NDT定位流程

Apollo NDT算法整体流程：
1.将激光雷达的实时点云数据通过体素滤波进行下采样作为输入点云；
2.将当前位置周边的高精度地图点云数据进行网格化处理，计算每个网格的正态分布模型（期望，方差，网格内的点云数量，质心坐标等）作为匹配目标；
3.利用组合导航的原始运动估计作为匹配算法的初始值；
4.采用NDT(正态分布变换)配准算法+牛顿法迭代求解，通过线搜索方法求步长，最终输出定位结果。

NDT定位源码解析

代码目录结构

核心代码简介
（1）map_creation 创建地图
创建NDT地图：ndt_map_creator.cc；
（2）ndt_locator NDT定位
激光雷达ndt定位：lidar_locator_ndt.cc、lidar_locator_ndt.h；
ndt求解：ndt_solver.h、ndt_solver.hpp：；
ndt体素网格协方差计算相关：ndt_voxel_grid_covariance.h、ndt_voxel_grid_covariance.hpp；
（3）ndt定位模块实现
定位位姿计算：localization_pose_buffer.cc、localization_pose_buffer.h；
ndt定位组件：ndt_localization_component.cc、ndt_localization_component.h；
ndt定位实现ndt_localization.cc、ndt_localization.h；

NDT地图制作

根据雷达点云信息（需要有强度信息）、准确的pose位姿信息，生成NDT地图。通过ndt_map_creator来实现。
调用方法：
ndt_map_creator --pcd_folders=/apollo/data/pcd --pose_files=/apollo/data/pcd/poses.txt --resolution_type=single --resolution=1 --zone_id=10 --map_folder=/apollo/modules/localization/map/ndt_map/local_map
参数简介：
pcd_folders：点云数据；
pose_files：点云位姿；
resolution_type：分辨率类型（single或multi）；
Resolution：分辨率
zone_id：UTM分度带编号；
map_folder：NDT地图数据存放地址。
如图所示，pcds文件夹为输入的激光雷达点云数据和位姿数据；ndt_map为输出的结果文件（NDT地图文件、配准文件）。

NDT建图过程：
1、读取所有pose，创建NdtMapConfig对象，根据分辨率等参数，生成ndt地图配置文件；
2、根据地图配置，初始化地图；读取点云数据，根据点云位置，获取对应的地图结点NdtMapNode。
3、设置地图结点NdtMapNode和NdtMapCell，计算质心，对点云强度信息采样并且放入对应的Cell（计算质心、强度的方差、协方差等等）。
具体可查看ndt_map_creator.cc主函数main()。
涉及Cell的AddSample方法，具体见msf文件夹下的ndtmap相关代码
apollo-master\modules\localization\msf\local_pyramid_map\ndt_map\ndt_map_matrix.cc

int NdtMapCells::AddSample(const float intensity, const float altitude,
                           const float resolution,
                           const Eigen::Vector3f& centroid, bool is_road) {
  int altitude_index = CalAltitudeIndex(resolution, altitude);
  NdtMapSingleCell& cell = cells_[altitude_index];
  cell.AddSample(intensity, altitude, centroid, is_road);
  if (altitude_index > max_altitude_index_) {
    max_altitude_index_ = altitude_index;
  }
  if (altitude_index < min_altitude_index_) {
    min_altitude_index_ = altitude_index;
  }
  if (is_road) {
    auto got = std::find(road_cell_indices_.begin(), road_cell_indices_.end(),
                         altitude_index);
    if (got == road_cell_indices_.end()) {
      road_cell_indices_.push_back(altitude_index);
    }
  }

  return altitude_index;
}

NDT配准流程

模块入口及初始化

ndt_localization_component.cc

//初始化参数及通道，message 输入，输出
Init()
//代码循环入口，每收到一次组合导航数据就循环一次
Proc( const std::shared_ptrlocalization::Gps& gps_msg)

相关的主要回调函数

//接收雷达点云数据，传入ndt_localization
LidarCallback(const std::shared_ptrdrivers::PointCloud& lidar_msg) 
//接收组合导航定位状态，传入ndt_localization
OdometryStatusCallback(const std::shared_ptrdrivers::gnss::InsStat& status_msg) 

