激光雷达采集的数据,可能由于颠簸或者雷达安装倾斜或者地面本身是有坡度的,造成地面在雷达坐标系中不是水平的。不是水平的,会影响我们后续的对点云的分割分类等处理,所以校准很有必要。
过程:用PCL中基于RANSAC的平面检测方法检测出平面,得到平面:ax+by+cz+d=0。检测平面可以有多种方法。对于一个平面,上式中xyz的系数,就是它的法向量。然后,雷达坐标系中的竖直向量是(0,0,1),计算出从平面法向量旋转到竖直向量的旋转矩阵,再把此旋转矩阵应用到点云,点云即可得到旋转。
以地面分割为例。因为是地面分割,所以更关注地面附近的点,下文拟合平面就是用地面附近的点,不应该用其他的点。
1 法向量计算
首先拿到地面附近的点云,可以用条件滤波(x,y,z字段的参数视情况而定)
pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_filter(pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_raw, int flag)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_filter(new pcl::PointCloud<pcl::PointXYZ>);//创建点云对象,用以存储滤波后点云
cout << "条件滤波" << endl;
//创建条件限定下的滤波器
pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>);
//添加在各字段上的比较算子
//GT greater than
//EQ equal
//LT less than
//GE greater than or equal
//LE less than
pcl::FieldComparison<pcl::PointXYZ>::ConstPtr cond_z1(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, -0.75f));
range_cond->addComparison(cond_z1);
pcl::FieldComparison<pcl::PointXYZ>::ConstPtr cond_z2(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, -0.68f));
range_cond->addComparison(cond_z2);
pcl::FieldComparison<pcl::PointXYZ>::ConstPtr cond_y1(new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GT, 0.0f));
range_cond->addComparison(cond_y1);
pcl::FieldComparison<pcl::PointXYZ>::ConstPtr cond_y2(new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::LT, 10.0f));
range_cond->addComparison(cond_y2);
pcl::FieldComparison<pcl::PointXYZ>::ConstPtr cond_x1(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, -8.0f));
range_cond->addComparison(cond_x1);
pcl::FieldComparison<pcl::PointXYZ>::ConstPtr cond_x2(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 8.0f));
range_cond->addComparison(cond_x2);
//创建滤波器并用条件定义对象初始化
pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
condrem.setCondition(range_cond);
condrem.setInputCloud(pointCloud_raw);
//condrem.setKeepOrganized(true);//设置保持为结构点云
// apply filter应用滤波器
condrem.filter(*pointCloud_filter);
return pointCloud_filter;
}
条件滤波将不符合条件的点置为nan,所以在滤波之后加一步
vector<int> mapping;
removeNaNFromPointCloud(*cloud_temp, *cloud_temp, mapping);//移除nan的点
然后计算得到地面点云拟合出的平面的法向量,下面函数返回的是法向量
Vector3f estimateGroundPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, const float in_distance_there)
{
Vector3f normal_vector;
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(in_distance_there);//视情况定,我的是0.05f
plane_seg.setInputCloud(cloud_in);
plane_seg.segment(*Plane_inliers, *plane_coefficients);
// cerr << "Model coefficeients:" << plane_coefficients->values[0] << " "
// << plane_coefficients->values[1] << " "
// << plane_coefficients->values[2] << " "
// << plane_coefficients->values[3] << endl;
normal_vector = { plane_coefficients->values[0],plane_coefficients->values[1],plane_coefficients->values[2] };
return normal_vector;
}
上面plane_coefficients->values四个值分别是平面:ax+by+cz+d=0的四个系数,(a,b,c)即为该平面的法向量
2,计算得到两个向量之间的旋转矩阵:
angle_before是前面的normal_vector
Vector3f angle_after = { 0.0f,0.0f,1.0f };
Eigen::Matrix4f CreateRoatationMatrix(Vector3f angle_before, Vector3f angle_after)
{
angle_before.normalize();
angle_after.normalize();
float angle = acos(angle_before.dot(angle_after));
Vector3f p_rotate = angle_before.cross(angle_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_raw, *cloud_final, rotationMatrix);//得到旋转之后的点云
旋转之后,地面上同一条激光线上的点的z值不会相差太大,1~2cm左右,我测试的是柏油路,路面上有小孔。
其实都是调用pcl库函数。