PCL库源代码阅读—配准模块(Registration)—收敛标准(ConvergenceCriteria)

想躺平的点云工程师

于 2024-05-24 19:45:00 发布

阅读量990

点赞数 11

文章标签：人工智能自动驾驶

本文链接：https://blog.csdn.net/mazheng11111111/article/details/138411042

版权

pcl库中收敛条件的判断是通过ConvergenceCriteria模块的相关文件实现的，具体实现文件为convergence_criteria.h，default_convergence_criteria.h，default_convergence_criteria.hpp。

收敛标准模块为点云配准提供不同的收敛标准，该模块作为具体配准算法的一部分来使用，收敛标准主要为：

1、迭代次数达到阈值；2、变换矩阵的变化值小于阈值；3、均方误差(MSE)的变化值小于阈值；

由于本部分的使用一般是作为点云配准算法的一部分，为了更好的理解本部分的源代码，建议结合配准模块(Registration)中的具体点云配准算法，如ICP算法：PCL库源代码阅读—配准模块(Registration)—ICP算法

文中有任何错误或疑惑的地方可在评论区留言，看到均会回复！！！

convergence_criteria.h 文件

命名空间嵌套，pcl::registration，注意与配准模块(Registration)区分，前者是小写。

namespace pcl {
namespace registration {...}    命名空间
}

ConvergenceCriteria类是收敛标准模块(ConvergenceCriteria)的基类，同时又是一个抽象类，关键函数为hasConverged()，它用来判断是否达到收敛的标准。operator操作符的类型转换，使用时将类对象转换为bool，并且执行return (hasConverged());语句。如有疑问可先学习C++中operator操作符的几种用法。

class PCL_EXPORTS ConvergenceCriteria{
public:
  using Ptr = shared_ptr<ConvergenceCriteria>;
  using ConstPtr = shared_ptr<const ConvergenceCriteria>;

  / 空构造函数 */
  ConvergenceCriteria() = default;

  / 空析构函数 */
  virtual ~ConvergenceCriteria() = default;

  / 纯虚函数，判断是否收敛 */
  virtual bool
  hasConverged() = 0;

  / operator类型转换，为每次迭代判断是否收敛 */
  operator bool() { return (hasConverged()); }
};
}

default_convergence_criteria.h文件

命名空间嵌套，pcl::registration，注意与配准模块(Registration)区分，前者是小写。

namespace pcl {
namespace registration {...}    命名空间
}

DefaultConvergenceCriteria类继承于ConvergenceCriteria类，本类实现了默认情况下的收敛判断，各种点云配准算法的收敛判断均可以与其结合使用。

template <typename Scalar = float>
class DefaultConvergenceCriteria : public ConvergenceCriteria {
    ...
}

下面将详细解读DefaultConvergenceCriteria类中的代码。

enum ConvergenceState {                  收敛状态的枚举类型
  CONVERGENCE_CRITERIA_NOT_CONVERGED,    未收敛
  CONVERGENCE_CRITERIA_ITERATIONS,       达到最大迭代次数的收敛标准
  CONVERGENCE_CRITERIA_TRANSFORM,        达到变换矩阵变化阈值的收敛标准
  CONVERGENCE_CRITERIA_ABS_MSE,          达到MSE绝对变化阈值的收敛标准
  CONVERGENCE_CRITERIA_REL_MSE,          达到MSE相对变化阈值的收敛标准
  CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,            无足够对应关系的不收敛
  CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS   达到最大迭代次数后的不收敛
};

构造函数及初始化列表，相关变量的初始化在后续变量定义部分进行详细介绍，此处只需注意构造函数是有参的即可。

DefaultConvergenceCriteria(const int& iterations,
                           const Matrix4& transform,
                           const pcl::Correspondences& correspondences)
: ...
{}

本部分是一些参数的设置函数，默认值在构造函数初始化列表时给出，配准算法设置的收敛阈值参数最终传递到了此处。此外，本部分为了快速阅读，简化了函数的书写方式，不常用的函数本部分将略去。

setMaximumIterationsSimilarTransforms(int);	设置旋转、平移和MSE变化很小时的最大迭代次数阈值，default:0
setMaximumIterations(int);					设置最大迭代次数，default:100
setFailureAfterMaximumIterations(bool);		设置达到最大迭代次数后是否收敛
setRotationThreshold(double);				设置旋转变化阈值，角度的余旋值，default:0.99999(0.256°)
setTranslationThreshold(double);			设置平移变化阈值，default:3e-4*3e-4
setRelativeMSE(double);				设置两组点对之间均方误差的相对变化阈值，default:0.00001
setAbsoluteMSE (double);			设置两组点对之间均方误差的绝对变化阈值，default:1e-12
setConvergenceState(ConvergenceState);		设置收敛状态，default:CONVERGENCE_CRITERIA_NOT_CONVERGED

hasConverged()函数用来判断是否达到了收敛状态，它是收敛模块的核心函数，相关代码的解读在.hpp文件部分展开。

bool hasConverged() override;

本部分解释了所定义的变量含义和MSE的计算，其中计算MSE所用到的对应关系相关内容可参考以下链接：PCL库源代码阅读—配准模块(Registration)—对应关系模块(Correspondence)

