ICode9

精准搜索请尝试: 精确搜索
首页 > 其他分享> 文章详细

SLAM框架:MULLS中的预处理文件cprocessing.hpp注释解析

2021-11-24 16:34:04  阅读:318  来源: 互联网

标签:float Xo hpp MULLS SLAM pcl boundary cloud points


激光slam框架,论文题目:

MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square

开源代码链接:

https://github.com/YuePanEdward/MULLS

最近在做一些特征提取的工作,发现MULLS框架对前端特征提取部分分的比较细致,特地阅读了一下论文和源码。以下是对预处理文件cprocessing.hpp的注释解析:

包含了:投影滤除地面,随机采样平面分割,凸包顶点检测,利用kdtree临近搜索提取角点

#ifndef _INCLUDE_CLOUD_PROCESSING_HPP
#define _INCLUDE_CLOUD_PROCESSING_HPP

//PCL
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

#include "utility.hpp"

using namespace std;
using namespace Eigen;

namespace lo
{

template <typename PointT>
class CProceesing : public CloudUtility<PointT>
{
public:
	// 滤除地面,投影算高度
	bool ground_filter_pmf(const typename pcl::PointCloud<PointT>::Ptr &cloud, typename pcl::PointCloud<PointT>::Ptr &gcloud,
						   typename pcl::PointCloud<PointT>::Ptr &ngcloud, int max_window_size,
						   float slope, float initial_distance, float max_distance)
	{
		pcl::PointIndicesPtr ground_points(new pcl::PointIndices);
		pcl::ProgressiveMorphologicalFilter<PointT> pmf;
		pmf.setInputCloud(cloud);
		pmf.setMaxWindowSize(max_window_size);	  //20
		pmf.setSlope(slope);					  //1.0f
		pmf.setInitialDistance(initial_distance); //0.5f
		pmf.setMaxDistance(max_distance);		  //3.0f
		pmf.extract(ground_points->indices);

		// Create the filtering object
		pcl::ExtractIndices<PointT> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(ground_points);
		extract.filter(*gcloud);               // 找出地面点

		//std::cout << "Ground cloud after filtering (PMF): " << std::endl;
		//std::cout << *gcloud << std::endl;

		// Extract non-ground returns
		extract.setNegative(true);
		extract.filter(*ngcloud);             // 非地面点

		//std::out << "Non-ground cloud after filtering (PMF): " << std::endl;
		//std::out << *ngcloud << std::endl;

		return 1;
	}

	// 随机一致性采样,分割平面
	bool plane_seg_ransac(const typename pcl::PointCloud<PointT>::Ptr &cloud,
						  float threshold, int max_iter, 
						  typename pcl::PointCloud<PointT>::Ptr &planecloud, 
						  pcl::ModelCoefficients::Ptr &coefficients) //Ransac
	{
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

		// Create the segmentation object
		pcl::SACSegmentation<PointT> sacseg;

		// Optional
		sacseg.setOptimizeCoefficients(true);

		// Mandatory
		sacseg.setModelType(pcl::SACMODEL_PLANE);
		sacseg.setMethodType(pcl::SAC_RANSAC);
		sacseg.setDistanceThreshold(threshold);
		sacseg.setMaxIterations(max_iter);

		sacseg.setInputCloud(cloud);
		sacseg.segment(*inliers, *coefficients);

		if (inliers->indices.size() == 0)
		{
			PCL_ERROR("Could not estimate a planar model for the given dataset.");
		}

		/*cout << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << std::endl;*/

		//LOG(INFO) << "Model inliers number: " << inliers->indices.size() << std::endl;

		for (size_t i = 0; i < inliers->indices.size(); ++i)
		{
			planecloud->push_back(cloud->points[inliers->indices[i]]);
		}
		return 1;
	}

	// 投影点云到z=0的面上
	bool ground_projection(const typename pcl::PointCloud<PointT>::Ptr &cloud,
						   typename pcl::PointCloud<PointT>::Ptr &projcloud)
	{
		// Create a set of planar coefficients with X=Y=Z=0
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
		coefficients->values.resize(4);
		coefficients->values[0] = coefficients->values[1] = coefficients->values[3] = 0;
		coefficients->values[2] = cloud->points[0].z;

		// Create the filtering object
		pcl::ProjectInliers<PointT> proj;
		proj.setModelType(pcl::SACMODEL_PLANE);
		proj.setInputCloud(cloud);
		proj.setModelCoefficients(coefficients);
		proj.filter(*projcloud);

		//cout << "Cloud projection completed" << endl;

		return 1;
	}

