pcl中MomentOfInertiaEstimation计算有向包围盒

chen_jared

已于 2023-04-23 14:48:32 修改

阅读量1.5k

点赞数 1

文章标签： c++ 算法人工智能

于 2023-04-23 14:31:19 首次发布

本文链接：https://blog.youkuaiyun.com/jacken123456/article/details/130321018

版权

pcl::MomentOfInertiaEstimation 是 Point Cloud Library (PCL) 中的一个类，用于计算点云中物体的矩。它可以提供点云物体的三个主轴及其长度，以及物体的惯性矩阵等信息。通过使用 pcl::MomentOfInertiaEstimation 类，可以实现物体形状分析、分类和识别等功能。

具体来说， pcl::MomentOfInertiaEstimation 类将点云中的点看作质点，并在空间中构建一个二次型矩阵，该矩阵描述了点云物体的惯性特性。然后通过对二次型矩阵进行特征值分解，可以得到物体的惯性矩阵及其特征向量和特征值，从而计算出物体的三个主轴及其长度、长宽高、体积等信息。

在点云处理中，矩特征分析是一个常用的工具，例如可以通过矩特征来计算物体的质心、面积、方向、形心、协方差矩阵等，从而实现物体分类、跟踪、位姿估计以及三维重建等应用。

template <typename PointT> void
pcl::MomentOfInertiaEstimation<PointT>::computeOBB ()
{
  obb_min_point_.x = std::numeric_limits <float>::max ();
  obb_min_point_.y = std::numeric_limits <float>::max ();
  obb_min_point_.z = std::numeric_limits <float>::max ();

  obb_max_point_.x = std::numeric_limits <float>::min ();
  obb_max_point_.y = std::numeric_limits <float>::min ();
  obb_max_point_.z = std::numeric_limits <float>::min ();

  unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
  {
    float x = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
              (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
              (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
    float y = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
              (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
              (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
    float z = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
              (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
              (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2);

    if (x <= obb_min_point_.x) obb_min_point_.x = x;
    if (y <= obb_min_point_.y) obb_min_point_.y = y;
    if (z <= obb_min_point_.z) obb_min_point_.z = z;

    if (x >= obb_max_point_.x) obb_max_point_.x = x;
    if (y >= obb_max_point_.y) obb_max_point_.y = y;
    if (z >= obb_max_point_.z) obb_max_point_.z = z;
  }

  obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0),
                            major_axis_ (1), middle_axis_ (1), minor_axis_ (1),
                            major_axis_ (2), middle_axis_ (2), minor_axis_ (2);

  Eigen::Vector3f shift (
    (obb_max_point_.x + obb_min_point_.x) / 2.0f,
    (obb_max_point_.y + obb_min_point_.y) / 2.0f,
    (obb_max_point_.z + obb_min_point_.z) / 2.0f);

  obb_min_point_.x -= shift (0);
  obb_min_point_.y -= shift (1);
  obb_min_point_.z -= shift (2);

  obb_max_point_.x -= shift (0);
  obb_max_point_.y -= shift (1);
  obb_max_point_.z -= shift (2);

  obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
}

这段代码是 pcl::MomentOfInertiaEstimation 类中的 computeOBB() 函数的实现，用于计算点云的最小有向边界盒（OBB）。

首先，通过遍历待计算的点云，计算每个点相对于主轴、中轴和次轴的投影，并更新 OBB 盒的最大、最小边界坐标。其中，主轴、中轴和次轴通过矩阵分解得到，并存储在类中的 major_axis_、middle_axis_ 和 minor_axis_ 中。

然后，将计算出的主轴、中轴和次轴组成旋转矩阵，并计算 OBB 盒的位置（即重心）和边界框大小。最后，将 OBB 盒边界坐标减去重心坐标，得到相对于 OBB 盒中心的坐标。

首先，通过以下语句初始化 OBB 盒顶点坐标的最小值和最大值：

obb_min_point_.x = std::numeric_limits <float>::max ();
obb_min_point_.y = std::numeric_limits <float>::max ();
obb_min_point_.z = std::numeric_limits <float>::max ();

obb_max_point_.x = std::numeric_limits <float>::min ();
obb_max_point_.y = std::numeric_limits <float>::min ();
obb_max_point_.z = std::numeric_limits <float>::min ();

然后遍历待计算的点云，对每个点计算其相对于主轴、中轴和次轴的投影，并根据这些投影来更新 OBB 盒的最大、最小边界坐标：

for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
{
  float x = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) +
            (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) +
            (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2);
  float y = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) +
            (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) +
            (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2);
  float z = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) +
            (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) +
            (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2);

  if (x <= obb_min_point_.x) obb_min_point_.x = x;
  if (y <= obb_min_point_.y) obb_min_point_.y = y;
  if (z <= obb_min_point_.z) obb_min_point_.z = z;

  if (x >= obb_max_point_.x) obb_max_point_.x = x;
  if (y >= obb_max_point_.y) obb_max_point_.y = y;
  if (z >= obb_max_point_.z) obb_max_point_.z = z;
}

这里首先计算了每个点相对于重心坐标系下的主轴、中轴和次轴的投影坐标，并将其保存在 x、y 和 z 变量中。然后，根据这些投影坐标来更新 OBB 盒的最大、最小边界坐标。

接下来，通过以下代码将主轴、中轴和次轴组成旋转矩阵：

obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0),
                          major_axis_ (1), middle_axis_ (1), minor_axis_ (1),
                          major_axis_ (2), middle_axis_ (2), minor_axis_ (2);

