If you wish for peace, prepare for war

数字的类型 2023-04-25|C++C++ 基础

数字后面带个U，L，F的含义

U表示该常数用无符号整型方式存储，相当于 unsigned int
L表示该常数用长整型方式存储，相当于 long
F表示该常数用浮点方式存储，相当于 float

比如 -1UL, 其实是一个很大很大的数

OpennCV的数据类型

S = 有符号整型 U = 无符号整型 F = 浮点型

CV_8U - 8位无符号整数（0…255）
CV_32F - 32位浮点数（-FLT_MAX…FLT_MAX，INF，NAN）
CV_8U - 8位无符号整数（0…255）
CV_8S - 8位有符号整数（-128…127）
CV_16U - 16位无符号整数（0…65535）
CV_16S - 16位有符号整数（-32768…32767）
CV_32S - 32位有符号整数（-2147483648…2147483647）
CV_32F - 32位浮点数（-FLT_MAX…FLT_MAX，INF，NAN）
CV_64F - 64位浮点数（-DBL_MAX…DBL_MAX，INF，NAN）

trimmer机制 - 移除重复子图 2023-04-04|激光SLAMCartographer源码解读

在PoseGraph2D::HandleWorkQueue里，RunOptimization();之后

// 子图的裁剪，如果没有裁剪器就不裁剪
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {
     trimmer->Trim(&trimming_handle);
   }
// 这里只有纯定位时才执行
// 裁剪器完成状态，删除裁剪器
   trimmers_.erase(
       std::remove_if(trimmers_.begin(), trimmers_.end(),
                      [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                        return trimmer->IsFinished();
                      }),
       trimmers_.end());

trimmers_的添加是 PoseGraph2D构造函数 —— PoseGraph2D::AddTrimmer

先看构造函数部分

if (options.has_overlapping_submaps_trimmer_2d()  )
{
    const auto& trimmer_options = options.overlapping_submaps_trimmer_2d();
    AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(
        trimmer_options.fresh_submaps_count(),
        trimmer_options.min_covered_area(),
        trimmer_options.min_added_submaps_count() )  );
}

三个参数:

fresh_submaps_count = 2 最新的2个submaps
min_covered_area = 3.0 最小未被覆盖的面积
min_added_submaps_count = 5 代表在5个submaps之后才会进行该项操作，这个参数不实用，主要是上面两个

Trims submaps that have less than min_covered_cells_count cells not overlapped by at least fresh_submaps_count submaps. 也就是说一些子图如果只有min_covered_cells_count个栅格没有被fresh_submaps_count个子图覆盖，那么这些子图被裁剪掉。

