点云OBB有向包围框处理思路

最近在做点云中的OBB算法工作，给定一个物体的点云，快速估计出物体的有向包围框。算法的主要原理是PCA（Principal Component Analysis，主成分分析），是数据分析的一种重要技术。它可以将高维数据投影到低维空间，找到数据的主要结构。在点云场景下，PCA用来提取点云的主成分方向和法向量方向。

PCL库已经实现好了OBB相关算法（pcl::MomentOfInertiaEstimation），MomentOfInertiaEstimation 类可以计算点云的 惯性矩、重心、主方向，同时可以得到 AABB（轴对齐包围盒）和OBB（方向对齐包围盒）。它内部会用点云的质心和协方差矩阵来求解主方向，从而得到有向包围盒。

注意1：该接口为模板函数，因为没有预编译pcl::PointXYZRGB类型的版本，编译时会报错。建议使用pcl::PointXYZ类型

注意2：该接口计算出的OBB不一定是最小包围盒

使用说明

// 头文件
#include <pcl/features/moment_of_inertia_estimation.h>

// 创建特征提取器对象，传入点云并计算
pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud(cloud_filtered);
feature_extractor.compute();

// 存储结果
pcl::PointXYZ min_point_OBB, max_point_OBB, position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);

计算结果：

min_point_OBB ：在 OBB坐标系下 的最小点坐标（即包围盒的八个顶点之一）。

max_point_OBB：在 OBB坐标系下 的最大点坐标（即包围盒的八个顶点之一）。

position_OBB：OBB 的中心点（世界坐标系下）。同时也是OBB坐标系到世界坐标系的平移量。

rotational_matrix_OBB：OBB 的旋转矩阵，用于把 OBB坐标系 转换到 世界坐标系。

有了OBB坐标系到世界坐标系的旋转平移矩阵后，就可以把OBB变换到世界坐标系下去包围住点云，或者将点云逆变换到OBB坐标系下填入包围盒内。在可视化窗口内绘制包围盒，可以使用pcl::visualization::PCLVisualizer类的addCube函数，addCube有三种重载形式。

addCube(x_min, x_max, y_min, y_max, z_min, z_max, r, g, b, id);
- 优点：简单，直接用坐标画。
- 缺点：无法指定旋转，立方体总是平行于世界坐标轴。
addCube(const pcl::ModelCoefficients &coefficients, id, viewport);
- coefficients 是长度为 10 的向量，前 3 个是中心，接着是三个方向向量的长度，最后是方向向量。
- 更适合从拟合模型直接创建立方体。
addCube(translation, rotation, width, height, depth, id, viewport);
- 完整的 刚体变换 方式。
- translation：中心点位置（Eigen::Vector3f）
- rotation：旋转（Eigen::Quaternionf）
- width/height/depth：尺寸

PCL库OBB（体素下采样加速）完整代码示例

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Dense>
#include <iostream>

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);

    if (argc < 2) {
        std::cerr << "Usage: " << argv[0] << " cloud.pcd" << std::endl;
        return -1;
    }

    if (pcl::io::loadPCDFile(argv[1], *cloud_rgb) == -1) {
        PCL_ERROR("Couldn't read file\n");
        return -1;
    }

    pcl::copyPointCloud(*cloud_rgb, *cloud_xyz);

    // ===== Step 0: 体素降采样 =====
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud_xyz);
    vg.setLeafSize(0.01f, 0.01f, 0.01f); // 5mm voxel,根据点云大小调整
    vg.filter(*cloud_filtered);

    // ====== Step 1: OBB 计算 ======
    pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud(cloud_filtered);
    feature_extractor.compute();

    pcl::PointXYZ min_point_OBB, max_point_OBB, position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);

    // ====== Step 2: 尺寸和位姿 ======
    float length = max_point_OBB.x - min_point_OBB.x;
    float width = max_point_OBB.y - min_point_OBB.y;
    float height = max_point_OBB.z - min_point_OBB.z;

    Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
    Eigen::Matrix3f R = rotational_matrix_OBB;

    std::cout << "OBB 尺寸: 长=" << length
        << " 宽=" << width
        << " 高=" << height << std::endl;

    std::cout << "旋转矩阵 R:\n" << R << std::endl;
    std::cout << "平移向量 t: " << position.transpose() << std::endl;

    // ====== Step 3: 可视化 ======
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("OBB Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud_rgb, "cloud");

    Eigen::Vector3f pos = position;
    Eigen::Quaternionf q(R);

    viewer->addCube(pos, q, length, width, height, "OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
        pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "OBB");

    viewer->addCoordinateSystem(0.1);

    while (!viewer->wasStopped()) {
        viewer->spinOnce(10);
    }

    return 0;
}

