#每周一篇论文2#[感知] 激光雷达外参数自标定

激光雷达外参数自标定

基于激光雷达的车载三维重建系统和感知系统工作时需要对激光雷达的外参数(三个旋转参数和三个平移参数)进行标定。
激光雷达的外参数的标定是指求解激光雷达测量坐标系相对于其他传感器测量坐标系的相对变换关系，即旋转平移变换矩阵。
本方法进行激光雷达外参标定使用的方法是求解激光雷达的地平面与理想地平面的变换矩阵，方法可分为以下步骤：

• 点云滤波
• 设置ROI
• 使用RANSCAN算法进行地平面分割
• 提取激光雷达的地平面向量，计算激光雷达的地平面向量与理想地 平面向量[0,0,1] 的变换矩阵
• 对激光雷达外参标定进行系统评价
• 使用DBSCAN算法对标定参数进行聚类，计算最大类的标定参数的均值，最为最优标定标定输出。

点云滤波

一般离激光雷达距离越近地面数据越多，这里截取激光雷达前后左右50米的作为有效数据。

/**********************************************************************************************************
	功能: 滤波

	输入：points(原始数据)、roi_point(滤波后数据)；

	返回：...
**********************************************************************************************************/
void set_roi(PointCloudT &points, PointCloudT::Ptr roi_point)
{
    // 创建条件滤波器：通过x, y范围来过滤点云
    pcl::PassThrough<pcl::PointXYZI> pass;
    // false表示保持这些点在范围内
    pass.setFilterLimitsNegative(false);
    // 设置输入点云
    pass.setInputCloud(points.makeShared());
    // 设置滤波对象x
    pass.setFilterFieldName("x");
    //  设置范围
    pass.setFilterLimits(-25.0, 25.0);
    // 进行滤波，并将返回值赋值给points
    pass.filter(points);
    // 设置滤波对象y
    pass.setInputCloud(points.makeShared());
    pass.setFilterFieldName("y");
    pass.setFilterLimits(-25.0, 20.0);
    pass.filter(*roi_point);
}

设置ROI

对点云数据的z轴数据进行排序，截取Z/3点云数据作为感兴趣区域。(地面的z轴小)

/**********************************************************************************************************
    @brief Compare function to sort points. Here use z axis.
    @return z-axis accent
    // 用户排序的参数
**********************************************************************************************************/
bool point_cmp(PointT a, PointT b){
    return a.z<b.z;
}

void extract_initial_seeds_(const PointCloudT::Ptr p_sorted, const PointCloudT::Ptr g_seeds_pc)
{
    sort((*p_sorted).points.begin(),(*p_sorted).end(),point_cmp);
    g_seeds_pc->clear();
    for(int i=0;i<p_sorted->points.size()/4;i++)
    {
        g_seeds_pc->points.push_back(p_sorted->points[i]);
    }
   
}

地平面分割

使用ransacn数据对ROI数据进行最大平面检测。(滤波、ROI阶段、点云特性，当前最大平面基本为地平面)

/**********************************************************************************************************
	功能: 使用 ransac检测地面

	输入：cloud_filtered(ROI数据)、out_point_cloud(地平面数据)；

	返回：...
**********************************************************************************************************/
void ransac_ground_detect(PointCloudT &cloud_filtered, PointCloudT & out_point_cloud)
{
    // 模型的系数 
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    // 地面上各点的指数 the indices of the points in the ground
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    //使用SAC_RANSAC进行分割
    pcl::SACSegmentation<PointT> seg;
    //设置对估计模型优化
    seg.setOptimizeCoefficients (true);
    //if the step before is true the following config is obligatory
    //设置分割模型类型，检测平面
    seg.setModelType (pcl::SACMODEL_PLANE);
    //设置方法【聚类或随机样本一致性】
    seg.setMethodType (pcl::SAC_RANSAC);
    // 设置内点到模型的距离允许最大值
    seg.setDistanceThreshold (0.1);
    // 设置输入点云
    seg.setInputCloud (cloud_filtered.makeShared());
    //实现了对接地点指数和接地面系数的分割和保存
    seg.segment (*inliers, *coefficients);
    // 可以通过内点的个数判断地面分割是否正确
    pcl::copyPointCloud(cloud_filtered, inliers->indices, out_point_cloud);
}

计算变换矩阵

