雷达相机标定3
0 环境
标定方案来自：cam_lidar_camlibration
雷达：雷神32线激光雷达
相机：海康球机
1 问题发现
在标定相机雷达的过程中，使用cam_lidar_camlibration
使用中发现经常出现平面寻找错误的情况。
1,2图为错误情况，3图为原博客的正常情况。4图为修改代码后的正常拟合情况
2 错误分析
从显示情况上可以看出，平面拟合是正确的，每条ring的左右点也找正确了，在正常情况下应该有四种颜色的点，通过对这四个点做直线拟合找到板子边沿四条边。但是这里颜色且分布不合理，判断应该是选择四条边的点除了问题,导致直线拟合出错。
/*-----------------------------找出 左上 左下 右上 右下的四个点云,进行拟合矩形的四个边-------------------------------*/
pcl::PointCloud::Ptr left_up_points(new pcl::PointCloud);
pcl::PointCloud::Ptr left_down_points(new pcl::PointCloud);
pcl::PointCloud::Ptr right_up_points(new pcl::PointCloud);
pcl::PointCloud::Ptr right_down_points(new pcl::PointCloud);
for (int m = 0; m < min_points->size(); ++m)
{ //根据center_ring分配每条ring最小点和最大点到四条边中
ROS_INFO_STREAM("center_ring is " << center_ring);
ROS_INFO_STREAM("min_points->points[m].ring is " << int(min_points->points[m].ring));
if (int(min_points->points[m].ring) < center_ring)
{
left_down_points->push_back(max_points->points[m]);
right_down_points->push_back(min_points->points[m]);
}
else
{
left_up_points->push_back(max_points->points[m]);
right_up_points->push_back(min_points->points[m]);
}
} 仔细阅读原作者博客和代码发现，根据这个方法的要求，标定板最好是以对角线水平放置，因为其是找到这个拟合平面的最长ring作为中心ring，以中心ring为分界找到左上右上左下右下四个边。按顺序打出平面每条ring的序号和长度，发现并不是我们想象的中间最长两边最短。
打印出四条边的拟合点数，可以看出两条边完全不正常 ，只根据两三个点拟合。
打印出每个ring的z轴的值也能发现，这个ring的排列和Z轴没有比例关系。
在原代码中也有着下面一句注释,velodyne的ring顺序是从0-31，所以每个ring的距离应该是从小到大再到小。
这里我们使用的是雷神的 32线激光雷达，根据上面的分析基本可以确定是ring的顺序不对导致不能找到中心的ring，使得四条边分错。
3 修改代码
整体思路：
/// POINT CLOUD FEATURES //
①根据ROI滤波后一个点（往往是最高点）的z，和板的长度确定Z的范围选定平面点云
//找Z的范围
//过滤出点云（通过最高点减去板子的长度可以滤除地面，防止地面被拟合成平面）
//在过滤出的点云中拟合平面
//投影点云内点到平面上
②创建一个点云容器，以一个ring的点云为单位，这个容器就是点云平面（注意：这里雷神的ring和我们想的不一样所以要重新根据z对ring进行排列）
③对每个ring进行操作,找出每个ring的最左边，并根据每个ring的最左边的z从新进行ring排序，使得这个ring和z是正比例关系
//找出 每个ring的最小值，根据这个点Z轴的值进行排序
//，map对上面ring的顺序按照Z自动进行排序后,迭代器进行迭代赋值给candidate_segments2(保证一个干净的点云)，如果直接用candidate_segments，之前的ring还有数据
③对排序好的candidate_segments2的点云容器，找出每个ring最左边和最右边的点并计算每个ring的距离，最大的默认为中间ring（这要标定板45度放置），并以此对每个ring最左边和最右边的点确定左上左下右上右下封装成点云。
//找出每个ring的最大点和最小点
//计算ring的距离
//用迭代器查看数据是否排序好
④找出 左上 左下 右上 右下的四个点云,进行拟合矩形的四个边根据center_ring分配每条ring最小点和最大点到四条边中核心：
①对拟合好平面之后的点云容器重新按照每个ring的最左边Z的值进行排序，找出对应关系
②新建一个点云容器，用map迭代器，逐一将原平面点云的每条ring对应点云安装前面对应关系赋给新的点云
效果：
下面是修改后点云特征提取的代码：
..............
/// POINT CLOUD FEATURES //
pcl::PointCloud::Ptr cloud(new pcl::PointCloud),
cloud_filtered(new pcl::PointCloud),
cloud_passthrough(new pcl::PointCloud),
corrected_plane(new pcl::PointCloud);
sensor_msgs::PointCloud2 cloud_final;
pcl::fromROSMsg(*pc, *cloud);
// Filter out the experimental region
pcl::PassThrough pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(bound.x_min, bound.x_max);
pass.filter(*cloud_passthrough);
pcl::PassThrough pass_z;
pass_z.setInputCloud(cloud_passthrough);
pass_z.setFilterFieldName("z");
pass_z.setFilterLimits(bound.z_min, bound.z_max);
pass_z.filter(*cloud_passthrough);
pcl::PassThrough pass_final;
pass_final.setInputCloud(cloud_passthrough);
pass_final.setFilterFieldName("y");
pass_final.setFilterLimits(bound.y_min, bound.y_max);
pass_final.filter(*cloud_passthrough);
// Filter out the board point cloud
// find the point with max height(z val) in cloud_passthrough
double z_max = cloud_passthrough->points[0].z;
size_t pt_index;
for (size_t i = 0; i < cloud_passthrough->points.size(); ++i) {
if (cloud_passthrough->points[i].z > z_max) {
pt_index = i;
z_max = cloud_passthrough->points[i].z;
}
}
/*-------------------------根据滤波后一个点（往往是最高点）的z，和板的长度确定Z的范围选定平面点云-------------------------------------------*/
// subtract by approximate diagonal length (in metres)
//找Z的范围
ROS_INFO_STREAM("z max is: " << z_max);
ROS_INFO_STREAM("diagonal max is: " << diagonal);
double z_min = z_max - diagonal;
ROS_INFO_STREAM("z min is: " << z_min);
//过滤出点云（通过最高点减去板子的长度可以滤除地面，防止地面被拟合成平面）
pass_z.setInputCloud(cloud_passthrough);
pass_z.setFilterFieldName("z");
pass_z.setFilterLimits(z_min, z_max);
pass_z.filter(*cloud_filtered); // board point cloud
// pcl::toROSMsg(*cloud_filtered, debug_pc_msg);
// debug_pc_pub.publish(debug_pc_msg);
//在过滤出的点云中拟合平面
// Fit a plane through the board point cloud
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
int i = 0, nr_points = static_cast(cloud_filtered->points.size());
pcl::SACSegmentation seg;
seg.setOptimizeCoefficients(true);
// 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云
seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型 定义为平面模型
seg.setMethodType(pcl::SAC_RANSAC); //设置随机采样一致性方法类型
seg.setMaxIterations(1000);
seg.setDistanceThreshold(plane_dist_threshold_); 设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件 //表示点到估计模型的距离最大值，
pcl::ExtractIndices extract;
seg.setInputCloud(cloud_filtered);
//引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients
seg.segment(*inliers, *coefficients);
// Plane normal vector magnitude
float mag = sqrt(pow(coefficients->values[0], 2) + pow(coefficients->values[1], 2)
+ pow(coefficients->values[2], 2));
pcl::PointCloud::Ptr cloud_seg(new pcl::PointCloud);
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_seg);
//投影内点到平面上
// Project the inliers on the fit plane //投影
pcl::PointCloud::Ptr cloud_projected(new pcl::PointCloud);
pcl::ProjectInliers proj;
proj.setModelType(pcl::SACMODEL_PLANE); //平面模型
proj.setInputCloud(cloud_seg);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
// pcl::toROSMsg(*cloud_seg, debug_pc_msg);
// debug_pc_pub.publish(debug_pc_msg);
// Publish the projected inliers
//发布平面内点的点云话题
pcl::toROSMsg(*cloud_projected, cloud_final);
pub_cloud.publish(cloud_final); //发布点云的投影
// FIND THE MAX AND MIN POINTS IN EVERY RING CORRESPONDING TO THE BOARD
/*------------------------------创建一个点云容器，以一个ring的点云为单位，这个容器就是点云平面-------------------------------------------------*/
//（注意：这里雷神的ring和我们想的不一样所以要重新根据z对ring进行排列）
// First: Sort out the points in the point cloud according to their ring numbers
//容器（双端队列（点云）） ， 双端队列（点云）就是一行ring, deque双端队列可以高效的在头尾两端插入和删除元素
std::vector> candidate_segments(i_params.lidar_ring_count);
std::vector> candidate_segments2(i_params.lidar_ring_count);
//std::vector capture_rings;
// double x_projected = 0; double y_projected = 0; double z_projected = 0;
for (size_t i = 0; i < cloud_projected->points.size(); ++i) //给这个平面点云 归属到对于的ring
{
int ring_number = static_cast(cloud_projected->points[i].ring);//读每个点云的ring
// ROS_INFO_STREAM( " cloud_projected->points[i].ring = " <points[i].ring)<<" i = "<< i <<" Z = "<< cloud_projected->points[i].z);
//push back the points in a particular ring number
candidate_segments[ring_number].push_back(&(cloud_projected->points[i])); //放到
}
//找出点云平面的每个ring的最左边max_x和最右边min_x
// Second: Arrange points in every ring in descending order of y coordinate
pcl::PointXYZIR max, min;
pcl::PointCloud::Ptr max_points(new pcl::PointCloud);
pcl::PointCloud::Ptr min_points(new pcl::PointCloud);
double max_distance = -9999.0;
int center_ring = -1;
ROS_INFO_STREAM( " candidate_segments.size() : " << candidate_segments.size());
std::map Z_ring;//创建Z和ring的对应关系
std::map::iterator Z_ring_Iter;//创建Z和ring的对应关系
/*------------------对每个ring进行操作,找出每个ring的最左边，并根据每个ring的最左边的z从新进行ring排序，使得这个ring和z是正比例关系--------------------*/
// velodyne lidar ring order is : from button to top 0->31
// so the distance of ring end points pair will be small->large->small
//找出 每个ring的最小值和最大值
for (int i = 0; static_cast(i) < candidate_segments.size(); i++)
{
if (candidate_segments[i].size() == 0) // If no points belong to a aprticular ring number
{
continue;
}
double x_min = 9999.0;
int x_min_index;
for (int p = 0; p < candidate_segments[i].size(); p++) //找出每个ring的和最左边点
{
if (candidate_segments[i][p]->x < x_min)
{
x_min = candidate_segments[i][p]->x;
x_min_index = p;
}
}
pcl::PointXYZIR min_p = *candidate_segments[i][x_min_index];
Z_ring.insert(std::pair < float, int > (min_p.z,i));
}
//，map对上面ring的顺序按照Z自动进行排序后,迭代器进行迭代赋值给candidate_segments2(保证一个干净的点云)，如果直接用candidate_segments，之前的ring还有数据
int ring_correct=0;
for (Z_ring_Iter = Z_ring.begin(); Z_ring_Iter != Z_ring.end();Z_ring_Iter++)
{
// ROS_INFO_STREAM(" Z_ring->first " << Z_ring_Iter->first << " Z_ring->second " << Z_ring_Iter->second);
for (int p = 0; p < candidate_segments[Z_ring_Iter->second].size(); p++) // 修改每个点的ring
{
(*candidate_segments[Z_ring_Iter->second][p]).ring = ring_correct;
candidate_segments2[ring_correct].push_back(&(*candidate_segments[Z_ring_Iter->second][p]));
}
ring_correct++;
}
Z_ring.clear();//清除迭代器，后面还需要用这个迭代器查看顺序
/*-----------------------------------对排序好的candidate_segments2的点云容器，找出每个ring最左边和最右边的点
计算每个ring的距离，最大的默认为中间ring（这要标定板45度放置），并以此对每个ring最左边和最右边的点确定左上左下右上右下封装成点云---------------*/
for (int i = 0; static_cast(i) < candidate_segments2.size(); i++)
{
if (candidate_segments2[i].size() == 0) // If no points belong to a aprticular ring number
{
continue;
}
double x_min = 9999.0;
double x_max = -9999.0;
int x_min_index, x_max_index;
for (int p = 0; p < candidate_segments2[i].size(); p++) //找出每个ring的最大点和最小点
{
if (candidate_segments2[i][p]->x > x_max)
{
x_max = candidate_segments2[i][p]->x;
x_max_index = p;
}
if (candidate_segments2[i][p]->x < x_min)
{
x_min = candidate_segments2[i][p]->x;
x_min_index = p;
}
}
pcl::PointXYZIR min_p = *candidate_segments2[i][x_min_index];
pcl::PointXYZIR max_p = *candidate_segments2[i][x_max_index];
double distance = pcl::euclideanDistance(min_p, max_p); //计算ring的距离
if (distance < 0.001){
continue;
}
//选出最大ring的距离 作为中心ring
if(distance > max_distance)
{
max_distance = distance;
center_ring = min_p.ring;
// ROS_INFO_STREAM("max_distance " << max_distance << " center_ring: " << center_ring);
}
ROS_INFO_STREAM("ring number: " << i << " distance: " << distance);
// velodyne lidar ring order is : from button to top 0->31
// so the distance of ring end points pair will be small->large->small
min_points->push_back(min_p);
max_points->push_back(max_p);
ROS_INFO_STREAM("min_p " << min_p.z);
Z_ring.insert(std::pair < float, int > (min_p.z,i));
}
//用迭代器查看数据是否排序好
for (Z_ring_Iter = Z_ring.begin(); Z_ring_Iter != Z_ring.end();Z_ring_Iter++)
{
ROS_INFO_STREAM(" Z_ring->first " << Z_ring_Iter->first << " Z_ring->second " << Z_ring_Iter->second);
}
/*-----------------------------找出 左上 左下 右上 右下的四个点云,进行拟合矩形的四个边-------------------------------*/
......
...... 使用过程中会出现如下错误：
检查后发现是平面没找到。这里查实是opencv没有检测到角点报的错误。
还要注意要调节ROI的Z值使得刚好落在标定板的最高点，否者算法会把点云的其他杂点当成标定板的最高点，然后减去标定板的板长，这时候这个点云的选择就会错误。