SLAM框架：MULLS中主成分分析文件pca.hpp代码原理解析

SLAM框架：MULLS中主成分分析文件pca.hpp代码原理解析

激光slam框架，论文题目：

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

开源代码链接：

https://github.com/YuePanEdward/MULLS

原理：

整个pca.hpp文件只有不到500行，还包换了多个函数的重载，其核心功能就是针对每一个点，使用kdtree找临近点，对该点进行pca(主成分分析)，最后根据pca结果判断点是平面特征还是角点特征。

部分代码解析：

开头先是三个结构体，分别定义了一个点pca所包含的元素。

eigenvalue_t定一个一个点pca计算时，协方差矩阵的三个由大到小的特征值。

struct eigenvalue_t // Eigen Value ,lamada1 > lamada2 > lamada3;
{
	double lamada1;
	double lamada2;
	double lamada3;
};

eigenvector_t定一个 三个特征值对应的特征向量。

struct eigenvector_t //the eigen vector corresponding to the eigen value
{
	Eigen::Vector3f principalDirection;
	Eigen::Vector3f middleDirection;
	Eigen::Vector3f normalDirection;
};

pca_feature_t包含了这个点的pca信息，及判断后的特征标签。

struct pca_feature_t //PCA
{
	eigenvalue_t values;
	eigenvector_t vectors;
	double curvature;
	double linear;
	double planar;
	double spherical;
	double linear_2;
	double planar_2;
	double spherical_2;
	double normal_diff_ang_deg;
	pcl::PointNormal pt;
	int ptId;
	int pt_num = 0;
	std::vector neighbor_indices;
	std::vector close_to_query_point;
};

 get_pc_pca_feature()，对指定点，使用kdtree寻找临近点来进行pca：

// R - K 邻域（已经构建了 kd 树）
//在半径内，我们会选择最近的K个点来计算PCA
bool get_pc_pca_feature(typename pcl::PointCloud::Ptr in_cloud,
						std::vector &features, typename pcl::KdTreeFLANN::Ptr &tree,
						float radius, int nearest_k, int min_k = 1, int pca_down_rate = 1,
						bool distance_adaptive_on = false, float unit_dist = 35.0)
{
	//LOG(INFO) << "[" << in_cloud->points.size() << "] points used for PCA, pca down rate is [" << pca_down_rate << "]";
	features.resize(in_cloud->points.size());   // 每个点都有pca的值，

	omp_set_num_threads(min_(6, omp_get_max_threads()));
	#pragma omp parallel for												 //Multi-thread
	// 按指定距离跳跃，求点的pca。。pca_down_rate == 1 ，实际是求的每一个点的
	for (int i = 0; i < in_cloud->points.size(); i += pca_down_rate) //faster way
	{
		// if (i % pca_down_rate == 0) {//this way is much slower
		std::vector search_indices_used; //points would be stored in sequence (from the closest point to the farthest point within the neighborhood)
		std::vector search_indices;	  //point index vector
		std::vector squared_distances; //distance vector

		float neighborhood_r = radius;
		int neighborhood_k = nearest_k;

		if (distance_adaptive_on)
		{
			// 如果点太远，调整
			double dist = std::sqrt(in_cloud->points[i].x * in_cloud->points[i].x +
									in_cloud->points[i].y * in_cloud->points[i].y +
									in_cloud->points[i].z * in_cloud->points[i].z);
			if (dist > unit_dist)
			{
				neighborhood_r = std::sqrt(dist / unit_dist) * radius;       // 这样调整的依据？
				//neighborhood_k = (int)(unit_dist / dist * nearest_k));
			}
		}
		//nearest_k=0 --> the knn is disabled, only the rnn is used
		tree->radiusSearch(i, neighborhood_r, search_indices, squared_distances, neighborhood_k);

		features[i].pt.x = in_cloud->points[i].x;
		features[i].pt.y = in_cloud->points[i].y;
		features[i].pt.z = in_cloud->points[i].z;
		features[i].ptId = i;
		features[i].pt_num = search_indices.size();

		//deprecated
		features[i].close_to_query_point.resize(search_indices.size());
		// 判断每个临近点距离中心的距离，分类
		for (int j = 0; j < search_indices.size(); j++)
		{
			if (squared_distances[j] < 0.64 * radius * radius) // 0.5^(2/3)
				features[i].close_to_query_point[j] = true;
			else
				features[i].close_to_query_point[j] = false;
		}
		// 提取该点的pca特征
		get_pca_feature(in_cloud, search_indices, features[i]);

		if (features[i].pt_num > min_k)
			assign_normal(in_cloud->points[i], features[i]);    // 将点法线指定为 pca 法线向量
		// 释放内存，回收空间
		std::vector().swap(search_indices);
		std::vector().swap(search_indices_used);
		std::vector().swap(squared_distances);
	}
	//}
	return true;
}