/**********************************************************************************************************
	功能: 进行平面拟合 得到法向量normal_和 th_dist_d_.计算法向量normal_与[0,0,1]向量的变换矩阵

	输入：g_ground_pc(最大平面数据)、roll(X轴)、pitch(Y轴)、yaw(Z轴)：欧拉角；
        (x, y, z):位移；

	返回：...
**********************************************************************************************************/
//  和 th_dist_d_. 更新拟合平面的A B C D
void estimate_plane_(PointCloudT::Ptr g_ground_pc, double &roll, double &pitch, double &yaw, double &offset_z)
{
    MatrixXf normal_;
    // Create covarian matrix in single pass.
    // TODO: compare the efficiency.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean; // 归一化坐标值
    // computeMeanAndCovarianceMatrix主要是PCA过程中计算平均值和协方差矩阵 ,对地面点(最小的n个点)进行计算协方差和平均值
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
    // Singular Value Decomposition: SVD
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2)); // 取最小的特征值对应的特征向量作为法向量
    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>(); // seeds_mean 地面点的平均值

    // according to normal.T*[x,y,z] = -d
    offset_z = -(normal_.transpose()*seeds_mean)(0,0); // 计算d   D=d
    // set distance threhold to `th_dist - d`
    //th_dist_d_ = th_dist_ - offset_z;// 这里只考虑在拟合的平面上方的点 小于这个范围的点当做地面
    
    // return the equation parameters
    
    Eigen::MatrixXf vectorAfter = MatrixXf::Zero(3, 1);
    vectorAfter << 0.0, 0.0,1.0;
    Eigen::Matrix4f rotationMatrix = rotation_matrix_from_vectors(normal_,vectorAfter);
    
    
    
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix<<   rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(0, 2),
                        rotationMatrix(1, 0), rotationMatrix(1, 1), rotationMatrix(1, 2),
                        rotationMatrix(2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2);

    // 2,1,0;
    Eigen::Vector3d eulerAngle = rotation_matrix.eulerAngles(0,1,2);
    yaw = eulerAngle(2);
    roll = eulerAngle(1);
    pitch = eulerAngle(0);
 
          
    yaw = fmod( ( yaw + 2*M_PI),  (2*M_PI) ) / M_PI * 180;
    roll = fmod( ( roll + 2*M_PI),  (2*M_PI) ) / M_PI * 180;
    pitch = fmod( ( pitch + 2*M_PI),  (2*M_PI) ) / M_PI * 180;
    
    /*
	std::cout << "yaw:"<< yaw << ", roll:" << roll << ", pitch:" << pitch << std::endl;  //45 -0 0
	std::cout << "offset_z:"<< offset_z << std::endl;  //45 -0 0
	cout<<  rotationMatrix(0, 0) <<","<< rotationMatrix(0, 1) <<","<< rotationMatrix(0, 2) <<","<< rotationMatrix(0, 3)<< endl; 
    cout<<  rotationMatrix(1, 0) <<","<< rotationMatrix(1, 1) <<","<< rotationMatrix(1, 2) <<","<< rotationMatrix(1, 3)<< endl;
    cout<<  rotationMatrix(2, 0) <<","<< rotationMatrix(2, 1) <<","<< rotationMatrix(2, 2) <<","<< rotationMatrix(2, 3)<< endl; 
    cout<<  rotationMatrix(3, 0) <<","<< rotationMatrix(3, 1) <<","<< rotationMatrix(3, 2) <<","<< rotationMatrix(3, 3)<< endl; 
    */
}

系统评价

// 点云变换
pcl::transformPointCloud(*g_seeds_pc, *cloud_filtered_ptr, transform_matrix);
/**********************************************************************************************************
    功能：评判校准是否有效
    
    输入：cloud_filtered_ptr(地面数据)、roi_point_num(roi中点的数量)

    输出：校准是否有效
    
    评判维度：
        1. 通过比较 ROI点云数量与 地面点云数量。
        2. 计算均值和方差

**********************************************************************************************************/

bool access_calibrate(PointCloudT::Ptr cloud_filtered_ptr, int roi_point_num)
{
    std::vector< double > resultSet;
	cloud_filtered_ptr->width = cloud_filtered_ptr->points.size();;
	cloud_filtered_ptr->height = 1;

	for (std::size_t i = 0; i < cloud_filtered_ptr->points.size(); ++i)
	{
	    if( cloud_filtered_ptr->points[i].z >= -0.1 && cloud_filtered_ptr->points[i].z <= 0.1)
        {
            resultSet.push_back(cloud_filtered_ptr->points[i].z);
        }
	}
	double sum = std::accumulate(std::begin(resultSet), std::end(resultSet), 0.0);
    double mean =  sum / resultSet.size(); //均值
    double accum  = 0.0;
    std::for_each (std::begin(resultSet), std::end(resultSet), [&](const double d) {accum  += (d-mean)*(d-mean);});
    double stdev = sqrt(accum/(resultSet.size()-1)); //方差
    double num_ratio = resultSet.size() *1.0 / roi_point_num; // 有效数据量/roi数据量
    
    
    cout<< roi_point_num <<"," << num_ratio << "," << resultSet.size() << "," << abs(mean) << "," << abs(stdev) <<endl;
    if( num_ratio>=min_num_ratio && resultSet.size() >= min_ground_num && abs(mean)<= max_mean && abs(stdev)<=max_stdev )
        return true;
        
   return false;
}

参数输出

