注意:本示例为手写计算点云最小包围盒
与PCL自带的计算OBB包围盒的区别为:
本示例(1)适合计算实际的小场景物体的最小包围盒。如【人、树、车子】等;
(2)计算更快;
工作中若需计算点云最小包围盒,一般使用此示例(就算会损失一些精确性)。
前置知识:包围盒及特征向量等、协方差矩阵及PCA主成分分析等
1.原理
通过计算点云的特征值和特征向量,按照特征值和特征向量的相关参数,计算得到最小包围盒的参数值。
(1)计算点云的协方差矩阵,得到特征值和特征向量T。
(2)三个特征向量T1,T2,T3为当前点云的最小包围盒状态下的xyz轴。
按照特征向量T的xyz方向,旋转点云,使点云的XYZ(特征向量的XYZ方向)轴和系统的XYZ(即x(1,0,0),y(0,1,0),z(0,0,1))轴对齐。并记录所使用的旋转矩阵M。
此时,旋转后的点云的姿态即为最小包围盒姿态。
类似于下图,想象将这个小象的坐标轴旋转的和途中的坐标轴对齐。
(3)按照计算AABB盒的方法,计算旋转后的点云包围盒。
(4)计算上文旋转矩阵M的逆矩阵,将AABB盒的各参数(如顶点)转换到原始点云的位置坐标内。
得到的包围盒参数即为最小包围盒参数。
2.使用场景
需要计算最小包围盒的场景。
之前计算悬垂绝缘子的方向和形态的时候用过。
3.注意事项
(1)实际工作中,使用该方法计算最小包围盒。
(2)适合小场景,类似于某个小物体的最小包围盒计算,不适合大场景,大物体的最小包围盒计算。(不准确)
(3)不适合扁平点云的最小包围盒计算(协方差矩阵计算会不准确)。
4.关键函数
本示例函数复杂,学习的话,需要整篇代码理一下,不能只学某些函数。
5.代码
方法一为调用PCL接口,方法二为手动计算协方差矩阵,得到最小包围盒。
#include <iostream>
#include <pcl/common/pca.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
// #define COMMON_PCA
int main()
{
/****************添加点云********************/
// 原始点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("D:/code/csdn/data/person4.pcd", *cloud); // 加载原始点云数据
// 计算pca主成分分析(方法一)
#ifdef COMMON_PCA
pcl::PCA<pcl::PointXYZ> pca; // 计算PCA主成分分析
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPcaProj(new pcl::PointCloud<pcl::PointXYZ>); // 投影后的结果点云
pca.setInputCloud(cloud);
pca.project(*cloud, *cloudPcaProj); // 计算投影后点云
std::cout << "特征向量: " << pca.getEigenVectors() << std::endl; // 计算特征向量
std::cout << "特征值: " << pca.getEigenValues() << std::endl; // 计算特征值
#endif
// 计算PCA主成分分析(方法二)
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)); // 校正主方向间垂直(两两叉乘,类似于OpenGL中的计算相机的流程)
eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
// PCA的应用(计算最小包围盒)
// 计算协方差矩阵的变换矩阵及逆变换矩阵
Eigen::Matrix4f tm = Eigen::Matrix4f::Identity(); // 变换矩阵
Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity(); // 逆变换矩阵
tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); // 计算协方差矩阵所得的变换矩阵
tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) * (pcaCentroid.head<3>());
tm_inv = tm.inverse(); // 计算协方差矩阵得到的逆变换矩阵
// 计算最小包围盒
pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *transformedCloud, tm); // 将点云转换为协方差矩阵所在的位置(也就是将点云转正,其主方向的XYZ轴和坐标系的XYZ轴平行,通过对此计算包围盒,得到最终结果)
// 计算此时的点云包围盒(按照协方差矩阵转正之后的包围盒)
pcl::PointXYZ min_p1, max_p1; // 转换后包围盒最大最小位置的参数
pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
Eigen::Vector3f centerTrans = (min_p1.getVector3fMap() + max_p1.getVector3fMap()) / 2.0; // 转换后中心点
Eigen::Vector3f centerOriginal; // 原始中心点
pcl::transformPoint(centerTrans, centerOriginal, Eigen::Affine3f(tm_inv)); // 通过逆矩阵转换回去
// 生成点云最小包围盒参数
// 顶点计算
pcl::PointCloud<pcl::PointXYZ>::Ptr transVertexCloud(new pcl::PointCloud<pcl::PointXYZ>); // 先计算变换后点云包围盒的6个顶点
transVertexCloud->width = 6;
transVertexCloud->height = 1;
transVertexCloud->is_dense = false;
transVertexCloud->points.resize(transVertexCloud->width * transVertexCloud->height);
transVertexCloud->points[0].x = max_p1.x;
transVertexCloud->points[0].y = max_p1.y;
transVertexCloud->points[0].z = max_p1.z;
transVertexCloud->points[1].x = max_p1.x;
transVertexCloud->points[1].y = max_p1.y;
transVertexCloud->points[1].z = min_p1.z;
transVertexCloud->points[2].x = max_p1.x;
transVertexCloud->points[2].y = min_p1.y;
transVertexCloud->points[2].z = min_p1.z;
transVertexCloud->points[3].x = min_p1.x;
transVertexCloud->points[3].y = max_p1.y;
transVertexCloud->points[3].z = max_p1.z;
transVertexCloud->points[4].x = min_p1.x;
transVertexCloud->points[4].y = min_p1.y;
transVertexCloud->points[4].z = max_p1.z;
transVertexCloud->points[5].x = min_p1.x;
transVertexCloud->points[5].y = min_p1.y;
transVertexCloud->points[5].z = min_p1.z;
pcl::PointCloud<pcl::PointXYZ>::Ptr minVertexCloud(new pcl::PointCloud<pcl::PointXYZ>); // 最小包围盒的顶点
pcl::transformPointCloud(*transVertexCloud, *minVertexCloud, tm_inv);
// 中心点
const Eigen::Vector3f bboxCenter(centerOriginal); // 最小包围盒中心点
const Eigen::Quaternionf bboxQua(tm_inv.block<3, 3>(0, 0)); // 从垂直于xyz的轴逆转换回来的四元数
double width = (max_p1.getVector3fMap() - min_p1.getVector3fMap())(0); // 宽
double height = (max_p1.getVector3fMap() - min_p1.getVector3fMap())(1); // 高
double depth = (max_p1.getVector3fMap() - min_p1.getVector3fMap())(2); // 深
/****************展示********************/
// 点云
pcl::visualization::PCLVisualizer viewer;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloudColor(cloud, 255, 255, 0);
viewer.addPointCloud(cloud, cloudColor, "cloud");
// 顶点
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> vertexColor(cloud, 255, 0, 0);
viewer.addPointCloud(minVertexCloud, vertexColor, "vertex");
// 包围盒
viewer.addCube(bboxCenter, bboxQua, width, height, depth, "minBbox");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "minBbox");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "minBbox");
// 原始坐标轴
viewer.addCoordinateSystem(0.5);
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
return 0;
}
6.结果展示
坐标轴为系统坐标轴,即x(1,0,0),y(0,1,0),z(0,0,1)。
绿色框线即为计算出的最小包围盒。