void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer)
{
  // C++11 does not allow us to move a unique_ptr into a lambda.
  PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
  AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    trimmers_.emplace_back(trimmer_ptr);
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

类PoseGraphTrimmer的派生类有 PureLocalizationTrimmer 和 OverlappingSubmapsTrimmer2D。前者属于纯定位部分，这里不看。后者其实主要就实现了一个Trim函数。

回头看trimmers_.erase这一句，发现OverlappingSubmapsTrimmer2D::IsFinished()永远返回 false，所以这一句只有纯定位时才会执行，建图不执行。

frozen轨迹的特点 2023-03-29|激光SLAMCartographer源码解读

frozen轨迹的子图和节点不参与优化问题中的优化，也就是5个残差的计算。但是参与inter global约束的计算，也就是纯定位。

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

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

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约束及雅格比矩阵的推导 2023-03-04|路径规划TEB算法各类约束及雅格比矩阵

时间约束

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α 得到

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

$|(cos{\theta_1}+cos{\theta_2})(y_2-y_1) - (sin{\theta_1}+sin{\theta_2})(x_2-x_1) |$

另一个是 positive-drive-direction约束

$-(x_2-x_1)cos{\theta_1} -(y_2-y_1)sin{\theta_1}$

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

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

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

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

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

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

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元边。

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

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里有一段:

if (start_vel)
  setVelocityStart(*start_vel);
if (free_goal_vel)
  setVelocityGoalFree();
else
  vel_goal_.first = true;

setVelocityStart给起始速度赋值：

// 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_

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个雅格比

_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的详细使用 2023-03-03|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>

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

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

# 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下，作为空函数
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的赋值可以这样：

_estimate.position() = position;
_estimate.x() = x;
_estimate.y() = y;
_estimate.theta() = theta;

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

oplusImpl

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

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

// 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，上面两个函数是

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也会进行数值求导，但是会比重写的雅可比慢。雅克比矩阵的维度为 误差维度 × 优化变量的维度 ，两个函数一般是这样的形式

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. 后一个顶点的类型是李群。可以这样写

// 相对 _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()

// 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

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

LOAM中的雅格比矩阵推导 2023-02-27|LOAM系列

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

代价函数使用的是欧拉角的方式进行求导解算的。一方面由于未采用矩阵的形式进行推导，导致整个推导过程非常复杂. 在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都只计算点到面的误差。

范数及求导 2023-02-27|数学基础

||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范数可以比较真实的矩阵和估计的矩阵值之间的误差。

ALOAM的流程 2023-02-24|LOAM系列

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

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

scanRegistration 流程

原始点云去除无效点和很近的点，成为无序点云
通过Lidar坐标系下点的仰角以及水平夹角计算点云的scanID和fireID(点在每个scan的ID为fireID)
以上是每根线作为独立的子点云，将子点云合并为一帧有序点云
根据曲率计算4种特征点，角点曲率大，面点曲率小。计算曲率，除了开始5个点和结束的5个点，以当前点为中心，前5个和后5个点都参与计算。然后针对每条scan，提取特征点：

曲率最大的前2个点认为是sharp点，最大的前20个点认为是less_sharp点
曲率最小的前4个点认为是flat点，将除了角点之外的所有点(包括flat)都加入 less_flat
发布4种特征点与滤波后的当前帧点云

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

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

laserOdometry 流程

// 后面可以看到，这是点云特征匹配时的优化变量
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);

订阅4种特征点的消息，各自加入队列，每次取一帧进行匹配
用当前帧的角点和面点，与上一帧的角点和面点建立约束。点到线以及点到面的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优化。

// 残差 = 点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优化。

// 残差 = 点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);
// 统计约束的数量

两种约束的数量不要太少

对于sharp点，将当前帧Lidar坐标系下的点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到线(找到的原上一帧点云的2个点)的残差。对于flat点，将当前帧Lidar坐标系下的点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到平面(找到的原上一帧点云的3个点组成的平面)的残差。
经过2次点到线以及点到面的ICP点云配准之后，得到最优的位姿增量 para_q 和 para_t，更新上一帧的位姿，得到当前帧的位姿，这里说的位姿都指世界坐标系下的位姿，世界坐标系本质就是第1帧的lidar坐标系。

$t_{cur} = t_{cur} + q_{cur} * t_{last}$ $q_{cur} = q_{cur} * q_{last}$

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

laserMapping 流程

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

接收laserOdometry发布的特征点，submap中的corner特征和surf特征在匹配中作为target；而当前帧点云中的两种特征在匹配中作为source。
基本流程与laserOdometry相同，同样地也分为点到线和点到面的匹配。submap中的点云都是world坐标系，而当前帧的点云都是Lidar坐标系，所以在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到world坐标系下
在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点，求出这5个点的中心。判断这5个点是不是呈线状分布，如果是线状，以中心点构建两个虚拟点。和laserOdometry处理sharp点一样，加入ceres优化，这样比laserOdometry中的ICP过程更鲁棒
对点到面的情况，也是找5个最近点，使用最小二乘拟合出平面(不是PCA了)，如果平面够平，把当前帧的点和平面法向量加入ceres优化，其实也是把点到平面的距离作为残差。
完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，更新增量wmap_wodom，为下一次Mapping做准备

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