选取一帧地面比较平的点云pcd,然后用PCL封装好的RANSAC地面分割算法来拟合。
平面方程一般式: A x 2 + B y + C z + D = 0 Ax^2 +By + Cz + D = 0 Ax2+By+Cz+D=0,法向量即为平面方程的系数 n = ( A , B , C ) n = (A, B, C) n=(A,B,C)。
void ransac_plane(std::string pcd_file, pcl::ModelCoefficients::Ptr coefficients)
{
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr in_cloud(new pcl::PointCloud);
if (pcl::io::loadPCDFile(pcd_file, *cloud) != 0)
{
return;
}
for (int i = 0; i < cloud->size(); i++)
{
if (cloud-points[i].x < 20 && abs(cloud->points[i].y < 5)
{
in_cloud->push_back(cloud->points[i]);
}
}
pcl::PointIndices::Ptr idx(new pcl::PointIndices);
pcl::SACSegmentation seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(in_cloud);
seg.segment(*idx, *coefficients);
Eigen::Vector3f plane_normal << coefficients->values[0], coefficients->values[1], coefficients->values[2];
}
这里直接调用pcl封装好的函数
Eigen::Vector3f vec_z;
vec_z << 0, 0, 1;
double angle = pcl::getAngle3D(vec_z, plane_normal);
可以参考(https://blog.csdn.net/fei_12138/article/details/110280338),因为只有俯仰角的校准,所以只需要求绕y轴的旋转矩阵。
https://en.wikipedia.org/wiki/Rotation_matrix
Eigen::Matrix4f rotation_y = Eigen::Matrix4f::Identity();
rotation_y(0,0) = cos(angle);
rotation_y(0,2) = sin(angle);
rotation_y(2,0) = -sin(angle);
rotation_y(2,2) = cos(angle);
已知转换矩阵的逆矩阵及转换后的点云 Y Y Y,可以求得校准原始理想点云(校准的点云)。这里点云转换依然采用PCL封装好的函数。如果点云向下倾斜则用旋转矩阵的逆矩阵,如果点云向上倾斜,则直接用旋转矩阵。
R A = Y RA = Y RA=Y
==> R − 1 R A = R − 1 Y R^{-1}RA = R^{-1}Y R−1RA=R−1Y
==> A = R − 1 Y A = R^{-1}Y A=R−1Y
Eigen::Matrix4f rotation_inverse = totation_y.inverse();(点云向下倾斜)
pcl::transformPointCloud(*cloud, *transform_cloud, rotation_inverse);
至此,即可将点云水平校准完成,转换后的点云应该是地面水平状态。