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

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

    • 1. RANSAC求地面拟合方程
    • 2. 计算两个空间法向量夹角
    • 3. 计算转换矩阵
    • 4. 点云转换

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

1. RANSAC求地面拟合方程

选取一帧地面比较平的点云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];
}

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 Y Y,可以求得校准原始理想点云(校准的点云)。这里点云转换依然采用PCL封装好的函数。如果点云向下倾斜则用旋转矩阵的逆矩阵,如果点云向上倾斜,则直接用旋转矩阵。
R A = Y RA = Y RA=Y
==> R − 1 R A = R − 1 Y R^{-1}RA = R^{-1}Y R1RA=R1Y
==> A = R − 1 Y A = R^{-1}Y A=R1Y

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

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

你可能感兴趣的:(PCL及ROS在实践中的应用,C++,PCL,激光点云,自动驾驶,c++)