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

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

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

%YAML:1.0
K: !!opencv-matrixrows: 3cols: 3dt: ddata: [1061.37439737547, 0, 980.706836288949,0, 1061.02435228316, 601.685030610243,0, 0, 1]
d: !!opencv-matrixrows: 5cols: 1dt: ddata: [-0.149007007770170, 0.0729485326193990, 0.000257753168848673, -0.000207183134328829, 0]Camera.width: 1920
Camera.height: 1200grid_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 <ros/ros.h>
#include <ros/package.h>
#include <iostream>/// read rosbag
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH#include <sensor_msgs/Image.h>        /// sensor_msgs::Image
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui.hpp>#include "ilcc2/ImageCornersEst.h"
//cv_bridge用于ROS图像和OpenCV图像的转换
cv::Mat get_image_from_msg(const sensor_msgs::ImageConstPtr& msg_img)
{//Create cv_brigde instancecv_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 structurecv::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");             // 对应的topicnh_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 <fstream>
#include <iostream>
#include <math.h>
#include <ros/package.h>        /// ros::package::getPath#include <Eigen/Core>#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>          /// 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<uchar>(y)[x*3] = 255;
//      image.ptr<uchar>(y)[x*3+1] = 0;
//      image.ptr<uchar>(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);                       //转换为模板点云fromROSMsgcv::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();}
}

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

halcon相机标定助手_Halcon 学习笔记---单相机标定（2）
一.单项机标定原因降低畸变(相差) 测量二.相机标定求出什么该方程是求取世界坐标系与像素坐标系之间转换矩阵,本质就是求出相机的内外参数.其中dx和dy为每个像素在图像坐标系(UVO)沿U和V方向 ...
【相机标定与三维重建原理及实现】学习笔记1——相机模型数学推导详解
目录前言一.小孔成像模型二.坐标系的变换 1.世界坐标系到相机坐标系的变换(刚体变换)[xw^→xc^\boldsymbol {\hat{x_{w}}}\rightarrow \boldsymb ...
SLAM学习 | 单目相机标定（附代码实测可用）
SLAM学习 | 单目相机标定(附代码实测可用) 1 针孔相机模型 2 张正友标定法 3 VS2013下配置openCV 4 标定过程与结果 5 经验总结与注意事项概要: 这篇文章介绍如何解决做SL ...
三维重建学习(2)：相机标定基础
前言在相机标定过程中,我们会碰到一些概念,比如:摄像机模型.世界坐标系.图像坐标系等等.为便于理解推导,所以又整理了相关的笔记,介绍的都是些比较基础的概念,也比较容易. 相机模型针孔相机模型注: ...
Halcon学习笔记：Halcon标定步骤-3d_coordinates.hdev示例
Halcon标定步骤 1.设置相机内部参数的初始值 StartCamPar := [0.016,0,0.0000074,0.0000074,326,247,652,494] set_calib_dat ...
学习OpenCV3 面阵相机标定方法
使用OpenCV实现张正友法相机标定之前,有几个问题事先要确认一下,那就是相机为什么需要标定,标定需要的输入和输出分别是哪些? 相机标定的目的:获取摄像机的内参和外参矩阵(同时也会得到每一幅标定图像的 ...
MVG学习笔记(7) --自动标定和3D图形模型
文章目录自动标定已知在无穷远处的平面给定图像中的方形像素进行自动校准 3D图形模型视频增强自动标定在不了解相机标定的情况下,不可能比射影重建做得更好. 在任意数量的视图中的一组特征 ...
学习笔记 6 — 照相机标定
一.什么是照相机标定在图像测量过程以及机器视觉应用中,为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系,必须建立相机成像的几何模型,这些几何模型参数就是相机参数.在大多数条件下这 ...
学习笔记5--摄像头标定之内参标定
本系列博客包括6个专栏,分别为:<自动驾驶技术概览>.<自动驾驶汽车平台技术基础>.<自动驾驶汽车定位技术>.<自动驾驶汽车环境感知>.<自动驾驶 ...
[MATLAB学习笔记]view相机视角
view(az,el) 为当前坐标区设置相机视线的方位角和仰角. 使用向量更改视图创建一组 x.y 和 z 坐标,并使用它们绘制一个曲面.然后标记每个轴. [X,Y] = meshgrid(-5:. ...

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

图像提取标定板角点libcbdetect

激光提取标定板角点ilcc2

标定激光与相机外参

激光投影回相机效果

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

最新文章

热门文章