基于凸包的最小 OBB 计算（体素降采样加速版）示例

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/convex_hull.h>
#include <Eigen/Dense>
#include <iostream>

int main(int argc, char** argv)
{
    if (argc < 2) {
        std::cerr << "Usage: " << argv[0] << " cloud.pcd" << std::endl;
        return -1;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile(argv[1], *cloud) == -1) {
        PCL_ERROR("Couldn't read file\n");
        return -1;
    }

    // ===== Step 0: 体素降采样 =====
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.005f, 0.005f, 0.005f); // 5mm voxel,根据点云大小调整
    vg.filter(*cloud_filtered);

    // ===== Step 1: 计算凸包 =====
    pcl::ConvexHull<pcl::PointXYZ> chull;
    chull.setInputCloud(cloud_filtered);
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull(new pcl::PointCloud<pcl::PointXYZ>);
    chull.reconstruct(*hull);

    // ===== Step 2: 枚举凸包三点组合 =====
    Eigen::Vector3f best_dims;
    Eigen::Matrix3f best_rot = Eigen::Matrix3f::Identity();
    Eigen::Vector3f best_pos;
    float min_volume = std::numeric_limits<float>::max();

    for (size_t i = 0; i < hull->size(); ++i)
    {
        for (size_t j = i + 1; j < hull->size(); ++j)
        {
            for (size_t k = j + 1; k < hull->size(); ++k)
            {
                Eigen::Vector3f v1 = hull->points[j].getVector3fMap() - hull->points[i].getVector3fMap();
                Eigen::Vector3f v2 = hull->points[k].getVector3fMap() - hull->points[i].getVector3fMap();
                Eigen::Vector3f axis1 = v1.normalized();
                Eigen::Vector3f axis2 = v2.normalized();
                Eigen::Vector3f axis3 = axis1.cross(axis2).normalized();
                if (axis3.hasNaN()) continue;
                axis2 = axis3.cross(axis1);

                Eigen::Matrix3f R;
                R.col(0) = axis1;
                R.col(1) = axis2;
                R.col(2) = axis3;

                // 点云旋转到候选方向
                Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
                transform.block<3, 3>(0, 0) = R.transpose();
                pcl::PointCloud<pcl::PointXYZ> rotated;
                pcl::transformPointCloud(*cloud_filtered, rotated, transform);

                // 计算包围盒尺寸
                pcl::PointXYZ min_pt, max_pt;
                pcl::getMinMax3D(rotated, min_pt, max_pt);
                Eigen::Vector3f dims = (max_pt.getVector3fMap() - min_pt.getVector3fMap());
                float volume = dims.prod();

                if (volume < min_volume)
                {
                    min_volume = volume;
                    best_dims = dims;
                    best_rot = R;
                    best_pos = R * ((max_pt.getVector3fMap() + min_pt.getVector3fMap()) * 0.5f);
                }
            }
        }
    }

    std::cout << "最小 OBB 尺寸: " << best_dims.transpose() << std::endl;
    std::cout << "旋转矩阵:\n" << best_rot << std::endl;
    std::cout << "中心位置: " << best_pos.transpose() << std::endl;

    // ===== Step 3: 可视化 =====
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Min OBB Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");

    Eigen::Quaternionf q(best_rot);
    viewer->addCube(best_pos, q, best_dims.x(), best_dims.y(), best_dims.z(), "OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
        pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "OBB");

    while (!viewer->wasStopped())
        viewer->spinOnce(10);

    return 0;
}