#每周一篇论文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平面:
在这里插入图片描述

资源下载

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/hgz_gs/article/details/113484065

智能推荐

队列的实现(C语言)_vitobo的博客-程序员秘密_c语言队列的实现以及操作

队列的定义队列的顺序存储--&gt;循环队列方案1:牺牲一个单元来区分队空和队满方案2:增设表示元素个数的变量(不牺牲存储单元)方案3:增设tag数据成员区分队空队满(不牺牲存储单元)链式队列的实现带头结点的链式队列不带头结点的链式队列队列的定义队列(Queue) 是一种操作受限的线性表,只允许在表的一端进行插入,而在表的另一端进行删除。队列操作的特性:先进先出(Frist In First Out)队列的顺序存储–&gt;循环队列方案1:牺牲一个单元来区分队空和队满实现:// .

ConcurrentHashMap分析与应用_mushui59的博客-程序员秘密

ConcurrentHashMap类与 Hashtable 相似,都是线程安全的,但与 HashMap 不同,它不 允许将 null 用作键或值。 可以使用Iterator和Enumeration进行遍历,且不会抛出ConcurrentModificationException。不过,迭代器被设计成每次仅由一个线程使用。ConcurrentHashMap可以做到读取数据不加锁,并且其内...

C#基础之Assembly_julianboom的博客-程序员秘密_assembly c#

 一直以来,我们都在用C#编写程序,编写程序的时候,我们用到继承、多态、接口以及泛型,我们也都明白子类可以继承抽象类,并能够重写父类的抽象方法,可是大家是否想过,如下几个问题:  1、凡树必有根和叶,类的继承也如此,如何通过程序集查找所有继承父类的之类的程序集名称?  2、如果程序B被其他程序调用,如何通过程序集查询调用B的所有程序?  3、如何查询当前项目通过添加引用了哪些程序集?...

使用Assembly打包Jar,可直接使用_此处一淌水的博客-程序员秘密_assembly jar

Pom文件 &lt;build&gt; &lt;resources&gt; &lt;resource&gt; &lt;directory&gt;${project.basedir}/src/main/resources&lt;/directory&gt; &lt;filtering&gt;true&lt;/filtering&gt; &lt;excludes&gt;

Pycharm和Python关系_CC_Lsh的博客-程序员秘密_pycharm与python的关系

Pycharm和Python关系简单来说:Pycharm是一个代码编辑器,是目前最流行的代码编辑器之一,用于编写python代码。Python是一个代码解释器,用于将Python代码翻译成计算机可以理解的指令。Pycharm下载地址:PyCharm: the Python IDE for Professional Developers by JetBrainsThe Python &amp; Django IDE with intellig...

pycharm怎样编写java_Pycharm改进和编写代码_伊利心情的博客-程序员秘密

PyCharm包含用于编写代码的各种标准,其中包含适用于Python的适当缩进。 这有助于提高代码标准并在PyCharm编辑器中编写完整的代码。改进代码完成PyCharm中的代码完成非常独特。 您可以使用许多其他功能进一步增强它。 请注意,编辑器提供了代码块的开始和结束。 以下代码编写一个名为demo.py的文件中 -message = 'GIEWIVrGMTLIVrHIQS' #encrypte...

随便推点

bitset 应用实例-埃拉托斯特尼筛法_DWonderOO的博客-程序员秘密

bitset 应用实例-埃拉托斯特尼筛法bitset在C++容器中,是一个特殊容器,用来处理二进制位。那么在这里讲一个非常有趣的例子,这个例子是用来查找质数,又称为素数,只能够被1和它自身整除的整数。1既不是质数,也不是合数。那么如何使用一个算法,高效率地查找质数呢? 这里介绍一下 埃拉托斯特尼筛法。原理:先把N个自然数按次序排列起来。1不是质数,也不是合数,要划去。第二个数是2是质数留下来,而吧2后面所有能被2整除的数都划去。2后面第一个没被划去的数是3,把3留,再把3后面所有能被3整除的数都划去

埃拉托斯特尼筛法(埃筛)_hambaga的博客-程序员秘密_埃拉托斯特尼筛法

埃筛的作用是找出区间内的所有素数,复杂度是O(nloglogn)。其基本思想是:素数的倍数一定是合数。#include &lt;bits/stdc++.h&gt;using namespace std;const int Max = 1e5;int n;int prime[Max]; // 1表示是素数void eratos() { memset(prime, 1, sizeof(...

删除chrome浏览器记住密码input自动填充背景色_DenggLin的博客-程序员秘密

/* Change the white to any color ;) */@-webkit-keyframes autofill { to { color: #333; //input中文字的颜色 background: transparent; }}.login-input input:-webkit-autofill { ...

芯片如何储存信息_简单通俗谈信息储存原理_weixin_39986027的博客-程序员秘密

简单通俗谈信息储存原理:网上有个塑料杯留声机的实验,即人对着纸杯喊话,纸杯底部连接一根钢针,钢针的另一头刻在塑料杯上,塑料杯安装在一个滚轴上。即喊话的声音频率刻在滚动的塑料杯上,然后回放滚动的塑料杯就可以还原原声。其实它这个也就是信息储存原理:1,刻下或保存信息能量频率,也即刻下或保存信息。2,刻下,记下,保存的信息能量频率可以振动回放,回放即输出原声。那我们人脑和电脑其实信息储存也是这个原理。人...

(数据结构)图的邻接表存储结构_是我来晚了!的博客-程序员秘密_图的邻接表存储

图的邻接表存储结构一般来说,图更多的是采用链表存储,具体的存储方法有 3 种,分别是邻接表、邻接多重表和十字链表本篇文章将优先介绍邻接表!!!邻接点:在图中,如果两个点相互连通,且通过其中一个顶点,可直接找到另一个顶点,则称它们互为邻接点邻接:指图中顶点之间有边或者弧的存在邻接表存储图的实现方式:给图中的各个顶点独自建立一个链表,用节点存储该顶点,用另一个链表中的节点存储其邻接点特殊之处是,为了便于管理这些链表,通常会将链表的头节点存储到数组中,也正因为各个链表的头节点存储的是各个顶