激光雷達點雲地面水平校准

博客轉載自：https://blog.csdn.net/ethan_guo/article/details/80683181

激光雷達采集的數據，可能由於顛簸或者雷達安裝傾斜或者地面本身是有坡度的，造成地面在雷達坐標系中不是水平的。不是水平的，會影響我們后續的對點雲的分割分類等處理，所以校准很有必要。

校准方法是（參考）：用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);