一文读懂自动驾驶中常用的定位算法之NDT点云配准算法

文中所用图片如无声明来源网络和论文。

Paper: The Three-Dimensional Normal-Distributions Transform— an Efficient Representation for Registration,Surface Analysis, and Loop Detection
下载地址
这是Martin Magnusson的2009年博士论文

写在前面

研究NDT算法开始由于我们在校园无人车项目中的车辆定位方案中使用了这种方法，此外autoware无人驾驶方案中核心定位算法也是NDT算法。作为一种常用的点云匹配算法，拿来欣赏学习实在是一种享受，那种透过数学推导看到算法背后的思想后，真的是佩服做出来这些算法的前辈们！这篇文章还没有写完，后续慢慢填上去，如有任何问题欢迎下方留言探讨。

Abstract

对原始采样点云进行应用NDT算法之后，可以将扫描到的表面使用一个光滑的函数，这个函数有可解析的一阶导数和二阶导数。这样的表示方法有一些比较好的特性可以利用,它使得可以利用标准优化方法，如：牛顿法，进行scan registration。这个算法将2d registration扩展到3d， 并进行了一些提升。

使用技术，克服了普通registration算法中的离散表示问题：

• multiresolution discretisation
• trilinear interpolation

相比于icp算法，ndt对于比较差的initial pose估计更加快速，精确，鲁棒。

更进一步，提出了kernel-based extension 3d NDT算法用于注册coloured data。

• original algorithm 对于每一个quantum of space使用one metric normal distribution
• colour-NDT使用三个components，每一个使用一种颜色。这种表示允许仅有少量geometric features的coloured scans可以被注册。当3d点和图像数据都可以获得，使用图像数据的lcoal visual freatures进行scan registration是可能的。然后基于local features的方法通常只是用获得的3d点中的一小部分用于注册。相反，colured-NDT使用所有获得的3d点。

• 如何处理大量数据
• 如何有效获得3d旋转不变性

所提出的方法仅使用3D点云的外观来检测循环并且不需要pose信息，它利用NDT表面表示来创建基于局部表面方向(local surface orientation)和平滑度(smoothness)的特征直方图(features histogram)。

• 表面形状直方图将输入数据压缩两到三个数量级。由于高压缩率，可以有效地匹配直方图以比较两次扫描的外观。
• 通过align扫描到的主要表面取向，来实现旋转不变性。

Chapter 1 Introduction

1.1 Contributions

• 3D-NDT surface representation
• 3D-NDT registration with extensions
  通过使用多分辨率离散化技术和三线性插值，可以克服基本3D-NDT配准算法中存在的一些离散化问题
  
  
• Colour-NDT registration
• Appearance-based loop detection from 3D laser scans
• Boulder detection from 3D laser scans

Chapter2 Common concepts

2.1 rotation

NDT中使用欧拉角zyx来表示，在这种情况下使用欧拉角的基本原理是避免在数值优化方法中加入非线性约束。对于执行扫描配准时遇到的相对较小的角度，不太可能发生万向节锁定。因此，通过更容易的问题公式来评估欧拉角的潜在缺点。

2.2 registration

• pose tracking，机器人定位
• modelling（三维重建），比如矿井下的map

2.3 sampling

在许多情况下，尤其是在走廊和隧道中进行扫描时，扫描仪位置附近的点分布比远处更密集。如果以均匀随机的方式对点进行采样，则采样的子集将具有类似的不均匀分布。从扫描的远端的重要几何结构中取样会很少或没有点，会导致配准不好。

为了克服这个问题，通常使用某种形式的空间分布式采样，以确保样本密度在整个扫描体积上均匀。

在当前工作中完成此操作的方式是通过创建具有相同大小的单元格的网格结构并将扫描点放置在相应的单元格中。该采样网格的单元尺寸通常在0.1和0.2米之间。从随机单元格中抽取随机点，直到达到所需的点数。如果cell分布足够，这种策略将给出均匀的点分布。

2.4 slam

slam主要的step：

• registration,
• pose-covariance estimation,
• loop detection,
• relaxation.

