PCL：基于PCL绘制包围盒代码实现（2）

本博客基于pcl::MomentOflnertiaEstimation类获取基于惯性矩（moment of inertia）与偏心率（eccentricity）的描述子，而该类的另一个功能就是提取有向包围盒（OBB）和坐标轴对齐包围盒（AABB），但是所提取的有向包围盒OBB并不一定是最小的包围盒。

主特征向量概念：

一个矩阵可以有多个特征值，在这些特征值中，模最大的那个特征值即主特征值（对于实数阵即绝对值最大的特征值），主特征值对应的特征向量称为主特征向量。（“主特征向量是指主特征值对应的特征向量而主特征值是指模最大(如果是实数的话就是绝对值最大)的特征值一般用幂迭代或者阿诺尔迪迭代等等可以求出主特征值和主特征向量”）

右手系原则：

右手系(right-hand system)是在空间中规定直角坐标系的方法之一。此坐标系中x轴，y轴和z轴的正方向是如下规定的：把右手放在原点的位置，使大姆指，食指和中指互成直角，把大姆指指向x轴的正方向，食指指向y轴的正方向时，中指所指的方向就是z轴的正方向。也可以按如下方法确定右手(左手)坐标系：如果当右手(左手)的大拇指指向第一个坐标轴(x轴)的正向，而其余手指以第二个轴(y轴)绕第一轴转动的方向握紧，就与第三个轴(z轴)重合，就称此坐标系为右手(左手)坐标系。

理论基础：

该特征提取的核心思想如下：首先计算点云的协方差矩阵，然后提取特征值和特征向量，并且构造局部坐标系，以点云重心为坐标系原心，得到的特征向量已被归一化，并且以主特征向量（Major Eigen Vector）作为X轴，最小特征值对应的特征向量（Minor Eigen Vector）作为Z轴，剩下的特征向量作为Y轴，符合右手坐标系原则。接下来进行迭代计算，每一次迭代，主特征向量即X轴按照循序绕其他两个轴（YZ轴）旋转，实现当中具体是外循环绕Y轴0到90度，内循环绕Z轴0到360度，每次添加一定的角度（用step_变量控制），最终的X轴正方向遍历了整个Z轴负方向的半球离散方向，我们将这个旋转主向量X轴作为当前轴。对于每个当前轴，计算一个惯性矩。另外，利用当前轴也可以计算一个偏心率，具体是将当前轴正向即旋转后的X轴正向视为假设平面的法向量，然后将输入点云投影到该假设平面上。接下来，根据获取的投影就可以构造一个偏心率，其中长轴和短轴的数值是通过投影后点云的协方差矩阵得到。这样就在每个当前轴上得到一个惯性矩和一个偏心率，迭代完成后得到两个向量来存储惯性矩和偏心率作为特征。类pcl::MomentOflnertiaEstimation不仅提供了对上述特征的获取过程，并且可以输出AABB（与坐标系本身的轴向对应），以及OBB，其中OBB就是局部坐标系对应的AABB。

PS：上述OBB对应的局部坐标系就是应用PCA找到的新的坐标系。AABB对应的是本身坐标系。

惯性矩：

惯性矩(moment of inertia of an area)是一个几何量，通常被用作描述截面抵抗弯曲的性质。

偏心率：

偏心率（Eccentricity）是用来描述圆锥曲线轨道形状的数学量，定义为曲线到定点（焦点）的距离与到定直线（准线）的距离之比。对于椭圆，偏心率即为两焦点间的距离（焦距，2c）和长轴长度（2a）的比值，即e=c/a（偏心率一般用e表示）。

圆锥曲线轨道形状与偏心率的关系。

代码如下：

#include<iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>using namespace std;int main(int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());pcl::io::loadPCDFile("F:\\coutsaved\\test\\boxes\\pcd\\boxes.pcd", *cloud);//------------创建pcl::MomentOfInertiaEstimation 类-------------pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;feature_extractor.setInputCloud(cloud);feature_extractor.compute();vector <float> moment_of_inertia;//存储惯性矩的特征向量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::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);//AABB对应的左下角和右上角坐标feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);//OBB对应的相关参数feature_extractor.getEigenValues(major_value, middle_value, minor_value);//三个特征值feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//三个特征向量feature_extractor.getMassCenter(mass_center);//点云中心坐标//------------------可视化---------------------------------cout << "蓝色的包围盒为OBB,红色的包围盒为AABB" << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("基于惯性矩与偏心率的描述子"));viewer->setBackgroundColor(0, 0, 0);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, 2, "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_COLOR, 0, 1, 0, "AABB");//绿框viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "AABB");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, "AABB");Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);cout << "position_OBB: " << position_OBB << endl;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.5, "OBB");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, "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");cout << "size of cloud :" << cloud->points.size() << endl;cout << "moment_of_inertia :" << moment_of_inertia.size() << endl;cout << "eccentricity :" << eccentricity.size() << endl;while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return (0);
}

结果如下：

蓝色框为OBB。

绿色框为AABB。

封装函数：https://blog.csdn.net/u012010729/article/details/104115932