cam_lidar_calibration代码详解（二）优化计算过程

目录

一、openga.h解析

二、optimiser.h解析

三、optimiser.cpp解析

先对特征点提取和优化过程进行了梳理，见博客：

​​​​​​​​​​​​cam_lidar_calibration代码详解（一）采样优化部分_可见一班的博客-CSDN博客

这篇博客主要对优化计算的过程及代码进行整理，用到的是遗传算法。主要包含3个文件：

• openga.h，遗传算法的实现
• optimiser.h，定义旋转矩阵、变化矩阵优化计算用到的函数
• optimiser.cpp，执行遗传算法的配置和流程

一、openga.h解析

是一个遗传算法开源库，有三种计算模式，使用的是SOGA模式

enum class GA_MODE
{
  SOGA,
  IGA,
  NSGA_III
};

定义染色体结构，ChromosomeType放在GenerationType里，又放在GenerationType_SO_abstract里

template <typename GeneType, typename MiddleCostType>
struct ChromosomeType
{
  GeneType genes;
  MiddleCostType middle_costs;  // individual costs
  double total_cost;            // for single objective
  vector<double> objectives;    // for multi-objective
};

template <typename GeneType, typename MiddleCostType>
struct GenerationType
{
  vector<ChromosomeType<GeneType, MiddleCostType>> chromosomes;
  double best_total_cost = (std::numeric_limits<double>::infinity());  // for single objective
  double average_cost = 0.0;                                           // for single objective

  int best_chromosome_index = -1;       // for single objective
  vector<int> sorted_indices;           // for single objective
  vector<vector<unsigned int>> fronts;  // for multi-objective
  vector<double> selection_chance_cumulative;
  double exe_time;
};

template <typename GeneType, typename MiddleCostType>
struct GenerationType_SO_abstract
{
  double best_total_cost = (std::numeric_limits<double>::infinity());  // for single objective
  double average_cost = 0.0;                                           // for single objective

  GenerationType_SO_abstract(const GenerationType<GeneType, MiddleCostType>& generation)
    : best_total_cost(generation.best_total_cost), average_cost(generation.average_cost)
  {
  }
};

对矩阵进行操作的一些函数，如置零、清除、获取行数列数等，在矩阵类Matrix中

class Matrix
{
  unsigned int n_rows, n_cols;
  vector<double> data;

public:  
//这是一个构造函数，用于初始化一个名为Matrix的类。其中，n_rows和n_cols是表示矩阵的行数和列数的变量，data是一个指向矩阵数据的指针。在这个构造函数中，n_rows和n_cols被初始化为0，data被初始化为空指针，即还没有分配矩阵数据的内存空间。
  Matrix() : n_rows(0), n_cols(0), data()   
  {
  }

  //这也是一个构造函数，用于初始化一个名为Matrix的类，并指定矩阵的行数和列数。在这个构造函数中，n_rows和n_cols被初始化为传入的参数n_rows和n_cols，data被初始化为一个大小为n_rows * n_cols的向量，也就是说，它为矩阵数据分配了一个连续的内存空间。这个构造函数可以用于创建一个空矩阵，也可以用于创建一个具有指定行数和列数的矩阵。
  Matrix(unsigned int n_rows, unsigned int n_cols) : n_rows(n_rows), n_cols(n_cols), data(n_rows * n_cols)
  {
  }

  void zeros()      //这个函数适用于已经存在的矩阵，可以将矩阵中的所有元素清零
  {
    std::fill(data.begin(), data.end(), 0);
  }

  void zeros(unsigned int rows, unsigned int cols)    //这个函数适用于创建一个新的矩阵，并将其中的所有元素初始化为0。
  {
    n_rows = rows;
    n_cols = cols;
    data.assign(rows * cols, 0);    //其中"assign"函数的第一个参数是要分配的元素数量，第二个参数是要分配的元素的值。
  }

  bool empty()
  {
    return (!n_rows) || (!n_cols);    //如果其中一个为0，则返回true，否则返回false。判断一个矩阵是否为空，即行数或列数是否为0。
  }

