雷达相机标定3

0 环境

          标定方案来自：cam_lidar_camlibration  

          雷达：雷神32线激光雷达

          相机：海康球机

1 问题发现

在标定相机雷达的过程中，使用cam_lidar_camlibration

使用中发现经常出现平面寻找错误的情况。

        1,2图为错误情况，3图为原博客的正常情况。4图为修改代码后的正常拟合情况

2 错误分析

        从显示情况上可以看出，平面拟合是正确的，每条ring的左右点也找正确了，在正常情况下应该有四种颜色的点，通过对这四个点做直线拟合找到板子边沿四条边。但是这里颜色且分布不合理，判断应该是选择四条边的点除了问题,导致直线拟合出错。

/*-----------------------------找出 左上 左下 右上 右下的四个点云,进行拟合矩形的四个边-------------------------------*/
pcl::PointCloud<pcl::PointXYZIR>::Ptr left_up_points(new pcl::PointCloud<pcl::PointXYZIR>);
pcl::PointCloud<pcl::PointXYZIR>::Ptr left_down_points(new pcl::PointCloud<pcl::PointXYZIR>);
pcl::PointCloud<pcl::PointXYZIR>::Ptr right_up_points(new pcl::PointCloud<pcl::PointXYZIR>);
pcl::PointCloud<pcl::PointXYZIR>::Ptr right_down_points(new pcl::PointCloud<pcl::PointXYZIR>);

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<pcl::PointXYZIR>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZIR>),
        cloud_filtered(new pcl::PointCloud<pcl::PointXYZIR>),
        cloud_passthrough(new pcl::PointCloud<pcl::PointXYZIR>),
        corrected_plane(new pcl::PointCloud<pcl::PointXYZIR>);
sensor_msgs::PointCloud2 cloud_final;
pcl::fromROSMsg(*pc, *cloud);
// Filter out the experimental region
pcl::PassThrough<pcl::PointXYZIR> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(bound.x_min, bound.x_max);
pass.filter(*cloud_passthrough);
pcl::PassThrough<pcl::PointXYZIR> 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<pcl::PointXYZIR> 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<int>(cloud_filtered->points.size());
pcl::SACSegmentation<pcl::PointXYZIR> seg;
seg.setOptimizeCoefficients(true);

// 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云 
seg.setModelType(pcl::SACMODEL_PLANE);  //设置模型类型   定义为平面模型
seg.setMethodType(pcl::SAC_RANSAC);   //设置随机采样一致性方法类型
seg.setMaxIterations(1000); 
seg.setDistanceThreshold(plane_dist_threshold_); 设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件 //表示点到估计模型的距离最大值，
pcl::ExtractIndices<pcl::PointXYZIR> 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<pcl::PointXYZIR>::Ptr cloud_seg(new pcl::PointCloud<pcl::PointXYZIR>);
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_seg);

//投影内点到平面上
// Project the inliers on the fit plane //投影
pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZIR>);
pcl::ProjectInliers<pcl::PointXYZIR> 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<std::deque<pcl::PointXYZIR *>> candidate_segments(i_params.lidar_ring_count); 
std::vector<std::deque<pcl::PointXYZIR *>> candidate_segments2(i_params.lidar_ring_count); 
//std::vector<RingFeature> 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<int>(cloud_projected->points[i].ring);//读每个点云的ring
    // ROS_INFO_STREAM( " cloud_projected->points[i].ring = " <<int(cloud_projected->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<pcl::PointXYZIR>::Ptr max_points(new pcl::PointCloud<pcl::PointXYZIR>);
pcl::PointCloud<pcl::PointXYZIR>::Ptr min_points(new pcl::PointCloud<pcl::PointXYZIR>);

double max_distance = -9999.0;
int center_ring = -1;
ROS_INFO_STREAM( " candidate_segments.size() : " << candidate_segments.size());

std::map<float,int> Z_ring;//创建Z和ring的对应关系
std::map<float,int>::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<size_t>(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<size_t>(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值使得刚好落在标定板的最高点，否者算法会把点云的其他杂点当成标定板的最高点，然后减去标定板的板长，这时候这个点云的选择就会错误。