根据相机内参向ROS发布camera

根据相机内参向ROS发布camera 需求

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

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 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

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

    vector D{0.000094, -0.011701, 0.000383, -0.000507, 0.000000};
    boost::array K = {
        404.005825, 0.000000, 335.580380,
        0.000000, 404.368809, 250.727020,
        0.000000, 0.000000, 1.000000  
    };
    
     boost::array 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 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("/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} )