5. ndt_cpu模块介绍及代码解析

一. 模块介绍

ndt_cpu模块是ndt算法的具体实现过程，通过ndt_matching提供的source（输入scan）、target（map）和init_pose,最终得到一个精确的final_pose.

2d和3d通用，需要先熟悉这张框架图，再进行源码解析，同论文中的公式来一一对应

二. 源码解析

关键函数VoxelGrid ：

void NormalDistributionsTransform<PointSourceType, PointTargetType>::setInputTarget(typename pcl::PointCloud<PointTargetType>::Ptr input)
{
  Registration<PointSourceType, PointTargetType>::setInputTarget(input);

  // Build the voxel grid
  // 将map分割成为多个cell，resolution_分辨率默认为1，分辨率越大算力越强
  if (input->points.size() > 0) {
    voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
    voxel_grid_.setInput(input);
  }
}

将map分割成为多个cell，resolution_分辨率默认为1，分辨率越大算力越强：

void VoxelGrid<PointSourceType>::setInput(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud)
{
  if (input_cloud->points.size() > 0) {
    /* If no voxel grid was created, then
     * build the initial voxel grid and octree
     */
    source_cloud_ = input_cloud;

    findBoundaries();

    std::vector<Eigen::Vector3i> voxel_ids(input_cloud->points.size());
    // 计算每一个输入point在分割后的voxel中的实际id
    for (int i = 0; i < input_cloud->points.size(); i++) {
      Eigen::Vector3i &vid = voxel_ids[i];
      PointSourceType p = input_cloud->points[i];

      vid(0) = static_cast<int>(floor(p.x / voxel_x_));
      vid(1) = static_cast<int>(floor(p.y / voxel_y_));
      vid(2) = static_cast<int>(floor(p.z / voxel_z_));
    }
    // 插入八叉树中
    octree_.setInput(voxel_ids, input_cloud);

    voxel_ids.clear();

    initialize();
    // 如下两个函数用于计算每个voxel的centroid（μ）和 covariance（Σ）
    scatterPointsToVoxelGrid();

    computeCentroidAndCovariance();
  }
}

同ndt_matching模块通过调用此函数正式进行ndt优化的运算：

//同ndt_matching模块通过调用此函数正式进行ndt优化的运算
template <typename PointSourceType, typename PointTargetType>
void Registration<PointSourceType, PointTargetType>::align(const Eigen::Matrix<float, 4, 4> &guess)
{
  converged_ = false;

  final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix<float, 4, 4>::Identity();

  trans_cloud_.points.resize(source_cloud_->points.size());

  for (int i = 0; i < trans_cloud_.points.size(); i++) {
    trans_cloud_.points[i] = source_cloud_->points[i];
  }

  computeTransformation(guess);
}

正式通过computeTransformation（）函数进行计算：