  //用于获取矩阵的行数和列数的。返回一个无符号整数类型的值
  unsigned int get_n_rows() const
  {
    return n_rows;
  }
  unsigned int get_n_cols() const
  {
    return n_cols;
  }

  void clear()
  {
    n_rows = 0;
    n_cols = 0;
    data.clear();
  }


  //这段代码定义了一个名为set_col的函数，它有两个参数：col_idx和col_vector。col_idx是一个无符号整数类型的值，表示要设置的列的索引。col_vector是一个双精度浮点数向量，表示要分配给该列的值。该函数使用assert函数来确保col_vector的大小与矩阵的行数相同。然后，它使用一个for循环来遍历矩阵的每一行，并将col_vector中对应的值分配给该行的col_idx列。这个函数可能是用于设置矩阵的某一列的值的。
  void set_col(unsigned int col_idx, const vector<double>& col_vector)
  {
    assert(col_vector.size() == n_rows && "Assigned column vector size mismatch."); //如果col_vector的大小与n_rows不同，assert函数将会抛出一个错误，提示“Assigned column vector size mismatch.”，并终止程序的执行。这个assert语句的作用是确保在设置矩阵的某一列时，分配给该列的向量的大小与矩阵的行数相同，以避免出现错误的结果。
    for (unsigned int i = 0; i < n_rows; i++)
      (*this)(i, col_idx) = col_vector[i];
  }

  void set_row(unsigned int row_idx, const vector<double>& row_vector)
  {
    assert(row_vector.size() == n_cols && "Assigned row vector size mismatch.");
    for (unsigned int i = 0; i < n_cols; i++)
      (*this)(row_idx, i) = row_vector[i];
  }

  void get_col(unsigned int col_idx, vector<double>& col_vector) const
  {
    col_vector.resize(n_rows);
    for (unsigned int i = 0; i < n_rows; i++)
      col_vector[i] = (*this)(i, col_idx);
  }

  void get_row(unsigned int row_idx, vector<double>& row_vector) const
  {
    row_vector.resize(n_cols);
    for (unsigned int i = 0; i < n_cols; i++)
      row_vector[i] = (*this)(row_idx, i);
  }

计时用高精度时钟类Chronometer的tic()和toc()函数

class Chronometer   //高度精确的钟表
{
protected:
  typedef std::chrono::time_point<std::chrono::high_resolution_clock> Timetype;
  Timetype time_start, time_stop;
  bool initialized;

public:
  Chronometer() : initialized(false)
  {
  }

  void tic()
  {
    initialized = true;
    time_start = std::chrono::high_resolution_clock::now();
  }

  double toc()
  {
    if (!initialized)
      throw runtime_error("Chronometer is not initialized!");
    time_stop = std::chrono::high_resolution_clock::now();
    return (double)std::chrono::duration<double>(time_stop - time_start).count();
  }
};

主要是类 Genetic，下面是各成员主要函数，protected里的函数较多

class Genetic
{
    private:  //私有成员,类的外部是不可访问的，只有类和友元函数可以访问私有成员
      std::mt19937_64 rng;  // random generator,这是一个C++标准库中的随机数生成器。它可以生成64位的随机数。
      std::uniform_real_distribution<double> unif_dist; //随机数分布器,可以生成指定范围内的均匀分布的随机数
      int average_stall_count;
      int best_stall_count;
      vector<double> ideal_objectives;           // for multi-objective
      Matrix extreme_objectives;                 // for multi-objective
      vector<double> scalarized_objectives_min;  // for multi-objective
      Matrix reference_vectors;
  // double shrink_scale;
      unsigned int N_robj;

    public:  //公有成员,在程序类的外部可以访问
      typedef ChromosomeType<GeneType, MiddleCostType> thisChromosomeType;
      typedef GenerationType<GeneType, MiddleCostType> thisGenerationType;
      typedef GenerationType_SO_abstract<GeneType, MiddleCostType> thisGenSOAbs;
      GA_MODE problem_mode;
      unsigned int population;
      double crossover_fraction;
      double mutation_rate;
      bool verbose;
      int generation_step;
      int elite_count;
      int generation_max;
      double tol_stall_average;
      i