障碍约束定义的误差函数表示机器人到障碍的最小距离。 $ \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.

时间约束

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

kinematics 约束

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

初始配置$x_i$和方向$d_i,d_{i+1}$之间的角度$\theta _i$ 必须等于配置$x_{i+1}$和方向$d_i,d_{i+1}$之间的夹角，即$\theta _i = \theta_{i+1}$

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

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

另一个是 positive-drive-direction约束

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

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

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

$\frac{error}{cost}$ 向量的维度是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;

在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);
}

雅格比矩阵的推导可以参照速度约束的，不同的地方是这次把角度单独拿出来了，这样有了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

代码中的 double aux0 = 2/sum_time_inv; 错了 应当是 double aux0 = 2/sum_time;

其他项以此类推

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>

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);// 不优化
}

参考一点儿也不萌的萌萌的文章

代价函数使用的是欧拉角的方式进行求导解算的。一方面由于未采用矩阵的形式进行推导，导致整个推导过程非常复杂. 在LEGO-LOAM、LIO-SAM中还能看到欧拉角，另外还有旋转矩阵等表示方法。

FLOAM使用SE(3)推导雅格比。

点到直线的误差函数 $d_e =||(Rp_s+t-p_t) \times \overrightarrow {n_e}\ ||_2$

$d_e$ 点到直线的距离

$p_t$ 目标（地图）点云中的角点

$p_s$ 源（当前帧）点云中的角点

$\overrightarrow {n_e}$ 近邻角点组成的直线对应的单位向量

这里我没太明白，LOAM中应该是两个目标点云中的角点，也就是 $\overrightarrow {PA} \times \overrightarrow {PB}$， 不明白为什么叉乘两个角点对应的单位向量。

作者讲的计算有点绕远，根据叉乘的求导公式，范数部分的求导如下

完整2范数的对R求导，直接套用范数求雅格比的公式

这样直接把上面的计算结果带入 $J_f(X)$，就得到最终结果。 关于t的导数同样计算。

点到平面误差函数就是 $d_p =||(Rp_s+t-p_t) * \overrightarrow {n_e}\ ||_2$ 没有疑问

$p_t$ 目标（地图）点云中的平面点；

$\overrightarrow {n_e}$ 近邻平面点组成的平面对应的法向量。

推导过程还是按我上面讲的更容易理解。

$\frac{(Rp_s+t-p_t) * \overrightarrow {n_e}}{||(Rp_s+t-p_t) * \overrightarrow {n_e}\ ||_2 }$ 的值是-1或者1

得到每一个误差的导数，组成一个大的雅克比矩阵(也有可能是向量)，有了雅克比矩阵之后带入高斯牛顿或者LM算法即可以求解最优的R和t。
点到线的误差项在优化过程中的贡献比较小，在r3live、fast-lio都只计算点到面的误差。

||x|| 是 1 范数

2范数其实就是：列向量 x, 2范数为 $\sqrt{x^Tx}$

2范数为 $\phi(x) =||Ax- \overrightarrow {b} ||_2$

对 x 求导

如果是求雅格比，

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

再看矩阵的范数

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

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

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

• 2 范数: 公式为 $\sqrt{\lambda_{max}(A^TA) }$， 其中 $\lambda_{max}(A^TA)$ 表示 $A^TA$的最大特征值。 2范数可以用来判断一个矩阵是不是病态矩阵

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

LOAM是一个激光里程计算法，没有闭环检测，也就没有加入图优化框架，该算法把SLAM问题分为两个算法并行运行：一个odometry算法，10Hz；另一个mapping算法，1Hz，最终将两者输出的pose做整合，实现10Hz的位姿实时输出。

LOAM源码主要由四个节点构成，分别完成特征点提取，高频低精度odom， 低频高精度odom， 双频odom融合的功能。实际上， 四个节点的执行顺序完全是串行的，很容易改成单进程的版本。

scanRegistration 流程

1. 原始点云去除无效点和很近的点，成为无序点云

2. 通过Lidar坐标系下点的仰角以及水平夹角计算点云的scanID和fireID(点在每个scan的ID为fireID)

3. 以上是每根线作为独立的子点云，将子点云合并为一帧有序点云

4. 根据曲率计算4种特征点，角点曲率大，面点曲率小。计算曲率，除了开始5个点和结束的5个点，以当前点为中心，前5个和后5个点都参与计算。然后针对每条scan，提取特征点：
   
   曲率最大的前2个点认为是sharp点，最大的前20个点认为是less_sharp点
   曲率最小的前4个点认为是flat点，将除了角点之外的所有点(包括flat)都加入 less_flat

5. 发布4种特征点与滤波后的当前帧点云

为什么选择线特征和面特征？

1. 在三维空间中或人造的建筑物中，常见的特征是线、面
2. 由于LOAM没有提取特征点，直接使用传统icp的点到点约束，会由于数据关联错误造成较大的估计误差

