lidar_camera_calib学习笔记（激光雷达相机标定）

参考项目链接：https://gitee.com/manifold/lidar_camera_calib/tree/master

• 项目提供的bag中的信息
  
• pointgrey.yaml 文件内容

%YAML:1.0
K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [1061.37439737547, 0, 980.706836288949,0, 1061.02435228316, 601.685030610243,0, 0, 1]
d: !!opencv-matrix
   rows: 5
   cols: 1
   dt: d
   data: [-0.149007007770170, 0.0729485326193990, 0.000257753168848673, -0.000207183134328829, 0]

Camera.width: 1920
Camera.height: 1200

grid_length: 0.15
corner_in_x: 7
corner_in_y: 5

• launch文件配置

<?xml version="1.0"?>
<launch>

  <node pkg="ilcc2" type="get_image_corners_bag" name="get_image_corners_bag" output="screen">

    <param name="bag_path_prefix"  value= "/home/stone/Desktop/rosws/src/bag/2018-12-03-" />
    <param name="bag_num"  value= "6" />
    <param name="camera_name"  value= "pointgrey" />
    <param name="image_topic"  value= "/camera/image_raw " />
    <param name="yaml_path"  value= "pointgrey.yaml" />
    <param name="save_image_flag"  value= "true" />

  </node>
</launch>

• 运行指令

roslaunch ilcc2 image_corners.launch

• 得到的结果（去畸变后的图片）
  
• 对应的代码

#include 
#include 
#include 

/// read rosbag
#include 
#include 
#include 
#define foreach BOOST_FOREACH

#include         /// sensor_msgs::Image
#include 
#include 
#include 


#include "ilcc2/ImageCornersEst.h"
//cv_bridge用于ROS图像和OpenCV图像的转换
cv::Mat get_image_from_msg(const sensor_msgs::ImageConstPtr& msg_img)
{
  //Create cv_brigde instance
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    cv_ptr=cv_bridge::toCvCopy(msg_img, sensor_msgs::image_encodings::MONO8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Not able to convert sensor_msgs::Image to OpenCV::Mat format %s", e.what());
    return cv::Mat();
  }
  // sensor_msgs::Image to OpenCV Mat structure
  cv::Mat Image = cv_ptr->image;
  return Image;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_corners");
    ros::NodeHandle nh;
    //显示出来ilcc2 package的路径
    std::string package_path = ros::package::getPath("ilcc2");
    ROS_INFO("ilcc2 package at %s", package_path.c_str());
	
    int bag_num;
    bool save_image_flag;
    std::string bag_path_prefix, image_topic, yaml_path, camera_name;
    ros::NodeHandle nh_private("~");
    //nh_private：一般用来参数配置。在launch文件中param标签会把参数解释为/node/param_name,相当于（~param_name）
    nh_private.param<std::string>("bag_path_prefix", bag_path_prefix, "20181101_");  //bag的前缀
    nh_private.param<int>("bag_num", bag_num, 1);                                    //bag的个数
    nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");               //参数文件
    nh_private.param<std::string>("image_topic", image_topic, "/front");             // 对应的topic
    nh_private.param<std::string>("camera_name", camera_name, "front");             //输出的文件前缀
    nh_private.param<bool>("save_image_flag", save_image_flag, "true");

    std::cout << "bag_num: " << bag_num << std::endl;
    std::cout << package_path << "/config/" << yaml_path << std::endl;

    ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);
    image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) );


    for(int bag_idx = 1; bag_idx <= bag_num; bag_idx++)
    {
        std::cout << bag_idx << "============================================================\n";
        std::string bag_path = bag_path_prefix+std::to_string(bag_idx)+".bag";
        ROS_INFO("bag_path: %s", bag_path.c_str());

        rosbag::Bag bag_read;
        bag_read.open(bag_path, rosbag::bagmode::Read);
        std::vector<std::string> topics;
        topics.push_back(image_topic);

        rosbag::View view(bag_read, rosbag::TopicQuery(topics));
        std::cout << "view: " << view.size() << std::endl;

        sensor_msgs::ImageConstPtr msg_image_last = NULL;
        foreach(rosbag::MessageInstance const m, view)
        {
            sensor_msgs::ImageConstPtr msg_image = m.instantiate<sensor_msgs::Image>();
            if (msg_image != NULL){
              msg_image_last = msg_image;
            }

            if(msg_image_last != NULL)
              break;
        }
        bag_read.close();

        if(msg_image_last == NULL)
        {
          ROS_WARN("can't read image topic");
          continue;
        }


        ROS_INFO("findCorners");
        cv::Mat image = get_image_from_msg(msg_image_last);

        cv::Mat rectifyImage;
        image_corners_est->undistort_image(image, rectifyImage);

        //image_corners_est->findCorners(image);
        printf(package_path.c_str());
        if(save_image_flag)
          cv::imwrite(package_path+"/process_data/"+camera_name+std::to_string(bag_idx)+".jpg", rectifyImage);

    }


    return 0;
}

