SLAM框架：MULLS中的预处理文件cprocessing.hpp注释解析
作者：互联网
激光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