自动驾驶感知【激光雷达】：一、标定

激光雷达的自动标定算法实现效果：

一、标定在自主驾驶中的作用

    激光雷达标定在自动驾驶中具有重要的作用。激光雷达是自动驾驶车辆中最为常用的传感器之一，它可以获取环境中障碍物的位置和形状等信息，并将这些信息转换成点云数据进行处理。激光雷达标定是指对激光雷达传感器内部参数和外部参数进行估计的过程，包括激光束旋转中心、扫描面的倾斜角度、激光束发散度等内部参数，以及激光雷达相对于车辆坐标系的位置和方向等外部参数。激光雷达标定的目的是使得激光雷达采集到的点云数据在车辆坐标系下准确地表示出来，以便后续的SLAM、目标检测和路径规划等任务。因此，激光雷达的精确标定对于自动驾驶车辆的定位和导航非常关键。

    在自动驾驶中，激光雷达标定的作用主要有以下几个方面：

    1、定位和导航：激光雷达标定可以准确地确定激光雷达相对于车辆坐标系的位置和方向，从而实现车辆的准确定位和导航。

    2、障碍物检测和跟踪：激光雷达标定可以提高激光雷达采集到的点云数据的准确性和稳定性，从而实现对环境中障碍物的更加精确的检测和跟踪。

    3、地图构建：激光雷达标定可以准确地将激光雷达采集到的点云数据转换成机器人坐标系下的坐标，从而构建出更加准确的地图，为自动驾驶提供更加可靠的环境信息。

二、坐标系转换

    开头说一声，刚开始的理解坐标系，即其中的转换关系时会有点乱，理解不了，简单说一下，坐标系的转换和点的转换是相反的关系，一般在教程中或者实际应用中，我们都是将点在不同坐标系之间转换，但要记住，点在地球这个绝对坐标系中是不随其它坐标系的变换而发生位置变换的，只是在不同坐标系中的xyz数值在变换。

    将激光雷达坐标系中的点云数据转换到车辆坐标系中，需要知道激光雷达坐标系与车辆坐标系之间的转换关系。常用的转换关系有旋转矩阵和平移向量两种，即利用旋转矩阵将激光雷达坐标系转换到车辆坐标系下，再通过平移向量将原点从激光雷达坐标系转换到车辆坐标系。激光雷达坐标系到其他坐标系的转换可以通过下面的公式进行计算：

                        $P_c=T_{cb}\cdot R_{cb}\cdot P_l$

            $\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& T_x\\ R_{21}& R_{22}& R_{23}& T_y\\ R_{31}& R_{32}& R_{33}& T_z\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}{x}_l\\ y_l\\ z_l\\ 1\end{array}\right]$

其中，$x_l$、$y_l$、$z_l$表示激光雷达坐标系下的点的坐标，$x_c$、$y_c$、$z_c$表示目标坐标系下的点的坐标，$R$表示旋转矩阵，$T$表示平移向量，表示从激光雷达坐标系到目标坐标系的转换关系。

将旋转矩阵$R_{cb}按照$x$、$y$、$z分解可以得到：

$R_{cb}=R_z(\psi )\cdot R_y(\theta )\cdot R_x(\varphi )$

其中，$R_x(\varphi )$、$R_y(\theta )$、$R_z(\psi )$分别表示绕$x$、$y$、$z$轴旋转的旋转矩阵，$\varphi$、$\theta$、$\psi$表示对应的旋转角度。其中值得一提的是：同一旋转矩阵x、y、z从左到右不同的相乘（分解）顺序将得到不同的$\varphi$、$\theta$、$\psi$。

二、单激光雷达标定

    在一辆无人车上一般装有多个激光雷达，在我的策略里，先将一个主激光雷达进行标定，转换到车辆坐标系下，然后其它雷达向主激光雷达依次标定，最终实现将所有激光雷达都标定到车辆坐标系下。

使用直通滤波，对x、y进行滤波，取z比较小的点云，作为地面：

    // 创建条件滤波器：通过x, y范围来过滤点云
    pcl::PassThrough<pcl::PointXYZI> pass;
    // false表示保持这些点在范围内
    pass.setFilterLimitsNegative(false);
    // 设置输入点云
    pass.setInputCloud(points.makeShared());
    // 设置滤波对象x
    pass.setFilterFieldName("x");
    //  设置范围
    pass.setFilterLimits(roi_x0, roi_x1);
    // 进行滤波，并将返回值赋值给points
    pass.filter(points);
    // 设置滤波对象y
    pass.setInputCloud(points.makeShared());
    pass.setFilterFieldName("y");
    pass.setFilterLimits(roi_y0, roi_y1);
    pass.filter(*roi_point);
    
    sort((*roi_point).points.begin(), (*roi_point).points.end(), point_cmp);
    PointCloudT::Ptr g_seeds_pc
    for (int i = 0; i < roi_point->points.size() / 4; i++) {
        g_seeds_pc->points.push_back(roi_point->points[i]);
    }

使用 ransac检测地面

    // 模型的系数 
    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);

求法向量并求法向量与目标向量（0，0，1）的旋转矩阵，并分解到x，y，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;

    单激光雷达程序需要一个大平面作为地面，然后计算法向量，从而可以对pitch（俯仰角）、roll（横滚角）、z方向偏移进行标定，因为主雷达安装在车中间，所以认为横向（y）偏移为0，纵向（x）方向靠测量，raw（偏航角）默认基本为0，程序启动见github链接

二、多激光雷达标定

多雷达标定程序，我目前采取分别将除主激光雷达外的其它雷达与主激光雷达两两标定，使用ndt算法，最终实现全车雷达的标定。标定一定要有共视区域，并需要特征明显的物体（比如房子之类的有面、线、直角等特征）。