void NormalDistributionsTransform<PointSourceType, PointTargetType>::computeTransformation(const Eigen::Matrix<float, 4, 4> &guess)
{
  nr_iterations_ = 0;
  converged_ = false;

  double gauss_c1, gauss_c2, gauss_d3;

  gauss_c1 = 10 * ( 1 - outlier_ratio_);
  gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
  // 这里的计算公式参考NDT那篇博士论文中的公式6.8，最终主要是为了求取d1和d2，也就是为之后建立优化函数计算系数
  gauss_d3 = - log(gauss_c2);
  gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3;
  gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3) / gauss_d1_);

  if (guess != Eigen::Matrix4f::Identity()) {
    final_transformation_ = guess;

    pcl::transformPointCloud(*source_cloud_, trans_cloud_, guess);
  }

  Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
  eig_transformation.matrix() = final_transformation_;

  Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
  Eigen::Vector3f init_translation = eig_transformation.translation();
  Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);

  p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), init_rotation(1), init_rotation(2);

  Eigen::Matrix<double, 6, 6> hessian;

  double score = 0;
  double delta_p_norm;
  //由此进入hession矩阵和score_gradient梯度矩阵的计算
  score = computeDerivatives(score_gradient, hessian, trans_cloud_, p);

  int points_number = source_cloud_->points.size();
  //进行迭代运算
  while (!converged_) {
    previous_transformation_ = transformation_;
    // 计算增量 H*delta_p=-g
    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
    
    delta_p = sv.solve(-score_gradient);

    delta_p_norm = delta_p.norm();

    if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) {
      trans_probability_ = score / static_cast<double>(points_number);
      converged_ = delta_p_norm == delta_p_norm;
      return;
    }
    // 给每次优化后的步长乘了一个步长系数,并且通过调用computeDerivatives()来更新score_gradient和hessian
    delta_p.normalize();
    //相当于一个补长搜索策略，涉及一些比较复杂的数学优化知识，感兴趣同学可以深入研究下，参考的论文和书籍已经附在函数里面
    delta_p_norm = computeStepLengthMT(p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, trans_cloud_);
    delta_p *= delta_p_norm;

    transformation_ = (Eigen::Translation<float, 3>(static_cast<float>(delta_p(0)), static_cast<float>(delta_p(1)), static_cast<float>(delta_p(2))) *
              Eigen::AngleAxis<float>(static_cast<float>(delta_p(3)), Eigen::Vector3f::UnitX()) *
              Eigen::AngleAxis<float>(static_cast<float>(delta_p(4)), Eigen::Vector3f::UnitY()) *
              Eigen::AngleAxis<float>(static_cast<float>(delta_p(5)), Eigen::Vector3f::UnitZ())).matrix();

    p = p + delta_p;

    //Not update visualizer
    // 收敛或达到最大迭代次数，停止迭代运算
    if (nr_iterations_ > max_iterations_ || (nr_iterations_ && (std::fabs(delta_p_norm) < transformation_epsilon_))) {
      converged_ = true;
    }

    nr_iterations_++;
  }

  if (source_cloud_->points.size() > 0) {
    trans_probability_ = score / static_cast<double>(source_cloud_->points.size());
  }
}

double NormalDistributionsTransform<PointSourceType, PointTargetType>::computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
                                              typename pcl::PointCloud<PointSourceType> &trans_cloud,
                                              Eigen::Matrix<double, 6, 1> pose, bool compute_hessian)
{
  PointSourceType x_pt, x_trans_pt;
  Eigen::Vector3d x, x_trans;
  Eigen::Matrix3d c_inv;

  score_gradient.setZero ();
  hessian.setZero ();

  //Compute Angle Derivatives
  // 根据输入6Dpose来计算一阶和二阶雅克比（Hession矩阵）中的关键项
  computeAngleDerivatives(pose);

  std::vector<int> neighbor_ids;
  Eigen::Matrix<double, 3, 6> point_gradient;
  Eigen::Matrix<double, 18, 6> point_hessian;
  double score = 0;

  point_gradient.setZero();
  point_gradient.block<3, 3>(0, 0).setIdentity();
  point_hessian.setZero();

  for (int idx = 0; idx < source_cloud_->points.size(); idx++) {
    neighbor_ids.clear();
    x_trans_pt = trans_cloud.points[idx];
    // 遍历输入cloud中的每一个point,寻找它在指定距离内的邻近voxels的id
    voxel_grid_.radiusSearch(x_trans_pt, resolution_, neighbor_ids);

    for (int i = 0; i < neighbor_ids.size(); i++) {
      int vid = neighbor_ids[i];

      x_pt = source_cloud_->points[idx];
      x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);

      x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
      //得到当前遍历voxel的均值和方差
      x_trans -= voxel_grid_.getCentroid(vid);
      c_inv = voxel_grid_.getInverseCovariance(vid);
      //根据当前遍历point，生成一阶和二阶雅克比矩阵
      computePointDerivatives(x, point_gradient, point_hessian, compute_hessian);

      score += updateDerivatives(score_gradient, hessian, point_gradient, point_hessian, x_trans, c_inv, compute_hessian);
    }
  }

  return score;
}