pcl中MomentOfInertiaEstimation解析与实例

news2024/11/23 12:33:02

pcl中features模块又基于惯性矩和偏心率的描述子，也可以求取点云的AABB和OBB包围盒，在计算的过程中法线一些问题，特此记录。
针对惯性矩和偏心率这两个数据的应用场景还不明确，因此暂时不做讨论，主要讨论求取OBB时的代码。

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;
}

如上所示，OBB求取的过程就是将点云转换到以中心为坐标原点，PCA计算的三个特征向量为轴方向的局部坐标系中，然后求取相应的AABB包围盒的过程，但是在文中最后一段

  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;

第一点：
shift是包围盒的在局部坐标系中的中心坐标，
obb_rotational_matrix_ 是将局部坐标系转为全局坐标的旋转矩阵，平移矩阵就是中心点坐标。
obb_position_ 就是OBB包围盒中心的全局坐标

注意：这里局部坐标系和全局坐标系的转换关系要注意，对点云计算协方差阵，得到的特征向量矩阵是列向量组成的，obb_rotational_matrix_ 就是列向量组成的；
分析：
全局坐标->局部坐标
RP=Pt,此时的R是特征向量作为行向量组成的，这与得到的特征向量矩阵是转置的关系
局部坐标->全局坐标
RtPt=P，此时Rt是R的逆，这才是正确的，但是由于R的转置*R=单位阵，因此使用转置取代求逆操作，也就是说，此时的Rt是特征向量作为列组成的。所以出现
obb_position_ = mean_value_ + obb_rotational_matrix_ * shift;
obb_position_ 就是OBB包围盒中心的全局坐标。

第二点：

  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);