	// 凸包顶点检测
	bool alpha_shape(const typename pcl::PointCloud<PointT>::Ptr &cloud, float alpha_value,
					 typename pcl::PointCloud<PointT>::Ptr &boundary_cloud) //Concave Hull Generation with alpha_shape
	{
		pcl::ConcaveHull<PointT> chull;
		chull.setInputCloud(cloud);
		chull.setAlpha(alpha_value);          // 限制结果的大小
		chull.reconstruct(boundary_cloud);
		//std::cout<< "Concave hull has: " << boundary_cloud->points.size() << " data points." << endl;
		return 1;
	}

	// 指定临近点数量,寻找角点
	bool cornerpoint_knn(const typename pcl::PointCloud<PointT>::Ptr &boundary_cloud, int K, float disthreshold, float maxcos,
						 typename pcl::PointCloud<PointT>::Ptr &corner_cloud) //KNN corner point extraction
	{
		// 创建kdtree
		pcl::KdTreeFLANN<PointT> kdtree;
		kdtree.setInputCloud(boundary_cloud);

		// 定义临近点的索引序列,距离序列
		vector<int> pointIdxNKNSearch(K);		  //index in the order of the distance    k个
		vector<float> pointNKNSquaredDistance(K); //distance square

		// 遍历所有点,计算是否为角点
		for (int i = 0; i < boundary_cloud->size(); i++)
		{
			// 查找临近点,并将临近点按由近到远的顺序,存放在距离和索引队列中
			kdtree.nearestKSearch(boundary_cloud->points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance); // K NN search result
			// 相当于两个点的距离??
			float max1_max2;
			max1_max2 = sqrt(pointNKNSquaredDistance[K - 1]) - sqrt(pointNKNSquaredDistance[K - 2]);       // - ?

			float Xa, Xb, Xo, Ya, Yb, Yo, AOpBO, AO, BO, cosAOB;

			// 取最远和次远点
			Xo = boundary_cloud->points[i].x;
			Yo = boundary_cloud->points[i].y;
			Xa = boundary_cloud->points[pointIdxNKNSearch[K - 1]].x;
			Ya = boundary_cloud->points[pointIdxNKNSearch[K - 1]].y;

			// 如果最远和次远点的‘距离’小于阀值,则认为这两个点在点的同一侧,此时将次远点替换为另一侧的最远点
			if (max1_max2 < disthreshold) 
			{
				float maxdis = 0;
				int maxindex = -1;
				float Xc, Yc, Xd, Yd;
				Xc = boundary_cloud->points[pointIdxNKNSearch[K - 2]].x;
				Yc = boundary_cloud->points[pointIdxNKNSearch[K - 2]].y;
				// 遍历其它所有临近点,找距离次远点最远的点,则认为此点是最远点另一侧,且最远的点
				for (int j = 0; j < K - 2; j++)
				{
					Xd = boundary_cloud->points[pointIdxNKNSearch[j]].x;
					Yd = boundary_cloud->points[pointIdxNKNSearch[j]].y;

					float dis = sqrt((Xd - Xc) * (Xd - Xc) + (Yd - Yc) * (Yd - Yc));

					if (dis > maxdis)
					{
						maxdis = dis;
						maxindex = j;
					}
				}
				Xb = boundary_cloud->points[pointIdxNKNSearch[maxindex]].x;
				Yb = boundary_cloud->points[pointIdxNKNSearch[maxindex]].y;
			}
			else
			{
				Xb = boundary_cloud->points[pointIdxNKNSearch[K - 2]].x;
				Yb = boundary_cloud->points[pointIdxNKNSearch[K - 2]].y;
			}
			// 计算两侧两点与当前点的夹角的cos值
			AOpBO = (Xa - Xo) * (Xb - Xo) + (Ya - Yo) * (Yb - Yo);
			AO = sqrt((Xa - Xo) * (Xa - Xo) + (Ya - Yo) * (Ya - Yo));
			BO = sqrt((Xb - Xo) * (Xb - Xo) + (Yb - Yo) * (Yb - Yo));
			cosAOB = abs(AOpBO / AO / BO);
			
			// 如果cos值小于某个值,则认为此点为一个角点
			if (cosAOB < maxcos)
				corner_cloud->push_back(boundary_cloud->points[i]); //if the angle is smaller than a threshold, we regard it as a corner point
		}

		return 1;
	}

