#每周一篇论文3#[感知] 激光雷达标定

激光雷达外参数自标定

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

• 手工选取大致地面数据
• 使用RANSCAN算法进行地平面分割
• 提取激光雷达的地平面向量，计算激光雷达的地平面向量与理想地 平面向量[0,0,1] 的变换矩阵
• 对激光雷达外参标定进行系统评价
• 标定输出。
  全自动激光雷达标定传送门

选取大致地面数据

使用pcl工具选取地面大致数据。

/**********************************************************************************************************
功能：pcl 鼠标选择数据
**********************************************************************************************************/
static void _pp_callback(const pcl::visualization::AreaPickingEvent& event, void* args)
{
}


/**********************************************************************************************************
功能：更新窗口数据
**********************************************************************************************************/
boost::shared_ptr LidarInitCalibration::updateVis (boost::shared_ptr viewer, PointCloudT::ConstPtr cloud)
{
    return (viewer);
}

/**********************************************************************************************************
功能：创建窗口和显示点云数据
**********************************************************************************************************/
boost::shared_ptr LidarInitCalibration::rgbVis (PointCloudT::ConstPtr cloud)
{
    return (viewer);
}

/**********************************************************************************************************
功能：点云感兴趣区域选择
**********************************************************************************************************/
void LidarInitCalibration::set_roi(PointCloudT &points, PointCloudT::Ptr roi_point)
{
    
}

地平面分割

使用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 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 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) <=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; 
    */
}

校准效果

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