回调函数的具体实现，在ndt_localization.cc文件中。

实时在线点云地图处理

激光雷达扫描得到的激光点云数据需要去除距离车体较近与较远的激光点集，然后利用体素滤波过滤剩下的激光点云数据，在保持点云统计特征的情况下，降低激光点云数据集的尺寸大小，然后将降采样后的过滤点云作为NDT配准算法的输入源点云。

ndt_localization.cc

void NDTLocalization::LidarCallback(
    const std::shared_ptr<drivers::PointCloud>& lidar_msg)

lidar_locator_ndt.cc
Update()
用组合导航来进行局部定位，利用组合导航数据差值作为当前时刻的预测值；
使用预测值，获取对应位置的点云地图，将地图点云转化到激光坐标系,
在激光坐标系下进行激光点云与地图点云的匹配；

int LidarLocatorNdt::Update(unsigned int frame_idx, const Eigen::Affine3d& pose,
                            const LidarFrame& lidar_frame)
 ...
// Filter online points 对输入点云数据进行滤波；
  AINFO << "Online point cloud leaf size: " << proj_reslution_;
  pcl::PointCloud<pcl::PointXYZ>::Ptr online_points_filtered(
      new pcl::PointCloud<pcl::PointXYZ>());
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(online_points);
  //proj_reslution_ 默认值为1.0
  sor.setLeafSize(proj_reslution_, proj_reslution_, proj_reslution_);
  sor.filter(*online_points_filtered);
  AINFO << "Online Pointcloud size: " << online_points->size() << "/"
        << online_points_filtered->size();
  online_filtered_timer.End("online point calc end.");
  ...

NDT地图网格化及赋值

对地图点云进行网格化，组成地图Cells的相关信息。
此操作是在ndt_map_creator输出的地图数据基础上进行，并且已经对地图进行了1m * 1m* 1m的体素化处理。
lidar_locator_ndt.cc
ComposeMapCells()
按照一定的方式分成了9块；分块完成后，遍历整个区域，由于事先对地图进行了1m * 1m* 1m的体素化处理,所以迭代次数不会特别多。最终得到每个网格的质心坐标（x,y,z），网格内点的个数，均值，方差，并保存在cell_map_中。

