PCL-计算点云AABB包围盒

一、概述
二、实验代码
三、效果展示
四、总结

一、概述

点云包围盒也叫外接最小矩形,是一种求解离散点集最优包围空间的算法，基本思想是用体积稍大且特性简单的几何体（称为包围盒）来近似地代替复杂的几何对象。常见的包围盒算法有AABB包围盒、包围球、方向包围盒OBB以及固定方向凸包FDH。
在这里插入图片描述
最小包围盒的计算过程大致如下:
1.利用PCA主元分析法获得点云的三个主方向，获取质心，计算协方差，获得协方差矩阵，求取协方差矩阵的特征值和特长向量，特征向量即为主方向。
2.利用1中获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重回，建立变换到原点的点云的包围盒。
3.给输入点云设置主方向和包围盒，通过输入点云到原点点云变换的逆变换实现。

AABB（Axis-Aligned Bounding Box，轴对齐包围盒）是一种简单的包围体积，用于快速地包含和近似复杂形状或点云。AABB的特点是它的所有边都与坐标轴平行，这使得它易于计算、存储和相交测试
对于点云数据，AABB可以通过找到点云在所有三个维度（X、Y、Z）上的最小和最大坐标值来构建。这些最小和最大坐标值定义了AABB的八个顶点（尽管实际上只需要六个参数来完全描述一个AABB：三个最小值和三个最大值）。

OBB相关

构建AABB的步骤：
1.初始化最小和最大坐标值：为X、Y、Z轴设置非常大的初始最大值和非常小的初始最小值。
2.遍历点云：对于点云中的每个点，更新X、Y、Z轴上的最小和最大坐标值。
如果点的X坐标小于当前最小X值，则更新最小X值。
如果点的X坐标大于当前最大X值，则更新最大X值。
3.对Y和Z坐标重复上述步骤。
计算AABB的中心点和尺寸：
中心点坐标是各轴最小和最大坐标值的平均值。
尺寸是各轴上最大和最小坐标值之差

二、实验代码

#include <iostream>
#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>using namespace std;
typedef pcl::PointXYZ PointType;int main(int argc, char** argv)
{pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());pcl::io::loadPCDFile("E:\\******.pcd", *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));cout << "特征值va(3x1):\n" << eigenValuesPCA << endl; // Eigen计算出来的特征值默认是从小到大排列cout << "特征向量ve(3x3):\n" << eigenVectorsPCA << endl;cout << "质心点(4x1):\n" << pcaCentroid << 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*ttm_inv = tm.inverse();std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);pcl::transformPointCloud(*cloud, *transformedCloud, tm);PointType min_p1, max_p1;Eigen::Vector3f c1, c;pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);c1 = 0.5f * (min_p1.getVector3fMap() + max_p1.getVector3fMap());cout << "型心c1(3x1):\n" << c1 << endl;Eigen::Affine3f tm_inv_aff(tm_inv);pcl::transformPoint(c1, c, tm_inv_aff);Eigen::Vector3f whd, whd1;whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();whd = whd1;float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3;  //点云平均尺度，用于设置主方向箭头大小cout << "width1=" << whd1(0) << endl;cout << "heght1=" << whd1(1) << endl;cout << "depth1=" << whd1(2) << endl;cout << "scale1=" << sc1 << endl;const Eigen::Quaternionf bboxQ1(Eigen::Quaternionf::Identity());const Eigen::Vector3f    bboxT1(c1);const Eigen::Quaternionf bboxQ(tm_inv.block<3, 3>(0, 0));const Eigen::Vector3f    bboxT(c);// 变换到原点的点云主方向PointType op;op.x = 0.0;op.y = 0.0;op.z = 0.0;Eigen::Vector3f px, py, pz;Eigen::Affine3f tm_aff(tm);pcl::transformVector(eigenVectorsPCA.col(0), px, tm_aff);pcl::transformVector(eigenVectorsPCA.col(1), py, tm_aff);pcl::transformVector(eigenVectorsPCA.col(2), pz, tm_aff);PointType pcaX;pcaX.x = sc1 * px(0);pcaX.y = sc1 * px(1);pcaX.z = sc1 * px(2);PointType pcaY;pcaY.x = sc1 * py(0);pcaY.y = sc1 * py(1);pcaY.z = sc1 * py(2);PointType pcaZ;pcaZ.x = sc1 * pz(0);pcaZ.y = sc1 * pz(1);pcaZ.z = sc1 * pz(2);// 初始点云的主方向PointType cp;cp.x = pcaCentroid(0);cp.y = pcaCentroid(1);cp.z = pcaCentroid(2);PointType pcX;pcX.x = sc1 * eigenVectorsPCA(0, 0) + cp.x;pcX.y = sc1 * eigenVectorsPCA(1, 0) + cp.y;pcX.z = sc1 * eigenVectorsPCA(2, 0) + cp.z;PointType pcY;pcY.x = sc1 * eigenVectorsPCA(0, 1) + cp.x;pcY.y = sc1 * eigenVectorsPCA(1, 1) + cp.y;pcY.z = sc1 * eigenVectorsPCA(2, 1) + cp.z;PointType pcZ;pcZ.x = sc1 * eigenVectorsPCA(0, 2) + cp.x;pcZ.y = sc1 * eigenVectorsPCA(1, 2) + cp.y;pcZ.z = sc1 * eigenVectorsPCA(2, 2) + cp.z;// 可视化pcl::visualization::PCLVisualizer viewer;viewer.setBackgroundColor(1.0, 1.0, 1.0);viewer.setWindowName("PCA获取点云包围盒");//输入的初始点云pcl::visualization::PointCloudColorHandlerCustom<PointType> color_handler(cloud, 255, 0, 0);viewer.addPointCloud(cloud, color_handler, "cloud");viewer.addCube(bboxT, bboxQ, whd(0), whd(1), whd(2), "bbox");viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "bbox");viewer.addArrow(pcX, cp, 1.0, 0.0, 0.0, false, "arrow_x");viewer.addArrow(pcY, cp, 0.0, 1.0, 0.0, false, "arrow_y");viewer.addArrow(pcZ, cp, 0.0, 0.0, 1.0, false, "arrow_z");//转换到原点的点云pcl::visualization::PointCloudColorHandlerCustom<PointType> tc_handler(transformedCloud, 0, 255, 0);viewer.addPointCloud(transformedCloud, tc_handler, "transformCloud");viewer.addCube(bboxT1, bboxQ1, whd1(0), whd1(1), whd1(2), "bbox1");viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox1");viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "bbox1");viewer.addArrow(pcaX, op, 1.0, 0.0, 0.0, false, "arrow_X");viewer.addArrow(pcaY, op, 0.0, 1.0, 0.0, false, "arrow_Y");viewer.addArrow(pcaZ, op, 0.0, 0.0, 1.0, false, "arrow_Z");viewer.addCoordinateSystem(0.5f * sc1);while (!viewer.wasStopped()){viewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(10000));}return 0;
}