这里使用了 Eigen 矩阵库来创建一个 3x3 的旋转矩阵，并将主轴、中轴和次轴作为矩阵的列向量。

然后，通过以下代码计算 OBB 盒的位置（即重心）和边界框大小：

Eigen::Vector3f shift (
  (obb_max_point_.x + obb_min_point_.x) / 2.0f,
  (obb_max_point_.y + obb_min_point_.y) / 2.0f,
  (obb_max_point_.z + obb_min_point_.z) / 2.0f);

obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;

obb_min_point_.x -= shift (0);
obb_min_point_.y -= shift (1);
obb_min_point_.z -= shift (2);

obb_max_point_.x -= shift (0);
obb_max_point_.y -= shift (1);
obb_max_point_.z -= shift (2);

这里首先计算了 OBB 盒的重心坐标 shift，即将盒子最大和最小顶点的坐标平均值作为重心。然后，通过旋转矩阵将盒子的重心坐标从重心坐标系转换到原始坐标系中，并将其保存在 obb_position_ 变量中。最后，再将盒子顶点坐标减去盒子的重心坐标，得到相对于盒子中心的坐标。这里使用了 Eigen 矢量库来计算向量之间的加法和减法。

总的来说，这段代码实现了计算点云 OBB 盒的全部步骤，并用各种成员变量来存储计算结果，方便后续应用程序进行分析和处理。

PCL官网中的例子：

#include <vector>
#include <thread>
 
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
 
using namespace std::chrono_literals;
 
int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile("table_scene_lms400.pcd", *cloud) == -1)
        return (-1);
 
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud(cloud);
    feature_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::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);
    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues(major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter(mass_center);
 
    //aabb外接立方体
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "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, 1.0, 0.0, "AABB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_POINTS, "AABB");
 
    //obb外接立方体，最小外接立方体
    Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
    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_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(100);
        std::this_thread::sleep_for(100ms);
    }
 
    return (0);
}

feature_extractor.getMomentOfInertia(moment_of_inertia) 用于获取点云的惯性矩。具体来说，该方法将计算得到的点云惯性矩赋值给输入的 moment_of_inertia 向量。这个向量的长度为 6，分别存储了点云绕 x、y、z 坐标轴旋转的惯性矩以及旋转坐标系后的三对相互垂直的惯性矩。这些数据是计算点云形状特征的重要信息。

点云的惯性矩与点云的形状密切相关。惯性矩是描述物体沿不同坐标轴旋转惯性大小的物理量，它可以被用来计算物体的质心、轴向方差和惯性轴等特征，是描述物体形状和旋转状态的重要信息。例如，在三维空间中，一个点云的长、宽、高等尺寸特征可以通过惯性矩计算得到。此外，惯性矩也可以用于识别旋转中的点云，如最小外接立方体 (OBB) 的方向就可以通过点云的惯性矩计算得出，并进一步展示点云在三维空间中的方向和大小。

getEccentricity(eccentricity)函数接收一个名为 eccentricity 的变量作为参数，并将计算得到的偏心率值赋值给该变量。在三维点云处理领域，偏心率用于描述点云形状的离心程度。在数学和物理学中，偏心率是一个描述椭圆形状的参数，其值介于 0 和 1 之间，当偏心率为 0 时，表示椭圆退化成圆形，而当偏心率越接近 1，表示椭圆越扁平。在计算机视觉和图像处理领域中，偏心率通常被用作图像特征的一种度量方式，用于描述图像中目标物体的形状偏向程度。

偏心率是指点云中每个点到定点(焦点)的距离与到定直线(准线)的距离之比，也可定义为二阶矩的长轴与短轴之比。对于点云形状更接近椭球形或圆柱形的物体，其偏心率值会较小，表示形状趋近于圆心对称；而对于点云形状离心程度较大的物体，例如长条形的木板等，其偏心率值会较大。