计算点云的包围盒OBB

　　文中的点云文件和代码在这里：https://github.com/robinvista/pointcloud-OBB
　　为什么要计算点云的包围盒，它有什么用？对于无人驾驶和机器人来说，包围盒主要用来做碰撞检测和显示。
　　最简单的包围盒叫Axis-Aligned Bounding Box (AABB)，它的边与坐标轴平行，所以与实际物体相差较大，不是太好用。稍微复杂点的是Orientation Bounding Box（OBB），它与AABB的区别就是考虑了包围盒的方向，不一定与坐标轴平行，可以旋转。本文就是计算OBB的。计算OBB最难的是怎么确定box的方向。计算box的中心很简单，求所有点的平均值或者最大最小点的中间值即可。但是方向却需要计算特征向量。不用担心，现有的库或者软件都提供这样的函数，直接调用即可。以Eigen库为例，它在ROS、百度无人驾驶中都被使用，它里面的函数特点是：
　　Eigen中的eigenvalues函数返回的特征值是从小到大排列的，如下：

The eigenvalues are sorted in increasing order.

　　Mathematica中的Eigenvalues函数返回的特征值是从大到小排列的，如下。对于数值矩阵，Eigenvectors函数返回的特征向量是单位向量。

特征值依绝对值递减排列

　　MATLAB中的eig函数返回的特征值顺序是未排序的，需要用户自己排序，如下：

默认情况下，eig 并不总是返回已排序的特征值和特征向量。可以使用 sort 函数将特征值按升序排序，并重新排序相应的特征向量。

　　所需的代码如下：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef pcl::PointXYZI PointType;

int main(int argc, char* argv[])
{
    // 1. 读入点云文件
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud());
    pcl::PLYReader reader;
    reader.read("pointcloud.ply", *cloud);

    //2. 开始欧式聚类
    pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
    tree->setInputCloud(cloud);
    std::vector local_indices;
    pcl::EuclideanClusterExtraction euclid;
    euclid.setInputCloud(cloud);
    float in_max_cluster_distance = 0.5;
    float MIN_CLUSTER_SIZE = 5;
    float MAX_CLUSTER_SIZE = 500000;
    euclid.setClusterTolerance(in_max_cluster_distance);
    euclid.setMinClusterSize(MIN_CLUSTER_SIZE);
    euclid.setMaxClusterSize(MAX_CLUSTER_SIZE);
    euclid.setSearchMethod(tree);
    euclid.extract(local_indices);

    //3. 取出第i个聚类，用户输入i的值
    std::cout << "Number of clusters = " << local_indices.size() << endl;
    size_t i = 0;
    pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud());
    PointType point;
    for (auto pit = local_indices[i].indices.begin(); pit != local_indices[i].indices.end(); ++pit)
    {
        point.x = cloud->points[*pit].x;
        point.y = cloud->points[*pit].y;
        point.z = cloud->points[*pit].z;
        cloud_cluster->points.push_back(point);
    }

    //5. 计算点云的几何中心和重心，注意这两个概念不一样，重心是所有点的平均值，中心是最大最小点的平均值
    Eigen::Vector4f centroid;     // 重心
    pcl::compute3DCentroid(*cloud_cluster, centroid);
    PointType min_pt, max_pt;
    Eigen::Vector3f center;       // 中心
    pcl::getMinMax3D(*cloud_cluster, min_pt, max_pt); 
    center = (max_pt.getVector3fMap() + min_pt.getVector3fMap())/2;

    // 6. 计算协方差矩阵的特征向量
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*cloud_cluster, centroid, covariance); // 这里必须用重心
    Eigen::SelfAdjointEigenSolver 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));

    // 7. 计算变换矩阵，只考虑绕全局坐标系Z轴的转动
    Eigen::Vector3f ea = (eigenVectorsPCA).eulerAngles(2, 1, 0);
    Eigen::AngleAxisf keep_Z_Rot(ea[0], Eigen::Vector3f::UnitZ());
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translate(center);  // translate与rotate书写的顺序不可搞反
    transform.rotate(keep_Z_Rot); // radians

    // 8. 计算包围盒的尺寸
    pcl::PointCloud::Ptr transformedCloud(new pcl::PointCloud);
    pcl::transformPointCloud(*cloud_cluster, *transformedCloud, transform.inverse());
    // 重新计算变换后的点云的中心，因为变换前的点云的中心不是其包围盒的几何中心
    PointType min_pt_T, max_pt_T;
    pcl::getMinMax3D(*transformedCloud, min_pt_T, max_pt_T);
    Eigen::Vector3f center_new = (max_pt_T.getVector3fMap() + min_pt_T.getVector3fMap()) / 2;
    Eigen::Vector3f box_dim;
    box_dim = max_pt_T.getVector3fMap() - min_pt_T.getVector3fMap();
    Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
    transform2.translate(center_new);
    Eigen::Affine3f transform3 = transform * transform2;

    //9. 显示
    pcl::visualization::PCLVisualizer viewer;
    pcl::visualization::PointCloudColorHandlerCustom handler_1(cloud_cluster, 0, 255, 0);  //转换到原点的点云
    pcl::visualization::PointCloudColorHandlerCustom handler_2(transformedCloud, 255, 0, 0);   //输入的初始点云
    viewer.addPointCloud(cloud_cluster, handler_1, "cloud1");
    viewer.addPointCloud(transformedCloud, handler_2, "cloud2");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud1");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud2");
    const Eigen::Quaternionf bboxQ(keep_Z_Rot);
    const Eigen::Vector3f    bboxT(transform3.translation()); // 这里用新的"中心"，因为要显示的包围盒采用几何中心作为参考点
    viewer.addCube(bboxT, bboxQ, box_dim(0), box_dim(1), box_dim(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.addCoordinateSystem(2.0);
    viewer.setBackgroundColor(1, 1, 1);
    viewer.spin();
    return 0;
}

　　PCLVisualizer中的addCube函数的输入参数如下图，其中位姿都是用的Eigen的类型，尺寸的顺序就是分别沿XYZ轴（从上往下看，X轴是宽度，Y轴是高度，Z轴是深度）。

　　其它类似的计算OBB的项目有：
　　https://github.com/LidarPerception/object_builders_lib

　　但是，OBB计算出来的包围盒并不完美，我们知道包围盒并没有贴合物体的边缘。要对角度矫正需要知道边缘的方向，这个可以用RANSAC估计出来，但是有些慢。