【PCL】激光雷达点云地面水平校准

激光雷达采集的数据，可能由于颠簸或者雷达安装倾斜或者地面本身是有坡度的，造成地面在雷达坐标系中不是水平的。不是水平的，会影响我们后续的对点云的分割分类等处理，所以校准很有必要。

校准方法是（参考）：用PCL中基于RANSAC的平面检测方法检测出平面，得到平面：ax+by+cz+d=0。对于一个平面，上式中xyz的系数，就是它的法向量。然后，雷达坐标系中的竖直向量是（0,0,1），计算出从平面法向量旋转到竖直向量的旋转矩阵，再把此旋转矩阵应用到点云，点云即可得到旋转。

一步一步来：

1，分割平面，得到平面的法向量：

    pcl::SACSegmentation<pcl::PointXYZ> plane_seg;
    pcl::PointIndices::Ptr plane_inliers ( new pcl::PointIndices );
    pcl::ModelCoefficients::Ptr plane_coefficients ( new pcl::ModelCoefficients );
    plane_seg.setOptimizeCoefficients (true);
    plane_seg.setModelType ( pcl::SACMODEL_PLANE );
    plane_seg.setMethodType ( pcl::SAC_RANSAC );
    plane_seg.setDistanceThreshold ( 0.3 );
    plane_seg.setInputCloud ( cloud_in );
    plane_seg.segment (*plane_inliers, *plane_coefficients);//得到平面系数，进而得到平面法向量

2，计算两个向量之间的旋转矩阵：

    关于在已知两个向量坐标的情况下如何求两者的旋转矩阵的问题，可以看这个和这个。其中，会用到两个向量的点乘和叉乘，什么是点乘和叉乘，看这里。怎么实现点乘和叉乘，看这里，用eigen库非常方便，都已经封装好了。

Eigen::Matrix4f CreateRotateMatrix(Vector3f before,Vector3f after)
{
    before.normalize();
    after.normalize();

    float angle = acos(before.dot(after));
    Vector3f p_rotate =before.cross(after);
    p_rotate.normalize();

    Eigen::Matrix4f rotationMatrix = Eigen::Matrix4f::Identity();
    rotationMatrix(0, 0) = cos(angle) + p_rotate[0] * p_rotate[0] * (1 - cos(angle));
    rotationMatrix(0, 1) = p_rotate[0] * p_rotate[1] * (1 - cos(angle) - p_rotate[2] * sin(angle));//这里跟公式比多了一个括号，但是看实验结果它是对的。
    rotationMatrix(0, 2) = p_rotate[1] * sin(angle) + p_rotate[0] * p_rotate[2] * (1 - cos(angle));


    rotationMatrix(1, 0) = p_rotate[2] * sin(angle) + p_rotate[0] * p_rotate[1] * (1 - cos(angle));
    rotationMatrix(1, 1) = cos(angle) + p_rotate[1] * p_rotate[1] * (1 - cos(angle));
    rotationMatrix(1, 2) = -p_rotate[0] * sin(angle) + p_rotate[1] * p_rotate[2] * (1 - cos(angle));


    rotationMatrix(2, 0) = -p_rotate[1] * sin(angle) +p_rotate[0] * p_rotate[2] * (1 - cos(angle));
    rotationMatrix(2, 1) = p_rotate[0] * sin(angle) + p_rotate[1] * p_rotate[2] * (1 - cos(angle));
    rotationMatrix(2, 2) = cos(angle) + p_rotate[2] * p_rotate[2] * (1 - cos(angle));

    return rotationMatrix;
}

3，利用旋转矩阵，将点云旋转

pcl::transformPointCloud(*cloud_in, *cloud_final, rotation);

效果演示：

绿的是原始点云，白的是校准后的点云。