ndt_solver
这是算法最核心的部分，详细描述了NDT算法的实现原理。
NDT算法的原文请参考Magnusson的博士论文：
Magnusson M. The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection[D]. Örebro universitet, 2009.
重点在第6章；
下面是整理的算法大纲；
参考：
https://blog.csdn.net/adamshan/article/details/79230612
https://blog.csdn.net/u013794793/article/details/89306901
文章目录
算法原理分析
1,对于1维正态分布，表达式如下：
其中的 μ 为正态分布的均值， σ2 为方差，这是对于维度 D=1 的情况而言的。
2，对于多维的情况：
其中 ,D表示维度，x⃗ 就表示均值向量，而 Σ 表示协方差矩阵,我们知道，协方差矩阵对角元素表示的是对应的元素的方差，非对角元素则表示对应的两个元素（行与列）的相关性。
二维正太分布如下图所示：
3、方差特性
通过方差来描述点云的特性：
在3D 场景中，1. 如果方差比较近似，局部点云形状为 点/球；2. 如果一个方差远大于另外两个方差，局部点云形状为 线；如果一个方差远小于另外两个方差，局部点云形状为 平面。如图 6.4。
4，对目标函数进行数学形式的简化
当使用NDT配准时，目标是找到当前扫描的姿态，使当前扫描的点位于参考扫描（3D地图）表面上的可能性最大化。那么我们需要优化的参数就是对当前扫描的点云的变换（旋转，平移等），我们使用一个变换参数 p⃗ 来描述。当前扫描为一个点云 X={x⃗ 1,…,x⃗ n} ,给定扫描点集合 X 和变换参数 p⃗ ，令空间转换函数 T(p⃗ ,x⃗ k) 表示使用使用姿态变换 p⃗ 来移动点 x⃗ k ,结合之前的一组状态密度函数（每个网格都有一个PDF），那么最好的变换参数 p⃗ 应该是最大化似然函数的姿态变换：
其中，c1，c2 由p(x)和为1来求出。（ 代码中直接令c1=10 * (1 - 0.55) ，c2=0.55 / pow(1, 3),完全看不懂了;）
这个形式没有simplest的一阶导数和二阶导数，我们可以用一个高斯函数来近似负对数函数，即：
在 x = 0, x = σ, 和 x = ∞时，令倆式相等，求出：
d3为常数项，可以省略：
5 ，求协方差矩阵的逆
6，对目标函数进行优化求解
高斯牛顿法：
其中J(x)为p(x)关于x的导数；
左边等于H，右边等于g;
高斯牛顿法的步骤如下：
1，给定初始值x0;
2, 对于第k次迭代，求出当前的雅可比矩阵J(x)和误差f(Xk);
3,求解增量方程：
4,若 xk足够小，则停止。否则，令x(k+1)=x(k)+d(xk),返回第2部。
所以最终解得：
代码
部分参数介绍
input_:激光点云；
一、Align()
Align函数为NDT算法实现函数入口。
void NormalDistributionsTransform<PointSource, PointTarget>::Align(
PointCloudSourcePtr output, const Eigen::Matrix4f &guess) {
// Resize the output dataset
if (output->points.size() != input_->size())
output->points.resize(input_->size());
// Copy the header
output->header = input_->header;
//判断是否为有序点云
if (input_->width == 1 || input_->height == 1) {
output->width = static_cast<uint32_t>(input_->size());
output->height = 1;
} else {
output->width = static_cast<uint32_t>(input_->width);
output->height = input_->height;
}
//是否有ini/inf等无限值，true 表示没有无限值，点云中的所有数据都是有限的
output->is_dense = input_->is_dense;
// Copy the point data to output
for (size_t i = 0; i < input_->size(); ++i) {
output->points[i] = input_->points[i];
}
// Perform the actual transformation computation
converged_ = false;
final_transformation_ = Eigen::Matrix4f::Identity();
transformation_ = Eigen::Matrix4f::Identity();
previous_transformation_ = Eigen::Matrix4f::Identity();
// Right before we estimate the transformation, we set all the point.data[3]
// values to 1 to aid the rigid transformation
for (size_t i = 0; i < input_->size(); ++i) {
output->points[i].data[3] = 1.0;
}
ComputeTransformation(output, guess);
}
二、ComputeTransformation（）
对输入的激光点云进行了旋转，转换到地图坐标系的角度，这样可以避免大量转化地图的点云，大大减少运算量；
计算点云变换矩阵；
根据正太分布法建立目标函数，并求出目标函数；
使用牛顿法对目标函数进行迭代求出最小值；
template <typename PointSource, typename PointTarget>
void NormalDistributionsTransform<PointSource, PointTarget>::
ComputeTransformation(PointCloudSourcePtr output,
const Eigen::Matrix4f &guess) {
apollo::common::time::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_);//搞不懂c1,c2为什么可以这么求
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();//g,H 参数初始化
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::time::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);// dx= H/-g
// 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());
}
1.ComputeDerivatives（）
2D 和 3D 场景的 g,H 的区别在于 x ⃗ 关于参数 p ⃗
的导数。论文中6.2.1, 6.2.2 分别介绍了 2D 和 3D 场景下的导数计算。
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]
//求出J 和 H 的参数的值（a,b,c,d,e,f）
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]
//每个点的J和H 值
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);
}
2.ComputeStepLengthMt（）
通过得到的方程，最终变成一个最小二乘问题，
利用高斯牛顿法求最小值；
其中牵涉到了梯度下降方向，步长等问题；
后面的【Thuente 1994】找到论文了，但是没看太懂。
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) {
// Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
double phi_0 = -(*score);
// Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
double d_phi_0 = -(score_gradient->dot(*step_dir));
Eigen::Matrix<double, 6, 1> x_t;
if (d_phi_0 >= 0) {
// Not a decent direction
if (d_phi_0 == 0) {
return 0;
} else {
// Reverse step direction and calculate optimal step.
d_phi_0 *= -1;
(*step_dir) *= -1;
}
}
// The Search Algorithm for T(mu) [More, Thuente 1994]
int max_step_iterations = 10;
int step_iterations = 0;
// Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
double mu = 1.e-4;
// Curvature condition constant, Equation 1.2 [More, Thuete 1994]
double nu = 0.9;
// Initial endpoints of Interval I,
double a_l = 0, a_u = 0;
// Auxiliary function psi is used until I is determined ot be a closed
// interval, Equation 2.1 [More, Thuente 1994]
double f_l = AuxilaryFunctionPsimt(a_l, phi_0, phi_0, d_phi_0, mu);
double g_l = AuxilaryFunctionDpsimt(d_phi_0, d_phi_0, mu);
double f_u = AuxilaryFunctionPsimt(a_u, phi_0, phi_0, d_phi_0, mu);
double g_u = AuxilaryFunctionDpsimt(d_phi_0, d_phi_0, mu);
// Check used to allow More-Thuente step length calculation to be skipped by
// making step_min == step_max
bool interval_converged = (step_max - step_min) > 0, open_interval = true;
double a_t = step_init;
a_t = std::min(a_t, step_max);
a_t = std::max(a_t, step_min);
x_t = x + (*step_dir) * a_t;
final_transformation_ =
(Eigen::Translation<float, 3>(static_cast<float>(x_t(0)),
static_cast<float>(x_t(1)),
static_cast<float>(x_t(2))) *
Eigen::AngleAxis<float>(static_cast<float>(x_t(3)),
Eigen::Vector3f::UnitX()) *
Eigen::AngleAxis<float>(static_cast<float>(x_t(4)),
Eigen::Vector3f::UnitY()) *
Eigen::AngleAxis<float>(static_cast<float>(x_t(5)),
Eigen::Vector3f::UnitZ()))
.matrix();
// New transformed point cloud
transformPointCloud(*input_, *trans_cloud, final_transformation_);
// Updates score, gradient and hessian. Hessian calculation is unnecessary
// but testing showed that most step calculations use the initial step
// suggestion and recalculation the reusable portions of the hessian would
// intail more computation time.
(*score) =
ComputeDerivatives(score_gradient, hessian, trans_cloud, &x_t, true);
// Calculate phi(alpha_t)
double phi_t = -(*score); //
// Calculate phi'(alpha_t)
double d_phi_t = -(score_gradient->dot(*step_dir));
// Calculate psi(alpha_t)
double psi_t = AuxilaryFunctionPsimt(a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t)
double d_psi_t = AuxilaryFunctionDpsimt(d_phi_t, d_phi_0, mu);
// Iterate until max number of iterations, interval convergance or a value
// satisfies the sufficient decrease, Equation 1.1, and curvature condition,
// Equation 1.2 [More, Thuente 1994]
while (!interval_converged && step_iterations < max_step_iterations &&
!(psi_t <= 0 /*Sufficient Decrease*/ &&
d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) {
// Use auxiliary function if interval I is not closed
if (open_interval) {
a_t = TrialValueSelectionMt(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t,
d_psi_t);
} else {
a_t = TrialValueSelectionMt(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t,
d_phi_t);
}
a_t = std::min(a_t, step_max);
a_t = std::max(a_t, step_min);
x_t = x + (*step_dir) * a_t;
final_transformation_ =
(Eigen::Translation<float, 3>(static_cast<float>(x_t(0)),
static_cast<float>(x_t(1)),
static_cast<float>(x_t(2))) *
Eigen::AngleAxis<float>(static_cast<float>(x_t(3)),
Eigen::Vector3f::UnitX()) *
Eigen::AngleAxis<float>(static_cast<float>(x_t(4)),
Eigen::Vector3f::UnitY()) *
Eigen::AngleAxis<float>(static_cast<float>(x_t(5)),
Eigen::Vector3f::UnitZ()))
.matrix();
// New transformed point cloud
// Done on final cloud to prevent wasted computation
transformPointCloud(*input_, *trans_cloud, final_transformation_);
// Updates score, gradient. Values stored to prevent wasted computation.
*score =
ComputeDerivatives(score_gradient, hessian, trans_cloud, &x_t, false);
// Calculate phi(alpha_t+)
phi_t = -(*score);
// Calculate phi'(alpha_t+)
d_phi_t = -(score_gradient->dot(*step_dir));
// Calculate psi(alpha_t+)
psi_t = AuxilaryFunctionPsimt(a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t+)
d_psi_t = AuxilaryFunctionDpsimt(d_phi_t, d_phi_0, mu);
// Check if I is now a closed interval
if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
open_interval = false;
// Converts f_l and g_l from psi to phi
f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
g_l = g_l + mu * d_phi_0;
// Converts f_u and g_u from psi to phi
f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
g_u = g_u + mu * d_phi_0;
}
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;
}
3、 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; //p(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;
}
总结
研究的越是深入，发现不懂的东西越多，后面有时间再对牛顿法及梯度下降算法进行深入的学习。
后面有时间再解析下ndt_map 相关内容。
版权申明：转载请注明出处，严禁用于商业用途。