PCL包围盒（详细介绍）

实现效果：

原理分析：
1.包围盒简介
包围盒也叫外接最小矩形,是一种求解离散点集最优包围空间的算法，基本思想是用体积稍大且特性简单的几何体（称为包围盒）来近似地代替复杂的几何对象。
常见的包围盒算法有AABB包围盒、包围球、方向包围盒OBB以及固定方向凸包FDH。碰撞检测问题在虚拟现实、计算机辅助设计与制造、游戏及机器人等领域有着广泛的应用，甚至成为关键技术。而包围盒算法是进行碰撞干涉初步检测的重要方法之一。

最小包围盒的计算过程大致如下:
1.利用PCA主元分析法获得点云的三个主方向，获取质心，计算协方差，获得协方差矩阵，求取协方差矩阵的特征值和特长向量，特征向量即为主方向。
2.利用1中获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重回，建立变换到原点的点云的包围盒。
3.给输入点云设置主方向和包围盒，通过输入点云到原点点云变换的逆变换实现。

PCA（Principal Component Analysis）主成分分析

主成分分析可以用来计算有多个变量组成的数据集的坐标系空间，数据集的多个变量可以分成多个不相关分量，存在一个顶点位置数组中的x,y,z坐标值就是这样的数据集（点云）。数据集中最不同的方向表示主分量的基本主元。
将每个点设为,点云就看作 N个点构成的数据集。首先利用以下公式计算位置平均数m

在构建一个协方差矩阵C，

协方差矩阵是由以下六个元素组成的对称矩阵：

协方差矩阵表示x,y,z坐标值之间的相互关系。若这三个坐标值两两无关，则他们在协方差矩阵上元素的值为0，为了将所有的点集中均匀的沿着坐标轴分布，需要将协方差矩阵转换成对角阵。我们可以利用线性代数里面的内容:

以作为该对象自然轴对应的方向，下面计算出从顶点沿X、Y、Z三个方向最大和最小位置，根据这些最大值和最小值可以容易的构建出边界盒的六个平面。

为了确定最大和最小的范围，可以通过计算每个顶点位置坐标Pi与单位向量的内积来完成。边界盒的6个平面分别为：

边界盒的尺寸就是X,Y,Z三个方向上相应内积的最大值与最小值之差，边界盒的中心O就是三对反向平面中三个平面的交点，令a,b,c分别为X,Y,Z上最大值与最小值的平均值：

PCA在很多地方还可以用到，笔者觉得了协方差矩阵、特征值、特征向量的几何意义是理解PCA的关键。上述代码中还用到四元数，四元数主要用于旋转变化。

代码如下：

