3D包容盒子

原理简述

        包围体（包容盒）是一个简单的几何空间，里面包含着复杂形状的物体。为物体添加包围体的目的是快速的进行碰撞检测或者进行精确的碰撞检测之前进行过滤（即当包围体碰撞，才进行精确碰撞检测和处理）。包围体类型包括球体、轴对齐包围盒（AABB）、有向包围盒（OBB）、8-DOP以及凸壳（CONVEX HULL）。

常见包容盒（ Bounding Volumes）分类：

• 包容球：SPHERE 用球体包围整个几何体，用于相交测试很方便，但是其紧密型差，周围空隙较大，当物体变形后，包围球需要重新计算。当对象进行旋转运动时，包围球不需要做任何更新，这是包围球的优势，即当几何对象频繁进行旋转运动时，使用包围球效率较高。
• AABB包容盒：Axially Aligned Bounding Box，3D环境下的AABB盒即一个六面体，每个边都平行于一个坐标平面，较简单，但紧密性较差，当物体旋转、形变之后需要对AABB进行更新。本身的长宽高根据物体大小而定。
• OBB包容盒：Oriented Bounding Box，此方法紧密型较好，可以降低参与相交测试的包容盒数目，因此性能要优于AABB和包容球。当物体发生旋转，仅需对OBB进行相同的旋转即可，但是当物体形变后，更新OBB的代价较大，故不适用那些软体的对象。
• 8-DOP：8-DOP是由八个平面构成的。每个平面都与其他七个平面相交，形成一个闭合的多边形。这些平面可以通过基于物体的最小和最大坐标值来计算。也就是说，对于给定的物体，可以找到其最小和最大的X、Y和Z坐标值，并使用这些值来计算八个平面。8-DOP可以用于快速估计物体的包围盒，以便进行碰撞检测等操作。由于它是使用八个面来近似包围物体的，因此可能不够精确，但通常能提供一个较好的近似结果。它在某些情况下比其他简单的包围体（如轴对齐AABB包围盒）更有效，因为它可以更好地适应物体的形状。
• 凸壳（CONVEX HULL）：凸壳（Convex Hull）是指包含一组点集的最小凸多边形或凸体。在三维空间中，凸壳是一个封闭的、由凸面构成的立体结构。给定一组三维点，凸壳可以用来估计点云的整体形状，并提供一种紧凑的表示形式。计算凸壳的算法有很多种，其中著名的算法包括：Graham扫描算法：基于极角排序的凸壳计算算法。Jarvis march算法（也称为Gift Wrapping算法）：通过不断选择点集中的最外层点来计算凸壳。QuickHull算法：利用分治法的思想，递归地划分空间以计算凸壳。3D Incremental算法：从一个简单的初始凸壳开始，逐步添加点以构建凸壳。

        如上图所示，，越靠右，包容效果好、越紧密。但是检测速度更慢，也更消耗内存资源。 

代码实现 

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

using namespace std::chrono_literals;

int main(int argc, char** argv)
{
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    // 从文件读取点云图
    pcl::PCDReader reader;
    reader.read("G:/vsdata/PCLlearn/PCDdata/bun0.pcd", *cloud);

    // 创建惯性矩估算对象，设置输入点云，并进行计算
    pcl::MomentOfInertiaEstimation  feature_extractor;
    feature_extractor.setInputCloud(cloud);
    feature_extractor.compute();

    std::vector  moment_of_inertia;
    std::vector  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);
    // 获取AABB盒子
    feature_extractor.getAABB(min_point_AABB, max_point_AABB);
    // 获取OBB盒子
    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues(major_value, middle_value, minor_value);
    // 获取主轴major_vector，中轴middle_vector，辅助轴minor_vector
    feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
    // 获取质心
    feature_extractor.getMassCenter(mass_center);

    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    viewer->addPointCloud(cloud, "sample cloud");
    // 添加AABB包容盒
    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, 1.0, 0.0, "AABB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB");

    // 添加OBB包容盒
    Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
    Eigen::Quaternionf quat(rotational_matrix_OBB);
    // 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  深度
    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_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");

    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");
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(10);
        std::this_thread::sleep_for(10ms);
    }

    return (0);
}

实现效果

        黄色立方体为AABB包容盒，白色立方体为OBB包容盒。