本文详细介绍了cam_lidar_calibration软件包在激光雷达和单目相机外参标定过程中的启动程序和主要代码，包括launch文件run_optimiser.launch和assess_results.launch的功能，以及feature_extraction_node.cpp和feature_extractor.cpp中的关键函数，如特征提取、重投影误差评估、棋盘格投影和边缘检测等。此外，还涉及点云滤波、平面拟合、边缘线识别等技术，用于确定标定板的几何信息和优化标定结果。
目录
1.1 run_optimiser.launch标定优化程序
1.2 assess_results.launch重投影误差评估程序
2.1 feature_extraction_node.cpp文件
2.2.1 FeatureExtractor::callback_camerainfo函数
2.2.4 FeatureExtractor::optimise函数
2.2.5 FeatureExtractor::publishBoardPointCloud函数
2.2.7 FeatureExtractor::visualiseSamples函数
2.2.8 FeatureExtractor::passthrough函数
2.2.9 FeatureExtractor::chessboardProjection函数
2.2.10 FeatureExtractor::locateChessboard函数
2.2.11 FeatureExtractor::extractBoard函数
2.2.12 FeatureExtractor::findEdges函数
2.2.14 FeatureExtractor::distoffset_passthrough 函数
2.2.15 FeatureExtractor::extractRegionOfInterest函数
2.2.16 FeatureExtractor::find_octant函数
2.2.17 FeatureExtractor::getDateTime函数
使用cam_lidar_calibration标定速腾激光雷达和单目相机外参（可见一班的博客）实现了功能，但是对于它内部的机理不是很懂。于是对采样、优化部分的代码进行了仔细阅读，同时把自己的自己的一些理解和笔记分享在这里，供大家一起探讨、进步！
一、launch启动程序
1.1 run_optimiser.launch标定优化程序
主要内容：
- 传入参数import_samples决定执行示例/自己的参数
- 执行feature_extraction_node.cpp
- num_lowestvoq
- 定义distance_offset_mm = 0，用于修正lidar内参
<?xml version="1.0" encoding="utf-8"?>
<!-- roslaunch cam_lidar_calibration run_optimiser.launch import_samples:=false -->
<launch>
<!-- 读取params里的参数 -->
<rosparam file="$(find cam_lidar_calibration)/cfg/params.yaml" />
<!-- 执行的是feature_extraction_node.cpp -->
<node pkg="cam_lidar_calibration" type="feature_extraction_node" name="feature_extraction" output="screen">
<param name="num_lowestvoq" type="int" value="50" />
<param name="import_samples" value="$(arg import_samples)"/>
<param name="import_path" value="$(find cam_lidar_calibration)/data/vlp/poses.csv"/>
<!-- If your lidar is not calibrated well interally, it may require a distance offset (millimetres) on each point -->
<param name="distance_offset_mm" value="0" />
</node>
<!-- Only open rviz and rqt if not importing samples -->
<!-- 运行示例时，输入参数为import_samples:=false，不执行以下内容，不打开rviz和rqt_reconfigure -->
<group unless="$(arg import_samples)">
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find cam_lidar_calibration)/rviz/cam_lidar_calibration.rviz" />
<node type="rqt_reconfigure" name="rqt_reconfigure" pkg="rqt_reconfigure"/>
</group>
</launch>1.2 assess_results.launch重投影误差评估程序
既使用camera_info，又在yaml中配置，为什么？因为要二次检验相机参数！
可以切换弧度和角度的表示。
- 执行visualise_results.py
- trans_binwidth
- rot_binwidth_deg
- visualise_pose_num
<!-- degree: true to display plot in degrees, false for radians -->
<param name="degree" value="false"/>二、主要代码
首先，根据标定优化程序展开学习，也就是开源程序的第一部分。
2.1 feature_extraction_node.cpp文件
主函数，包含
- 定义类，feature_extractor特征提取，调用feature_extractor.cpp里函数的功能
- 参数优化 SimpleActionServer<cam_lidar_calibration::RunOptimiseAction> optimise_action，通过actionlib库实现，ROS的一种通讯机制action通信机制
简介：Action通信简介及案例1：发送单导航目标点_action 通信
2.2 feature_extractor.cpp文件
一系列函数名和命名空间定义在头文件feature_extractor.h里，命名空间为cam_lidar_calibration，含为类FeatureExtractor，具体包括以下函数：
2.2.1 FeatureExtractor::callback_camerainfo函数
功能：将相机参数从“camera_info”话题里读进来，包括内参和畸变系数。
2.2.2 serviceCB函数
与通讯机制action有关的函数
传入：Optimise::Request类型的req和Optimise::Response类型的res。
2.2.3 compare_voq函数
这个很简单，比较标定质量指标voq大小的。使用的数据格式是SetAssess，自定义类型。
2.2.4 FeatureExtractor::optimise函数
这个是cam_lidar_calibration标定程序包的核心思路。这里暂时只进行思路梳理，后面再仔细研究一下。总的目标是根据提取的特征，计算VOQ指标，对标定位姿进行优化。
功能1：根据启动程序时import_samples的值读取或写入参数，进行优化求解。import_samples为true时从文件里读取，也就是调用作者数据集的时候。import_samples为false时，往文件里写入。读取流程：按行读取数据，装入point3D向量row，写入时与此相反：
- 把read_samples里的数据读入到line
- 用逗号将line分割为word
- 把word装入向量line_double，一个一个push_back到后面
- 把{line_double[0], line_double[1], line_double[2]}装入row
包括相机中心、法向量；雷达中心、法向量；宽度、高度、偏移距离等等，19个为一组，。
功能2：从N个数据中构建3个为一组（set）的求解对象，如果少于100个样本，我们可以得到所有的100C3，再多一些样本，NC3就会变得太大，所以为了速度我们只能随机抽样。
2.2.5 FeatureExtractor::publishBoardPointCloud函数
发布激光点云采样的点，就是标定板上的点。
2.2.6 boundsCB函数
传入：boundsConfig类型的引用变量config和一个uint32_t类型的变量level
返回：void
功能：由cam_lidar_calibration::boundsConfig支持，重新读取gui中滑动条运动对应的值
2.2.7 FeatureExtractor::visualiseSamples函数
功能：可视化函数，显示提取激光点云提取到的标定板，绿色方框表述位置（单位为m），蓝色箭头表示法向量，赋值marker后进行发布。
2.2.8 FeatureExtractor::passthrough函数
功能：根据串口滑动条配置的区间，进行直通滤波。输入input_pc，按照xzy顺序进行滤波，定义了中间点云x、z，输出output_pc，不定义中间点云或者少定义中间点云可以节省内存。
2.2.9 FeatureExtractor::chessboardProjection函数
功能：提取图像中的棋盘格、标定板。标定板有四个角点和一个中心点；棋盘格有好多个内角点，一般加工方式下有白边。并且将世界坐标系下的物理坐标值，投影到图像坐标系下。
输入：角点向量corners和图像cv_ptr
输出：旋转矩阵、平移向量、板子的三维点（物理坐标，mm）
auto FeatureExtractor::chessboardProjection(const std::vector<cv::Point2d>& corners,
const cv_bridge::CvImagePtr& cv_ptr)
{
// Find the chessboard in 3D space - in it's own object frame (position is arbitrary, so we place it flat)
// 在3D空间中找到棋盘——在它自己的对象框架中(位置是任意的，所以我们把它放平)
// Location of board frame origin from the bottom left inner corner of the chessboard
//从棋盘左下角内角开始的棋盘框架原点位置
// 这段代码是用于生成棋盘格的三维坐标点的。首先，通过输入参数i_params中的棋盘格大小和方格长度计算出棋盘格左下角的坐标。然后，通过两个for循环遍历整个棋盘格，计算每个方格的三维坐标点，并将其存储在corners_3d向量中。具体地，对于每个方格，先计算出其在棋盘格中的二维坐标，然后将其乘以方格长度并减去棋盘格左下角的坐标，即可得到该方格的三维坐标点。最终，corners_3d向量中存储的就是整个棋盘格的所有三维坐标点。
cv::Point3d chessboard_bleft_corner((i_params.chessboard_pattern_size.width - 1) * i_params.square_length / 2,
(i_params.chessboard_pattern_size.height - 1)*i_params.square_length/2, 0);
std::vector<cv::Point3d> corners_3d;
for (int y = 0; y < i_params.chessboard_pattern_size.height; y++)
{
for (int x = 0; x < i_params.chessboard_pattern_size.width; x++)
{
corners_3d.push_back(cv::Point3d(x, y, 0) * i_params.square_length - chessboard_bleft_corner);
}
}
// chessboard corners, middle square corners, board corners and centre
std::vector<cv::Point3d> board_corners_3d;
// Board corner coordinates from the centre of the chessboard
// 这段代码是用于计算棋盘角点在三维空间中的坐标。其中，board_corners_3d是一个存储棋盘角点坐标的向量。代码中使用了cv::Point3d来表示三维坐标点，其中x、y、z分别表示点在x、y、z轴上的坐标值。i_params是一个结构体，包含了棋盘的尺寸和平移误差等参数。代码中通过计算棋盘中心点到棋盘四个角点的距离，来确定棋盘角点在三维空间中的坐标。具体来说，代码中先计算出棋盘中心点到棋盘左上角点的距离，然后根据棋盘尺寸和平移误差计算出棋盘左上角点在三维空间中的坐标，接着按照同样的方式计算出棋盘的其他三个角点的坐标。最后将这些坐标点存储到board_corners_3d向量中。
board_corners_3d.push_back(cv::Point3d((i_params.board_dimensions.width - i_params.cb_translation_error.x)/2.0,
(i_params.board_dimensions.height - i_params.cb_translation_error.y)/2.0,0.0));
board_corners_3d.push_back(cv::Point3d(-(i_params.board_dimensions.width + i_params.cb_translation_error.x)/2.0,
(i_params.board_dimensions.height - i_params.cb_translation_error.y)/2.0,0.0));
board_corners_3d.push_back(cv::Point3d(-(i_params.board_dimensions.width + i_params.cb_translation_error.x)/2.0,
-(i_params.board_dimensions.height + i_params.cb_translation_error.y)/2.0,0.0));
board_corners_3d.push_back(cv::Point3d((i_params.board_dimensions.width - i_params.cb_translation_error.x)/2.0,
-(i_params.board_dimensions.height + i_params.cb_translation_error.y)/2.0,0.0));
// Board centre coordinates from the centre of the chessboard (due to incorrect placement of chessboard on board)从棋盘中心开始的棋盘中心坐标(由于棋盘格在板子上的位置不正确)
board_corners_3d.push_back(cv::Point3d(-i_params.cb_translation_error.x/2.0, -i_params.cb_translation_error.y/2.0, 0.0));
std::vector<cv::Point2d> inner_cbcorner_pixels, board_image_pixels;
cv::Mat rvec(3, 3, cv::DataType<double>::type); // Initialization for pinhole and fisheye cameras
cv::Mat tvec(3, 1, cv::DataType<double>::type);
最低0.47元/天 解锁文章
扫一扫
1288
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
