C++:利用PCL实现点云的水平校准

• 由于激光雷达在安装过程中，会出现安装误差，造成点云倾斜，这里假设激光点云只有俯仰的倾斜角度，所以我们需要通过水平校准来求得俯仰角，并将点云进行转换。
• 过程1：对地面点云求地面拟合方程的法向量。
• 过程2：根据地面的法向量及水平面的法向量（0，0，1）求夹角，即为激光雷达的安装俯仰角。
• 过程3：跟据求得夹角，得到点云绕y轴的旋转矩阵（不考虑平移）。
• 过程4：知道旋转矩阵，就可以对点云进行转换校准。

1. RANSAC求地面拟合方程

选取一帧地面比较平的点云pcd,然后用PCL封装好的RANSAC地面分割算法来拟合。

平面方程一般式： $A{x}^2+By+Cz+D=0$，法向量即为平面方程的系数$n=(A,B,C)$。

void ransac_plane(std::string pcd_file, pcl::ModelCoefficients::Ptr coefficients)
{
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZI>);
	if (pcl::io::loadPCDFile<pcl::PointXYZI>(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<pcl::PointXYZI> 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];
}

2. 计算两个空间法向量夹角

这里直接调用pcl封装好的函数

Eigen::Vector3f vec_z;
vec_z << 0, 0, 1;
double angle = pcl::getAngle3D(vec_z, plane_normal);

3. 计算转换矩阵

可以参考（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);

4. 点云转换

已知转换矩阵的逆矩阵及转换后的点云 $Y$，可以求得校准原始理想点云（校准的点云）。这里点云转换依然采用PCL封装好的函数。如果点云向下倾斜则用旋转矩阵的逆矩阵，如果点云向上倾斜，则直接用旋转矩阵。
$RA=Y$
==> $R^{-1}RA=R^{-1}Y$
==> $A=R^{-1}Y$

Eigen::Matrix4f rotation_inverse = totation_y.inverse();（点云向下倾斜）
pcl::transformPointCloud(*cloud, *transform_cloud, rotation_inverse);

至此，即可将点云水平校准完成，转换后的点云应该是地面水平状态。