protected:
  
  / 计算给定对应关系的均方误差MSE /
  inline double
  calculateMSE(const pcl::Correspondences& correspondences) const
  {
    double mse = 0;
    for (const auto& correspondence : correspondences)
      mse += correspondence.distance;    求和
    mse /= static_cast<double>(correspondences.size());    求平均
    return (mse);
  }

  const int& iterations_;            定义当前迭代次数
  const Matrix4& transformation_;    定义当前变换矩阵
  const pcl::Correspondences& correspondences_;    定义源点云和目标点云点对的对应关系
  double correspondences_prev_mse_;  定义上一次对应点对的MSE,default:无穷大
  double correspondences_cur_mse_;   定义当前对应点对的MSE,default:无穷大
  int max_iterations_;               定义最大迭代次数
  bool failure_after_max_iter_;      定义达到最大迭代次数后的收敛状态
  double rotation_threshold_;        定义旋转角度变化的阈值，角度的余旋值
  double translation_threshold_;     定义平移变化的阈值
  double mse_threshold_relative_;    定义两组点对之间MSE的相对变化阈值
  double mse_threshold_absolute_;    定义两组点对之间MSE的绝对变化阈值
  int iterations_similar_transforms_;        定义旋转、平移和MSE变化很小时的当前迭代次数
  int max_iterations_similar_transforms_;    定义旋转、平移和MSE变化很小时的最大迭代次数阈值
  ConvergenceState convergence_state_;       定义收敛状态对象，枚举类型

default_convergence_criteria.hpp文件

hasConverged()函数是整个收敛模块判断收敛与否的核心函数，它通过最大迭代次数、旋转角度变化阈值、平移变化阈值、MSE的相对变化阈值、MSE的绝对变化阈值实现了收敛与否的判断。

template <typename Scalar>
bool
DefaultConvergenceCriteria<Scalar>::hasConverged()
{...}

每次迭代判断是否收敛时先将收敛状态变量初始化重置。

if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED) {
  / 每次迭代判断收敛时的收敛状态重置 /
  iterations_similar_transforms_ = 0;
  convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;
}
bool is_similar = false;    旋转、平移、MSE变化是否很小

本部分通过迭代次数与最大迭代次数的关系判断收敛状态。

/ 达到最大迭代次数时 /
if (iterations_ >= max_iterations_) {
  / 达到最大迭代次数后收敛的情况 /
  if (!failure_after_max_iter_) {    
    convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;
    return (true);
  }
  convergence_state_ = CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS;
}

本部分通过旋转角度变化阈值、平移变化阈值判断收敛状态，其中所用到的旋转矩阵的迹与旋转角度之间的关系可以查阅相关数学原理。

平移变换的阈值用到的是距离平方之和，这与构造函数初始化列表中默认值3e-4 * 3e-4很好的对应上了，transformation_.coeff()函数用来获取矩阵的元素值。

/ 通过旋转矩阵的迹与旋转角度之间的关系求解 /
double cos_angle = 0.5 * (transformation_.coeff(0, 0) + transformation_.coeff(1, 1) +
                          transformation_.coeff(2, 2) - 1);
/ 平移变换的距离平方之和 /
double translation_sqr = transformation_.coeff(0, 3) * transformation_.coeff(0, 3) +
                         transformation_.coeff(1, 3) * transformation_.coeff(1, 3) +
                         transformation_.coeff(2, 3) * transformation_.coeff(2, 3);

/ 旋转变化阈值、平移变化阈值满足时 /
if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_) {
  / 旋转、平移、MSE变化很小时，相似迭代次数满足阈值时 /
  if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
    convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM;
    return (true);
  }
  is_similar = true;    旋转、平移变化满足阈值时表示变化很小
}

本部分通过MSE的绝对变化阈值、MSE的相对变化阈值判断收敛状态。从该部分和上部分可知，当旋转变化、平移变化、MSE的绝对变化、MSE的相对变化满足阈值时，is_similar = true;即认为两次变换矩阵很相似了。

/ 计算当前对应关系的MSE /
correspondences_cur_mse_ = calculateMSE(correspondences_);

/ MSE的绝对变化满足阈值 /
if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) <
    mse_threshold_absolute_) {
  / 旋转、平移、MSE变化很小时，相似迭代次数满足阈值时 /
  if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
    convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE;
    return (true);
  }
  is_similar = true;
}

/ MSE的相对变化满足阈值 /
if (std::abs(correspondences_cur_mse_ - correspondences_prev_mse_) /
        correspondences_prev_mse_ <
    mse_threshold_relative_) {
  / 旋转、平移、MSE变化很小时，相似迭代次数满足阈值时 /
  if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) {
    convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE;
    return (true);
  }
  is_similar = true;
}

if (is_similar) {
  / 相似的迭代次数+1 /
  ++iterations_similar_transforms_;
}
else {
  iterations_similar_transforms_ = 0;
}
/ 将当前对应关系计算得到的MSE赋予上一次，为了计算MSE的变化值 /
correspondences_prev_mse_ = correspondences_cur_mse_;

总结

pcl库中点云配准的收敛判断是由独立的收敛模块(ConvergenceCriteria)实现的，该模块的核心函数是hasConverged()，它实现了通过最大迭代次数、旋转角度变化阈值、平移变化阈值、MSE的相对变化阈值、MSE的绝对变化阈值判断是否达到收敛状态。

想躺平的点云工程师

关注

11
点赞
踩
15

收藏

觉得还不错? 一键收藏
打赏
0
评论
PCL库源代码阅读—配准模块(Registration)—收敛标准(ConvergenceCriteria)

pcl库中收敛条件的判断是通过ConvergenceCriteria模块的相关文件实现的，具体实现文件为convergence_criteria.h，default_convergence_criteria.h，default_convergence_criteria.hpp。收敛标准模块为点云配准提供不同的收敛标准，该模块作为具体配准算法的一部分来使用，收敛标准主要为：1、迭代次数达到阈值；2、变换矩阵的变化值小于阈值；3、均方误差(MSE)的变化值小于阈值；
复制链接

扫一扫