get_pca_feature（）函数，具体的针对某个点进行pca：

// 计算pca的值，
bool get_pca_feature(typename pcl::PointCloud::Ptr in_cloud,
						std::vector &search_indices,
						pca_feature_t &feature)
{
	int pt_num = search_indices.size();

	if (pt_num <= 3)
		return false;

	typename pcl::PointCloud::Ptr selected_cloud(new pcl::PointCloud());
	for (int i = 0; i < pt_num; ++i)
		selected_cloud->points.push_back(in_cloud->points[search_indices[i]]);

	pcl::PCA pca_operator;
	pca_operator.setInputCloud(selected_cloud);

	// Compute eigen values and eigen vectors
	Eigen::Matrix3f eigen_vectors = pca_operator.getEigenVectors();
	Eigen::Vector3f eigen_values = pca_operator.getEigenValues();
	// 赋值给主方向和最小方向的特征向量，并单位化
	feature.vectors.principalDirection = eigen_vectors.col(0);
	feature.vectors.normalDirection = eigen_vectors.col(2);

	feature.vectors.principalDirection.normalize();
	feature.vectors.normalDirection.normalize();
	// 得出特征值
	feature.values.lamada1 = eigen_values(0);
	feature.values.lamada2 = eigen_values(1);
	feature.values.lamada3 = eigen_values(2);
	// 协防差矩阵的迹为0
	if ((feature.values.lamada1 + feature.values.lamada2 + feature.values.lamada3) == 0)
		feature.curvature = 0;
	else
		feature.curvature = feature.values.lamada3 / (feature.values.lamada1 + feature.values.lamada2 + feature.values.lamada3);

	// feature.linear_2 = (sqrt(feature.values.lamada1) - sqrt(feature.values.lamada2)) / sqrt(feature.values.lamada1);
	// feature.planar_2 = (sqrt(feature.values.lamada2) - sqrt(feature.values.lamada3)) / sqrt(feature.values.lamada1);
	// feature.spherical_2 = sqrt(feature.values.lamada3) / sqrt(feature.values.lamada1);
	feature.linear_2 = ((feature.values.lamada1) - (feature.values.lamada2)) / (feature.values.lamada1);
	feature.planar_2 = ((feature.values.lamada2) - (feature.values.lamada3)) / (feature.values.lamada1);
	feature.spherical_2 = (feature.values.lamada3) / (feature.values.lamada1);
	// 交换的方式清理内存
	search_indices.swap(feature.neighbor_indices);
	return true;
}

assign_normal（），最后根据pca结果，对特征点进行分类：

// is_palne_feature (true: 将点法线指定为 pca 法线向量, false: 将点法线指定为 pca 主方向向量
bool assign_normal(PointT &pt, pca_feature_t &pca_feature, bool is_plane_feature = true)
{
	// 是平面特征点时
	if (is_plane_feature)
	{
		pt.normal_x = pca_feature.vectors.normalDirection.x();
		pt.normal_y = pca_feature.vectors.normalDirection.y();
		pt.normal_z = pca_feature.vectors.normalDirection.z();
		pt.normal[3] = pca_feature.planar_2; //planrity
	}
	// 线特征点或角点时
	else
	{
		pt.normal_x = pca_feature.vectors.principalDirection.x();
		pt.normal_y = pca_feature.vectors.principalDirection.y();
		pt.normal_z = pca_feature.vectors.principalDirection.z();
		pt.normal[3] = pca_feature.linear_2; //linarity
	}
	return true;
}