NDT 公式推导及源码解析（2）
本文以 Autoware 的 ndt_cpu 为例对 NDT 的实现进行解析。源码解析顺序为 Octree、VoxelGrid、Registration、NormalDistributionTransform，如果觉得内容太过分散也可以看下 [2] 里的完整代码注释。
Octree
Octree 是在 VoxelGrid 基础上构建的，用以加速近邻查找。关于这个问题我一开始想了很久为啥要用 Octree 加速，VoxelGrid 已经是对所有点云进行了一个划分了，要找最近邻直接将点投影到对应的 voxel 不就行了么，之后才想起来查询点并不一定在 VoxelGrid 里的，这样就不能投影找了，而要遍历所有的 voxel 计算最短距离了。而 Octree 是在 VoxelGrid 的基础上构建的（也就是说一个 OctreeNode 包含了多个空间相邻的 voxel），类似于 KDTree，在查找近邻 OctreeNode 时复杂度与树的深度成比例也就是 O(h)，之后我们可以对 Node 内部的所有 voxel 进行遍历找到最近的 voxel，最终达到了加速近邻查找的效果。
先上 Octree 的头文件，可以看到用户能使用的也就是构造、设置输入、更新
/* The octree is built on top of a voxel grid to fasten the nearest neighbor search */
namespace cpu {
template <typename PointSourceType>
class Octree {
public:
Octree();
/* Input is a vector of boundaries and ptsum of the voxel grid
* Those boundaries is needed since the number of actually occupied voxels may be
* much smaller than reserved number of voxels */
/**
* @brief Set the Input object 虽然 Octree 是在 VoxelGrid 的基础上构造的，但还是需要 point_cloud 以计算每个 OctreeNode 的点云 bounding box
* 一个 OctreeNode 包含 leaf_size 个 voxel（会向上/向下取整），自底向上进行构造 Octree
* @param occupied_voxels point_cloud 中每个点对应的 voxel 坐标
* @param point_cloud
*/
void setInput(std::vector<Eigen::Vector3i> occupied_voxels, typename pcl::PointCloud<PointSourceType>::Ptr point_cloud);
/**
* @brief 使用新增的 new_voxels 更新 Octree
* 先 updateBoundaries，扩充边界，修改数组大小和维度，从 old_tree 自底向上往 new_tree 拷贝。updateOctreeContent 再根据 new_voxels 对 new_tree 进行更新
* @param new_voxels
* @param new_cloud
*/
void update(std::vector<Eigen::Vector3i> new_voxels, typename pcl::PointCloud<PointSourceType>::Ptr new_cloud);
/**
* @brief 返回距离查询点 q 最近（距离 centroid）的 OctreeNode 的 bounding box，查询过程与 KDTree 十分类似
* 1. 先从根节点到叶节点找到距离 q 最近的 OctreeNode
* 2. 自底向上到根节点（goUp）检查在其兄弟节点中是否存在更近的 OctreeNode，对每个兄弟节点向下（goDown）遍历至叶节点找更近的 OctreeNode，会计算 q 与 OctreeNode 的 bounding box 的距离，如果更大则剪枝，如果更小则向下到叶节点
* @param q 查询点
* @return Eigen::Matrix 距离查询点 q 最近的 OctreeNode 的 bounding box
*/
Eigen::Matrix<float, 6, 1> nearestOctreeNode(PointSourceType q);
private:
// Octree 的一个节点
typedef struct {
// OctreeNode 包含的点云的 bounding box
float lx, ux;
float ly, uy;
float lz, uz;
// OctreeNode 包含的点云的中心位置
Eigen::Vector3d centroid;
// OctreeNode 包含的点数
int point_num;
} OctreeNode;
// Octree 每层的边界，用于计算每一层节点的 index
typedef struct {
int lower_x, upper_x;
int lower_y, upper_y;
int lower_z, upper_z;
} OctreeLevelBoundaries;
// Octree 每层在 x,y,z 维度上数量，与 OctreeLevelBoundaries 配合计算每层 OctreeNode 的 index
typedef struct {
int x, y, z;
} OctreeLevelDim;
// 根据 OctreeNode 的三维 index 计算实际存储在 vector 中的 index
// Convert from 3d indexes and level of the tree node to the actual index in the array
int index2id(int idx, int idy, int idz, int level);
int index2id(int idx, int idy, int idz, OctreeLevelBoundaries bounds, OctreeLevelDim dims);
// Convert from the index in the array to the 3d indexes of the tree node
Eigen::Vector3i id2index(int id, int level);
Eigen::Vector3i id2index(int id, OctreeLevelBoundaries bounds, OctreeLevelDim dims);
// 根据下一层的子 OctreeNode 构造上一层的父 OctreeNode
void buildLevel(int level);
// 给定 node_id 和所在层（最底层为 0），根据 (*occupancy_check)[level] 判断是否被 occupied，即是否包含点云
bool isOccupied(int node_id, int level);
bool isOccupied(std::vector<unsigned int> occupancy, int node_id);
void setOccupied(int node_id, int level);
void setOccupied(std::vector<unsigned int> &occupancy, int node_id);
// 根据 new_voxels 更新每层的 boundaries
void updateBoundaries(std::vector<Eigen::Vector3i> new_voxels);
// 以 factor 为单位向上/向下取整
int roundUp(int input, int factor);
int roundDown(int input, int factor);
int div(int input, int divisor);
// 根据新点云自底向上更新 octree
void updateOctreeContent(std::vector<Eigen::Vector3i> new_voxels, typename pcl::PointCloud<PointSourceType>::Ptr new_cloud);
// 计算点 q 距离 node 的 bounding box 的距离
double dist(OctreeNode node, PointSourceType q);
/* Three functions to search for the nearest neighbor of a point */
// 参数名起的太烂了，跟 voxel 没什么关系，就是找距离 q 最近的 OctreeNode
void initRange(PointSourceType q, double &min_range, int ¤t_nn_voxel);
void goUp(Eigen::Matrix<int, 4, 1 > tree_node, PointSourceType q, double &min_range, int ¤t_nn_voxel);
void goDown(Eigen::Matrix<int, 4, 1> tree_node, PointSourceType q, double &min_range, int ¤t_nn_voxel);
// 实际的 Octree，每层 OctreeNode 存储在一个 vector 中，最终形成了一个二维数组
boost::shared_ptr<std::vector<std::vector<OctreeNode> > > octree_;
// 保存每一层 OctreeNode 的边界，以便 3d index 与 1d index 的转换
boost::shared_ptr<std::vector<OctreeLevelBoundaries> > reserved_size_;
// 保存每一层 OctreeNode 的维度，以便 3d index 与 1d index 的转换
boost::shared_ptr<std::vector<OctreeLevelDim> > dimension_;
/* Used for checking if an octree node is occupied or not
* If an octree node is occupied (containing some points),
* then the corresponding bit is set
*/
/**
* @brief 用于检查一个 OctreeNode 是否被 occupied，可以用于构造树阶段 OctreeNode 的初始化，也可以用来查询阶段的提前剪枝
* 每一层 OctreeNode 的 occupied 情况被保存在一个 vector 中，每个 OctreeNode 的 occupied 情况用 1bit 保存。相较于直接使用 bool 占据的空间更小。
*/
boost::shared_ptr<std::vector<std::vector<unsigned int> > > occupancy_check_;
// 每个维度保存的 voxel 数量（代码里的“维度”有时候指的是 x,y,z，有时候指的是 x,y,z 方向上的 size，注意区分）
int leaf_x_, leaf_y_, leaf_z_; // Number of voxels contained in each leaf
// 取整用的，不太明白这么做的好处，是为了避免频繁更新吗？
static const int MAX_BX_ = 8;
static const int MAX_BY_ = 8;
static const int MAX_BZ_ = 4;
};
}
#endif
这里只介绍建树和查找的实现，其余源码解析可以见源码注释。
void setInput(std::vectorEigen::Vector3i occupied_voxels, typename pcl::PointCloud::Ptr point_cloud)
- 先根据每个点的 voxel 确定 Octree 整体的 boundary，其实也就是所有叶节点的 boundary；根据这个 boundary 确定 Octree 叶节点这一层的节点数和各维度的上下限（方便计算 index）
- 自底向上确定上一层的节点数和各维度上下限，一直到一层的节点数小于 8
- 构建最底层（0 层）的 OctreeNode（根据 voxel 来构建）
- 从底向上构建每一层的 OctreeNode（根据下一层 OctreeNode 构建）
Eigen::Matrix nearestOctreeNode(PointSourceType q)
- 先从根节点到叶节点找到距离 q 最近的叶 OctreeNode（initRange）
- 自底向上到根节点（goUp）检查在其兄弟节点中是否存在更近的 OctreeNode，对每个兄弟节点向下（goDown）遍历至叶节点找更近的 OctreeNode，会计算 q 与 OctreeNode 的 bounding box 的距离，如果更大则剪枝，如果更小则向下到叶节点
VoxelGrid
VoxelGrid 就是对点云的 bounding box 空间在 x,y,z 三个维度上根据 voxel_x, voxel_y, voxel_z 进行等分，因此可能会有部分 voxel 中是没有点云的。在看完 Octree 之后，回过头来看 VoxelGrid 就比较简单了，这个类的主要作用就是保存各个 voxel 的 μ , Σ \mu, \Sigma μ,Σ 了，以及给定 point 查找其附近的 voxel（radiusSearch）。
还是先上代码大致看一下。
namespace cpu {
template <typename PointSourceType>
class VoxelGrid {
public:
VoxelGrid();
/* Set input points */
void setInput(typename pcl::PointCloud<PointSourceType>::Ptr input);
/* For each input point, search for voxels whose distance between their centroids and
* the input point are less than radius.
* The output is a list of candidate voxel ids */
void radiusSearch(PointSourceType query_point, float radius, std::vector<int> &voxel_ids, int max_nn = INT_MAX);
int getVoxelNum() const;
float getMaxX() const;
float getMaxY() const;
float getMaxZ() const;
float getMinX() const;
float getMinY() const;
float getMinZ() const;
float getVoxelX() const;
float getVoxelY() const;
float getVoxelZ() const;
int getMaxBX() const;
int getMaxBY() const;
int getMaxBZ() const;
int getMinBX() const;
int getMinBY() const;
int getMinBZ() const;
int getVgridX() const;
int getVgridY() const;
int getVgridZ() const;
void setLeafSize(float voxel_x, float voxel_y, float voxel_z);
/* Searching for the nearest point of each input query point.
* Return the distance between the query point and its nearest neighbor.
* If the distance is larger than max_range, then return DBL_MAX. */
double nearestNeighborDistance(PointSourceType query_point, float max_range);
Eigen::Vector3d getCentroid(int voxel_id) const;
Eigen::Matrix3d getCovariance(int voxel_id) const;
Eigen::Matrix3d getInverseCovariance(int voxel_id) const;
void update(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud);
private:
typedef struct {
int x, y, z;
} OctreeDim;
/* Construct the voxel grid and the build the octree. */
void initialize();
/* Put points into voxels */
void scatterPointsToVoxelGrid();
/* Compute centroids and covariances of voxels. */
void computeCentroidAndCovariance();
/* Find boundaries of input point cloud and compute
* the number of necessary voxels as well as boundaries
* measured in number of leaf size */
void findBoundaries();
void findBoundaries(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud,
float &max_x, float &max_y, float &max_z,
float &min_x, float &min_y, float &min_z);
int voxelId(PointSourceType p);
int voxelId(PointSourceType p,
float voxel_x, float voxel_y, float voxel_z,
int min_b_x, int min_b_y, int min_b_z,
int vgrid_x, int vgrid_y, int vgrid_z);
int voxelId(int idx, int idy, int idz,
int min_b_x, int min_b_y, int min_b_z,
int size_x, int size_y, int size_z);
/* Private methods for merging new point cloud to the current point cloud */
void updateBoundaries(float max_x, float max_y, float max_z,
float min_x, float min_y, float min_z);
void updateVoxelContent(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud);
int nearestVoxel(PointSourceType query_point, Eigen::Matrix<float, 6, 1> boundaries, float max_range);
int roundUp(int input, int factor);
int roundDown(int input, int factor);
int div(int input, int divisor);
//Coordinate of input points
typename pcl::PointCloud<PointSourceType>::Ptr source_cloud_;
int voxel_num_; // Number of voxels
float max_x_, max_y_, max_z_; // Upper bounds of the grid (maximum coordinate)
float min_x_, min_y_, min_z_; // Lower bounds of the grid (minimum coordinate)
float voxel_x_, voxel_y_, voxel_z_; // Leaf size, a.k.a, size of each voxel
int max_b_x_, max_b_y_, max_b_z_; // Upper bounds of the grid, measured in number of voxels
int min_b_x_, min_b_y_, min_b_z_; // Lower bounds of the grid, measured in number of voxels
int vgrid_x_, vgrid_y_, vgrid_z_; // Size of the voxel grid, measured in number of voxels
int min_points_per_voxel_; // Minimum number of points per voxel. If the number of points
// per voxel is less than this number, then the voxel is ignored
// during computation (treated like it contains no point)
boost::shared_ptr<std::vector<Eigen::Vector3d> > centroid_; // 3x1 Centroid vectors of voxels
boost::shared_ptr<std::vector<Eigen::Matrix3d> > icovariance_; // Inverse covariance matrixes of voxel
boost::shared_ptr<std::vector<std::vector<int> > > points_id_; // Indexes of points belong to each voxel
boost::shared_ptr<std::vector<int> > points_per_voxel_; // Number of points belong to each voxel
// (may differ from size of each vector in points_id_
// because of changes made during computing covariances
boost::shared_ptr<std::vector<Eigen::Vector3d> > tmp_centroid_;
boost::shared_ptr<std::vector<Eigen::Matrix3d> > tmp_cov_;
int real_max_bx_, real_max_by_, real_max_bz_;
int real_min_bx_, real_min_by_, real_min_bz_;
Octree<PointSourceType> octree_;
static const int MAX_BX_ = 16;
static const int MAX_BY_ = 16;
static const int MAX_BZ_ = 8;
};
}
#endif
这里只介绍 setInput、radiusSearch。
void setInput(typename pcl::PointCloud::Ptr input)
- 确定 VoxelGrid 每个维度的 size（会做一个向上/向下取整）以及 voxel 总数（findBoundaries）
- 确定 input_cloud 中每个点所在的 voxel id
- 调用 Octree 的 setInput 进行初始化
- 接下来就是计算每个 voxel 的 centroid（ μ \mu μ）和 covariance（ Σ \Sigma Σ），代码实现是先调用 scatterPointsToVoxelGrid 计算中间值，再根据中间值调用 computeCentroidAndCovariance 计算实际的 centroid 和 covariance（ Σ = 1 n ∑ k = 1 n ( x ⃗ − μ ⃗ ) ( x ⃗ − μ ⃗ ) T = 1 n ( ∑ k = 1 n x ⃗ x ⃗ T − ∑ k = 1 n x ⃗ μ ⃗ T ) + μ ⃗ μ ⃗ T \Sigma = \frac{1}{n}\sum\limits_{k=1}^n(\vec{x}-\vec{\mu})(\vec{x}-\vec{\mu})^T = \frac{1}{n}(\sum\limits_{k=1}^n\vec{x}\vec{x}^T - \sum\limits_{k=1}^n\vec{x}\vec{\mu}^T) + \vec{\mu}\vec{\mu}^T Σ=n1k=1∑n(x−μ)(x−μ)T=n1(k=1∑nxxT−k=1∑nxμT)+μμT）。这里也会使用论文中提到的方法对接近奇异的 Σ \Sigma Σ 进行一个 inflate，然后直接保存 Σ − 1 \Sigma^{-1} Σ−1
void radiusSearch(PointSourceType query_point, float radius, std::vector &voxel_ids, int max_nn = INT_MAX)
这个就很简单了，就是在 query_point 的 radius bounding box 范围内遍历找 voxel，将 dist(query_point, centroid) 小于 radius 的 voxel 加入到结果中。这个方法最终是在 NDT 为每个 source point 找对应的 voxel 时调用的（是的，没错，并不是找距离 source point 最近的，而是用 radius 范围内的所有 voxel 计算梯度，这样应该能获得类似 trilinear interpolation 的平滑效果）
其实代码看到这有个想吐槽的是，貌似我们前面构造的 octree 并没有用到嘛，事实上 octree 也只是在 nearstNeighborDistance 里用于找最近的 OctreeNode，然后遍历找最近的 voxel。然而这个方法在实际使用中只在计算 FitnessScore 时才用到，也就是说对于最终的求解并没有帮助（一脸懵逼。。。）。
Registration
这个类没啥好说的，就是个基类，用来继承实现内部的 computeTransformation 虚方法的，值得一说的是里面的 transformation_epsilon_ 成员，这个成员一般是用来规定最小变化量的（也就是最小的 Δ [ x , y , z , r o l l , p i t c h , y a w ] \Delta[x,y,z,roll,pitch,yaw] Δ[x,y,z,roll,pitch,yaw]），具体在 NDT 里是用来规定 MT 的最小更新步长的。
NormalDistributionTransform
说了这么多，终于到正主了，如果看完了 NDT 公式推导及源码解析（1） 以及以上内容的话，这部分代码理解起来基本没有什么问题（除了 line search 部分）。
namespace cpu
{
template <typename PointSourceType, typename PointTargetType>
class NormalDistributionsTransform : public Registration<PointSourceType, PointTargetType>
{
public:
NormalDistributionsTransform();
// 拷贝构造
NormalDistributionsTransform(const NormalDistributionsTransform &other);
void setStepSize(double step_size);
// 设置 ndt 中 voxel 的大小
void setResolution(float resolution);
// 设置离群点的比例，用于计算混合分布中均值和高斯的权重
void setOutlierRatio(double olr);
double getStepSize() const;
float getResolution() const;
double getOutlierRatio() const;
// TODO: 如何度量的？实际意义是什么？
double getTransformationProbability() const;
int getRealIterations();
/* Set the input map points */
void setInputTarget(typename pcl::PointCloud<PointTargetType>::Ptr input);
// Euclidean fitness score，output 点云与 target 中最近点距离的平方和。感觉作用不是很大，在环境变化明显的时候，即使位姿比较准确，这个值应该也会挺大的。而且有一定计算量，最好不好经常调用
/* Compute and get fitness score */
double getFitnessScore(double max_range = DBL_MAX);
// 调用 voxel_grid_ 的 update 进行更新
void updateVoxelGrid(typename pcl::PointCloud<PointTargetType>::Ptr new_cloud);
protected:
// 给定初始位姿估计，牛顿迭代计算位姿
void computeTransformation(const Eigen::Matrix<float, 4, 4> &guess);
using Registration<PointSourceType, PointTargetType>::transformation_epsilon_;
using Registration<PointSourceType, PointTargetType>::max_iterations_;
using Registration<PointSourceType, PointTargetType>::source_cloud_;
using Registration<PointSourceType, PointTargetType>::trans_cloud_;
using Registration<PointSourceType, PointTargetType>::converged_;
using Registration<PointSourceType, PointTargetType>::nr_iterations_;
using Registration<PointSourceType, PointTargetType>::final_transformation_;
using Registration<PointSourceType, PointTargetType>::transformation_;
using Registration<PointSourceType, PointTargetType>::previous_transformation_;
using Registration<PointSourceType, PointTargetType>::target_cloud_updated_;
using Registration<PointSourceType, PointTargetType>::target_cloud_;
private:
//Copied from ndt.h
double auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4);
//Copied from ndt.h
double auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4);
double updateIntervalMT(double &a_l, double &f_l, double &g_l,
double &a_u, double &f_u, double &g_u,
double a_t, double f_t, double g_t);
double trialValueSelectionMT(double a_l, double f_l, double g_l,
double a_u, double f_u, double g_u,
double a_t, double f_t, double g_t);
void computeAngleDerivatives(Eigen::Matrix<double, 6, 1> pose, bool compute_hessian = true);
double 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,
typename pcl::PointCloud<PointSourceType> &trans_cloud);
void computeHessian(Eigen::Matrix<double, 6, 6> &hessian, typename pcl::PointCloud<PointSourceType> &trans_cloud, Eigen::Matrix<double, 6, 1> &p);
double 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 = true);
void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6> &point_gradient, Eigen::Matrix<double, 18, 6> &point_hessian, bool computeHessian = true);
double updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
Eigen::Matrix<double, 3, 6> point_gradient, Eigen::Matrix<double, 18, 6> point_hessian,
Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, bool compute_hessian = true);
void updateHessian(Eigen::Matrix<double, 6, 6> &hessian,
Eigen::Matrix<double, 3, 6> point_gradient, Eigen::Matrix<double, 18, 6> point_hessian,
Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv);
double gauss_d1_, gauss_d2_;
double outlier_ratio_;
Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
Eigen::Vector3d h_ang_a2_, h_ang_a3_,
h_ang_b2_, h_ang_b3_,
h_ang_c2_, h_ang_c3_,
h_ang_d1_, h_ang_d2_, h_ang_d3_,
h_ang_e1_, h_ang_e2_, h_ang_e3_,
h_ang_f1_, h_ang_f2_, h_ang_f3_;
// [x,y,z,roll,pitch,yaw] 的最小变化量(m, rad)，当小于这个值时就停止 align
// double transformation_epsilon;
// More-Thuente line search 的最大步长，大一些可以更快的下降，但也可能 overshoot 导致陷入局部最优
double step_size_;
// ndt 中 voxel 的大小，每个 voxel 中会保存 mean，covariance 和 点云，这个值是最 scale dependent 的，应该足够大（一个 voxel 至少包含 6 个点），也不能太大（要反映局部空间的特征）
float resolution_;
// 还不知道怎么度量的
double trans_probability_;
int real_iterations_;
VoxelGrid<PointSourceType> voxel_grid_;
};
} // namespace cpu
#endif
这部分代码的阅读我们可以从 ndt 的使用步骤顺序来阅读。
- ndt.setTransformationEpsilon (0.01) 这个调用的是基类方法，之前介绍过作用了
- ndt.setStepSize (0.1) 设置 MT 的最大更新步长
- ndt.setResolution (1.0) 设置 VoxelGrid 的 leaf_size
- ndt.setMaximumIterations (35) 设置最大迭代次数
- ndt.setInputTarget (target_cloud) 设置 reference point cloud，一般就是 map point cloud。这个方法内会对 voxel_grid_ 调用 setInput 进行初始化
- ndt.setInputSource (filtered_cloud) 这个就是把要匹配的 source point cloud 保存一下
- ndt.align (init_guess) 以 init_guess 为初始值进行迭代优化，将结果保存在 final_transformation_ 中。这个方法最终调用的还是 computeTransformation 完成最终的计算，这里还是贴下这部分的代码注释介绍一下吧，虽然代码也很简单，部分公式的代码在 computeDerivatives 和 computeStepLengthMT 里，这里就不贴了，具体可以看源码注释。
template <typename PointSourceType, typename PointTargetType>
void NormalDistributionsTransform<PointSourceType, PointTargetType>::computeTransformation(const Eigen::Matrix<float, 4, 4> &guess)
{
nr_iterations_ = 0;
converged_ = false;
double gauss_c1, gauss_c2, gauss_d3;
// 公式 6.8，但还不知道 gauss_c1 是怎么计算的
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()) {
transformation_ = guess;
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;
// 这个 score 不知道含义是什么。计算 score function 关于 位姿 p 的 Jacobian 和 Hessian
score = computeDerivatives(score_gradient, hessian, trans_cloud_, p);
int points_number = source_cloud_->points.size();
while (!converged_) {
previous_transformation_ = transformation_;
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;
}
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 (points_number > 0) {
trans_probability_ = score / static_cast<double>(points_number);
}
}
简单的总结
似乎没啥可总结的，除了 line search 部分的代码由于没有推公式就没有看之外，其他的代码还是比较好理解的，而且在代码实现里也没有什么论文里没有提到的 trick。
Reference
- autowarefoundation/autoware
- https://github.com/jyakaranda/autoware_ros/tree/dev_zh/Autoware/ros/src/computing/perception/localization/lib/ndt_cpu