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

目录

特征点

地面点

讨论

信息保存

特征提取数量

时耗

   

特征点

  对于edge和plane特征点,我们使用常规的曲率提取的方式,对于一条线,曲率等于其曲率圆半径的倒数,如下图,线比较曲,圆比较小,半径倒数就很大,相反,如果一个绝对的平面,曲率圆非常大,圆半径非常大,半径倒数非常小,也就是曲率非常小。   微信图片_20210120154626   计算中我们使用如下公式:   微信图片_20210120154727   对应程序如下:  
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线,在一坡上有两点,夹角\Theta如果小于10度,就认为两点为地面点。   微信图片_20210120154749   二是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实时特征点和我们程序的效果。   微信图片_20210120154945   微信图片_20210120154947   二是一部分现在的非实时的激光建图是使用尽可能多的面特征点的,我们保存下来在第四节博客对比它们效果。  

时耗

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