障碍约束和雅格比矩阵的推导|路径规划TEB算法各类约束及雅格比矩阵

障碍约束定义的误差函数表示机器人到障碍的最小距离。 $ \min \textrm{penaltyBelow}( dist2point ) \cdot weight $. 

1
2
3
4
5
6
7
8
9
10
11
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
// calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) 计算机器人到障碍物的距离
// current_pose为当前机器人的位姿。
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);

if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
  _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, 
  										cfg_->optim.obstacle_cost_exponent);
}

obstacle_cost_exponent和min_obstacle_dist的设置，一般都会进入if情况。 注意 max cost (before weighting) is the same as the straight line version and that all other costs are below the straight line (for positive exponent), so it may be necessary to increase weight_obstacle and/or the inflation_weight when using larger exponents.

时间约束和kinematics约束及雅格比矩阵的推导|路径规划TEB算法各类约束及雅格比矩阵

时间约束

EdgeTimeOptimal类太简单了，误差函数 _error[0] = timediff->dt();，优化 ，那么对其求偏导，显然只有一个矩阵，一个元素 1 ： _jacobianOplusXi( 0 , 0 ) = 1;

kinematics 约束

两个误差方程，一个是 non-holonomic约束， ：

初始配置和方向之间的角度 必须等于配置和方向之间的夹角，即

根据二维叉积 A×B=|A| |B|⋅sinα 得到

所以代码中的目标函数写成

另一个是 positive-drive-direction约束

两个configure，所以两个雅格比，维度明显是 2x3，两个误差方程分别对(x, y, angle)求偏导，源码里的求导很简单，还不如速度约束的求导复杂，就不写过程了。

值得注意的是绝对值的求导结果会用sign函数表示。

加速度约束和雅格比矩阵的推导|路径规划TEB算法各类约束及雅格比矩阵

默认为5元边约束。 但是包含3个类：EdgeAcceleration, EdgeAccelerationStart, EdgeAccelerationGoal，后两个为3元边

向量的维度是2，第一个元素代表线加速度，第二个是角加速度。
EdgeAccelerationStart() and EdgeAccelerationGoal() 用于边界值

加速度约束和速度约束类似，不过变成两段圆弧和角度差、两个时间差、两个线速度和角速度。两个线速度的差除以两个时间差之和就是加速度。

1
2
3
4
5
6
7
8
9
double vel1 = dist1 / dt1->dt();
double vel2 = dist2 / dt2->dt();
const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x, cfg_->optim.penalty_epsilon);

const double omega1 = angle_diff1 / dt1->dt();
const double omega2 = angle_diff2 / dt2->dt();
const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta, cfg_->optim.penalty_epsilon);

起点的加速度约束

注意：起点加速度约束和终点加速度约束都是3元边。

对于起点的加速度约束，源码有以下变化:

1
2
3
4
5
6
7
8
9
void setInitialVelocity(const geometry_msgs::Twist& vel_start)
{
	_measurement = &vel_start;
}

ROS_ASSERT_MSG(cfg_ && _measurement, 
"You must call setTebConfig() and setInitialVelocity() on EdgeAccelerationStart()");
vel1 = _measurement->linear.x;
omega1 = _measurement->angular.z;

其他全一样，对于终点的加速度约束，则是vel2和omega2换成了_measurement的成员。那么这个_measurement从何而来？

在TebOptimalPlanner::plan里有一段:

1
2
3
4
5
6
if (start_vel)
  setVelocityStart(*start_vel);
if (free_goal_vel)
  setVelocityGoalFree();
else
  vel_goal_.first = true;

setVelocityStart给起始速度赋值：

1
2
3
4
5
6
7
8
9
// std::pair<bool, geometry_msgs::Twist>  vel_start_;
// 注意vel_start_ 和 vel_start 不同
void TebOptimalPlanner::setVelocityStart(const geometry_msgs::Twist& vel_start)
{
  vel_start_.first = true;
  vel_start_.second.linear.x = vel_start.linear.x;
  vel_start_.second.linear.y = vel_start.linear.y;
  vel_start_.second.angular.z = vel_start.angular.z;
}

