网站导航
> 文章列表 > pcl中MomentOfInertiaEstimation计算有向包围盒

pcl中MomentOfInertiaEstimation计算有向包围盒

网友: 文章列表 2024-03-22 04:46:56

pcl中MomentOfInertiaEstimation计算有向包围盒

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，表示椭圆越扁平。在计算机视觉和图像处理领域中，偏心率通常被用作图像特征的一种度量方式，用于描述图像中目标物体的形状偏向程度。

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

网络标签：

相关问题