图像提取标定板角点libcbdetect

激光提取标定板角点ilcc2

• 运行程序

roslaunch ilcc2 lidar_corners.launch

• 程序会弹出两个pcl_viewer,分别是visual_corners与visual_chessboard

• 打开rviz ，更改Fixed Frame 并添加PointCloud2 （把topic改为volodyne_points）
  

• 选中rviz的Publish Point功能,在标定板点云的中间点一个点.之后在visual_chessboard界面会显示提取结果,提取结果用粉色点云表示.
  

• 若提取正确,按o键后,visual_corners界面会显示角点提取结果

• 一共有两张标定板的图,一张是在激光原点,一张是在原始位置.其中在原始位置的标定板有粉色点,也就是激光角点.

• 最终正确的结果如下图所示.粉色角点提取正确.按k键确认.
  

• 循环操作完成其他的bag

标定激光与相机外参

• 运行程序

roslaunch ilcc2 calib_lidar_cam.launch

• 若初始外参设置正确,在lidar_camera_corners窗口中可以看到绿色相机角点与红色激光角点.该主要是为了确保lidar角点与相机角点的对应关系一致,只要红色点和绿色的排序方式一样即可(一般不会出问题,按任意键跳过即可).
  

• 标定好之后,会显示重投影结果(如上图所示).最终标定的结果会在终端输出,也会保存为bin文件存在config文件夹下.

激光投影回相机效果

roslaunch ilcc2 pcd2image.launch

rosbag play xxx.bag -l

• 对应代码

#include 
#include 
#include 
#include         /// ros::package::getPath

#include 

#include 
#include 
#include 

#include 
#include 
#include 

#include 
#include 
#include 


#include 
#include           /// pcl::transformPointCloud

#include "ilcc2/ImageCornersEst.h"

using namespace std;
using namespace message_filters;

ImageCornersEst::Ptr image_corners_est(new ImageCornersEst);

double distance_valid;


// void cv::undistort	
// (	InputArray 	src,                        // 原始图像
//         OutputArray 	dst,                        // 矫正图像
//         InputArray 	cameraMatrix,               // 原相机内参矩阵
//         InputArray 	distCoeffs,                 // 相机畸变参数
//         InputArray 	newCameraMatrix = noArray() // 新相机内参矩阵
// )	