void LidarLocatorNdt::ComposeMapCells(
    const Eigen::Vector2d& left_top_coord2d, int zone_id,
    unsigned int resolution_id, float map_pixel_resolution,
    const Eigen::Affine3d& inverse_transform) {
  apollo::common::util::Timer timer;
  timer.Start();

  unsigned int map_node_size_x = map_.GetMapConfig().map_node_size_x_;
  unsigned int map_node_size_y = map_.GetMapConfig().map_node_size_y_;
  unsigned int filter_size_x = filter_x_;
  unsigned int filter_size_y = filter_y_;

  // get the left top coordinate of input online pointcloud
  Eigen::Vector2d coord2d = left_top_coord2d;
  coord2d[0] -= map_pixel_resolution * static_cast<float>(filter_size_x / 2);
  coord2d[1] -= map_pixel_resolution * static_cast<float>(filter_size_y / 2);

  // get the node index of left top corner and global coordinate
  MapNodeIndex map_id = MapNodeIndex::GetMapNodeIndex(
      map_.GetMapConfig(), coord2d, resolution_id, zone_id);
  NdtMapNode* map_node_lt =
      dynamic_cast<NdtMapNode*>(map_.GetMapNodeSafe(map_id));
  assert(map_.IsMapNodeExist(map_id));
  unsigned int map_coord_x = 0;
  unsigned int map_coord_y = 0;
  map_node_lt->GetCoordinate(coord2d, &map_coord_x, &map_coord_y);

  std::vector<std::vector<int>> map_nodes_zones;

  int top_left_start_x = static_cast<int>(map_coord_x);
  int top_left_start_y = static_cast<int>(map_coord_y);
  int top_left_end_x = static_cast<int>(map_node_size_x) - 1;
  int top_left_end_y = static_cast<int>(map_node_size_y) - 1;
  std::vector<int> node_zone;
  node_zone.push_back(top_left_start_x);
  node_zone.push_back(top_left_start_y);
  node_zone.push_back(top_left_end_x);
  node_zone.push_back(top_left_end_y);
  map_nodes_zones.push_back(node_zone);
  ...
  // start obtain cells in MapNdtMatrix
        const Eigen::Vector2d& left_top_corner =
            map_node_ptr->GetLeftTopCorner();
        double resolution = map_node_ptr->GetMapResolution();
        double resolution_z = map_node_ptr->GetMapResolutionZ();
        for (int map_y = map_nodes_zones[y * 3 + x][1];
             map_y <= map_nodes_zones[y * 3 + x][3]; ++map_y) {
          for (int map_x = map_nodes_zones[y * 3 + x][0];
               map_x <= map_nodes_zones[y * 3 + x][2]; ++map_x) {
            const NdtMapCells& cell_ndt = map_cells.GetMapCell(map_y, map_x);
            if (cell_ndt.cells_.size() > 0) {
              for (auto it = cell_ndt.cells_.begin();
                   it != cell_ndt.cells_.end(); ++it) {
                unsigned int cell_count = it->second.count_;
                if (cell_count >= 6) {
                  Leaf leaf;
                  leaf.nr_points_ = static_cast<int>(cell_count);

                  Eigen::Vector3d eigen_point(Eigen::Vector3d::Zero());
                  eigen_point(0) = left_top_corner[0] + map_x * resolution +
                                   it->second.centroid_[0];
                  eigen_point(1) = left_top_corner[1] + map_y * resolution +
                                   it->second.centroid_[1];
                  eigen_point(2) =
                      resolution_z * it->first + it->second.centroid_[2];
                  leaf.mean_ = (eigen_point + R_inv_t);
                  if (it->second.is_icov_available_ == 1) {
                    leaf.icov_ = it->second.centroid_icov_.cast<double>();
                  } else {
                    leaf.nr_points_ = -1;
                  }

                  cell_map_.push_back(leaf);
                }
              }
            }
          }
        }
      }
    }
  }

  timer.End("Compose map cells.");
}

这里计算的是Leaf的协方差、均值、点个数，Leaf相关的定义和方法可以在以下文件中查看。
ndt_voxel_grid_covariance.hpp
ndt_voxel_grid_covariance.h

GNSS组合导航运动估计

把组合导航输出的定位结果当里程计用,并用来做瞬时定位，即当前定位结果=上一时刻激光定位的结果+时间间隔内里程计变化；
保存组合导航定位结果到链表list中,便于激光数据输入时查找和插值，主要根据时间戳进行判断。
ndt_localization.cc
OdometryCallback()