然后到了AddEdgesAcceleration里添加起始加速度约束，才用到vel_start_

1
2
3
4
5
6
7
8
9
10
11
12
if (vel_start_.first)
{
  EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart;
  acceleration_edge->setVertex(0,teb_.PoseVertex(0));
  acceleration_edge->setVertex(1,teb_.PoseVertex(1));
  acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0));
  // 这里就是上面的 setInitialVelocity, _measurement 就是 vel_start_.second
  acceleration_edge->setInitialVelocity(vel_start_.second);
  acceleration_edge->setInformation(information);
  acceleration_edge->setTebConfig(*cfg_);
  optimizer_->addEdge(acceleration_edge);
}

AddEdgesAcceleration然后添加普通的加速度约束，再就是终点的加速度约束，这里就奇怪了，相应的setVelocityGoal函数没有调用的地方，这里是vel_goal_.second唯一赋值的地方。即使free_goal_vel为false，也没有赋值，意思是终点的速度为0？

雅格比矩阵的推导可以参照速度约束的，不同的地方是这次把角度单独拿出来了，这样有了8个雅格比

1
2
3
4
5
6
7
8
_jacobianOplus[0].resize(2,2); // conf1
_jacobianOplus[1].resize(2,2); // conf2
_jacobianOplus[2].resize(2,2); // conf3
_jacobianOplus[3].resize(2,1); // deltaT1
_jacobianOplus[4].resize(2,1); // deltaT2
_jacobianOplus[5].resize(2,1); // angle1
_jacobianOplus[6].resize(2,1); // angle2
_jacobianOplus[7].resize(2,1); // angle3

既然拿出了角度，那么对conf的雅格比矩阵的维度就变成了 2x2，不是2x3。两个误差函数，一个是线速度的，一个是角速度的

代码中的 double aux0 = 2/sum_time_inv; 错了 应当是 double aux0 = 2/sum_time;
J[0](0,0)的推导过程
其他项以此类推

g2o的详细使用|SLAM工具

G2O 与ceres风格不同，它是专门用于进行图优化的库，提供了大量预先定义的节点和边供用户调用。

曲线拟合的例子其实不太适合用来理解g2o，它涉及的顶点只有一个，边是一元边，有很多个。还是用图优化的例子更容易理解。

TEB算法的base_teb_edges.h, g2o_types\vertex_pose.h 和 g2o_types\vertex_timediff.h很有学习价值，以及edge_obstacle.h, edge_shortest_path.h, edge_velocity.h

顶点

最基础的BaseVertex类，template <int D, typename T> class BaseVertex，

  • D: minimal dimension of the vertex, 例如 3 for rotation in 3D. D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示
  • T: 节点估计值的类型,例如 Quaternion for rotation in 3D

g2o本身内部定义了一些常用的顶点类型

  • VertexSE2 : public BaseVertex<3, SE2> // 2D pose Vertex, (x,y,theta)
  • VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (注意不管四元数的 w)
  • VertexPointXY : public BaseVertex<2, Vector2>
  • VertexPointXYZ : public BaseVertex<3, Vector3>
  • VertexSBAPointXYZ : public BaseVertex<3, Vector3>
  • VertexSE3Expmap : public BaseVertex<6, SE3Quat>

如果我们需要的顶点类型这里面没有，就得自己定义了

重新定义顶点一般需要考虑重写如下函数：

1
2
3
4
5
# 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下，作为空函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

setToOriginImpl 和 _estimate

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。有时可以不用实现。具体来说就是初始化成员变量 _estimate， _estimate的类型就是继承 BaseVertex时的模板参数