void processData(cv::Mat image, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)  
{

  cv::Mat rectifyImage;       //矫正的图像
  cv::undistort(image, rectifyImage, image_corners_est->camK, image_corners_est->distort_param, image_corners_est->camK);

  pcl::PointXYZI ptmp;

  /// 生成每个点的颜色
  double inten_low=255, inten_high=0;    
  std::vector<double> datas;            
  for(unsigned int index=0; index<cloud->size(); index++)  //找深度的最大值和最小值
  {
      ptmp = cloud->points[index];
      if(inten_low > ptmp.intensity)
          inten_low = ptmp.intensity;
      if(inten_high < ptmp.intensity)
          inten_high = ptmp.intensity;
      datas.push_back(ptmp.intensity);
  }

  inten_low = 0;
  inten_high = 60;


//  std::cout<<"start project "<< cloud->size() << " ,  ";
  int counter = 0;
  for(unsigned int index=0; index<cloud->size(); index++)
  {

    ptmp = cloud->points[index];
    Eigen::Vector3d Pw(ptmp.x, ptmp.y, ptmp.z);
    Eigen::Vector2d Pcam;
       //转换平面
    if ( image_corners_est->spaceToPlane(Pw, Pcam, distance_valid) ) {

      int x = Pcam[0];
      int y = Pcam[1];

      unsigned char r, g, b;

      double h = (ptmp.intensity-inten_low)/(inten_high-inten_low)*255;  //对深度进行归一化

      image_corners_est->HSVtoRGB(h, 100, 100, &r, &g, &b);

      cv::circle(rectifyImage, cv::Point2d(x,y), 0.6, cv::Scalar(r,g,b), 2);
//      cv::circle(rectifyImage, cv::Point2d(x,y), 0.8, cv::Scalar(r,g,b), 0.8);
//      image.ptr(y)[x*3] = 255;
//      image.ptr(y)[x*3+1] = 0;
//      image.ptr(y)[x*3+2] = 0;

      counter++;
    }
  }
//  std::cout << counter << " points ok\n";
  cv::resize(rectifyImage, rectifyImage, cv::Size(rectifyImage.cols/1, rectifyImage.rows/1));
  cv::imshow("img_liar_point", rectifyImage);  //矫正后的图像
  cv::waitKey(5);
}


void callback_LidarCam(const sensor_msgs::PointCloud2ConstPtr& msg_pc,
          const sensor_msgs::ImageConstPtr& msg_img)
{
//  ROS_INFO_STREAM("Velodyne scan received at " << msg_pc->header.stamp.toSec());
//  ROS_INFO_STREAM("image received at " << msg_img->header.stamp.toSec());
  pcl::PointCloud<pcl::PointXYZI> input_cloud;
  pcl::fromROSMsg(*msg_pc, input_cloud);                       //转换为模板点云fromROSMsg
  cv::Mat img = cv_bridge::toCvCopy(msg_img,"bgr8")->image;    //image从ros转为opencv常用格式
  processData(img, input_cloud.makeShared());
}



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

  ros::init(argc,argv,"pcd2image");
  ros::NodeHandle nh;
  std::string package_path = ros::package::getPath("ilcc2");
  ROS_INFO("ilcc2 package at %s", package_path.c_str());


  std::string yaml_path, image_topic, lidar_topic;
  std::string extrinsic_file;
  ros::NodeHandle nh_private("~");

  nh_private.param<double>("distance_valid", distance_valid, 5);
  nh_private.param<std::string>("extrinsic_file", extrinsic_file, "/process_data/pose.bin");
  nh_private.param<std::string>("yaml_path", yaml_path, "30w.yaml");
  nh_private.param<std::string>("image_topic", image_topic, "/front");
  nh_private.param<std::string>("lidar_topic", lidar_topic, "/velodyne_points");


  std::string extrinsic_path = package_path + extrinsic_file;             //外参路径
  ROS_INFO("load T_lidar2cam from %s", extrinsic_path.c_str());             

  image_corners_est = ImageCornersEst::Ptr(new ImageCornersEst(package_path+"/config/"+yaml_path, true) ); //内参路径
  image_corners_est->txt2extrinsic( extrinsic_path );

  message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, lidar_topic, 2);      //点云接收
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, image_topic, 2);            // 图像接收

  typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;   //时间戳同步
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(2), cloud_sub, image_sub);
  sync.registerCallback(boost::bind(&callback_LidarCam, _1, _2));                                  //回调函数
  ROS_INFO("waiting for lidar image topic %s %s", lidar_topic.c_str(), image_topic.c_str());       


  while (ros::ok())
  {
    ros::spin();
  }
}