// receive odometry message
void NDTLocalization::OdometryCallback(
    const std::shared_ptr<localization::Gps>& odometry_msg) {
    //获取时间戳
  double odometry_time = odometry_msg->header().timestamp_sec();
  static double pre_odometry_time = odometry_time;
  double time_delay = odometry_time - pre_odometry_time;
  if (time_delay > 0.1) {
    AINFO << "Odometry message loss more than 10ms, the pre time and cur time: "
          << pre_odometry_time << ", " << odometry_time;
  } else if (time_delay < 0.0) {
    AINFO << "Odometry message's time is earlier than last one, "
          << "the pre time and cur time: " << pre_odometry_time << ", "
          << odometry_time;
  }
  pre_odometry_time = odometry_time;
  Eigen::Affine3d odometry_pose = Eigen::Affine3d::Identity();
  if (odometry_msg->has_localization()) {
    odometry_pose.translation()[0] =
        odometry_msg->localization().position().x();
    odometry_pose.translation()[1] =
        odometry_msg->localization().position().y();
    odometry_pose.translation()[2] =
        odometry_msg->localization().position().z();
    Eigen::Quaterniond tmp_quat(
        odometry_msg->localization().orientation().qw(),
        odometry_msg->localization().orientation().qx(),
        odometry_msg->localization().orientation().qy(),
        odometry_msg->localization().orientation().qz());
    odometry_pose.linear() = tmp_quat.toRotationMatrix();
  }
  if (ZeroOdometry(odometry_pose)) {
    AINFO << "Detect Zero Odometry";
    return;
  }
//保存组合导航输出的定位结果当作里程计来使用
  TimeStampPose timestamp_pose;
  timestamp_pose.timestamp = odometry_time;
  timestamp_pose.pose = odometry_pose;
  {
    std::lock_guard<std::mutex> lock(odometry_buffer_mutex_);
    if (odometry_buffer_size_ < max_odometry_buffer_size_) {
      odometry_buffer_.push_back(timestamp_pose);
      odometry_buffer_size_++;
    } else {
      odometry_buffer_.pop_front();
      odometry_buffer_.push_back(timestamp_pose);
    }
  }

  if (ndt_debug_log_flag_) {
    AINFO << "NDTLocalization Debug Log: odometry msg: "
          << std::setprecision(15) << "time: " << odometry_time << ", "
          << "x: " << odometry_msg->localization().position().x() << ", "
          << "y: " << odometry_msg->localization().position().y() << ", "
          << "z: " << odometry_msg->localization().position().z() << ", "
          << "qx: " << odometry_msg->localization().orientation().qx() << ", "
          << "qy: " << odometry_msg->localization().orientation().qy() << ", "
          << "qz: " << odometry_msg->localization().orientation().qz() << ", "
          << "qw: " << odometry_msg->localization().orientation().qw();
  }
//上一时刻激光点云的位置加上间隔时间内组合导航的相对位移作为当前时刻激光点云定位的预测值
  Eigen::Affine3d localization_pose = Eigen::Affine3d::Identity();
  if (lidar_locator_.IsInitialized()) {
    localization_pose =
        pose_buffer_.UpdateOdometryPose(odometry_time, odometry_pose);
  }
  ComposeLocalizationEstimate(localization_pose, odometry_msg,
                              &localization_result_);
  drivers::gnss::InsStat odometry_status;
  FindNearestOdometryStatus(odometry_time, &odometry_status);
  ComposeLocalizationStatus(odometry_status, &localization_status_);
  is_service_started_ = true;
}

lidar_locator_ndt.cc