如果_estimate是Eigen::Vector3d，那么setToOriginImpl的内容可以是: _estimate << 0,0,0;。 如果是PoseSE2， _estimate的赋值可以这样：

1
2
3
4
5
6
7
8
9
_estimate.position() = position;
_estimate.x() = x;
_estimate.y() = y;
_estimate.theta() = theta;

virtual void setToOriginImpl()
{
    _estimate.setZero();
}

oplusImpl

顶点更新函数。非常重要的一个函数，主要用于优化过程中增量 △x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

比较简单的类型是这样实现：

1
2
3
4
5
6
7
8
9
10
11
// double 也可以换成 number_t
virtual void oplusImpl( const double* update ) // 更新
{
     // update为增量△m 迭代的时候，估计值 m= m + △m, m是向量（a,b,c）
    _estimate += Eigen::Vector3d(update);  
}

virtual void oplusImpl(const double* update)
{
    _estimate.plus(update);
}

李代数的类VertexSE3Expmap，上面两个函数是

1
2
3
4
5
6
7
8
9
virtual void setToOriginImpl() {    // 重置
    _estimate = SE3Quat();
}

virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    //更新方式 类似于左扰动？
    setEstimate(SE3Quat::exp(update)*estimate());        
}

边

g2o自带的边

  • EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
  • EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>
  • EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>
仅优化一个变量的时候选择单边

以一元边为例template <int D, typename E, typename VertexXi> class BaseUnaryEdge : public BaseEdge<D,E>。

  • D 表示测量值的维度
  • E 表示测量值的数据类型
  • VertexXi，VertexXj 分别表示不同顶点的类型。（一元边只有一个顶点）

例如，BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>是个二元边。2是说测量值是2维的，也就是图像的像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap。

  • read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

  • computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差

  • linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian。

重要的成员变量：

  • _measurement：存储观测值

  • _error：存储computeError() 函数计算的误差

  • _vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int有关（0 或1）

  • setId(int)： 来定义边的编号（决定了在H矩阵中的位置）

  • setMeasurement(type) 函数来定义观测值

  • setVertex(int, vertex) 来定义顶点

  • setInformation() 来定义协方差矩阵的逆

最重要的就是computeError()，linearizeOplus()两个函数了。如果我们没有给出雅可比，g2o也会进行数值求导，但是会比重写的雅可比慢。 雅克比矩阵的维度为 误差维度 × 优化变量的维度 ，两个函数一般是这样的形式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
virtual void computeError() override
{
  // ...
    _error = _measurement - Something;
}
virtual void linearizeOplus() override
{
    _jacobianOplusXi(pos, pos) = something;
    // ...

    /*
    _jocobianOplusXj(pos, pos) = something;
    ...
    */
}

雅格比矩阵要针对每个顶点求解，上面说的维度是每个顶点求解的维度。比如边的类型继承
1
2
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>

由于是一元边，那么只针对一个顶点求解，误差维度是边的第一个模板类型2，优化变量的维度是顶点的第一个模板参数6.

TEB算法中的class EdgeVelocity : public BaseTebMultiEdge<2, double>，是三元边，两个VertexPose顶点，类型PoseSE2。顶点VertexTimeDiff，类型double。对于前者，雅格比矩阵是 2x3，对后者是 2x1，也就是如下

1
2
3
_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT

对于边class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>，这是二元边，雅格比包含两部分，维度分别为 2x3 和 2x6. 后一个顶点的类型是李群。可以这样写

1
2
3
4
5
// 相对 _jacobianOplus[2] 的另一种写法
_jacobianOplusXi(0,0) =
......
_jacobianOplusXj(0,0) = 
......

removeEdge()

removeEdge 和 removeVertex只有在ORB-SLAM中看到。

After running optimize() once, I am finding a high number of outliers. Is there any way to delete a single edge out of the graph and run optimize again, or do I have to construct it again?

