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<float, 6, 1> 距離查詢點 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 &current_nn_voxel);

	void goUp(Eigen::Matrix<int, 4, 1 > tree_node, PointSourceType q, double &min_range, int &current_nn_voxel);

	void goDown(Eigen::Matrix<int, 4, 1> tree_node, PointSourceType q, double &min_range, int &current_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<float, 6, 1> 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（ $\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$ ）。這裏也會使用論文中提到的方法對接近奇異的 $\Sigma$ 進行一個 inflate，然後直接保存 $\Sigma^{-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_ 成員，這個成員一般是用來規定最小變化量的（也就是最小的 $\Delta[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。

NDT 公式推導及源碼解析（2）

Octree

void setInput(std::vectorEigen::Vector3i occupied_voxels, typename pcl::PointCloud::Ptr point_cloud)

Eigen::Matrix<float, 6, 1> nearestOctreeNode(PointSourceType q)

VoxelGrid

void setInput(typename pcl::PointCloud::Ptr input)

void radiusSearch(PointSourceType query_point, float radius, std::vector &voxel_ids, int max_nn = INT_MAX)

Registration

NormalDistributionTransform

簡單的總結

Reference

RANSAC 學習筆記

ov_eval說明

pointnet 系列論文總結

實時系統介紹

NDT 公式推導及源碼解析（2）

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結