//用组合导航来进行局部定位，利用组合导航数据差值作为当前时刻的预测值；
int LidarLocatorNdt::Update(unsigned int frame_idx, const Eigen::Affine3d& pose,
                            const LidarFrame& lidar_frame) {
  // Increasement from INSPVA
  Eigen::Vector3d trans_diff =
      pose.translation() - pre_input_location_.translation();
  Eigen::Vector3d trans_pre_local =
      pre_estimate_location_.translation() + trans_diff;
  Eigen::Quaterniond quatd(pose.linear());
  Eigen::Translation3d transd(trans_pre_local);
  Eigen::Affine3d center_pose = transd * quatd;

  Eigen::Quaterniond pose_qbn(pose.linear());
  AINFO << "original pose: " << std::setprecision(15) << pose.translation()[0]
        << ", " << pose.translation()[1] << ", " << pose.translation()[2]
        << ", " << pose_qbn.x() << ", " << pose_qbn.y() << ", " << pose_qbn.z()
        << ", " << pose_qbn.w();

  // Get lidar pose Twv = Twb * Tbv
  Eigen::Affine3d transform = center_pose * velodyne_extrinsic_;
  predict_location_ = center_pose;

NDT配准算法

在lidar_locator_ndt.cc的Update函数中调用。

 // Set left top corner for reg
  reg_.SetLeftTopCorner(map_left_top_corner_);
  // Ndt calculation
  reg_.SetInputTarget(cell_map_, pcl_map_point_cloud);
  reg_.SetInputSource(online_points_filtered);

  apollo::common::util::Timer ndt_timer;
  ndt_timer.Start();
  Eigen::Matrix3d inv_R = transform.inverse().linear();
  Eigen::Matrix4d init_matrix = Eigen::Matrix4d::Identity();
  init_matrix.block<3, 3>(0, 0) = inv_R.inverse();

  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(
      new pcl::PointCloud<pcl::PointXYZ>);
  reg_.Align(output_cloud, init_matrix.cast<float>());

ndt_solver.hpp
1、设置目标点云
SetInputTarget()
对地图点云进行了滤波处理；设置地图点云的体素滤波分辨率;输入算法目标点云，即地图点云；

/**@brief Provide a pointer to the input target. */
  inline void SetInputTarget(const std::vector<Leaf> &cell_leaf,
                             const PointCloudTargetConstPtr &cloud) {
    if (cell_leaf.empty()) {
      AWARN << "Input leaf is empty.";
      return;
    }
    if (cloud->points.empty()) {
      AWARN << "Input target is empty.";
      return;
    }

    target_ = cloud;
    target_cells_.SetVoxelGridResolution(resolution_, resolution_, resolution_);
    target_cells_.SetInputCloud(cloud);
    target_cells_.filter(cell_leaf, true);
  }

2、设置源点云
SetInputSource()

3、对地图点云进行滤波
filter()
对地图点云进行滤波，去掉小于6个点的网格（NDT算法原理决定的），并将结果放入kdtree_,便于搜索；

 /**@brief Initializes voxel structure. */
  inline void filter(const std::vector<Leaf> &cell_leaf,
                     bool searchable = true) {
    voxel_centroids_ = PointCloudPtr(new PointCloud);
    SetMap(cell_leaf, voxel_centroids_);
    if (voxel_centroids_->size() > 0) {
      kdtree_.setInputCloud(voxel_centroids_);
    }
  }

ndt_voxel_grid_covariance.hpp
SetMap()
对地图点云进行滤波处理，去掉网格内小于6个的点，输出点云坐标（x,y,z）及对应中心点的地图坐标编号；
最后将结果保存到output中；
至少需要包含 6 个点是为了防止共面或共线，以计算 Σ的逆；

template <typename PointT>
void VoxelGridCovariance<PointT>::SetMap(const std::vector<Leaf>& map_leaves,
                                         PointCloudPtr output) {
  voxel_centroids_leaf_indices_.clear();

  // Has the input dataset been set already
  if (!input_) {
    AWARN << "No input dataset given. ";
    output->width = output->height = 0;
    output->points.clear();
    return;
  }

  // Copy the header + allocate enough space for points
  output->height = 1;
  output->is_dense = true;
  output->points.clear();

  // Get the minimum and maximum dimensions
  Eigen::Vector4f min_p, max_p;
  pcl::getMinMax3D<PointT>(*input_, min_p, max_p);

  Eigen::Vector4f left_top = Eigen::Vector4f::Zero();
  left_top.block<3, 1>(0, 0) = map_left_top_corner_.cast<float>();
  min_p -= left_top;
  max_p -= left_top;

  // Compute the minimum and maximum bounding box values
  min_b_[0] = static_cast<int>(min_p[0] * inverse_leaf_size_[0]);
  max_b_[0] = static_cast<int>(max_p[0] * inverse_leaf_size_[0]);
  min_b_[1] = static_cast<int>(min_p[1] * inverse_leaf_size_[1]);
  max_b_[1] = static_cast<int>(max_p[1] * inverse_leaf_size_[1]);
  min_b_[2] = static_cast<int>(min_p[2] * inverse_leaf_size_[2]);
  max_b_[2] = static_cast<int>(max_p[2] * inverse_leaf_size_[2]);

  // Compute the number of divisions needed along all axis
  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones();
  div_b_[3] = 0;

  // Set up the division multiplier
  divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0);

  // Clear the leaves
  leaves_.clear();

  output->points.reserve(map_leaves.size());
  voxel_centroids_leaf_indices_.reserve(leaves_.size());

  for (unsigned int i = 0; i < map_leaves.size(); ++i) {
    const Leaf& cell_leaf = map_leaves[i];
    Eigen::Vector3d local_mean = cell_leaf.mean_ - map_left_top_corner_;
    int ijk0 =
        static_cast<int>(local_mean(0) * inverse_leaf_size_[0]) - min_b_[0];
    int ijk1 =
        static_cast<int>(local_mean(1) * inverse_leaf_size_[1]) - min_b_[1];
    int ijk2 =
        static_cast<int>(local_mean(2) * inverse_leaf_size_[2]) - min_b_[2];

    // Compute the centroid leaf index
    int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];

    Leaf& leaf = leaves_[idx];
    leaf = cell_leaf;

    if (cell_leaf.nr_points_ >= min_points_per_voxel_) {
      output->push_back(PointT());
      output->points.back().x = static_cast<float>(leaf.mean_[0]);
      output->points.back().y = static_cast<float>(leaf.mean_[1]);
      output->points.back().z = static_cast<float>(leaf.mean_[2]);
      voxel_centroids_leaf_indices_.push_back(idx);
    }
  }
  output->width = static_cast<uint32_t>(output->points.size());
}