The method removeEdge() removes the edge from a graph and unlinks it from all of the vertices it was attached to. After you call it on your edges, I think you need to call initializeOptimization() to reset all of g2o’s internal data structures to the new graph configuration. You should then be able to call optimize()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
// 剔除一些误差大的边
// Check inliers
int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
    g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
    g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
    if(!e12 || !e21)
        continue;

    if(e12->chi2()>th2 || e21->chi2()>th2)
    {
        size_t idx = vnIndexEdge[i];
        vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        optimizer.removeEdge(e12);
        optimizer.removeEdge(e21);
        vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
        vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
        nBad++;
    }
}

setLevel(int level)

也是在ORB-SLAM中用到

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化

setLevel(int ) is useful when you call optimizer.initializeOptimization(int ). If you assign initializeOptimization(0), the optimizer will include all edges up to level 0 in the optimization, and edges set to level >=1 will not be included

1
2
3
4
5
6
// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
if(e->chi2()>5.991 || !e->isDepthPositive())
{
    e->setLevel(1);// 不优化
}
范数及求导|数学基础

||x|| 是 1 范数

2范数其实就是：列向量 x, 2范数为

2范数为

对 x 求导

如果是求雅格比，

也就是只需求f(x)的雅格比，再转置。


再看矩阵的范数

  • 0 范数: 矩阵中非零元素的个数

  • 1 范数: 又称为 列和范数，将每列元素进行绝对值求和，取最大值

  • 范数: 又称为 列和范数，将每列元素进行绝对值求和，取最大值

  • 2 范数: 公式为 ， 其中 表示 的最大特征值。 2范数可以用来判断一个矩阵是不是病态矩阵

2范数是由向量范数诱导而来，F范数是直接定义，是两种不同的度量方式。F范数可以比较真实的矩阵和估计的矩阵值之间的误差。

NDT算法的应用|激光SLAMICP和NDT
abstract Welcome to my blog, enter password to read.
Read more
NDT算法|算法推导

推导过程

求雅格比和海森矩阵

算法流程分三步：

  1. 对环境进行 cell 的划分，为每一个 cell 构造一个高斯分布
  2. 根据初始解把当前帧的激光点转换到参考帧上，并确定属于哪个 cell
  3. 用牛顿法/高斯牛顿法进行迭代求解
  • 为什么想到用正态分布表示描述点云

几个点在一条直线上，则容易知道该协方差矩阵的秩为1；而当这几个点近似在一条直线上时，则协方差矩阵的一个特征值会明显大于另外一个。当大致均匀分布时，两个特征值会大小类似。所以说点云能够反映点云的局部形态。

NDT 改进方向

  1. 对点云进行预处理（在 3D 情况下尤其需要进行下采样）
  2. 取 cell 的策略：有以下几种思路：
  • 固定尺寸进行划分
  • 八叉树划分
  • 通过聚类后再取栅格
  1. 增加鲁棒性
  • 上面提到我们将空间划分成不同 cell，每个 cell 中维护一个高斯分布，那这样不可避免地在相邻栅格地交界处，分布会出现不连续性，因此一个思路可以是在每两个栅格之间通过插值计算出交界处附近的分布

目前的nod_omp是改进版本，使用OpenMP多线程，以及提升了搜索速度

参考：
NDT点云配准算法原理及PCL实现

高斯牛顿法的ICP|激光SLAMICP和NDT

参考这位博主的文章，工程optimized_ICP，看完这位大姐写的，再去看ALOAM，感觉容易理解了很多。ALOAM就是两种特征点的ICP匹配，比本例稍复杂，改用ceres优化(张楫是手推的雅格比)

  1. 对原始点云，给定初值T，使用pcl::transformPointCloud获得变换后的待匹配点云。
  2. 在目标点云中，nearestKSearch搜索距离待匹配点云最近的一个点，记录下此点的索引和二者之间的距离的平方resultant_distances
  3. 判断resultant_distances和参数max_correspond_distance_的大小，大于则continue
  4. 找出了待匹配点云和目标点云对应的一对点，也就是取最近点作为关联点，求它们的差值 error

