一起做激光SLAM[二]提取特征点和地面点

本节目标：提取edge点和plane点与地面点并显示   预期效果：  
  rosbag数据: https://pan.baidu.com/s/1GaZ2eGZdfc-cluSc-bkQng 提取码: 9zsp   程序：https://gitee.com/eminbogen/one_liom  

目录

特征点

地面点

讨论

信息保存

特征提取数量

时耗

特征点

  对于edge和plane特征点，我们使用常规的曲率提取的方式，对于一条线，曲率等于其曲率圆半径的倒数，如下图，线比较曲，圆比较小，半径倒数就很大，相反，如果一个绝对的平面，曲率圆非常大，圆半径非常大，半径倒数非常小，也就是曲率非常小。  
  计算中我们使用如下公式：  
  对应程序如下：  

float diffX = laserCloudScans[i].points[j - 3].x + laserCloudScans[i].points[j - 2].x + laserCloudScans[i].points[j - 1].x - 6 * laserCloudScans[i].points[j].x + laserCloudScans[i].points[j + 1].x + laserCloudScans[i].points[j + 2].x + laserCloudScans[i].points[j + 3].x;
float diffY = laserCloudScans[i].points[j - 3].y + laserCloudScans[i].points[j - 2].y + laserCloudScans[i].points[j - 1].y - 6 * laserCloudScans[i].points[j].y + laserCloudScans[i].points[j + 1].y + laserCloudScans[i].points[j + 2].y + laserCloudScans[i].points[j + 3].y;
float diffZ = laserCloudScans[i].points[j - 3].z + laserCloudScans[i].points[j - 2].z + laserCloudScans[i].points[j - 1].z - 6 * laserCloudScans[i].points[j].z + laserCloudScans[i].points[j + 1].z + laserCloudScans[i].points[j + 2].z + laserCloudScans[i].points[j + 3].z;
curvature=double(diffX * diffX + diffY * diffY + diffZ * diffZ);

  即xyz三种导数平方和。   根据阈值可以区分edge和plane点。（比如loam选0.1，小于plane，大于edge）    

地面点

  地面点的提取有两种办法。   一是LEGO_LOAM的根据相邻两线的上两点之间的仰角，度数小于10即两点为地面点。下图中中激光雷达发射两条scan线，在一坡上有两点，夹角
如果小于10度，就认为两点为地面点。  
  二是HDL graph的方式，根据高度信息粗提取，再根据法向量和地面法向量差异精确提取，再聚类为地平面。  

  boost::optional<Eigen::Vector4f> detect(const pcl::PointCloud<PointT>::Ptr& cloud) const {
    // 矫正激光雷达的倾斜度，因为有时候激光雷达并不是水平按照
    Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
    tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix();
 
    //垂直方向的滤波，方法比较粗暴，即认为地面点存在于[sensor_height-height_clip_range,sensor_height+height_clip_range]之间。
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
    pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);
	//估计每一个点的法向量.从存在地面的点中进一步（可选择）使用法线滤波，即过滤与地面法向量夹角过大的点
    if(use_normal_filtering) {
      filtered = normal_filtering(filtered);
    }
	//乘以转换矩阵的逆矩阵，把滤波后的点云转换到原坐标系下
    pcl::transformPointCloud(*filtered, *filtered, static_cast<Eigen::Matrix4f>(tilt_matrix.inverse()));
	//发布转换回去后的滤波之后的点云
    if(floor_filtered_pub.getNumSubscribers()) {
      filtered->header = cloud->header;
      floor_filtered_pub.publish(filtered);
    }
 
    // 点云数量太少，不足以给到RANSAC算法
    if(filtered->size() < floor_pts_thresh) {
      return boost::none;
    }
 
    // 开始使用RANSAC算法提取平面模型，关于RANSAC提取平面可另行百度
    pcl::SampleConsensusModelPlane<PointT>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointT>(filtered));
    pcl::RandomSampleConsensus<PointT> ransac(model_p);
    ransac.setDistanceThreshold(0.1);
    ransac.computeModel();
 
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    ransac.getInliers(inliers->indices);
 
    // too few inliers
    if(inliers->indices.size() < floor_pts_thresh) {
      return boost::none;
    }
 
    // 验证地面法线，这里的reference其实就是平面的一个法向量
    Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();
 
    Eigen::VectorXf coeffs;
    ransac.getModelCoefficients(coeffs);//得到提取出来的平面模型的四个参数
 
    double dot = coeffs.head<3>().dot(reference.head<3>());
    //如果提取平面的法向量与参考法向量之间夹角大于10°，说明该平面模型参数不满足条件，即法线不垂直
    if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) 
    {
      // the normal is not vertical
      return boost::none;
    }
 
    // 让法线方向朝上
    if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) 
    {
      coeffs *= -1.0f;
    }
 
    if(floor_points_pub.getNumSubscribers()) 
    {
      pcl::PointCloud<PointT>::Ptr inlier_cloud(new pcl::PointCloud<PointT>);
      pcl::ExtractIndices<PointT> extract;
      extract.setInputCloud(filtered);
      extract.setIndices(inliers);
      extract.filter(*inlier_cloud);
      inlier_cloud->header = cloud->header;
 
      floor_points_pub.publish(inlier_cloud);
    }
 
    return Eigen::Vector4f(coeffs);
  }

  我们采用lego的方式。只对线号为0-5的scan线处理。  

for(int i=0;i<5;i++)
{
    for(int j=0;j<int(laserCloudScans[i].size())&&j<int(laserCloudScans[i+1].size());j++)
    {
	float diffX,diffY,diffZ,angle;
	diffX = laserCloudScans[i+1].points[j].x - laserCloudScans[i].points[j].x;
	diffY = laserCloudScans[i+1].points[j].y - laserCloudScans[i].points[j].y;
	diffZ = laserCloudScans[i+1].points[j].z - laserCloudScans[i].points[j].z;
	angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
	if(abs(angle)<10) 
	{
	    laserCloudground->push_back(laserCloudScans[i+1].points[j]);
	    laserCloudground->push_back(laserCloudScans[i].points[j]);
	}
    }
}

讨论

信息保存

  在程序中我们使用了pcl::PointXYZRGB的点云格式，这是为了让点云的一些信息跟着点走，而不用再写东西另保存，要是专门写个vector<int>去储存信息还要在ros中传递节点信息，就很麻烦，而且我们也不用点云颜色信息hhh。  

//这里使用pcl的rgb并不是颜色，只是拿来储存信息
//r是是否为地面，1：非地面 2：地面
//g是曲率，小于一定值（比如0.1）认为是plane point
//b是scan的线号0-15
typedef pcl::PointXYZRGB PointType;

  loam中使用intensity的整数部分保存线号，用小数部分保存点在该线中的位置，达到后续匹配搜索使用的目的。  

特征提取数量

  与loam相比，我们提取的点数非常多，一是因为提取少，节目效果不好，下图loam实时特征点和我们程序的效果。  
 
  二是一部分现在的非实时的激光建图是使用尽可能多的面特征点的，我们保存下来在第四节博客对比它们效果。  

时耗

  大概2ms一帧,会比loam快1-2ms，这是因为判断内容少，使得时间变少，当然这里耗时都是毛毛雨，由于特征点数量可能多十倍，这会使得后面里程计部分时耗达到170ms一帧。所以，终究特征点数量在实时slam中是必须要控制的。