牛顿梯度下降法

ndt_solver.hpp
ComputeTransformation()
对输入的激光点云进行了旋转，转换到地图坐标系的角度，这样可以避免大量转化地图的点云，大大减少运算量；
计算点云变换矩阵；
根据正态分布法建立目标函数，并求出目标函数；
使用牛顿法对目标函数进行迭代求出最小值；

template <typename PointSource, typename PointTarget>
void NormalDistributionsTransform<PointSource, PointTarget>::
    ComputeTransformation(PointCloudSourcePtr output,
                          const Eigen::Matrix4f &guess) {
  apollo::common::util::Timer timer;
  timer.Start();

  nr_iterations_ = 0;
  converged_ = false;
  double gauss_c1, gauss_c2, gauss_d3;

  // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
  gauss_c1 = 10 * (1 - outlier_ratio_);
  gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
  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()) {
    // Initialise final transformation to the guessed one
    final_transformation_ = guess;
    // Apply guessed transformation prior to search for neighbours
    transformPointCloud(*output, *output, guess);
  }

  // Initialize Point Gradient and Hessian
  point_gradient_.setZero();
  point_gradient_.block<3, 3>(0, 0).setIdentity();
  point_hessian_.setZero();

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

  // Convert initial guess matrix to 6 element transformation vector
  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;

  // Calculate derivates of initial transform vector, subsequent derivative
  // calculations are done in the step length determination.
  score = ComputeDerivatives(&score_gradient, &hessian, output, &p);
  timer.End("Ndt ComputeDerivatives");

  apollo::common::util::Timer loop_timer;
  loop_timer.Start();
  while (!converged_) {
    // Store previous transformation
    previous_transformation_ = transformation_;

    // Solve for decent direction using newton method, line 23 in Algorithm
    // 2 [Magnusson 2009]
    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> sv(
        hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
    // Negative for maximization as opposed to minimization
    delta_p = sv.solve(-score_gradient);

    // Calculate step length with guarnteed sufficient decrease [More,
    // Thuente 1994]
    delta_p_norm = delta_p.norm();

    if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) {
      trans_probability_ = score / static_cast<double>(input_->points.size());
      converged_ = delta_p_norm == delta_p_norm;
      return;
    }

    delta_p.normalize();
    delta_p_norm = ComputeStepLengthMt(p, &delta_p, delta_p_norm, step_size_,
                                       transformation_epsilon_ / 2, &score,
                                       &score_gradient, &hessian, output);
    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;

    if (nr_iterations_ > max_iterations_ ||
        (nr_iterations_ &&
         (std::fabs(delta_p_norm) < transformation_epsilon_))) {
      converged_ = true;
    }

    nr_iterations_++;
  }
  loop_timer.End("Ndt loop.");

  // Store transformation probability.  The relative differences within each
  // scan registration are accurate but the normalization constants need to be
  // modified for it to be globally accurate
  trans_probability_ = score / static_cast<double>(input_->points.size());
}

ComputeDerivatives()
trans_cloud：输入点云
p：初始转换矩阵