目标函数

i 为通过最近邻搜索匹配上的点对， 和 分别为目标点云和待匹配点云中的点

R, t 分别为两个点云之间的旋转和平移变换。目标函数对R和t求导，文章写的是左扰动模型

但是代码里用的是右扰动模型

最终的求 的方程还是我们熟悉的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
for()
{
	// 残差对优化变量 R t 的雅可比矩阵
	Eigen::Matrix<float, 3, 6> Jacobian = Eigen::Matrix<float, 3, 6>::Zero();
	// 构建雅克比矩阵
	Jacobian.leftCols(3) = Eigen::Matrix3f::Identity();
	// 右扰动模型
	Jacobian.rightCols(3) = -T.block<3, 3>(0, 0) * Hat(origin_point_eigen);
	// 构建海森矩阵
	Hessian += Jacobian.transpose() * Jacobian;
	B += -Jacobian.transpose() * error;
}

// 可以改用QR分解：Eigen::Matrix<float, 6, 1> delta_x = Hessian.householderQr().solve(B);
Eigen::Matrix<float, 6, 1> delta_x = Hessian.inverse() * B;

T.block<3, 1>(0, 3) = T.block<3, 1>(0, 3) + delta_x.head(3);
// 由于使用的是右扰动模型求解，因此更新旋转时也应该右乘迭代量
T.block<3, 3>(0, 0) *= SO3Exp(delta_x.tail(3)).matrix();
C++的各种报错和报警|C++C++ 面向对象

还是Qt的环境好，很多时候，不用编译直接就提示有错误。

  • 报错invalid new-expression of abstract class type

有的编译器也会报错 error: allocating an object of abstract class type ‘child’。说明基类中有纯虚函数没有实现，必须在派生类实现所有纯虚函数，不使用派生类时是看不出来的。

  • marked ‘override’, but does not override
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
class Parent
{
public:
  virtual std::string getName1(int x)
  {
    return "Parent";
  }

  virtual std::string getName2(int x)
  {
    return "Parent";
  }
};

class Child : public Parent
{
public:
  virtual std::string getName1(double x) override
  {
    return "Sub";
  }
  virtual std::string getName2(int x) const override
  {
    return "Sub";
  }
};

派生类的同名函数和基类的参数类型不同，而且多了const。如果没有override，那么是派生类新加的函数，但是添加override，编译器认为基类有同名函数，但是没发现，所以报错。

如果是重写基类方法，建议使用override标识符，让编译器检查出可能的错误，从而避免无意的错误。

PCL中的NDT|PCL点云
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/console/time.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  （使用体素网格过滤器处理的效果比较好）
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <boost/thread/thread.hpp>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> PointCloud;
typedef pcl::PointCloud<PointType>::Ptr PointCloudPtr;
typedef pcl::PointCloud<PointType>::ConstPtr PointCloudConstPtr;