#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>#include <pcl/visualization/cloud_viewer.h>int main (int argc, char** argv)
{if (argc != 2)return (0);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());if (pcl::io::loadPCDFile (argv[1], *cloud) == -1)return (-1);pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;feature_extractor.setInputCloud (cloud); //default clouldfeature_extractor.compute ();std::vector <float> moment_of_inertia;  //¹ßÐÔ¾àstd::vector <float> eccentricity;  //ÀëÐÄÂÊpcl::PointXYZ min_point_AABB;pcl::PointXYZ max_point_AABB;pcl::PointXYZ min_point_OBB;pcl::PointXYZ max_point_OBB;pcl::PointXYZ position_OBB;Eigen::Matrix3f rotational_matrix_OBB;float major_value, middle_value, minor_value; //eigenÊÇ¼ÆËã¾ØÕóµÄ¿ªÔ´¿âEigen::Vector3f major_vector, middle_vector, minor_vector;Eigen::Vector3f mass_center;feature_extractor.getMomentOfInertia (moment_of_inertia); //ÌØÕ÷ÌáÈ¡»ñÈ¡¹ßÐÔ¾àfeature_extractor.getEccentricity (eccentricity);  //ÌØÕ÷ÌáÈ¡»ñÈ¡ÀëÐÄÂÊfeature_extractor.getAABB (min_point_AABB, max_point_AABB); //ÌØÕ÷ÌáÈ¡AABBfeature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); //ÌØÕ÷ÌáÈ¡OBBfeature_extractor.getEigenValues (major_value, middle_value, minor_value); //»ñÈ¡ÌØÕ÷Öµfeature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); //ÌØÕ÷ÏòÁ¿feature_extractor.getMassCenter (mass_center); //»ñÈ¡×î´óÖÊÐÄ//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("µãÔÆ¿âPCLÑ§Ï°½Ì³ÌµÚ¶þ°æ-»ùÓÚ¹ßÐÔ¾ØÓëÆ«ÐÄÂÊµÄÃèÊö×Ó"));boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));viewer->setBackgroundColor (1, 1, 1); //±³¾°viewer->addCoordinateSystem (1.0); //Ìí¼Ó×ø±êÏµviewer->initCameraParameters ();  //³õÊ¼»¯Ïà»ú²ÎÊýviewer->addPointCloud<pcl::PointXYZ> (cloud,pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud,0,255,0), "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,5,"sample cloud");viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 0.0, 0.0, "AABB"); //Ìí¼ÓÕý·½ÐÎviewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,0.1,"AABB");  //ÉèÖÃ¶ÔÓ¦µãÁ¬Ïßviewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,4,"AABB");  //ÉèÖÃ¶ÔÓ¦µãÁ¬ÏßµÄ¿í¶ÈEigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); //ÏòÁ¿Î»ÖÃstd::cout<<"position_OBB: "<<position_OBB<<endl;std::cout<<"mass_center: "<<mass_center<<endl;Eigen::Quaternionf quat (rotational_matrix_OBB);  //ËÄÔªÓÃ·¨viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,1,"OBB");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,0.1,"OBB");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,4,"OBB");viewer->setRepresentationToWireframeForAllActors();pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");std::cout<<"size of cloud :"<<cloud->points.size()<<endl;std::cout<<"moment_of_inertia :"<<moment_of_inertia.size()<<endl;std::cout<<"eccentricity :"<<eccentricity.size()<<endl;//Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);//Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);//Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);//Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);//Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);//Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);//Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);//Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);//p1 = rotational_matrix_OBB * p1 + position;//p2 = rotational_matrix_OBB * p2 + position;//p3 = rotational_matrix_OBB * p3 + position;//p4 = rotational_matrix_OBB * p4 + position;//p5 = rotational_matrix_OBB * p5 + position;//p6 = rotational_matrix_OBB * p6 + position;//p7 = rotational_matrix_OBB * p7 + position;//p8 = rotational_matrix_OBB * p8 + position;//pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2));//pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2));//pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2));//pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2));//pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2));//pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2));//pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2));//pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2));//viewer->addLine (pt1, pt2, 1.0, 0.0, 0.0, "1 edge");//viewer->addLine (pt1, pt4, 1.0, 0.0, 0.0, "2 edge");//viewer->addLine (pt1, pt5, 1.0, 0.0, 0.0, "3 edge");//viewer->addLine (pt5, pt6, 1.0, 0.0, 0.0, "4 edge");//viewer->addLine (pt5, pt8, 1.0, 0.0, 0.0, "5 edge");//viewer->addLine (pt2, pt6, 1.0, 0.0, 0.0, "6 edge");//viewer->addLine (pt6, pt7, 1.0, 0.0, 0.0, "7 edge");//viewer->addLine (pt7, pt8, 1.0, 0.0, 0.0, "8 edge");//viewer->addLine (pt2, pt3, 1.0, 0.0, 0.0, "9 edge");//viewer->addLine (pt4, pt8, 1.0, 0.0, 0.0, "10 edge");//viewer->addLine (pt3, pt4, 1.0, 0.0, 0.0, "11 edge");//viewer->addLine (pt3, pt7, 1.0, 0.0, 0.0, "12 edge");while(!viewer->wasStopped()){viewer->spinOnce (100);boost::this_thread::sleep (boost::posix_time::microseconds (100000));}return (0);
}

Compiling and running the program
创建 CMakeLists.txt 文件，复制下面内容到该文件（文件和）。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(moment_of_inertia)find_package(PCL 1.8 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (moment_of_inertia moment_of_inertia.cpp)
target_link_libraries (moment_of_inertia ${PCL_LIBRARIES})

通常在build文件夹下，执行下面命令：

$ ./moment_of_inertia lamppost.pcd