template <typename PointSource, typename PointTarget>
double
NormalDistributionsTransform<PointSource, PointTarget>::ComputeDerivatives(
    Eigen::Matrix<double, 6, 1> *score_gradient,
    Eigen::Matrix<double, 6, 6> *hessian, PointCloudSourcePtr trans_cloud,
    Eigen::Matrix<double, 6, 1> *p, bool compute_hessian) {
  // Original Point and Transformed Point
  PointSource x_pt, x_trans_pt;
  // Original Point and Transformed Point (for math)
  Eigen::Vector3d x, x_trans;
  // Occupied Voxel
  TargetGridLeafConstPtr cell;
  // Inverse Covariance of Occupied Voxel
  Eigen::Matrix3d c_inv;

  score_gradient->setZero();
  hessian->setZero();
  double score = 0;

  // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
  ComputeAngleDerivatives(*p);

  // Update gradient and hessian for each point, line 17 in Algorithm 2
  // [Magnusson 2009]
  for (size_t idx = 0; idx < input_->points.size(); idx++) {
    x_trans_pt = trans_cloud->points[idx];

    // Find neighbors (Radius search has been experimentally faster than
    // direct neighbor checking.
    std::vector<TargetGridLeafConstPtr> neighborhood;
    std::vector<float> distances;
    target_cells_.RadiusSearch(x_trans_pt, resolution_, &neighborhood,
                               &distances);

    for (typename std::vector<TargetGridLeafConstPtr>::iterator
             neighborhood_it = neighborhood.begin();
         neighborhood_it != neighborhood.end(); neighborhood_it++) {
      cell = *neighborhood_it;
      x_pt = input_->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);

      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
      x_trans -= cell->GetMean();
      // Uses precomputed covariance for speed.
      c_inv = cell->GetInverseCov();

      // Compute derivative of transform function w.r.t. transform vector,
      // J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
      ComputePointDerivatives(x);
      // Update score, gradient and hessian, lines 19-21 in Algorithm 2,
      // according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson
      // 2009]
      score += UpdateDerivatives(score_gradient, hessian, x_trans, c_inv,
                                 compute_hessian);
    }
  }
  return (score);
}

ComputeAngleDerivatives

template <typename PointSource, typename PointTarget>
void NormalDistributionsTransform<PointSource, PointTarget>::
    ComputeAngleDerivatives(const Eigen::Matrix<double, 6, 1> &p,
                            bool compute_hessian) {
                            ...
                            // Compute derivative of transform function w.r.t. transform vector,
      // J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
      ComputePointDerivatives(x);
      // Update score, gradient and hessian, lines 19-21 in Algorithm 2,
      // according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson
      // 2009]
      score += UpdateDerivatives(score_gradient, hessian, x_trans, c_inv,
                                 compute_hessian);
    }
  }
  return (score);
}

ComputeStepLengthMt()
通过得到的方程，最终变成一个最小二乘问题，
利用高斯牛顿法求最小值；
其中牵涉到了梯度下降方向，步长等问题；

template <typename PointSource, typename PointTarget>
double
NormalDistributionsTransform<PointSource, PointTarget>::ComputeStepLengthMt(
    const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> *step_dir,
    double step_init, double step_max, double step_min, double *score,
    Eigen::Matrix<double, 6, 1> *score_gradient,
    Eigen::Matrix<double, 6, 6> *hessian, PointCloudSourcePtr trans_cloud) {
...
if (open_interval) {
      // Update interval end points using Updating Algorithm [More,
      // Thuente 1994]
      interval_converged = UpdateIntervalMt(&a_l, &f_l, &g_l, &a_u, &f_u, &g_u,
                                            a_t, psi_t, d_psi_t);
    } else {
      // Update interval end points using Modified Updating Algorithm
      // [More, Thuente 1994]
      interval_converged = UpdateIntervalMt(&a_l, &f_l, &g_l, &a_u, &f_u, &g_u,
                                            a_t, phi_t, d_phi_t);
    }

    step_iterations++;
  }

  // If inner loop was run then hessian needs to be calculated.
  // Hessian is unnecessary for step length determination but gradients are
  // required so derivative and transform data is stored for the next
  // iteration.
  if (step_iterations) ComputeHessian(hessian, *trans_cloud, &x_t);

  return a_t;
}

UpdateDerivatives()

