【Autoware】之ndt_mapping理论公式及代码对比
Autoware之ndt_mapping详解
- Pub和Sub
- NDT算法流程
-
- 1. 将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网格或体素（Voxel）
- 2. 初始化变换参数
- 3. 对于要配准的点云（second scan），通过变换 T T T 将其转换到参考点云的网格中 x i ′ = T ( x i , p ) x^′_i=T(x_i,p) xi′=T(xi,p)
- 4. 根据正态分布参数计算每个转换点的概率密度
- 5. NDT配准得分（score）通过对每个网格计算出的概率密度相加得到
- 6. 根据牛顿优化算法对目标函数 − s c o r e −score −score 进行优化
- 7. 跳转到第3步继续执行，直到达到收敛条件为止
- 实际ndt_mapping中的参数
-
- Resolution
- 无约束优化中的牛顿法
-
- StepSize
- TransformationEpsilon
- MaximumIterations
- Leaf Size
- Min&Max Scan Range
- Minimum Add Scan Shift
- 室内室外参数及建图效果
前言：这里是张聪明&茗凯在移植使用Autoware建图的算法时，学习ndt_mapping路程
一开始在notion上写的 所以后续如果有些许修改请点击此处跳转notion的外链 如果有转载请直接注明出处
详情代码可见下面的分析，结合论文看效果更佳其中参考引用：
- NDT（Normal Distributions Transform）算法原理与公式推导
- autoware.ai/core_perception/lidar_localizer/ndt_mapping
- 2009年Martin Magnusson 的博士论文[Magnusson 2009]
更新：
20220919：为重构的录了一个教程，非常简单 感觉应该能随时部署在自己的机器人上，https://www.bilibili.com/video/BV18e4y1k7cA
20220916：重构了一下slam，更为轻便型，完全从autoware处剥离开了，无需其依赖；不过还有挺多todo待做，现在仅使用LiDAR 就能有odom信息，详情见：https://github.com/Kin-Zhang/simple_ndt_slam 同时gitee也同步更新
20220420：添加单独抽取出来mapping代码到repo，更轻量化 详情见repo：https://gitee.com/kin_zhang/mapping_ws
NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。
因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好
Pub和Sub
map.header.frame_id = "map";
ndt_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/ndt_map", 1000);
current_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);
ros::Subscriber param_sub = nh.subscribe("config/ndt_mapping", 10, param_callback);
ros::Subscriber output_sub = nh.subscribe("config/ndt_mapping_output", 10, output_callback);
ros::Subscriber points_sub = nh.subscribe("points_raw", 100000, points_callback);
ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", 100000, odom_callback);
ros::Subscriber imu_sub = nh.subscribe(_imu_topic, 100000, imu_callback);
可以注意到这里的三个消息的输入，points_raw /vehicle/odom _imu_topic 在实际使用中，可以只有points_raw，也就是说只有激光雷达的数据，注意如果要收odom数据记得提前检查odom数据是否正确，另外是/vehicle/odom的frame_id
NDT算法流程
NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好
整体步骤为：
if (_method_type == MethodType::PCL_GENERIC)
{
ndt.align(*output_cloud, init_guess); // 初始变换参数
fitness_score = ndt.getFitnessScore();//
t_localizer = ndt.getFinalTransformation();
has_converged = ndt.hasConverged();
final_num_iteration = ndt.getFinalNumIteration();
transformation_probability = ndt.getTransformationProbability();
}
算法的基本步骤如下：【为了和参考的论文公式统一查找，请详细看参考论文的第六章公式】
1. 将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网格或体素（Voxel）
并计算每个网格的多维正态分布参数：公式6.2&6.3
- 均值 μ ⃗ = 1 m ∑ k = 1 m y ⃗ k \vec{\mu} =\frac{1}{m} \sum_{k=1}^{m} \vec{y}_{k} μ=m1∑k=1myk
- 协方差矩阵 Σ = 1 m − 1 ∑ k = 1 m ( y ⃗ k − μ ⃗ ) ( y ⃗ k − μ ⃗ ) T \boldsymbol{\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}} Σ=m−11∑k=1m(yk−μ)(yk−μ)T
其中 y ⃗ k = 1 , … , m \vec{y}_{k}=1,\dots,m yk=1,…,m参考点云在网格内的位置
//Input are supposed to be in device memory
template <typename PointSourceType>
void VoxelGrid<PointSourceType>::setInput(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud)
{
if (input_cloud->points.size() > 0) {
/* If no voxel grid was created, then
* build the initial voxel grid and octree
*/
source_cloud_ = input_cloud;
findBoundaries();
std::vector<Eigen::Vector3i> voxel_ids(input_cloud->points.size());
for (int i = 0; i < input_cloud->points.size(); i++) {
Eigen::Vector3i &vid = voxel_ids[i];
PointSourceType p = input_cloud->points[i];
vid(0) = static_cast<int>(floor(p.x / voxel_x_));
vid(1) = static_cast<int>(floor(p.y / voxel_y_));
vid(2) = static_cast<int>(floor(p.z / voxel_z_));
}
octree_.setInput(voxel_ids, input_cloud);
voxel_ids.clear();
initialize();
scatterPointsToVoxelGrid();
computeCentroidAndCovariance();
}
}
/* Put points into voxels */
template <typename PointSourceType>
void VoxelGrid<PointSourceType>::scatterPointsToVoxelGrid()
{
for (int pid = 0; pid < source_cloud_->points.size(); pid++) {
int vid = voxelId(source_cloud_->points[pid]);
PointSourceType p = source_cloud_->points[pid];
Eigen::Vector3d p3d(p.x, p.y, p.z);
if ((*points_id_)[vid].size() == 0) {
(*centroid_)[vid].setZero();
(*points_per_voxel_)[vid] = 0;
(*tmp_centroid_)[vid].setZero();
(*tmp_cov_)[vid].setIdentity();
}
(*tmp_centroid_)[vid] += p3d;
(*tmp_cov_)[vid] += p3d * p3d.transpose();
(*points_id_)[vid].push_back(pid);
(*points_per_voxel_)[vid]++;
}
}
/* Compute centroids and covariances of voxels. */
template <typename PointSourceType>
void VoxelGrid<PointSourceType>::computeCentroidAndCovariance()
{
for (int idx = real_min_bx_; idx <= real_max_bx_; idx++)
for (int idy = real_min_by_; idy <= real_max_by_; idy++)
for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
int i = voxelId(idx, idy, idz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
int ipoint_num = (*points_id_)[i].size();
double point_num = static_cast<double>(ipoint_num);
Eigen::Vector3d pt_sum = (*tmp_centroid_)[i];
if (ipoint_num > 0) {
(*centroid_)[i] = pt_sum / point_num;
}
Eigen::Matrix3d covariance;
if (ipoint_num >= min_points_per_voxel_) {
covariance = ((*tmp_cov_)[i] - 2.0 * (pt_sum * (*centroid_)[i].transpose())) / point_num + (*centroid_)[i] * (*centroid_)[i].transpose();
covariance *= (point_num - 1.0) / point_num;
SymmetricEigensolver3x3 sv(covariance);
sv.compute();
Eigen::Matrix3d evecs = sv.eigenvectors();
Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();
if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
(*points_per_voxel_)[i] = -1;
continue;
}
double min_cov_eigvalue = evals(2, 2) * 0.01;
if (evals(0, 0) < min_cov_eigvalue) {
evals(0, 0) = min_cov_eigvalue;
if (evals(1, 1) < min_cov_eigvalue) {
evals(1, 1) = min_cov_eigvalue;
}
covariance = evecs * evals * evecs.inverse();
}
(*icovariance_)[i] = covariance.inverse();
}
}
}
2. 初始化变换参数
p = ( t x , t y , ϕ ) T p=(t_x,t_y,ϕ)^T p=(tx,ty,ϕ)T（赋予零值或者使用里程计数据赋值）
3. 对于要配准的点云（second scan），通过变换 T T T 将其转换到参考点云的网格中 x i ′ = T ( x i , p ) x^′_i=T(x_i,p) xi′=T(xi,p)
这里的公式展示的是二维状态下的
T : ( x ′ y ′ ) = ( c o s ϕ − s i n ϕ s i n ϕ c o s ϕ ) ( x y ) + ( t x t y ) T:\left(\begin{array}{l} x^′\\ y^′ \end{array} \right)=\left(\begin{array}{l} cosϕ&-sinϕ\\ sinϕ&cosϕ \end{array} \right)\left(\begin{array}{l} x\\ y \end{array} \right)+\left(\begin{array}{l} t_x\\ t_y \end{array} \right) T:(x′y′)=(cosϕsinϕ−sinϕcosϕ)(xy)+(txty)
Eigen::Matrix4f init_guess =
(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol;
Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation
Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation
Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
tf_ltob = tf_btol.inverse();
最佳的变换参数 p ⃗ \vec{p} p应该是选取最大化此似然函数：
Ψ = ∏ k = 1 n p ( T ( p ⃗ , x ⃗ k ) ) \Psi=\prod_{k=1}^{n} p\left(T\left(\vec{p}, \vec{x}_{k}\right)\right) Ψ=k=1∏np(T(p,xk))
然后从阶乘到求和，对两边取log，然后两边加负号也就是取最小的negative log-likelihood：
− log Ψ = − ∑ k = 1 n log ( p ( T ( p ⃗ , x ⃗ k ) ) ) ( 6.6 ) p ˉ ( x ⃗ ) = c 1 exp ( − ( x ⃗ − μ ⃗ ) T Σ − 1 ( x ⃗ − μ ⃗ ) 2 ) + c 2 p o ( 6.7 ) -\log \Psi =-\sum_{k=1}^{n} \log \left(p\left(T\left(\vec{p}, \vec{x}_{k}\right)\right)\right)\qquad(6.6)\\\\\bar{p}(\vec{x})=c_{1} \exp \left(-\frac{(\vec{x}-\vec{\mu})^{\mathrm{T}} \Sigma^{-1}(\vec{x}-\vec{\mu})}{2}\right)+c_{2} p_{o}\qquad(6.7) −logΨ=−k=1∑nlog(p(T(p,xk)))(6.6)pˉ(x)=c1exp(−2(x−μ)TΣ−1(x−μ))+c2po(6.7)
在Biber, Fleck和StraBer的文章里使用了混合正态分布和均匀分布，其中的 p ˉ ( x ⃗ ) \bar{p}(\vec{x}) pˉ(x)用了 c 1 , c 2 c_1,c_2 c1,c2 两个参数表示，其中 p 0 p_0 p0是outliers的期望比率，参数 c 1 , c 2 c_1,c_2 c1,c2由一个网格的扩张来决定（看代码里的gauss_c1,gauss_c2 ）
d 3 = − log ( c 2 ) d 1 = − log ( c 1 + c 2 ) − d 3 d 2 = − 2 log ( ( − log ( c 1 exp ( − 1 / 2 ) + c 2 ) − d 3 ) / d 1 ) ( 6.8 ) \begin{array}{l}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}\qquad(6.8) d3=−log(c2)d1=−log(c1+c2)−d3d2=−2log((−log(c1exp(−1/2)+c2)−d3)/d1)(6.8)
template <typename PointSourceType, typename PointTargetType>
void NormalDistributionsTransform<PointSourceType, PointTargetType>::computeTransformation(const Eigen::Matrix<float, 4, 4> &guess)
{
nr_iterations_ = 0;
converged_ = fals
double gauss_c1, gauss_c2, gauss_d3;
gauss_c1 = 10 * ( 1 - outlier_ratio_);
gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
//论文中公式6.8
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_);
//论文中公式6.8
if (guess != Eigen::Matrix4f::Identity()) {
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 = 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 (source_cloud_->points.size() > 0) {
trans_probability_ = score / static_cast<double>(source_cloud_->points.size());
}
}
4. 根据正态分布参数计算每个转换点的概率密度
在下面这段代码中用到了上面给出的gauss_d1_ ,gauss_d2_ 来计算现在这一帧的扫描的NDT分数函数：
p ~ ( x ⃗ k ) = − d 1 exp ( − d 2 2 ( x ⃗ k − μ ⃗ k ) T Σ k − 1 ( x ⃗ k − μ ⃗ k ) ) ( 6.9 ) \tilde{p}\left(\vec{x}_{k}\right)=-d_{1} \exp \left(-\frac{d_{2}}{2}\left(\vec{x}_{k}-\vec{\mu}_{k}\right)^{\mathrm{T}} \Sigma_{k}^{-1}\left(\vec{x}_{k}-\vec{\mu}_{k}\right)\right) \qquad(6.9) p~(xk)=−d1exp(−2d2(xk−μk)TΣk−1(xk−μk))(6.9)
5. NDT配准得分（score）通过对每个网格计算出的概率密度相加得到
s ( p ⃗ ) = − ∑ k = 1 n p ~ ( T ( p ⃗ , x ⃗ k ) ) ( 6.10 ) s(\vec{p})=-\sum_{k=1}^{n} \tilde{p}\left(T\left(\vec{p}, \vec{x}_{k}\right)\right)\qquad(6.10) s(p)=−k=1∑np~(T(p,xk))(6.10)
template <typename PointSourceType, typename PointTargetType>
double NormalDistributionsTransform<PointSourceType, PointTargetType>::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)
{
PointSourceType x_pt, x_trans_pt;
Eigen::Vector3d x, x_trans;
Eigen::Matrix3d c_inv;
score_gradient.setZero ();
hessian.setZero ();
//Compute Angle Derivatives
computeAngleDerivatives(pose);//这里是第六步中的一系列关于雅各比矩阵和海森矩阵内的参数计算
std::vector<int> neighbor_ids;
Eigen::Matrix<double, 3, 6> point_gradient;
Eigen::Matrix<double, 18, 6> point_hessian;
double score = 0;
point_gradient.setZero();
point_gradient.block<3, 3>(0, 0).setIdentity();
point_hessian.setZero();
for (int idx = 0; idx < source_cloud_->points.size(); idx++) {
neighbor_ids.clear();
x_trans_pt = trans_cloud.points[idx];
voxel_grid_.radiusSearch(x_trans_pt, resolution_, neighbor_ids);
for (int i = 0; i < neighbor_ids.size(); i++) {
int vid = neighbor_ids[i];
x_pt = source_cloud_->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);
x_trans -= voxel_grid_.getCentroid(vid);
c_inv = voxel_grid_.getInverseCovariance(vid);
computePointDerivatives(x, point_gradient, point_hessian, compute_hessian);//对应到公式6.20的hessian矩阵计算
score += updateDerivatives(score_gradient, hessian, point_gradient, point_hessian, x_trans, c_inv, compute_hessian);//return值是公式6.10，但是里面包含了更新梯度的操作
}
}
return score;
}
6. 根据牛顿优化算法对目标函数 − s c o r e −score −score 进行优化
即寻找变换参数 p \mathbf{p} p 使得score的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵
根据链式求导法则以及向量、矩阵求导的公式， s s s 梯度方向为
g i = δ s δ p i = ∑ k = 1 n d 1 d 2 x ⃗ k ′ T Σ k − 1 δ x ⃗ k ′ δ p i exp ( − d 2 2 x ⃗ k ′ T Σ k − 1 x ⃗ k ′ ) ( 6.12 ) g_{i}=\frac{\delta s}{\delta p_{i}}=\sum_{k=1}^{n} d_{1} d_{2} \vec{x}_{k}^{\prime}{ }^{\mathrm{T}} \Sigma_{k}^{-1} \frac{\delta \vec{x}_{k}^{\prime}}{\delta p_{i}} \exp \left(\frac{-d_{2}}{2} \vec{x}_{k}^{\prime}{ }^{\mathrm{T}} \Sigma_{k}^{-1} \vec{x}_{k}^{\prime}\right)\qquad(6.12) gi=δpiδs=k=1∑nd1d2xk′TΣk−1δpiδxk′exp(2−d2xk′TΣk−1xk′)(6.12)
template <typename PointSourceType, typename PointTargetType>
double NormalDistributionsTransform<PointSourceType, PointTargetType>::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)
{
Eigen::Vector3d cov_dxd_pi;
double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);//对应公式6.9
double score_inc = -gauss_d1_ * e_x_cov_x;//对应公式6.9
e_x_cov_x = gauss_d2_ * e_x_cov_x;
if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) {
return 0.0;
}
e_x_cov_x *= gauss_d1_;
for (int i = 0; i < 6; i++) {
cov_dxd_pi = c_inv * point_gradient.col(i);
score_gradient(i) += x_trans.dot(cov_dxd_pi) * e_x_cov_x;//对应公式6.12 从右往左看简单点...
if (compute_hessian) {
for (int j = 0; j < hessian.cols(); j++) {
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;//直接return的是score 也就是上一步：5 NDT配准得分（score）通过对每个网格计算出的概率密度相加得到
}
其中 x x x对变换参数 p i p_i pi的偏导数即为变换 T T T的雅克比矩阵：
J E = [ 1 0 0 0 c f 0 1 0 a d g 0 0 1 b e h ] ( 6.15 ) \mathbf{J}_{E}=\left[\begin{array}{llllll} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{array}\right]\qquad(6.15) JE=⎣ ⎡1000100010abcdefgh⎦ ⎤(6.15)
a = x 1 ( − s x s z + c x s y c z ) + x 2 ( − s x c z − c x s y s z ) + x 3 ( − c x c y ) b = x 1 ( c x s z + s x s y c z ) + x 2 ( − s x s y s z + c x c z ) + x 3 ( − s x c y ) c = x 1 ( − s y c z ) + x 2 ( s y s z ) + x 3 ( c y ) d = x 1 ( s x c y c z ) + x 2 ( − s x c y s z ) + x 3 ( s x s y ) e = x 1 ( − c x c y c z ) + x 2 ( c x c y s z ) + x 3 ( − c x s y ) f = x 1 ( − c y s z ) + x 2 ( − c y c z ) g = x 1 ( c x c z − s x s y s z ) + x 2 ( − c x s z − s x s y c z ) h = x 1 ( s x c z + c x s y s z ) + x 2 ( c x s y c z − s x s z ) ( 6.19 ) \begin{array}{l}a=x_{1}\left(-s_{x} s_{z}+c_{x} s_{y} c_{z}\right)+x_{2}\left(-s_{x} c_{z}-c_{x} s_{y} s_{z}\right)+x_{3}\left(-c_{x} c_{y}\right) \\b=x_{1}\left(c_{x} s_{z}+s_{x} s_{y} c_{z}\right)+x_{2}\left(-s_{x} s_{y} s_{z}+c_{x} c_{z}\right)+x_{3}\left(-s_{x} c_{y}\right) \\c=x_{1}\left(-s_{y} c_{z}\right)+x_{2}\left(s_{y} s_{z}\right)+x_{3}\left(c_{y}\right) \\d=x_{1}\left(s_{x} c_{y} c_{z}\right)+x_{2}\left(-s_{x} c_{y} s_{z}\right)+x_{3}\left(s_{x} s_{y}\right) \\e=x_{1}\left(-c_{x} c_{y} c_{z}\right)+x_{2}\left(c_{x} c_{y} s_{z}\right)+x_{3}\left(-c_{x} s_{y}\right) \\f=x_{1}\left(-c_{y} s_{z}\right)+x_{2}\left(-c_{y} c_{z}\right) \\g=x_{1}\left(c_{x} c_{z}-s_{x} s_{y} s_{z}\right)+x_{2}\left(-c_{x} s_{z}-s_{x} s_{y} c_{z}\right) \\h=x_{1}\left(s_{x} c_{z}+c_{x} s_{y} s_{z}\right)+x_{2}\left(c_{x} s_{y} c_{z}-s_{x} s_{z}\right)\end{array}\qquad(6.19) a=x1(−sxsz+cxsycz)+x2(−sxcz−cxsysz)+x3(−cxcy)b=x1(cxsz+sxsycz)+x2(−sxsysz+cxcz)+x3(−sxcy)c=x1(−sycz)+x2(sysz)+x3(cy)d=x1(sxcycz)+x2(−sxcysz)+x3(sxsy)e=x1(−cxcycz)+x2(cxcysz)+x3(−cxsy)f=x1(−cysz)+x2(−cycz)g=x1(cxcz−sxsysz)+x2(−cxsz−sxsycz)h=x1(sxcz+cxsysz)+x2(cxsycz−sxsz)(6.19)
根据上面梯度的计算结果，继续求s关于变量 p i p_i pi、 p j p_j pj的二阶偏导：
H i j = δ 2 s δ p i δ p j = ∑ k = 1 n d 1 d 2 exp ( − d 2 2 x ⃗ k ′ T Σ k − 1 x ⃗ k ′ ) ( − d 2 ( x ⃗ k ′ T Σ k − 1 δ x ⃗ k ′ δ p i ) ( x ⃗ k ′ T Σ k − 1 δ x ⃗ k ′ δ p j ) + x ⃗ k ′ T Σ k − 1 δ 2 x ⃗ k ′ δ p i δ p j + δ x ⃗ k ′ δ p j T Σ k − 1 δ x ⃗ k ′ δ p i ) ( 6.13 ) H_{i j}=\frac{\delta^{2} s}{\delta p_{i} \delta p_{j}}= \sum_{k=1}^{n} d_{1} d_{2} \exp \left(\frac{-d_{2}}{2} \vec{x}_{k}^{\prime}{ }^{\mathrm{T}} \boldsymbol{\Sigma}_{k}^{-1} \vec{x}_{k}^{\prime}\right)\left(-d_{2}\left(\vec{x}_{k}^{\prime}{ }^{\mathrm{T}} \boldsymbol{\Sigma}_{k}^{-1} \frac{\delta \vec{x}_{k}^{\prime}}{\delta p_{i}}\right)\left(\vec{x}_{k}^{\prime}{ }^{\mathrm{T}} \boldsymbol{\Sigma}_{k}^{-1} \frac{\delta \vec{x}_{k}^{\prime}}{\delta p_{j}}\right)+\vec{x}_{k}^{\prime}{ }^{\mathrm{T}} \boldsymbol{\Sigma}_{k}^{-1} \frac{\delta^{2} \vec{x}_{k}^{\prime}}{\delta p_{i} \delta p_{j}}+\frac{\delta \vec{x}_{k}^{\prime}}{\delta p_{j}}{ }^{T} \boldsymbol{\Sigma}_{k}^{-1} \frac{\delta \vec{x}_{k}^{\prime}}{\delta p_{i}}\right)\qquad(6.13) Hij=δpiδpjδ2s=k=1∑nd1d2exp(2−d2xk′TΣk−1xk′)(−d2(xk′TΣk−1δpiδxk′)(xk′TΣk−1δpjδxk′)+xk′TΣk−1δpiδpjδ2xk′+δpjδxk′TΣk−1δpiδxk′)(6.13)
H E = [ H ⃗ 11 ⋯ H ⃗ 16 ⋮ ⋱ ⋮ H ⃗ 61 ⋯ H ⃗ 66 ] = [ 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → 0 → a ⃗ b ⃗ c ⃗ 0 → 0 → 0 → b ⃗ d ⃗ e ⃗ 0 → 0 → 0 → c ⃗ e ⃗ f ⃗ ] ( 6.20 ) \mathbf{H}_{E}=\left[\begin{array}{ccc}\vec{H}_{11} & \cdots & \vec{H}_{16} \\\vdots & \ddots & \vdots \\\vec{H}_{61} & \cdots & \vec{H}_{66}\end{array}\right]=\left[\begin{array}{cccccc}\overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} \\\overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} \\\overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} \\\overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \vec{a} & \vec{b} & \vec{c} \\\overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \vec{b} & \vec{d} & \vec{e} \\\overrightarrow{0} & \overrightarrow{0} & \overrightarrow{0} & \vec{c} & \vec{e} & \vec{f}\end{array}\right]\qquad(6.20) HE=⎣ ⎡H11⋮H61⋯⋱⋯H16⋮H66⎦ ⎤=⎣ ⎡000000000000000000000abc000bde000cef⎦ ⎤(6.20)
template <typename PointSourceType, typename PointTargetType>
void NormalDistributionsTransform<PointSourceType, PointTargetType>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6> &point_gradient, Eigen::Matrix<double, 18, 6> &point_hessian, bool compute_hessian)
{
point_gradient(1, 3) = x.dot(j_ang_a_);
point_gradient(2, 3) = x.dot(j_ang_b_);
point_gradient(0, 4) = x.dot(j_ang_c_);
point_gradient(1, 4) = x.dot(j_ang_d_);
point_gradient(2, 4) = x.dot(j_ang_e_);
point_gradient(0, 5) = x.dot(j_ang_f_);
point_gradient(1, 5) = x.dot(j_ang_g_);
point_gradient(2, 5) = x.dot(j_ang_h_);
if (compute_hessian) {
Eigen::Vector3d a, b, c, d, e, f;
a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_);
b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_);
c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_);
d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_);
e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_);
f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_);
point_hessian.block<3, 1>(9, 3) = a;
point_hessian.block<3, 1>(12, 3) = b;
point_hessian.block<3, 1>(15, 3) = c;
point_hessian.block<3, 1>(9, 4) = b;
point_hessian.block<3, 1>(12, 4) = d;
point_hessian.block<3, 1>(15, 4) = e;
point_hessian.block<3, 1>(9, 5) = c;
point_hessian.block<3, 1>(12, 5) = e;
point_hessian.block<3, 1>(15, 5) = f;
}
}
a ⃗ = [ 0 x 1 ( − c x s z − s x s y c z ) + x 2 ( − c x c z + s x s y s z ) + x 3 ( s x c y ) x 1 ( − s x s z + c x s y c z ) + x 2 ( − c x s y s z − s x c z ) + x 3 ( − c x c y ) ] b ⃗ = [ 0 x 1 ( c x c y c z ) + x 2 ( − c x c y s z ) + x 3 ( c x s y ) x 1 ( s x c y c z ) + x 2 ( − s x c y s z ) + x 3 ( s x s y ) ] c ⃗ = [ 0 x 1 ( − s x c z − c x s y s z ) + x 2 ( − s x s z − c x s y c z ) x 1 ( c x c z − s x s y s z ) + x 2 ( − s x s y c z − c x s z ) ] d ⃗ = [ x 1 ( − c y c z ) + x 2 ( c y s z ) + x 3 ( − s y ) x 1 ( − s x s y c z ) + x 2 ( s x s y s z ) + x 3 ( s x c y ) x 1 ( c x s y c z ) + x 2 ( − c x s y s z ) + x 3 ( − c x c y ) ] e ⃗ = [ x 1 ( s y s z ) + x 2 ( s y c z ) x 1 ( − s x c y s z ) + x 2 ( − s x c y c z ) x 1 ( c x c y s z ) + x 2 ( c x c y c z ) ] e ⃗ = [ x 1 ( − c y c z ) + x 2 ( c y s z ) x 1 ( − c x s z − s x s y c z ) + x 2 ( − c x c z + s x s y s z ) x 1 ( − s x s z + c x s y c z ) + x 2 ( − c x s y s z − s x c z ) ] ( 6.21 ) \begin{array}{ll}\vec{a} =&\left[\begin{array}{c}0 \\x_{1}\left(-c_{x} s_{z}-s_{x} s_{y} c_{z}\right)+x_{2}\left(-c_{x} c_{z}+s_{x} s_{y} s_{z}\right)+x_{3}\left(s_{x} c_{y}\right) \\x_{1}\left(-s_{x} s_{z}+c_{x} s_{y} c_{z}\right)+x_{2}\left(-c_{x} s_{y} s_{z}-s_{x} c_{z}\right)+x_{3}\left(-c_{x} c_{y}\right)\end{array}\right] \\\vec{b}= & {\left[\begin{array}{c}0 \\x_{1}\left(c_{x} c_{y} c_{z}\right)+x_{2}\left(-c_{x} c_{y} s_{z}\right)+x_{3}\left(c_{x} s_{y}\right) \\x_{1}\left(s_{x} c_{y} c_{z}\right)+x_{2}\left(-s_{x} c_{y} s_{z}\right)+x_{3}\left(s_{x} s_{y}\right)\end{array}\right]} \\\vec{c}= & {\left[\begin{array}{c}0 \\x_{1}\left(-s_{x} c_{z}-c_{x} s_{y} s_{z}\right)+x_{2}\left(-s_{x} s_{z}-c_{x} s_{y} c_{z}\right) \\x_{1}\left(c_{x} c_{z}-s_{x} s_{y} s_{z}\right)+x_{2}\left(-s_{x} s_{y} c_{z}-c_{x} s_{z}\right)\end{array}\right]} \\\vec{d}= & {\left[\begin{array}{c}x_{1}\left(-c_{y} c_{z}\right)+x_{2}\left(c_{y} s_{z}\right)+x_{3}\left(-s_{y}\right) \\x_{1}\left(-s_{x} s_{y} c_{z}\right)+x_{2}\left(s_{x} s_{y} s_{z}\right)+x_{3}\left(s_{x} c_{y}\right) \\x_{1}\left(c_{x} s_{y} c_{z}\right)+x_{2}\left(-c_{x} s_{y} s_{z}\right)+x_{3}\left(-c_{x} c_{y}\right)\end{array}\right]} \\\vec{e}= & {\left[\begin{array}{c}x_{1}\left(s_{y} s_{z}\right)+x_{2}\left(s_{y} c_{z}\right) \\x_{1}\left(-s_{x} c_{y} s_{z}\right)+x_{2}\left(-s_{x} c_{y} c_{z}\right) \\x_{1}\left(c_{x} c_{y} s_{z}\right)+x_{2}\left(c_{x} c_{y} c_{z}\right)\end{array}\right]} \\\vec{e}= & {\left[\begin{array}{c}x_{1}\left(-c_{y} c_{z}\right)+x_{2}\left(c_{y} s_{z}\right) \\x_{1}\left(-c_{x} s_{z}-s_{x} s_{y} c_{z}\right)+x_{2}\left(-c_{x} c_{z}+s_{x} s_{y} s_{z}\right) \\x_{1}\left(-s_{x} s_{z}+c_{x} s_{y} c_{z}\right)+x_{2}\left(-c_{x} s_{y} s_{z}-s_{x} c_{z}\right)\end{array}\right]}\end{array}\qquad(6.21) a=b=c=d=e=e=⎣ ⎡0x1(−cxsz−sxsycz)+x2(−cxcz+sxsysz)+x3(sxcy)x1(−sxsz+cxsycz)+x2(−cxsysz−sxcz)+x3(−cxcy)⎦ ⎤⎣ ⎡0x1(cxcycz)+x2(−cxcysz)+x3(cxsy)x1(sxcycz)+x2(−sxcysz)+x3(sxsy)⎦ ⎤⎣ ⎡0x1(−sxcz−cxsysz)+x2(−sxsz−cxsycz)x1(cxcz−sxsysz)+x2(−sxsycz−cxsz)⎦ ⎤⎣ ⎡x1(−cycz)+x2(cysz)+x3(−sy)x1(−sxsycz)+x2(sxsysz)+x3(sxcy)x1(cxsycz)+x2(−cxsysz)+x3(−cxcy)⎦ ⎤⎣ ⎡x1(sysz)+x2(sycz)x1(−sxcysz)+x2(−sxcycz)x1(cxcysz)+x2(cxcycz)⎦ ⎤⎣ ⎡x1(−cycz)+x2(cysz)x1(−cxsz−sxsycz)+x2(−cxcz+sxsysz)x1(−sxsz+cxsycz)+x2(−cxsysz−sxcz)⎦ ⎤(6.21)
template <typename PointSourceType, typename PointTargetType>
void NormalDistributionsTransform<PointSourceType, PointTargetType>::computeAngleDerivatives(Eigen::Matrix<double, 6, 1> pose, bool compute_hessian)
{
double cx, cy, cz, sx, sy, sz;
if (fabs(pose(3)) < 10e-5) {
cx = 1.0;
sx = 0.0;
} else {
cx = cos(pose(3));
sx = sin(pose(3));
}
if (fabs(pose(4)) < 10e-5) {
cy = 1.0;
sy = 0.0;
} else {
cy = cos(pose(4));
sy = sin(pose(4));
}
if (fabs(pose(5)) < 10e-5) {
cz = 1.0;
sz = 0.0;
} else {
cz = cos(pose(5));
sz = sin(pose(5));
}
//-------------------------------这部分是关于公式6.19
j_ang_a_(0) = -sx * sz + cx * sy * cz;
j_ang_a_(1) = -sx * cz - cx * sy * sz;
j_ang_a_(2) = -cx * cy;
j_ang_b_(0) = cx * sz + sx * sy * cz;
j_ang_b_(1) = cx * cz - sx * sy * sz;
j_ang_b_(2) = -sx * cy;
j_ang_c_(0) = -sy * cz;
j_ang_c_(1) = sy * sz;
j_ang_c_(2) = cy;
j_ang_d_(0) = sx * cy * cz;
j_ang_d_(1) = -sx * cy * sz;
j_ang_d_(2) = sx * sy;
j_ang_e_(0) = -cx * cy * cz;
j_ang_e_(1) = cx * cy * sz;
j_ang_e_(2) = -cx * sy;
j_ang_f_(0) = -cy * sz;
j_ang_f_(1) = -cy * cz;
j_ang_f_(2) = 0;
j_ang_g_(0) = cx * cz - sx * sy * sz;
j_ang_g_(1) = -cx * sz - sx * sy * cz;
j_ang_g_(2) = 0;
j_ang_h_(0) = sx * cz + cx * sy * sz;
j_ang_h_(1) = cx * sy * cz - sx * sz;
j_ang_h_(2) = 0;
//-------------------------------这部分是关于公式6.19
if (compute_hessian) {
h_ang_a2_(0) = -cx * sz - sx * sy * cz;
h_ang_a2_(1) = -cx * cz + sx * sy * sz;
h_ang_a2_(2) = sx * cy;
h_ang_a3_(0) = -sx * sz + cx * sy * cz;
h_ang_a3_(1) = -cx * sy * sz - sx * cz;
h_ang_a3_(2) = -cx * cy;
h_ang_b2_(0) = cx * cy * cz;
h_ang_b2_(1) = -cx * cy * sz;
h_ang_b2_(2) = cx * sy;
h_ang_b3_(0) = sx * cy * cz;
h_ang_b3_(1) = -sx * cy * sz;
h_ang_b3_(2) = sx * sy;
h_ang_c2_(0) = -sx * cz - cx * sy * sz;
h_ang_c2_(1) = sx * sz - cx * sy * cz;
h_ang_c2_(2) = 0;
h_ang_c3_(0) = cx * cz - sx * sy * sz;
h_ang_c3_(1) = -sx * sy * cz - cx * sz;
h_ang_c3_(2) = 0;
h_ang_d1_(0) = -cy * cz;
h_ang_d1_(1) = cy * sz;
h_ang_d1_(2) = sy;
h_ang_d2_(0) = -sx * sy * cz;
h_ang_d2_(1) = sx * sy * sz;
h_ang_d2_(2) = sx * cy;
h_ang_d3_(0) = cx * sy * cz;
h_ang_d3_(1) = -cx * sy * sz;
h_ang_d3_(2) = -cx * cy;
h_ang_e1_(0) = sy * sz;
h_ang_e1_(1) = sy * cz;
h_ang_e1_(2) = 0;
h_ang_e2_(0) = -sx * cy * sz;
h_ang_e2_(1) = -sx * cy * cz;
h_ang_e2_(2) = 0;
h_ang_e3_(0) = cx * cy * sz;
h_ang_e3_(1) = cx * cy * cz;
h_ang_e3_(2) = 0;
h_ang_f1_(0) = -cy * cz;
h_ang_f1_(1) = cy * sz;
h_ang_f1_(2) = 0;
h_ang_f2_(0) = -cx * sz - sx * sy * cz;
h_ang_f2_(1) = -cx * cz + sx * sy * sz;
h_ang_f2_(2) = 0;
h_ang_f3_(0) = -sx * sz + cx * sy * cz;
h_ang_f3_(1) = -cx * sy * sz - sx * cz;
h_ang_f3_(2) = 0;
}
}
根据变换方程，向量 x ⃗ \mathbf{\vec{x}} x对变换参数p的二阶导数的向量为：【二维状态下】
δ 2 x ⃗ ′ δ p i δ p j = { [ − x 1 cos ϕ + x 2 cos ϕ − x 1 sin ϕ − x 2 cos ϕ ] if i = j = 3 [ 0 0 ] otherwise ( 6.16 ) \frac{\delta^{2} \vec{x}^{\prime}}{\delta p_{i} \delta p_{j}}=\left\{\begin{array}{cl}{\left[\begin{array}{l}-x_{1} \cos \phi+x_{2} \cos \phi \\-x_{1} \sin \phi-x_{2} \cos \phi\end{array}\right]} & \text { if } i=j=3 \\{\left[\begin{array}{l}0 \\0\end{array}\right]} & \text { otherwise }\end{array}\right.\qquad(6.16) δpiδpjδ2x′=⎩ ⎨ ⎧[−x1cosϕ+x2cosϕ−x1sinϕ−x2cosϕ][00] if i=j=3 otherwise (6.16)
7. 跳转到第3步继续执行，直到达到收敛条件为止
在以上的计算中，我们将默认以下三点从而简化其中的计算：
sin ϕ ≈ ϕ cos ϕ ≈ 1 ϕ 2 ≈ 0 \begin{aligned}\sin \phi & \approx \phi \\\cos \phi & \approx 1 \\\phi^{2} & \approx 0\end{aligned} sinϕcosϕϕ2≈ϕ≈1≈0
实际ndt_mapping中的参数
Resolution
/** \brief Set/change the voxel grid resolution.
- \param[in] resolution side length of voxels
*/
设置每一格代表多少米，原参数中一格代表1m，类似于分辨率
voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
// Build the voxel grid
if (input->points.size() > 0) {
voxel_grid_.setLeafSize(resolution_, resolution_, resolution_);
voxel_grid_.setInput(input);
}
无约束优化中的牛顿法
牛顿法是梯度法的进一步发展，梯度法在确定搜索方向时只考虑目标函数在选择迭代点的局部性质，即利用一阶偏导数的信息，而牛顿法进一步利用了目标函数的二阶偏导数，考虑了梯度的变化趋势，因而可更全面地确定合适的搜索方向，以便更快地搜索到极小点。
这一步也就是ndt算法步骤中的第六步 计算一些系列的偏导矩阵(Hessian, Jacobian)
牛顿法主要缺点是每次迭代过程都要计算函数二阶导数矩阵（Hessian矩阵），并且要对该矩阵求逆。这样工作量相当大，特别是矩阵求逆计算，当设计变量维数较高时工作量更大。因此，牛顿法很少被直接采用，但是对于那些容易计算一阶导数和海塞矩阵及其逆的二次函数采用牛顿法还是很方便的
StepSize
/** \brief Set/change the newton line search maximum step length.
- \param[in] step_size maximum step length
*/
Newton线步长，用于以下这个地方
delta_p_norm = computeStepLengthMT(p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, trans_cloud_)
对于这个computeStepLengthMT 详细的介绍是
/** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
* \note Search Algorithm [More, Thuente 1994]
* \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
* \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
* \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
* \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994)
* \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994)
* \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009]
* \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
* \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009]
* \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
* \return final step length
*/
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,
PointCloudSource &trans_cloud);
TransformationEpsilon
/** \brief Set the transformation epsilon (maximum allowable difference between two consecutive
* transformations) in order for an optimization to be considered as having converged to the final
* solution.
* \param[in] epsilon the transformation epsilon in order for an optimization to be considered as having
* converged to the final solution.
*/
MaximumIterations
/** \brief Set the maximum number of iterations the internal optimization should run for.
* \param[in] nr_iterations the maximum number of iterations the internal optimization should run for
*/
所以其实这三个参数不改应该也可以的吧… 是进行无约束的优化
Leaf Size
filter下的网格体素大小
// Apply voxelgrid filter
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
voxel_grid_filter.setInputCloud(scan_ptr);
voxel_grid_filter.filter(*filtered_scan_ptr);
Min&Max Scan Range
最大最小扫描范围(用来过滤距离车辆较近以及较远的点云数据) 这个也是主要要调整的参数，5-200m距离之间的点云才算数
p与车距离在范围内
r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
if (min_scan_range < r && r < max_scan_range)
Minimum Add Scan Shift
两帧点云的偏移在哪个范围内被接受，且更新地图操作
// Calculate the shift between added_pos and current_pos
double shift = sqrt(pow(current_pose.x - added_pose.x, 2.0) + pow(current_pose.y - added_pose.y, 2.0));
if (shift >= min_add_scan_shift)
{
submap_size += shift;
map += *transformed_scan_ptr;
submap += *transformed_scan_ptr;
added_pose.x = current_pose.x;
added_pose.y = current_pose.y;
added_pose.z = current_pose.z;
added_pose.roll = current_pose.roll;
added_pose.pitch = current_pose.pitch;
added_pose.yaw = current_pose.yaw;
isMapUpdate = true;
}
室内室外参数及建图效果
第一幅照片是原始默认的参数，也就是autoware的Bag包在日本道路上跑着的时候建的图的参数；第二幅是在A420 一个平方米小于20平方米的空间内的建图参数；两个建图均在都是仅有激光雷达数据的情况下，point_raw
Attention! v1.14中有点BAG详情见: ndt_mapping 无法建图 [GO TO Autoware: ndt_mapping doesn’t compute transformation matrix] 修改代码并编译后就OK了
最后的Filter Resolution是指要Filter到多少个点比如0.5 滤掉一半，0.2滤掉20%保留80%
最后保存的时候，对于大马路上0.2挺好，但是室内本来点就不多，所以0.01 或者完全不去都可以的
这张是：Autowrae在大马路上的默认建图参数
这张是：Autoware官方包（也就是大马路上）的建图效果
这张是：自己建图时的参数，室内平方米小于20
