雷达相机标定3

0 环境

          标定方案来自:cam_lidar_camlibration  

          雷达:雷神32线激光雷达

          相机:海康球机

1 问题发现

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

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

雷达相机标定3_第1张图片雷达相机标定3_第2张图片

雷达相机标定3_第3张图片雷达相机标定3_第4张图片

        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的序号和长度,发现并不是我们想象的中间最长两边最短。

雷达相机标定3_第5张图片

打印出四条边的拟合点数,可以看出两条边完全不正常 ,只根据两三个点拟合。

打印出每个ring的z轴的值也能发现,这个ring的排列和Z轴没有比例关系。

雷达相机标定3_第6张图片

在原代码中也有着下面一句注释,velodyne的ring顺序是从0-31,所以每个ring的距离应该是从小到大再到小。

这里我们使用的是雷神的 32线激光雷达,根据上面的分析基本可以确定是ring的顺序不对导致不能找到中心的ring,使得四条边分错。

雷达相机标定3_第7张图片

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);
}


/*-----------------------------找出 左上 左下 右上 右下的四个点云,进行拟合矩形的四个边-------------------------------*/
......
......

使用过程中会出现如下错误:

雷达相机标定3_第8张图片

检查后发现是平面没找到。这里查实是opencv没有检测到角点报的错误。

雷达相机标定3_第9张图片

还要注意要调节ROI的Z值使得刚好落在标定板的最高点,否者算法会把点云的其他杂点当成标定板的最高点,然后减去标定板的板长,这时候这个点云的选择就会错误。

你可能感兴趣的:(标定工作,深度学习)