	// 指定临近点距离范围,寻找角点
	bool cornerpoint_rnn(const typename pcl::PointCloud<PointT>::Ptr &boundary_cloud,
						 float radius, float disthreshold, float maxcos, typename pcl::PointCloud<PointT>::Ptr &corner_cloud) //Radius corner point extraction
	{
		pcl::KdTreeFLANN<PointT> kdtree;
		kdtree.setInputCloud(boundary_cloud);

		// Neighbors within radius search
		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;

		for (int i = 0; i < boundary_cloud->size(); i++)
		{

			if (kdtree.radiusSearch(boundary_cloud->points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 2)
			{

				int K = pointIdxRadiusSearch.size();

				float max1_max2;
				max1_max2 = sqrt(pointRadiusSquaredDistance[K - 1]) - sqrt(pointRadiusSquaredDistance[K - 2]);

				float Xa, Xb, Xo, Ya, Yb, Yo, AOpBO, AO, BO, cosAOB;

				Xo = boundary_cloud->points[i].x;
				Yo = boundary_cloud->points[i].y;
				Xa = boundary_cloud->points[pointIdxRadiusSearch[K - 1]].x;
				Ya = boundary_cloud->points[pointIdxRadiusSearch[K - 1]].y;

				if (max1_max2 < disthreshold) //If the distance between the farthest and second farthest point is smaller than a threshold, then regard them points on the same side
				{
					float maxdis = 0;
					int maxindex = -1;
					float Xc, Yc, Xd, Yd;
					Xc = boundary_cloud->points[pointIdxRadiusSearch[K - 2]].x;
					Yc = boundary_cloud->points[pointIdxRadiusSearch[K - 2]].y;
					//The second farthest point find the farthest point in the former neighborhood
					for (int j = 0; j < K - 2; j++)
					{
						Xd = boundary_cloud->points[pointIdxRadiusSearch[j]].x;
						Yd = boundary_cloud->points[pointIdxRadiusSearch[j]].y;

						float dis = sqrt((Xd - Xc) * (Xd - Xc) + (Yd - Yc) * (Yd - Yc));

						if (dis > maxdis)
						{
							maxdis = dis;
							maxindex = j;
						}
					}
					Xb = boundary_cloud->points[pointIdxRadiusSearch[maxindex]].x;
					Yb = boundary_cloud->points[pointIdxRadiusSearch[maxindex]].y;
				}

				else
				{
					Xb = boundary_cloud->points[pointIdxRadiusSearch[K - 2]].x;
					Yb = boundary_cloud->points[pointIdxRadiusSearch[K - 2]].y;
				}
				//Calculate the intersection angle
				AOpBO = (Xa - Xo) * (Xb - Xo) + (Ya - Yo) * (Yb - Yo);
				AO = sqrt((Xa - Xo) * (Xa - Xo) + (Ya - Yo) * (Ya - Yo));
				BO = sqrt((Xb - Xo) * (Xb - Xo) + (Yb - Yo) * (Yb - Yo));
				cosAOB = abs(AOpBO / AO / BO);

				if (cosAOB < maxcos)
					corner_cloud->push_back(boundary_cloud->points[i]); //if the angle is smaller than a threshold, we regard it as a corner point
			}
		}
		return 1;
	}


protected:
private:
};
} // namespace lo
#endif //_INCLUDE_CLOUD_PROCESSING_HPP

标签:float,Xo,hpp,MULLS,SLAM,pcl,boundary,cloud,points
来源: https://blog.csdn.net/qq_35102059/article/details/121518137

本站声明: 1. iCode9 技术分享网(下文简称本站)提供的所有内容,仅供技术学习、探讨和分享;
2. 关于本站的所有留言、评论、转载及引用,纯属内容发起人的个人观点,与本站观点和立场无关;
3. 关于本站的所有言论和文字,纯属内容发起人的个人观点,与本站观点和立场无关;
4. 本站文章均是网友提供,不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属;如您发现该文章侵犯了您的权益,可联系我们第一时间进行删除;
5. 本站为非盈利性的个人网站,所有内容不会用来进行牟利,也不会利用任何形式的广告来间接获益,纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

专注分享技术,共同学习,共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有