点云水平校准

激光雷达采集的数据，可能由于颠簸或者雷达安装倾斜或者地面本身是有坡度的，造成地面在雷达坐标系中不是水平的。不是水平的，会影响我们后续的对点云的分割分类等处理，所以校准很有必要。

过程：用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库函数。

猜你喜欢