int main (int argc, char** argv)
{
    ros::init(argc, argv, "test_ndt");
    ros::NodeHandle nh;

    PointCloudPtr target_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/user/test_nodes/src/test_pcl/room_scan1.pcd", *target_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << target_cloud->size() << " points" << endl; 

    PointCloudPtr input_cloud(new PointCloud);
    if(pcl::io::loadPCDFile<PointType>("/home/user/test_nodes/src/test_pcl/room_scan2.pcd", *input_cloud) == -1)
    {
        ROS_ERROR("load pcd file error !");
        return -1;
    }
    cout << "load target pcl " << input_cloud->size() << " points" << endl;

    PointCloudPtr filtered_cloud(new PointCloud);
    pcl::VoxelGrid<PointType> filter;
    filter.setLeafSize(0.1, 0.1, 0.1);
    filter.setInputCloud(input_cloud);
    filter.filter(*filtered_cloud);
    cout << "after filtered, input cloud has "<< filtered_cloud->size() << " points" << endl;
    
    pcl::NormalDistributionsTransform<PointType, PointType>::Ptr ndt_ptr(new pcl::NormalDistributionsTransform<PointType, PointType>() );
    ndt_ptr->setMaximumIterations(35);
    ndt_ptr->setTransformationEpsilon(0.01);
    ndt_ptr->setStepSize(0.1);
    ndt_ptr->setResolution(0.1);
    ndt_ptr->setInputSource(filtered_cloud);
    ndt_ptr->setInputTarget(target_cloud);

    Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

    PointCloudPtr output_cloud(new PointCloud);
    ndt_ptr->align(*output_cloud, init_guess);
    cout << "output cloud points size: "<< output_cloud->size() << endl;
    cout << "ndt transform: "<< endl << ndt_ptr->getFinalTransformation() << endl;
    cout << "score: "<< ndt_ptr->getFitnessScore() << endl;
    
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );
    pcl::io::savePCDFileASCII("/home/user/test_nodes/src/test_pcl/final.pcd", *output_cloud );

    // 初始化点云可视化对象
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
    viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色

    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    // 对转换后的源点云着色 (green)可视化.
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
    output_color(output_cloud, 0, 0, 255);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");
    // 启动可视化
    viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
    viewer_final->initCameraParameters ();   //初始化摄像头参数

    // 等待直到可视化窗口关闭
    while (!viewer_final->wasStopped ())
    {
      viewer_final->spinOnce (101);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

三个迭代终止条件设置：

  1. setMaximumIterations(30): 设置最大的迭代次数。 一般来说在这个限制值之前，优化程序会在epsilon（最大容差）变换阀值下终止，添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  2. setTransformationEpsilon: 终止条件设置最小转换差异，设置前后两次迭代的转换矩阵的最大容差epsilion，在变换中Epsilon参数分别从长度和弧度，定义了变换矢量[x, y, z, roll, pitch, yaw]的最小许可的递增量，一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，默认值为：0
  3. setEuclideanFitnessEpsilon: 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max

把setTransformationEpsilon的参数从0.01改为0.001，FitnessScore 只有细微变化，而且相对之前有时变大有时变小。但是改为0.1，FitnessScore与0.01时相比，也是有大有小。迭代次数不管怎么调整，FitnessScore毫无变化，可见确实是以epsilon为优先终止条件。

  • setStepSize(0.1): More-Thuente线搜索设置最大步长，即设置牛顿法优化的最大步长。当你靠近最优解时该算法会缩短迭代步长，在最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。 调试的经验 ：step的话可以先放大后变小。

  • setResolution(1.0);: 设置网格化时立方体的边长，网格大小设置在NDT中非常重要，太大会导致精度不高，太小导致内存过高，并且只有两幅点云相差不大的情况才能匹配。它需要足够大，每个体素包含与点有关的统计数据，平均值，协方差等。每个体素至少包含6个点，但小到足以唯一地描述环境。 调试的经验 ：resolution在追求精度的情况下最好与点密度相等或者低一个数量级,这里是激光点云，所以设了1米

  • getFitnessScore()：欧几里得适合度得分 FitnessScore ，该分数计算为从输出云到目标云中最近点的距离的平方。分数越大，准确率越低，配准效果也就越差。

align函数中，output_cloud保存变换后的input_cloud，它和target_cloud其实很相近了。但是它不能作为最终的源点云变换，因为上面对input_cloud进行了滤波处理，所以又经过pcl::transformPointCloud(*input_cloud, *output_cloud, ndt_ptr->getFinalTransformation() );获得未经滤波的output_cloud

注意的地方：

  • 开始做体素滤波，栅格太大，过滤太多，最后的output_cloud和target_cloud看着不太契合
  • 只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理


参考: 参考程序