利用ros进行双目相机标定（发布双目相机话题再用cameracalibrator.py文件进行标定）

1、创建工作环境

mkdir -p opencv_test/src
cd opencv_test/src/
catkin_create_pkg stereo_camera std_msgs roscpp rospy  

 2、修改CMakeLists.txt（cmakelist里要包含到opencv和ros的包）

3、再写一个打开双目相机并把获取到的图像进行发布并保存

#include <ros/ros.h>
#include "iostream"
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <algorithm>
#include <fstream>
#include <iomanip>
#include <chrono>
#include "camera_info_manager/camera_info_manager.h"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub_L = it.advertise("camera/left/image_raw", 1);
  image_transport::Publisher pub_R = it.advertise("camera/right/image_raw", 1);
  cv::Mat imLeft, imRight, imLeftRect, imRightRect;
  // USB双目相机初始化与启动
  int width=2560;
  int height=960;
  int fps=60;

  cv::VideoCapture cap(2);
  cap.set(cv::CAP_PROP_FRAME_WIDTH,width);//分辨率
  cap.set(cv::CAP_PROP_FRAME_HEIGHT,height);
  cap.set(cv::CAP_PROP_FPS,fps);//帧率
  cap.set(cv::CAP_PROP_FOURCC,cv::VideoWriter::fourcc('Y','U','Y','2'));
  float current_pose_q[7]={0};
  char key=' ';
  cv::Mat stereo_img;
  std::cout<<"开始发布图像数据..."<<std::endl;
	std::cout<<"分辨率:"<<width*0.5<<"*"<<height<<std::endl;	 
	std::cout<<"帧率:"<<fps<<"fps"<<std::endl;	

  while(nh.ok())
  {
    cap >> stereo_img;//获取当前帧图像
    imLeft=stereo_img.colRange(0,width/2).clone();
    imRight=stereo_img.colRange(width/2,width).clone();
	  cv::cvtColor(imLeft,imLeft,cv::COLOR_BGR2GRAY);
	  cv::cvtColor(imRight,imRight,cv::COLOR_BGR2GRAY);

    sensor_msgs::ImagePtr msg_L = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
    sensor_msgs::ImagePtr msg_R = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();

    ros::Rate loop_rate(60);
    pub_L.publish(msg_L);
    pub_R.publish(msg_R);
    loop_rate.sleep();
    }
}

4、开始编译(已经编译就开始下一步)

catkin_make

5、开始发布(rosrun前要再一个终端roscore)

cd opencv_test/
source ./devel/setup.bash 
rosrun stereo_camera my_publisher_usb_960 

再开一个终端

roscore

再开一个终端（查看发布的左右目相机话题）

rviz

 6、运行标定程序（程序中跑动的是包中的标定的py程序）  注：size为网格数，square为网网格边长，right/left都为发布的图像话题（在rviz或rostopic  list中查看话题）

rosrun camera_calibration cameracalibrator.py --size 9x6 --square 0.025 --approximate=0.01 right:=/camera/right/image_raw left:=/camera/left/image_raw 

 打开标定界面。