根据相机内参向ROS发布camera_info

最新推荐文章于 2025-05-28 18:20:39 发布

橘子皮一下

最新推荐文章于 2025-05-28 18:20:39 发布

阅读量5.4k

点赞数 7

CC 4.0 BY-SA版权

分类专栏：解决方法文章标签：自动驾驶 c++ 计算机视觉

本文链接：https://blog.youkuaiyun.com/weixin_43807148/article/details/122277420

解决方法专栏收录该内容

15 篇文章

订阅专栏

该博客介绍了如何将相机内参，包括distortion_coefficients、camera_matrix、projection_matrix和rectification_matrix等，从标定结果转换为ROS中的CameraInfo消息，并通过C++代码发布到话题`/camera/camera_info`，供其他节点使用。代码展示了如何构建并发布CameraInfo消息，确保了相机参数的有效传播。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

需求

使用相机内参标定算法完成相机标定之后得到了一个相机内参文件，主要包括以下内容：

image_width: 640
image_height: 480
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [409.36132,   0.     , 326.28708,
           0.     , 408.53318, 250.17083,
           0.     ,   0.     ,   1.     ]
camera_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.003776, -0.016523, 0.000289, -0.004807, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1., 0., 0.,
         0., 1., 0.,
         0., 0., 1.]
projection_matrix:
  rows: 3
  cols: 4
  data: [407.81525,   0.     , 322.70656,   0.     ,
           0.     , 409.13113, 250.40422,   0.     ,
           0.     ,   0.     ,   1.     ,   0.     ]

这些就是相机的内参。现在需要把这些信息发布到ROS中，以供其他程序可以调用。

C++源码

使用了直接赋值的方式，distortion_coefficients就是D，camera_matrix就是K，projection_matrix就是P，rectification_matrix就是R，当然其他参数也是相互对应的。


#include <ros/ros.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <math.h>

using namespace std;
using namespace cv;

sensor_msgs::CameraInfo getCameraInfo(void){        // extract cameraInfo.
    sensor_msgs::CameraInfo cam;

    vector<double> D{0.000094, -0.011701, 0.000383, -0.000507, 0.000000};
    boost::array<double, 9> K = {
        404.005825, 0.000000, 335.580380,
        0.000000, 404.368809, 250.727020,
        0.000000, 0.000000, 1.000000  
    };
    
     boost::array<double, 12> P = {
        402.124725, 0.000000, 335.482488, 0.000000,
        0.000000, 403.765045, 250.954855, 0.000000,
        0.000000, 0.000000, 1.000000, 0.000000
    };
    boost::array<double, 9> r = {1, 0, 0, 0, 1, 0, 0, 0, 1};

    cam.height = 480;
    cam.width = 640;
    cam.distortion_model = "plumb_bob";
    cam.D = D;
    cam.K = K;
    cam.P = P;
    cam.R = r;
    cam.binning_x = 0;
    cam.binning_y = 0;
    cam.header.frame_id = "camera";  //frame_id为camera，也就是相机名字
    cam.header.stamp = ros::Time::now();
    cam.header.stamp.nsec = 0;
    return cam;
}

int main(int argc, char** argv) {

  ros::init(argc, argv, "camera_info");  //初始化了一个节点，名字为camera_info
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<sensor_msgs::CameraInfo>("/camera/camera_info", 1000);  
  sensor_msgs::CameraInfo camera_info_dyn;
  ros::Rate rate(1);  //点云更新频率0.5Hz

  while (ros::ok())
  {
      camera_info_dyn = getCameraInfo();
      pub.publish(camera_info_dyn); //发布出去
      rate.sleep();
  }
    ros::spin();
    return 0;
}

CMakeLists.txt

安装正常的catkin_make编译或者cmake编译即可

cmake_minimum_required(VERSION 3.0.2)
project(camera_info)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
)
find_package(OpenCV REQUIRED)


catkin_package(

)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_LIBRARIES}
)

add_executable(camera_info src/camera_info.cpp)
target_link_libraries(camera_info ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES} )