laserOdometry 流程

1
2
3
4
5
6
7
8

// 后面可以看到，这是点云特征匹配时的优化变量
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};

// 下面的2个分别是优化变量 para_q 和 para_t 的映射：表示的是
// world坐标系下的两个位姿之间的增量，例如 △P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond>  q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d>  t_last_curr(para_t);

1. 订阅4种特征点的消息，各自加入队列，每次取一帧进行匹配
2. 用当前帧的角点和面点，与上一帧的角点和面点建立约束。点到线以及点到面的ICP，迭代2次

2.1 sharp点： 将当前帧的sharp点 $P_{cur}$, 从当前帧的lidar坐标系转换到上一帧的Lidar坐标系，转换后的点云记做 $\hat P_{last}$, 同时将所有点补偿到起始时刻。
每个点的转换公式： $p_o = Rp_i + t$

使用KD树，寻找上一帧点云 $P_{last}$ 和 $\hat P_{last}$ 中的每个点 $\hat P_{last_\ i}$ 最相近的那个，记做 $closest_i$，最小距离记做min_square_dis。这是找到了第1个点，还要找第2个。

第2个点和第1个不在同一条scan，在上下的线上寻找。找到比min_square_dis更小，而且是最小的距离min_square_dis2，对应点的索引为 $closest_j$。必须有两个点，否则不加入ceres优化。

1
2
3
4

// 残差 = 点O到直线AB的距离
ceres::CostFunction *cost_function = LidarEdgeFactor::Create( $$ p_{cur} $$, $$ P_{closest_\ i} $$, $$ P_{closest_\ j} $$, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
// 统计约束的数量

2.2 flat点： 找平面点O最近的3个点A B C，第1个A点也是用KD树找到，一般情况下，B和A在同一条scan，C在不同的scan。必须有三个点，否则不加入ceres优化。

1
2
3
4

// 残差 = 点O到平面ABC的距离
ceres::CostFunction* cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
// 统计约束的数量

1. 对于sharp点，将当前帧Lidar坐标系下的点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到线(找到的原上一帧点云的2个点)的残差。 对于flat点，将当前帧Lidar坐标系下的点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到平面(找到的原上一帧点云的3个点组成的平面)的残差。
   

2. 经过2次点到线以及点到面的ICP点云配准之后，得到最优的位姿增量 para_q 和 para_t，更新上一帧的位姿，得到当前帧的位姿，这里说的位姿都指世界坐标系下的位姿，世界坐标系本质就是第1帧的lidar坐标系。

1. 发布当前帧在世界坐标系在的位姿，也就是 $t_{cur}$ 和 $q_{cur}$ , corner_less_sharp特征点、surf_less_flat特征点、滤波后的点云

laserMapping 流程

地图的移动机制的目的：尽量将雷达保持在特征地图中心处，以保证在做点云配准时，可以保证在雷达附近可以找到特征点云地图中的特征。

1. 接收laserOdometry发布的特征点，submap中的corner特征和surf特征在匹配中作为target；而当前帧点云中的两种特征在匹配中作为source。

2. 基本流程与laserOdometry相同，同样地也分为点到线和点到面的匹配。submap中的点云都是world坐标系，而当前帧的点云都是Lidar坐标系，所以在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到world坐标系下

3. 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点，求出这5个点的中心。判断这5个点是不是呈线状分布，如果是线状，以中心点构建两个虚拟点。和laserOdometry处理sharp点一样，加入ceres优化，这样比laserOdometry中的ICP过程更鲁棒

4. 对点到面的情况，也是找5个最近点，使用最小二乘拟合出平面(不是PCA了)，如果平面够平，把当前帧的点和平面法向量加入ceres优化，其实也是把点到平面的距离作为残差。

5. 完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，更新增量wmap_wodom，为下一次Mapping做准备

Mapping线程耗时长，容易丢帧。LOAM最终输出的实时位姿是Mapping线程计算的低频位姿和Odometry线程计算的高频位姿做了整合。

算法流程分三步：

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

• 为什么想到用正态分布表示描述点云

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

NDT 改进方向

1. 对点云进行预处理（在 3D 情况下尤其需要进行下采样）
2. 取 cell 的策略：有以下几种思路：

• 固定尺寸进行划分
• 八叉树划分
• 通过聚类后再取栅格

1. 增加鲁棒性

• 上面提到我们将空间划分成不同 cell，每个 cell 中维护一个高斯分布，那这样不可避免地在相邻栅格地交界处，分布会出现不连续性，因此一个思路可以是在每两个栅格之间通过插值计算出交界处附近的分布

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

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

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

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

目标函数 $f= minE(R,t) = min \frac{1}{N_p}∑∥Rp_i+t−\hat p_i∥$

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

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

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

最终的求 $\Delta x$ 的方程还是我们熟悉的

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();