registrater机器人的连续视图以跟踪机器人的pose（localization）并构建metric map（mapping）。对于每个视图（或从一组视图生成的submaps），将node插入到scene graphe中，其edge将当前submap连接到其neighbours。还需要估计相邻submaps之间的相对pose的covariance，并将其附加到edge of graph。 一旦机器人检测到它已返回到先前访问过的位置，就可以计算在遍历回环上累积的error。基于与每个edge相关联的covariance估计，通过执行relaxation of the graph，将地图重新形成为consistent state。

Chapter3 Range sensing

通常，较短的波长导致更高的距离分辨率（更准确的范围读数）和更少的镜面反射（从发光表面读取的读数更少），但由于吸收和散射（衰减）而导致更短的最大范围。 空气中的气体和颗粒吸收并散射能量，使得较少的光束返回到接收器，从而降低了测量的可靠性。

• radar
  因为雷达使用相对较长的波长，所以它可以在很长的范围内使用，尽管与波长较短的传感器相比分辨率较差。 此外，雷达不易受到灰尘，雾，雨，雪，真空或光线条件变化的影响。 雷达的一个独特功能是它可以检测多个物体向下范围。 然而，它不会穿透钢铁或坚硬的岩石
  
  
• lidar
  与雷达和声纳的光束相比，激光束可以高度聚焦，没有旁瓣。 这也是短波长的影响。
  与雷达相比，激光传感器的主要缺点是在扫描多尘或有雾环境时对衰减和散射的敏感性，尽管红外激光在穿透烟雾和灰尘方面比可见光更好。
  最近的激光雷达设备可以测量每个光束的多个回波，因此可以测量灰尘和超出它的表面。
  可以使用三角测量或通过测量发射光束的飞行时间或其相移来测量到物体的距离;
  • Triangulation
  • time of flight

  
  
  
  
  
  
  
  

• sonar
• steroeo vision
• Time-of-flight cameras

Chapter5 Related work on scan registration

点云匹配算法有很多种，下面的是一些经典算法

• ICP
• IDC
• PIC
• Point-based probabilistic registration
• NDT
• Gaussian fields
• Quadratic patches
• Likelihood-field matching
• CRF matching
• Branch-and-bound registration
• Registration using local geometric features

Chapter6 The normal-distributions transform

6.1 NDT for representing surfaces

但是，使用点云来表示曲面有许多限制。例如，点云不包含有关表面特征的明确信息，例如方向，平滑度或孔。根据传感器配置，点云也可能效率低下，需要不必要的大量存储。 为了获得远离传感器位置的足够的sample resolution，通常需要以从传感器附近的表面产生大量冗余数据的方式配置传感器。

NDT的这种transform的核心思想是将点云映射到平滑表面来表示，使用一组局部概率密度函数（PDF）来描述，每个PDF描述表面的一部分的形状。

该算法的第一步是将扫描所占用的空间细分为单元网格（2D情况下的正方形或3D中的立方体），基于cell内的点的分布计算每个cell的PDF。

每个cell中的PDF可以被解释为cell内表面点$\vec{x}$的generative process（生成过程）。 换句话说，假设可以通过对该cell内点的分布的刻画来生成$\vec{x}$的位置。

假设参考扫描表面点 surface point的位置是由D-dimensional normal random process(D维正态随机过程)产生的，则测量得到$\vec{x}$的概率是