/**********************************************************************************************************
功能：存储校准参数

 输入：roll(X轴)、pitch(Y轴)、yaw(Z轴)：欧拉角；
        (x, y, z):位移；
        lidar_matrix(变换矩阵)

 输出：校准文件

 返回：void
**********************************************************************************************************/
void generate_yaml_file( double yaw,double roll,double pitch,
                            double offset_x,double offset_y,double offset_z,
                            Eigen::Matrix4f lidar_matrix)
{
    // 用于读写yaml文件的对象
    static CalibrationYaml cYaml;
    Calibration_Lidar cLidar;
    cLidar.x = offset_x;
    cLidar.y = offset_y;
    cLidar.z = offset_z;
    cLidar.roll = roll;
    cLidar.pitch = pitch;
    cLidar.yaw = yaw;
    cLidar.lidar_matrix = lidar_matrix;
      // Write the results in an yaml file
    cYaml.yamlWrite(output_file_path, cLidar);
    
    /*
    // 读校准参数
    Calibration_Lidar cLidar2  = cYaml.yamlRead(output_file_path);
    cout<<  cLidar2.lidar_matrix(0, 0) <<","<< cLidar2.lidar_matrix(0, 1) <<","<< cLidar2.lidar_matrix(0, 2) <<","<< cLidar2.lidar_matrix(0, 3)<< endl; 
    cout<<  cLidar2.lidar_matrix(1, 0) <<","<< cLidar2.lidar_matrix(1, 1) <<","<< cLidar2.lidar_matrix(1, 2) <<","<< cLidar2.lidar_matrix(1, 3)<< endl;
    cout<<  cLidar2.lidar_matrix(2, 0) <<","<< cLidar2.lidar_matrix(2, 1) <<","<< cLidar2.lidar_matrix(2, 2) <<","<< cLidar2.lidar_matrix(2, 3)<< endl; 
    cout<<  cLidar2.lidar_matrix(3, 0) <<","<< cLidar2.lidar_matrix(3, 1) <<","<< cLidar2.lidar_matrix(3, 2) <<","<< cLidar2.lidar_matrix(3, 3)<< endl; 
    */
}

最优输出

	if( access_value )
        {
            std::cout << "The init calibration is finished, you could now stop the program!" << std::endl;
            roll_correction = roll_correction + roll;
            yaw_correction = yaw_correction + yaw;
            pitch_correction = pitch_correction + pitch;
            x_correction = x_correction + offset_x;
            y_correction = y_correction + offset_y;
            z_correction = z_correction + offset_z;
            counter++;
        
       
        }
        else
        {
            std::cout << "The init calibration is not finished, you could now stop the program!" << std::endl;
        }
        std::cout << "roll_correction " << roll << std::endl;
        std::cout << "pitch_correction " << pitch << std::endl;
        std::cout << "yaw_correction " << yaw << std::endl;
        std::cout << "z_correction " << offset_z << std::endl;
        
        if(counter == 1000) 
        {
            isCalibrationFinished = true;
            roll_correction = roll_correction/counter;
            yaw_correction = yaw_correction/counter;
            pitch_correction = pitch_correction/counter;
            x_correction = x_correction/counter;
            y_correction = y_correction/counter;
            z_correction = z_correction/counter;
            // 创建变换矩阵
            Eigen::Matrix4f transform_matrix1 = create_transform_matrix( yaw_correction/ 180 * M_PI, roll_correction/ 180 * M_PI, pitch_correction/ 180 * M_PI, 
                                            x_correction, y_correction, z_correction);
            // 存储校准参数
            generate_yaml_file( yaw_correction, roll_correction, pitch_correction,
                        x_correction, y_correction, z_correction,
                        transform_matrix1);
            std::cout<< "finished---------------------"<<std::endl;
            std::cout << "roll_correction " << roll_correction << std::endl;
            std::cout << "pitch_correction " << pitch_correction << std::endl;
            std::cout << "yaw_correction " << yaw_correction << std::endl;
            std::cout << "x_correction " << x_correction << std::endl;
            std::cout << "y_correction " << y_correction << std::endl;
            std::cout << "z_correction " << z_correction << std::endl;
            cout<<  transform_matrix1(0, 0) <<","<< transform_matrix1(0, 1) <<","<< transform_matrix1(0, 2) <<","<< transform_matrix1(0, 3)<< endl; 
            cout<<  transform_matrix1(1, 0) <<","<< transform_matrix1(1, 1) <<","<< transform_matrix1(1, 2) <<","<< transform_matrix1(1, 3)<< endl;
            cout<<  transform_matrix1(2, 0) <<","<< transform_matrix1(2, 1) <<","<< transform_matrix1(2, 2) <<","<< transform_matrix1(2, 3)<< endl; 
            cout<<  transform_matrix1(3, 0) <<","<< transform_matrix1(3, 1) <<","<< transform_matrix1(3, 2) <<","<< transform_matrix1(3, 3)<< endl; 
        
        }

校准效果

x-z平面：
y-z平面：

资源下载

• 全自动开源付费版:https://download.csdn.net/download/hgz_gs/14980826
• 全自动-半开源-粉丝回馈-体验版:https://download.csdn.net/download/hgz_gs/14982919
• 半自动开源付费版:https://download.csdn.net/download/hgz_gs/14987523
• 半自动-半开源-粉丝回馈-体验版:https://download.csdn.net/download/hgz_gs/14987503