template <typename PointSource, typename PointTarget>
double
NormalDistributionsTransform<PointSource, PointTarget>::UpdateDerivatives(
    Eigen::Matrix<double, 6, 1> *score_gradient,
    Eigen::Matrix<double, 6, 6> *hessian, const Eigen::Vector3d &x_trans,
    const Eigen::Matrix3d &c_inv, bool compute_hessian) {
  Eigen::Vector3d cov_dxd_pi;
  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson
  // 2009]
  double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
  // Calculate probability of transtormed points existence, Equation 6.9
  // [Magnusson 2009]
  double score_inc = -gauss_d1_ * e_x_cov_x;

  e_x_cov_x = gauss_d2_ * e_x_cov_x;

  // Error checking for invalid values.
  if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) {
    return 0;
  }

  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
  e_x_cov_x *= gauss_d1_;

  for (int i = 0; i < 6; i++) {
    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13
    // [Magnusson 2009]
    cov_dxd_pi = c_inv * point_gradient_.col(i);

    // Update gradient, Equation 6.12 [Magnusson 2009]
    (*score_gradient)(i) += x_trans.dot(cov_dxd_pi) * e_x_cov_x;

    if (compute_hessian) {
      for (int j = 0; j < hessian->cols(); j++) {
        // Update hessian, Equation 6.13 [Magnusson 2009]
        (*hessian)(i, j) +=
            e_x_cov_x *
            (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
                 x_trans.dot(c_inv * point_gradient_.col(j)) +
             x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
             point_gradient_.col(j).dot(cov_dxd_pi));
      }
    }
  }

  return score_inc;
}

ComputeHessian

template <typename PointSource, typename PointTarget>
void NormalDistributionsTransform<PointSource, PointTarget>::ComputeHessian(
    Eigen::Matrix<double, 6, 6> *hessian, const PointCloudSource &trans_cloud,
    Eigen::Matrix<double, 6, 1> *p) {
  // Original Point and Transformed Point
  PointSource x_pt, x_trans_pt;
  // Original Point and Transformed Point (for math)
  Eigen::Vector3d x, x_trans;
  // Occupied Voxel
  TargetGridLeafConstPtr cell;
  // Inverse Covariance of Occupied Voxel
  Eigen::Matrix3d c_inv;

  hessian->setZero();

  // Precompute Angular Derivatives unnecessary because only used after regular
  // derivative calculation

  // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
  for (size_t idx = 0; idx < input_->points.size(); idx++) {
    x_trans_pt = trans_cloud.points[idx];

    // Find neighbors (Radius search has been experimentally faster than
    // direct neighbor checking.
    std::vector<TargetGridLeafConstPtr> neighborhood;
    std::vector<float> distances;
    target_cells_.RadiusSearch(x_trans_pt, resolution_, &neighborhood,
                               &distances);

    for (typename std::vector<TargetGridLeafConstPtr>::iterator
             neighborhood_it = neighborhood.begin();
         neighborhood_it != neighborhood.end(); neighborhood_it++) {
      cell = *neighborhood_it;
      {
        x_pt = input_->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);

        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson
        // 2009]
        x_trans -= cell->GetMean();
        // Uses precomputed covariance for speed.
        c_inv = cell->GetInverseCov();

        // Compute derivative of transform function w.r.t. transform
        // vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson
        // 2009]
        ComputePointDerivatives(x);
        // Update hessian, lines 21 in Algorithm 2, according to
        // Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
        UpdateHessian(hessian, x_trans, c_inv);
      }
    }
  }
}

UpdateHessian

template <typename PointSource, typename PointTarget>
void NormalDistributionsTransform<PointSource, PointTarget>::UpdateHessian(
    Eigen::Matrix<double, 6, 6> *hessian, const Eigen::Vector3d &x_trans,
    const Eigen::Matrix3d &c_inv) {
  Eigen::Vector3d cov_dxd_pi;
  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9
  // [Magnusson 2009]
  double e_x_cov_x =
      gauss_d2_ * exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);

  // Error checking for invalid values.
  if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) {
    return;
  }

  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
  e_x_cov_x *= gauss_d1_;

  for (int i = 0; i < 6; i++) {
    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13
    // [Magnusson 2009]
    cov_dxd_pi = c_inv * point_gradient_.col(i);

    for (int j = 0; j < hessian->cols(); j++) {
      // Update hessian, Equation 6.13 [Magnusson 2009]
      (*hessian)(i, j) +=
          e_x_cov_x *
          (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
               x_trans.dot(c_inv * point_gradient_.col(j)) +
           x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
           point_gradient_.col(j).dot(cov_dxd_pi));
    }
  }
}

参考资料：
https://blog.csdn.net/li812732767/article/details/110927143