$p(\vec{x})=\frac{1}{(2\pi {)}^{D/2}\sqrt{|\mathbf{\Sigma }|}}\exp \left(-\frac{(\vec{x}-\vec{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\vec{x}-\vec{\mu })}{2}\right)$
其中 ${\left((2\pi {)}^{D/2}\sqrt{|\mathbf{\Sigma }|}\right)}^{-1}$可以用一个常数$c_0$来表示。均值和方差使用下面的公式来计算
$\begin{array}{rl}\vec{\mu }& =\frac{1}{m}\sum _{k=1}^m{\vec{y}}_k\\ \mathbf{\Sigma }& =\frac{1}{m-1}\sum _{k=1}^m\left({\vec{y}}_k-\vec{\mu }\right){\left({\vec{y}}_k-\vec{\mu }\right)}^{\mathrm{T}}\end{array}$
其中${\vec{y}}_{k=1,\dots ,m}$是cell中point的位置。

正态分布给出了点云的分段平滑表示，具有连续导数。每个PDF都可以看作是局部表面的近似值，描述了表面的位置及其方向和平滑度。 2D激光扫描及其相应的正态分布如下图所示。 下下图显示了矿井隧道扫描的3D正态分布。

来自矿井隧道的2D激光扫描（显示为点）和描述表面形状的PDF。 在这种情况下，每个cell是边长为2米的正方形。 更亮的区域代表更高的概率。 仅针对超过五个点的cell计算PDF。

从上方看到的隧道剖面的3D-NDT表面表示。 更明亮，更密集的部分代表更高的概率。 cell的边长为1米。

由于目前的工作主要集中在正态分布上，让我们更仔细地研究单变量和多变量正态分布的特征。在一维情况下，正态分布的随机变量$x$具有一定的期望值$\mu$，并且关于该值的不确定性用方差$\sigma$表示，相当于维度$D=1$。
$p(x)=\frac{1}{\sigma \sqrt{2\pi }}\exp \left(-\frac{(x-\mu {)}^2}{2{\sigma }^2}\right)$
这是多变量的正态分布表示，
$p(\vec{x})=\frac{1}{(2\pi {)}^{D/2}\sqrt{|\mathbf{\Sigma }|}}\exp \left(-\frac{(\vec{x}-\vec{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\vec{x}-\vec{\mu })}{2}\right)$

在2D和3D情况下，可以从协方差矩阵的特征向量和特征值来评估表面取向orientation和平滑度smoothness。 特征向量描述了分布的主要组成部分; 也就是说，一组正交矢量对应于变量的协方差的主导方向。

线状分布如下图类似，

在3D情况下，如下图所示，正态分布可以描述一个点或球体（如果方差的幅度在所有方向上相似），一条线（如果一个方向上的方差远大于另外两个方向） 或平面（如果一个方向的方差远小于另外两个方向）。

不同形状的3D正态分布，取决于$\mathbf{\Sigma }$的特征值之间的关系。箭头方向是分布的特征向量方向，长度由特征向量对应的特征值决定。

总结： 通过上面的内容解释了

• 如何使用PDF刻画一个cell内点的分布情况
• 点的分布在不同维度（一维二维三维）下，协方差矩阵的物理含义

6.2 NDT scan registration

6.2.1 优化目标函数构建

使用NDT进行扫描配准时，目标是找到当前帧的pose，使得可以最大化当前帧位于参考帧上的可能性，也就是说需要估计一个当前帧相对参考帧的$R,t$，这里使用$\vec{p}$来表示这个变换。
current scan point cloud 表示如下，
X = { x ⃗ 1 , … , x ⃗ n } \mathcal{X}=\left\{\vec{x}_{1}, \ldots, \vec{x}_{n}\right\} X={
x
1​,…,x
n​}
假设有一个空间变换函数，$T(\vec{p},\vec{x})$将空间中的点$\vec{x}$通过$\vec{p}$变换到另外一个位置。

给定扫描点的一些PDF,$p(\vec{x})$（多变量的正态分布表示），最佳变换$\vec{p}$应该是最大化下面似然函数时候的$\vec{p}$
$\Psi ={\prod }_{k=1}^{n}p\left(T\left(\vec{p},{\vec{x}}_k\right)\right)$
等价于最小化下面的式子,
$-\log \Psi =-{\sum }_{k=1}^n\log \left(p\left(T\left(\vec{p},{\vec{x}}_k\right)\right)\right)$

这里解释一下为什么是最大化这个似然函数？

• $T(\vec{p},\vec{x})$是经过source pointcloud,记为$p_{source}$经$\vec{p}$变换到target pointcloud后的点，记为$p_{target}$
• $p(x)$表示的是描述$p_{target}$中一个点在所在cell中的PDF
• 那么$p(T(\vec{p},{\vec{x}}_k))$则表示$p_{source}$经过经$\vec{p}$变换到$p_{target}$坐标系下后在target pointcloud分布下的PDF
• 我们的变换$\vec{p}$越接近真实值，那么$p(T(\vec{p},{\vec{x}}_k))$就越贴近$p_{target}$中点的分布，如果所有的点都一一对应上了，相当于所有点的变换这件事情同时发生，所以用连乘机的形式表示，这其实就是最大似然函数的直观意义

举一个二维的例子如下图，

6.2.2 对目标函数进行数学形式的简化

PDF不一定限于正态分布。 任何可以在局部捕获表面点结构并且对异常值稳健的PDF都是合适的。 正态分布的负对数似然增长在远离均值的点处无界。 因此，扫描数据中的异常值可能对结果产生很大影响。 在这项工作中（如Biber，Fleck和Straßer8的论文），使用了正态分布和均匀分布的混合，我称之为混合分布。

$\overline{p}(\vec{x})=c_1\exp \left(-\frac{(\vec{x}-\vec{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\vec{x}-\vec{\mu })}{2}\right)+c_2{p}_o$

• $p_0$ 是异常值的预期比率。 使用此函数，可以限制异常值的影响，如下图所示。
• 常数c1和c2可以通过求$\overline{p}(\vec{x})$的概率质量得到，它equals one
  within the space spanned by a cell。
  
  

（什么鬼？）详细的解释参考论文【A Probabilistic Framework for Robust andAccurate Matching of Point Clouds】总之在数学上可以用来近似处理。

上图是比较正态分布$p(\vec{x})$和混合模型$\overline{p}(\vec{x})$。
负对数似然是进行NDT registration时的目标函数，其导数表征了一次特定测量的偏差。（不太理解）
$p(x)$对于非常大的$x$的，the influence grows without bounds,但是$\overline{p}(\vec{x})$却是有界限的。

回顾一下我们的目标函数，

$-\log \Psi =-{\sum }_{k=1}^n\log \left(p\left(T\left(\vec{p},{\vec{x}}_k\right)\right)\right)$

我们要处理的目标函数中的每一个子项$-\log (p(T(\vec{p},{\vec{x}}_k)))$具有下列式子的形式（使用前面说的混合模型$\overline{p}$来描述分布），

$-\log \left(c_1\exp \left(-\left((\vec{x}-\vec{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\vec{x}-\vec{\mu })\right)/2\right)+c_2\right)$

遗憾是的这个形式没有simplest的一阶导数和二阶导数，像上面的图中展示的那样，我们可以用一个高斯函数来近似负对数函数，也就是可以用

$\tilde{p}(x)=d_1\exp \left(-d_2{x}^2/\left(2{\sigma }^2\right)\right)+d_3$

来近似

$\overline{p}(x)=-\log \left(c_1\exp \left(-x^2/\left(2{\sigma }^2\right)\right)+c_2\right)$，

通过拟合参数$d_1,d_2,d_3$使得$\tilde{p}(x)$近似$\overline{p}(x)$,for $x=0,x=\sigma ,x=\infty$, 解方程求得
$\begin{array}{rl}{d}_3& =-\log \left(c_2\right)\\ d_1& =-\log \left(c_1+c_2\right)-d_3\\ d_2& =-2\log \left(\left(-\log \left(c_1\exp (-1/2)+c_2\right)-d_3\right)/d_1\right)\end{array}$

6.2.3 协方差矩阵的逆${\mathbf{\Sigma }}^{-1}$求解

似然函数hu需要协方差矩阵的逆，${\mathbf{\Sigma }}^{-1}$。如果cell中的点完全共面或共线，则协方差矩阵是奇异的就无法求逆转。 在3D情况下，从三点或更少点计算的协方差矩阵将始终是奇异的。因此，仅为包含五个以上点的cell计算PDF。 此外，作为对数值问题的预防措施，只要发现$\mathbf{\Sigma }$ is slightly inflated whenever it is found to be nearly singular。 如果$\mathbf{\Sigma }$的最大特征值${\lambda }_3$大于${\lambda }_1$或${\lambda }_2$的100倍，则较小的特征值${\lambda }_j$被替换为${\lambda }_j={\lambda }_3/100$。
${\Lambda }^{\prime }=\left[\begin{array}{ccc}{\lambda }_1^{\prime }& 0& 0\\ 0& {\lambda }_2^{\prime }& 0\\ 0& 0& {\lambda }_3\end{array}\right]$

使用${\mathbf{\Sigma }}^{\prime }=\mathbf{V}{\mathbf{\Lambda }}^{\prime }\mathbf{V}$来代替$\mathbf{\Sigma }$，$\mathbf{V}$中包含的是$\mathbf{\Sigma }$的特征向量。

6.2.4 对目标函数进行优化求解

使用Newton法进行求解优化$s(\vec{p})$，核心是求解下面的式子$\mathbf{H}\Delta \vec{p}=-\vec{g}$,其中，

• $H$是$s(\vec{p})$的hessian matrix
• $\vec{g}$是$s(\vec{p})$的gradient vector，或者叫做$s(\vec{p})$的雅克比矩阵的转置$J^T$

定义${\vec{x}}_k^{\prime }\equiv T\left(\vec{p},{\vec{x}}_k\right)-{\vec{\mu }}_k$，换句话说，$\vec{x}{k}^{\prime} $是点$\vec{x}{k} $变换到reference cell后与reference cell的center的差值。
将目标函数写成，
$s(\vec{p})={\sum }_{k=1}^n{d}_1\exp (-\frac{d_2}{2}({\vec{x}}_k^{\prime }{)}^{\mathrm{T}}{\Sigma }_k^{-1}{\vec{x}}_k^{\prime })$

The entries $g_i$ of the gradient vector $\vec{g}$ can be written,
$g_i=\frac{\delta s}{\delta p_i}={\sum }_{k=1}^n{d}_1{d}_2{\vec{x}}_k^{\prime \mathrm{T}}{\mathbf{\Sigma }}_k^{-1}\frac{\delta {\vec{x}}_k^{\prime }}{\delta p_i}\exp \left(\frac{-d_2}{2}{\vec{x}}_k^{\prime \mathrm{T}}{\mathbf{\Sigma }}_k^{-1}{\vec{x}}_k^{\prime }\right)$

其中$p_i$是$p$的第$i$的量，比如有三维（6个）和二维（3个）两种情况。

The entries $H_{ij}$ of the Hessian matrix $H$ are

6.2.5 算法流程如下

6.2.5.1 2D NDT公式细节

优化变量为， $\vec{p}={\left[t_x,t_y,\varphi \right]}^{\top }$

$T_2(\vec{p},\vec{x})=\left[\begin{array}{cc}\cos \varphi & -\sin \varphi \\ \sin \varphi & \cos \varphi \end{array}\right]\vec{x}+\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right]$

$\delta {\vec{x}}^{\prime }/\delta p_i={\mathrm{J}}_2=\left[\begin{array}{ccc}1& 0& -x_1\sin \varphi -x_2\cos \varphi \\ 0& 1& x_1\cos \varphi -x_2\sin \varphi \end{array}\right]$

$\frac{{\delta }^2{\vec{x}}^{\prime }}{\delta p_i\delta p_j}=\{\begin{array}{ll}\left[\begin{array}{c}-x_1\cos \varphi +x_2\sin \varphi \\ -x_1\sin \varphi -x_2\cos \varphi \end{array}\right]& \text{ if }i=j=3\\ \left[\begin{array}{c}0\\ 0\end{array}\right]& \text{ otherwise }\end{array}$

6.2.5.2 3D NDT公式细节

优化变量为，${\vec{p}}_6={\left[t_x,t_y,t_z,{\varphi }_x,{\varphi }_y,{\varphi }_z\right]}^{\mathrm{T}}$

$T_E\left({\vec{p}}_6,\vec{x}\right)={\mathbf{R}}_x{\mathbf{R}}_y{\mathbf{R}}_z\vec{x}+\vec{t}=\left[\begin{array}{ccc}{c}_y{c}_z& -c_y{s}_z& s_y\\ c_x{s}_z+s_x{s}_y{c}_z& c_x{c}_z-s_x{s}_y{s}_z& -s_x{c}_y\\ s_x{s}_z-c_x{s}_y{c}_z& c_x{s}_y{s}_z+s_x{c}_z& c_x{c}_y\end{array}\right]\vec{x}+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$

6.3 3D-NDT extensions

• 3D-NDT最重要的参数是 cell size
  • size too large, less precise registration
  • cell的影响区域只延伸到其边界，如果cells太小, registration will only succeed if the two scans are initially close together
  • 另一个问题是，对于较小的cell，可能根本不使用具有低点密度的部分扫描，因为计算可信的协方差矩阵至少需要5个点
  • Using smaller cells also requires more memory

因此，cell的最佳尺寸和分布取决于输入数据的形状和密度，还取决于我们对scan表示的保真度的要求。

使用方形或立方体cell的固定格子需要用户选择良好的cell尺寸。

更灵活的cell结构将是优选的：在可能的情况下使用大cell并且在单个正态分布不能令人满意地描述表面的地方进行更精细的细分。 本节介绍了许多用于创建NDT cell结构和相关似然函数的替代方法

6.3.1 Fixed discretisation

不考虑刚才提到的缺点，使用固定单元格的好处是初始化和使用单元结构的开销非常小。 每个单元只需要计算一组参数，每个单元的定位都很简单。 对于算法的性能而言，更重要的是点对单元查找可以在恒定时间内非常快速地完成，因为单元可以存储在简单的阵列中。

6.3.2 Octree discretisation

八叉树是一种常用的树形结构，可用于存储三维空间的hierarchical discretisation。在八叉树中，每个节点表示空间的有界分区。每个内部节点有八个子节点，表示其父节点的空间分区的全等和非重叠细分。创建八叉树时，根节点的大小适合包含整个参考扫描。然后递归地构建树，分割包含多于一定数量点的所有节点。所有数据点都包含在八叉树的叶节点中。八叉树与kD树非常相似，但每个内部节点有八个而不是两个子节点。

如前所述，八叉树版本的3D-NDT以fixed regular cells开始，不同之处在于每个单元格是八叉树的根节点。分布的方差大于特定阈值的单元被递归地分割，形成八叉树林。

对于NDT的效率而言，点到cell查找速度快是很重要的，这是使用forest of octrees的主要原因，而不是具有跨越所有扫描的根节点的单个八叉树。对于许多类型的扫描数据，可以指定合理的基本单元尺寸，从而仅需要分割扫描表面特别不均匀的部分中的少数单元。因此，对于大多数点，找到正确的单元只需要单个数组访问，而每个点遍历一个八叉树需要更多的时间。森林需要比单个树更多的内存，特别是如果每棵树的根节点的单元格大小很小，但是对于本文提出的实验，这种影响可以忽略不计

6.3.3 Iterative discretisation

另一种选择是使用每次运行的最终pose作为下一次运行的初始pose，以连续更精细的cell分辨率执行多次NDT运行。 第一次运行有利于将对齐的scan更紧密地结合在一起，后面的运行可以改善粗略的初始匹配。

6.3.4 Adaptive clustering

更自适应的离散化方法是使用聚类算法，该算法基于扫描点的位置将扫描点划分为多个聚类，并且为每个聚类使用一个NDT单元。一种易于实现的常见聚类算法是k-means聚类[29]，

6.3.6 Trilinear interpolation

$\hat{p}(\vec{x})={\sum }_{b=1}^8-d_{1(b)}w\left(\vec{x},{\vec{\mu }}_b\right)\exp \left(-\frac{d_{2(b)}}{2}{\left(\vec{x}-{\vec{\mu }}_b\right)}^{\mathrm{T}}{\Sigma }_b^{-1}\left(\vec{x}-{\vec{\mu }}_b\right)\right)$

将NDT应用于矿井隧道扫描的图示，具有（右）和不带（左）三线性插值。 密度较大的区域代表较大的分数值。 (暗网格图案不代表较小的分数值，但仅显示底层单元格的边界)

• trilinear interpolation耗时4倍
• cell间更平滑

PCL代码细节

代码中的公式均和论文一致，感兴趣的可以详细查看pcl源码pcl/registration/include/pcl/registration/impl/ndt.hpp文件

// template<typename PointSource, typename PointTarget> void pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) { 
    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); 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; // Update Visualizer (untested) if (update_visualizer_ != 0) update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() ); if (nr_iterations_ > max_iterations_ || (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_))) { 
    converged_ = true; } nr_iterations_++; } // Store transformation probability. The realtive 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 ()); }