描述
使用PCL库对点云的一些工程化操作，接上一篇。这一篇主要以PCA为核心。
原理
PCA代表主成分分析，它是一种用于降低数据集维度的技术，同时保留尽可能多的原始信息。在点云的背景下，主成分分析可以用来找到点云的主轴，然后可以用来表示低维空间中的点云。从PCA获得的特征向量表示点云的主轴。这些轴按重要性排序，第一个轴最重要，最后一个轴最不重要。每个特征向量是指向相应主轴方向的单位向量。特征向量的长度表示相应主轴的重要性。
特征向量和原始点云之间的关系是，特征向量可以用于表示低维空间中的点云。具体而言，特征向量可用于将点云投影到较低维度的子空间上，该子空间捕获点云中最重要的信息。这种低维表示可以用于诸如聚类、分类和可视化之类的任务。
如果点云以任何方式进行变换，例如通过平移或旋转，则特征向量也会发生变化。因此，在应用任何变换之前，将主成分分析应用于原始点云是很重要的。
代码
1. 点云主成分分析（PCA）
#include <iostream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
int main(int argc, char **argv)
{
// 输入点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
// std::string fileName = "/home/chen/CodeBase/c++_try/dragon.pcd";
// pcl::io::loadPCDFile(fileName.c_str(), *cloud);
std::string fileName = "/home/chen/CodeBase/c++_try/bunny.ply";
pcl::io::loadPLYFile(fileName.c_str(), *cloud);
Eigen::Vector4f pcaCentroid; // 点云的质心
pcl::compute3DCentroid(*cloud, pcaCentroid); // 计算质心
Eigen::Matrix3f covariance; // 协方差矩阵
pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors(); // 特征向量
Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues(); // 特征值
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
std::cout << "原始点云的特征值value(3x1):\n" << eigenValuesPCA << std::endl;
std::cout << "原始点云的特征向量vector(3x3):\n" << eigenVectorsPCA << std::endl;
std::cout << "原始点云的质心(4x1):\n" << pcaCentroid << std::endl;
/*
// 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量相似特征向量
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloudSegmented);
pca.project(*cloudSegmented, *cloudPCAprojection);
std::cerr << std::endl << "EigenVectors: " << pca.getEigenVectors() << std::endl;//计算特征向量
std::cerr << std::endl << "EigenValues: " << pca.getEigenValues() << std::endl;//计算特征值
*/
Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); // R
tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());// -R*t
tm_inv = tm.inverse();
std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;
std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *transformedCloud, tm); // 原始点云变换到特征点云
pcl::PointXYZ min_p1, max_p1;
Eigen::Vector3f c_feature, c;
pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
c_feature = 0.5f*(min_p1.getVector3fMap() + max_p1.getVector3fMap());
std::cout << "特征形心(3x1):\n" << c_feature << std::endl;
Eigen::Affine3f tm_inv_aff(tm_inv);
pcl::transformPoint(c_feature, c, tm_inv_aff); // 特征形心去求原始形心
std::cout << "原始形心(3x1):\n" << c << std::endl;
// ！最重要的变量!
// 代表：任意形状点云，它的三个特征主方向的长度值
Eigen::Vector3f feature;
feature = max_p1.getVector3fMap() - min_p1.getVector3fMap();
float vectorXYZ_mean = (feature(0) + feature(1) + feature(2)) / 3; //点云平均尺度，用于设置主方向箭头大小
std::cout << "width = " << feature(0) << std::endl; // 点云的宽（特征方向，非原始xyz方向）
std::cout << "height = " << feature(1) << std::endl; // 点云的高（特征方向，非原始xyz方向）
std::cout << "depth = " << feature(2) << std::endl; // 点云的长（特征方向，非原始xyz方向）
std::cout << "mean = " << vectorXYZ_mean << std::endl; // 点云长宽高的平均值
const Eigen::Quaternionf box_rotate_feature(Eigen::Quaternionf::Identity());
const Eigen::Vector3f box_pose_feature(c_feature); // 点云的特征形心
const Eigen::Quaternionf box_rotate_origin(tm_inv.block<3, 3>(0, 0));
const Eigen::Vector3f box_pose_origin(c); // 原始点云的形心
pcl::PointXYZ center; // 原始点云的质心
center.x = pcaCentroid(0); // 原始点云质心的xyz坐标
center.y = pcaCentroid(1);
center.z = pcaCentroid(2);
// 原始点云的三个特征方向的轴射线，每个轴射线设置一个固定长度，每个轴的方向取决于点云的三个特征主方向
// 每个轴射线的末端点 = 轴长 * 轴方向 + 质心坐标
pcl::PointXYZ x_axis_origin;
x_axis_origin.x = vectorXYZ_mean * eigenVectorsPCA(0, 0) + center.x;
x_axis_origin.y = vectorXYZ_mean * eigenVectorsPCA(1, 0) + center.y;
x_axis_origin.z = vectorXYZ_mean * eigenVectorsPCA(2, 0) + center.z;
pcl::PointXYZ y_axis_origin;
y_axis_origin.x = vectorXYZ_mean * eigenVectorsPCA(0, 1) + center.x;
y_axis_origin.y = vectorXYZ_mean * eigenVectorsPCA(1, 1) + center.y;
y_axis_origin.z = vectorXYZ_mean * eigenVectorsPCA(2, 1) + center.z;
pcl::PointXYZ z_axis_origin;
z_axis_origin.x = vectorXYZ_mean * eigenVectorsPCA(0, 2) + center.x;
z_axis_origin.y = vectorXYZ_mean * eigenVectorsPCA(1, 2) + center.y;
z_axis_origin.z = vectorXYZ_mean * eigenVectorsPCA(2, 2) + center.z;
pcl::PointXYZ center_zero; // 原点
center_zero.x = 0.0;
center_zero.y = 0.0;
center_zero.z = 0.0;
Eigen::Affine3f tm_aff(tm);
Eigen::Vector3f px = eigenVectorsPCA.col(0); // 原点云的三个特征向量
Eigen::Vector3f py = eigenVectorsPCA.col(1);
Eigen::Vector3f pz = eigenVectorsPCA.col(2);
pcl::transformVector(px, px, tm_aff); // 计算得到在特征空间下，xyz三个方向的值
pcl::transformVector(py, py, tm_aff);
pcl::transformVector(pz, pz, tm_aff);
// 原始点云的三个特征方向，均变换到原始坐标系xyz方向
pcl::PointXYZ x_axis_feature;
x_axis_feature.x = vectorXYZ_mean * px(0);
x_axis_feature.y = vectorXYZ_mean * px(1);
x_axis_feature.z = vectorXYZ_mean * px(2);
pcl::PointXYZ y_axis_feature;
y_axis_feature.x = vectorXYZ_mean * py(0);
y_axis_feature.y = vectorXYZ_mean * py(1);
y_axis_feature.z = vectorXYZ_mean * py(2);
pcl::PointXYZ z_axis_feature;
z_axis_feature.x = vectorXYZ_mean * pz(0);
z_axis_feature.y = vectorXYZ_mean * pz(1);
z_axis_feature.z = vectorXYZ_mean * pz(2);
//visualization
pcl::visualization::PCLVisualizer viewer;
// 特征点云（绿色）
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tc_handler(transformedCloud, 0, 255, 0); // rgb
viewer.addPointCloud(transformedCloud, tc_handler, "transformCloud");
viewer.addCube(box_pose_feature, box_rotate_feature, feature(0), feature(1), feature(2), "box_feature");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,
"box_feature");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "box_feature"); // rgb
// 特征点云的三个轴
viewer.addArrow(x_axis_feature, center_zero, 1.0, 0.0, 0.0, false, "arrow_X"); // 红， 格式rgb
viewer.addArrow(y_axis_feature, center_zero, 0.0, 1.0, 0.0, false, "arrow_Y"); // 绿， 格式rgb
viewer.addArrow(z_axis_feature, center_zero, 0.0, 0.0, 1.0, false, "arrow_Z"); // 蓝， 格式rgb
// 原始点云（红色）
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler(cloud, 255, 0, 0); // rgb
viewer.addPointCloud(cloud, color_handler, "cloud");
viewer.addCube(box_pose_origin, box_rotate_origin, feature(0), feature(1), feature(2), "box_origin");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,
"box_origin");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "box_origin");
// 原始点云的三个轴
viewer.addArrow(x_axis_origin, center, 1.0, 0.0, 0.0, false, "arrow_x");
viewer.addArrow(y_axis_origin, center, 0.0, 1.0, 0.0, false, "arrow_y");
viewer.addArrow(z_axis_origin, center, 0.0, 0.0, 1.0, false, "arrow_z");
viewer.addCoordinateSystem(0.5f * vectorXYZ_mean);
viewer.setBackgroundColor(1.0, 1.0, 1.0);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
}
return 0;
}
这是运行结果的截图，数据使用的是斯坦福兔子
代码输出结果为
原始点云的特征值value(3x1):
0.000710877
0.0011751
0.00231208
原始点云的特征向量vector(3x3):
0.147761 -0.724689 0.673048
0.323219 -0.607769 -0.72536
0.934718 0.324722 0.144428
原始点云的质心(4x1):
-0.0267599
0.0952158
0.00894712
1
变换矩阵tm(4x4):
0.147761 0.323219 0.934718 -0.0351845
-0.724689 -0.607769 0.324722 0.0355712
0.673048 -0.72536 0.144428 0.0857842
0 0 0 1
逆变矩阵tm'(4x4):
0.147761 -0.724688 0.673047 -0.0267599
0.323218 -0.607768 -0.72536 0.0952158
0.934717 0.324721 0.144428 0.00894712
-0 0 -0 1
特征形心(3x1):
-0.00569136
-0.00116282
-0.00634349
原始形心(3x1):
-0.0310277
0.0986843
0.00233355
width = 0.104996
height = 0.150444
depth = 0.193079
mean = 0.149506
由于PCA是比较重要的知识，这里对代码多做出一些解释。PCA是主成分分析，具体知识请去学习矩阵论的相关知识。这里只介绍应用。
-
目的
我们的目的是，对目标点云，求出它的三个主成分，并将点云框出来
-
分析
显然，对于任意形状的点云来说，它的三个主方向极大概率不平行于原坐标系的xyz三个轴。我们要做的，就是通过PCA分析，得到点云的三个最大的数据方向，并记录。同时，我们可以根据求出的特征方向，得到一系列的信息。该特征方向就是点云实际的长宽高，而不是xyz方向上的范围值
-
代码输入及输出
输入： 点云数据
输出： 原始点云的外包框、点云数据三个特征方向的值（长宽高） -
用途
我们可以利用PCA，得到某一团点云数据的特征，例如我们想寻找一个方向任意的长方体，它可能是平行于xyz坐标轴方向的，但它大概率是以任意角度出现在数据中的。这时我们可以先找到可能是它的点云簇，对这团点云簇进行PCA分析，对得到的三个值进行判断，找到满足长方体特征的点云簇
2. 点云特征分析
主要以第一节的代码为输入，上一节说道通过PCA我们可以得到点云簇的三个任意方向的值。在上述代码中，这个值是这样计算出来的
Eigen::Vector3f feature;
feature = max_p1.getVector3fMap() - min_p1.getVector3fMap();
float vectorXYZ_mean = (feature(0) + feature(1) + feature(2)) / 3;
阅读代码可以发现这三个值分别代表点云数据实际的长宽高，与坐标系无关的长宽高
feature(0) // 宽
feature(0) // 高
feature(0) // 长
3. 画出点云外包框
addCube(box_pose_origin, box_rotate_origin, feature(0), feature(1), feature(2), "box_origin");
输入参数分别是：长方体的中心、长方体的旋转四元数、宽、高、长、名字
这是PCL头文件中对AddCube的定义，可以参考
/** \brief Add a cube from a set of given model coefficients
* \param[in] translation a translation to apply to the cube from 0,0,0
* \param[in] rotation a quaternion-based rotation to apply to the cube
* \param[in] width the cube's width
* \param[in] height the cube's height
* \param[in] depth the cube's depth
* \param[in] id the cube id/name (default: "cube")
* \param[in] viewport (optional) the id of the new viewport (default: 0)
*/
bool addCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation,
double width, double height, double depth,
const std::string &id = "cube",
int viewport = 0);
4. 点云的质心与形心
简单来说，点云的质心体现着点云的密度，形心体现着点云的外包框。二者并不相同，无论是从数学上还是工程上，质心与形心并不一致，上述代码已经说明了如何求出质心与形心了。
观察输出结果
原始点云的质心(4x1):
-0.0267599
0.0952158
0.00894712
1
我们会发现，质心与形心虽然数据值相近，但还是存在差值。
原始形心(3x1):
-0.0310277
0.0986843
0.00233355
5. SVD分解求奇异值与奇异向量
struct Ground {
float x;
float y;
float z;
};
std::vector<Ground> ground;
for (int i = 0; i < 5; i++) {
Ground g;
g.x = i;
g.y = i + 1;
g.z = i * i;
ground.push_back(g);
}
std::cout<<"input:"<<std::endl;
for (int i = 0; i < ground.size(); i++) {
std::cout<<ground[i].x<<" "<<ground[i].y<<" "<<ground[i].z<<std::endl;
}
std::cout<<std::endl;
Eigen::MatrixX3f eigen_ground(ground.size(), 3);
int j=0;
for (auto &p: ground) {
eigen_ground.row(j++) << p.x, p.y, p.z;
}
Eigen::MatrixX3f centered = eigen_ground.rowwise() - eigen_ground.colwise().mean();
Eigen::MatrixX3f cov = (centered.adjoint() * centered) / double(eigen_ground.rows() - 1);
Eigen::VectorXf mean;
mean.resize(3);
mean << eigen_ground.colwise().mean()(0), eigen_ground.colwise().mean()(1), eigen_ground.colwise().mean()(2);
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(cov, Eigen::DecompositionOptions::ComputeFullU);
Eigen::VectorXf singular_values;
singular_values = svd.singularValues();
// use the least singular vector as normal
Eigen::MatrixX3f singular_normal;
singular_normal = svd.matrixU();
std::cout<<"singular values: \n"<<singular_values<<std::endl;
std::cout<<"singular normal: \n"<<singular_normal<<std::endl;
std::cout<<"one of singular normal: \n"<<singular_normal.col(2)<<std::endl;
补充
猴子的原始数据是红色的，如果单独画出猴子原始数据，你会发现你需要移动鼠标滚轮才能在下方找到猴子。我们用上述代码将猴子的特征数据用绿色画出来了，绿色数据的原点就是（0，0，0）位置
总结
基于PCA进行点云数据的分析
评论（0）
您还未登录，请登录后发表或查看评论