个人学习总结--图优化与g2o

发布时间：2024-05-13 09:43:32

state estimation for robotics: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf

图优化图优化图优化图优化图优化图优化图优化图优化图优化图优化

摘要：本篇主要解析lio-sam框架下，是如何进行回环检测及位姿计算的。

本文分享自华为云社区《lio-sam框架：回环检测及位姿计算》，作者：月照银海似蛟龙。

图优化本身有成形的开源的库，例如

g2o
ceres
gtsam

lio-sam 中就是通过 gtsam 库进行图优化的，其中约束因子就包括回环检测因子

本篇主要解析lio-sam框架下，是如何进行回环检测及位姿计算的。

用一个图(Graph 图论)来表示SLAM问题

图中的节点来表示机器人的位姿二维的话即为 (x,y,yaw)

两个节点之间的边表示两个位姿的空间约束(相对位姿关系以及对应方差或线性矩阵)

边分为了两种边

帧间边:连接的前后,时间上是连续的
回环边:连接的前后,时间上是不连续的,但是直接也是两个位姿的空间约束

构建了回环边才会有误差出现,没有回环边是没有误差的

出现回环边,有了误差之后.构建图,并且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小

一旦形成回环即可进行优化消除误差

里程积分的相对位姿视为预测值 图上的各个节点就是通过里程(激光里程计\\轮速里程计)积分得到的
回环计算的相对位姿视为观测值 图上就是说通过 X2和X8的帧间匹配作为观测值

构建图并调整各节点的位姿,让预测与观测的误差最小

在点云匹配之后，可以来看回环检测部分的代码了

这部分的代码入口在 main函数中

std::thread loopthread(&mapOptimization::loopClosureThread, &MO);

单独开了一个回环检测的线程

下面来看loopClosureThread这个函数

 void loopClosureThread()
{
 if (loopClosureEnableFlag==false)
 return;

如果不需要进行回环检测，那么就退出这个线程

ros::Rate rate(loopClosureFrequency);

设置回环检测的频率 loopClosureFrequency默认为 1hz

没有必要太频繁

 while (ros::ok())
{
 rate.sleep();
 performLoopClosure();
 visualizeLoopClosure();
 }

设置完频率后，进行一个while的死循环。

执行完一次就必须sleep一段时间，否则该线程的cpu占用会非常高，通过performLoopClosure visualizeLoopClosure 执行回环检测

下面来看performLoopClosure 函数的具体内容

 void performLoopClosure()
{
 if (cloudKeyPoses3D->points.empty()==true)
 return;

如果没有关键帧，就没法进行回环检测了

就直接退出

 mtx.lock();
 *copy_cloudKeyPoses3D=*cloudKeyPoses3D;
 *copy_cloudKeyPoses6D=*cloudKeyPoses6D;
 mtx.unlock();

把存储关键帧额位姿的点云copy出来，避免线程冲突 cloudKeyPoses3D就是关键帧的位置 cloudKeyPoses6D就是关键帧的位姿

if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre)==false)

首先看一下外部通知的回环信息

 if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre)==false)
 return;

然后根据里程计的距离来检测回环

如果还没有则直接返回

来看detectLoopClosureDistance 函数的具体内容

        int loopKeyCur=copy_cloudKeyPoses3D->size() - 1;
        int loopKeyPre=-1;

检测最新帧是否和其它帧形成回环，取出最新帧的索引

        auto it=loopIndexContainer.find(loopKeyCur);
 if (it !=loopIndexContainer.end())
 return false;

检查一下较晚帧是否和别的形成了回环，如果有就算了

因为当前帧刚刚出现，不会和其它帧形成回环，所以基本不会触发

kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);

把只包含关键帧位移信息的点云填充kdtree

        kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

根据最后一个关键帧的平移信息，寻找离他一定距离内的其它关键帧

 for (int i=0; i < (int)pointSearchIndLoop.size(); ++i)
{

遍历找到的候选关键帧

            int id=pointSearchIndLoop[i];
 if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
{ 
 loopKeyPre=id;
 break;
 }

历史帧，必须比当前帧间隔30s以上

必须满足时间上超过一定阈值，才认为是一个有效的回环

如果时间上满足要做就找到了历史回环帧，那么赋值id 并且 break

一次找一个回环帧就行了

 if (loopKeyPre==-1 || loopKeyCur==loopKeyPre)
 return false;

如果没有找到回环或者回环找到自己身上去了，就认为是本次回环寻找失败

 *latestID=loopKeyCur;
 *closestID=loopKeyPre;
 return true;
 }

至此则找到了当真关键帧和历史回环帧

赋值当前帧和历史回环帧的id

如果在一个地方静止不动的时候，那么按照这个逻辑也会形成关键帧，可以通过以关键帧序列号的方式加以改进

如果检测回环存在了，那么则可以进行下面内容，就是计算检测出这两帧的位姿变换

 pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
 pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());

声明当前关键帧的点云

声明历史回环帧周围的点云（局部地图）

loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);

当前关键帧把自己取了出来

来看 loopFindNearKeyframes 这个函数

 void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
{
 for (int i=-searchNum; i <=searchNum; ++i)
{

searchNum 是搜索范围，遍历帧的范围

int keyNear=key + i;

找到这个 idx

 if (keyNear < 0 || keyNear >=cloudSize )
 continue;

如果超出范围了就算了

 *nearKeyframes +=*transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
 *nearKeyframes +=*transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);

否则吧对应角点和面点的点云转到世界坐标系下去

 if (nearKeyframes->empty())
 return;

如果没有有效的点云就算了

 pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
 downSizeFilterICP.setInputCloud(nearKeyframes);
 downSizeFilterICP.filter(*cloud_temp);
 *nearKeyframes=*cloud_temp;

吧点云下采样

然后会到之前的地方：

loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);

回环帧把自己周围一些点云取出来，也就是构成一个帧局部地图的一个匹配问题

 if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
 return;

如果点云数目太少就算了

 if (pubHistoryKeyFrames.getNumSubscribers() !=0)
 publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);

把局部地图发布出来供rviz可视化使用

现在有了当前关键帧投到地图坐标系下的点云和历史回环帧投到地图坐标系下的局部地图，那么接下来就可以进行两者的icp位姿变换求解

 static pcl::IterativeClosestPoint<PointType, PointType> icp;

使用简单的icp来进行帧到局部地图的配准

icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);

设置最大相关距离

icp.setMaximumIterations(100);

最大优化次数

icp.setTransformationEpsilon(1e-6);

单次变换范围

icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);

残差设置

 icp.setInputSource(cureKeyframeCloud);
 icp.setInputTarget(prevKeyframeCloud);

设置两个点云

 pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
 icp.align(*unused_result);

执行配准

 if (icp.hasConverged()==false || icp.getFitnessScore() > historyKeyframeFitnessScore)
 return;

检测icp是否收敛且得分是否满足要求

 if (pubIcpKeyFrames.getNumSubscribers() !=0)
{
 pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
 pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
 publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
 }

把修正后的当前点云发布供可视化使用

 correctionLidarFrame=icp.getFinalTransformation();

获得两个点云的变换矩阵结果

 Eigen::Affine3f tWrong=pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);

取出当前帧的位姿

 Eigen::Affine3f tCorrect=correctionLidarFrame * tWrong;

将icp结果补偿过去，就是当前帧的更为准确的位姿结果

pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);

将当前帧补偿后的位姿转换成平移和旋转

gtsam::Pose3 poseFrom=Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo=pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);

将当前帧补偿后的位姿转换成 gtsam的形式

From 和 To相当于帧间约束的因子，To是历史回环帧的位姿

gtsam::Vector Vector6(6);
float noiseScore=icp.getFitnessScore();
noiseModel::Diagonal::shared_ptr constraintNoise=noiseModel::Diagonal::Variances(Vector6);

使用icp的得分作为他们的约束噪声项

 loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));//两帧索引
 loopPoseQueue.push_back(poseFrom.between(poseTo));//当前帧与历史回环帧相对位姿
 loopNoiseQueue.push_back(constraintNoise);//噪声

将两帧索引，两帧相对位姿和噪声作为回环约束送入对列

loopIndexContainer[loopKeyCur]=loopKeyPre;

保存已经存在的约束对

lio-sam回环检测的方式

构建关键帧，将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧，通过kdtree寻找历史关键帧中距离和时间满足条件的一个关键帧。然后就认为形成了回环。

形成回环后，历史帧周围25帧，构建局部地图，与当前关键帧进行icp匹配求解位姿变换。

lio-sam 认为里程计累计漂移比较小，所以通过距离与时间这两个概念进行的关键帧的回环检测。

点击关注，第一时间了解华为云新鲜技术~

假设一个机器人初始起点在0处，然后机器人向前移动，通过编码器测得它向前移动了1m，到达第二个地点x1。接着，又向后返回，编码器测得它向后移动了0.8米。但是，通过闭环检测，发现它回到了原始起点。可以看出，编码器误差导致计算的位姿和观测到有差异，那机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢？

首先构建位姿之间的关系，即图的边：

线性方程组中变量小于方程的个数，要计算出最优的结果，使出杀手锏最小二乘法。先构建残差平方和函数：

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0.

整理得到：

接着矩阵求解线性方程组：

所以调整以后为满足这些边的条件，机器人的位姿为：

可以看到，因为有了x2->x0的闭环检测，几个位置点间才能形成互相约束。这对使用最小二乘解决该优化问题起到了决定性的作用。

该问题描述来源于：https://heyijia.blog.csdn.net/article/details/47686523

下面利用G2O来解上面的问题，以便理解如何使用G2O。

在该问题中，一个位置点就是图优化中的一个顶点。一个顶点可以包含多个需优化量。比如二维环境下的机器人位置一般是3维的(x,y,theta)，即一个顶点有三个需要优化的量。

在此问题中，我们只需优化求解一个一维的距离值。即是，一个顶点只包含一个需优化量。

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class SimpleVertex : public g2o::BaseVertex<1, double>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SimpleVertex(bool fixed=false)
{
    setToOriginImpl();
    setFixed(fixed);
  }

  SimpleVertex(double e, bool fixed=false)
{
    _estimate=e;
    setFixed(fixed);
  }

  // 重置
  virtual void setToOriginImpl() override{
    _estimate=0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override{
    _estimate +=*update;
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in){return true;}

  virtual bool write(ostream &out) const{return true;}
};

上面代码中g2o::BaseVertex<1, double>就表示了优化的量是一维的，数据类型是double。

如果查看TEB中设置的优化量，可以发现它是这样写的：

g2o::BaseVertex<3, PoseSE2 >

TEB中的优化量是三维的，即机器人的位姿(x,y,theta)。此处的PoseSE2便是描述机器人位姿的结构体。

自定义一个顶点通常需要重新实现setToOriginImpl()和oplusImpl(const double *update)这两个函数。至于read(istream &in)和write(ostream &out)是为了保存和读取当前顶点数据的。用不上可留空。

setToOriginImpl()函数用于重置顶点的数据。顶点包含的数据变量是_estimate。该变量的类型即是g2o::BaseVertex<1, double>中设置的double。该函数正是用于重置_estimate和使顶点恢复默认状态。

oplusImpl(const double *update)函数用于叠加优化量的步长。注意有时候这样的叠加操作并不是线性的。比如2D-SLAM中位置步进叠加操作则不是线性的。

在此问题中我们是直接线性叠加一维的距离值。

TEB中的位置叠加也是线性叠加的。放置在下面以作参考。

void plus(const double* pose_as_array)
{
    _position.coeffRef(0) +=pose_as_array[0];
    _position.coeffRef(1) +=pose_as_array[1];
    _theta=g2o::normalize_theta( _theta + pose_as_array[2]);
}

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

另外，顶点是可以设置成固定的。当不需要变动某个顶点时，使用setFixed函数来固定。通常，一个优化问题中，至少需要固定一个顶点，否则所有的顶点都在浮动，优化效果也不会好。

边即是顶点之间的约束。在该问题中就是两位置间的测量值和观测值之间的差值要趋近于0。这里需要定义的边其实就是下面的等式。

实际上，G2O中边有三种：一元边(g2o::BaseUnaryEdge)，二元边(g2o::BaseBinaryEdge)和多元边(g2o::BaseMultiEdge)。

g2o::BaseUnaryEdge<D, E, VertexXi>
g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
g2o::BaseMultiEdge<D, E>

不同类型的边有不同数量的模板参数。其中D 是 int 型，表示误差值的维度（dimension）， E 表示测量值的数据类型（即_measurement的类型）， VertexXi，VertexXj 分别表示不同顶点的类型。当D为2时，_error的类型变为Eigen::Vector2d，当D为3时，_error的类型变为Eigen::Vector3d。

在上面的约束中，有一个一元边(f1)和三个二元边(f2,f3,f4)。在G2O中可如下定义：

// 误差模型 模板参数：测量值维度，测量值类型，连接顶点类型
class SimpleUnaryEdge : public g2o::BaseUnaryEdge<1, double, SimpleVertex>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SimpleUnaryEdge() : BaseUnaryEdge(){}

  // 计算模型误差
  virtual void computeError() override{
    const SimpleVertex *v=static_cast<const SimpleVertex *> (_vertices[0]);
    const double abc=v->estimate();
    _error(0, 0)=_measurement - abc;
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override{
    _jacobianOplusXi[0]=-1;//一元边只有一个顶点，所以只有_jacobianOplusXi。_jacobianOplusXi的维度和顶点需优化的维度是一致的
  }

  virtual bool read(istream &in){}

  virtual bool write(ostream &out) const{}

};

// 误差模型 模板参数：测量值维度，测量值类型，连接顶点类型
class SimpleBinaryEdge : public g2o::BaseBinaryEdge<1, double, SimpleVertex, SimpleVertex>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SimpleBinaryEdge(){}

  // 计算模型误差
  virtual void computeError() override{
    const SimpleVertex *v1=static_cast<const SimpleVertex *> (_vertices[0]);
    const SimpleVertex *v2=static_cast<const SimpleVertex *> (_vertices[1]);
    const double abc1=v1->estimate();
    const double abc2=v2->estimate();
    _error[0]=_measurement - (abc1 - abc2);
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override{
    _jacobianOplusXi(0,0)=-1;//二元边有两个顶点，偏差_error对每个顶点的偏导数都需要求解;如果_error是多维的，则每一维的偏导都需要求解。即会出现_jacobianOplusXi(1,0)

    _jacobianOplusXj(0,0)=1;//因为误差_error只有一维，所以_jacobianOplusXi只有_jacobianOplusXi(0,0)项。此时_jacobianOplusXi[0]与_jacobianOplusXi(0,0)效果等价。
  }

  virtual bool read(istream &in){}

  virtual bool write(ostream &out) const{}

};

因为我们这里测量值是一维的距离值（double类型）而顶点类型为SimpleVertex，所以边需设置成g2o::BaseUnaryEdge<1, double, SimpleVertex>和g2o::BaseBinaryEdge<1, double, SimpleVertex, SimpleVertex>类型。

自定义边同样需要特别关注两个函数computeError()和linearizeOplus()。

computeError()是用于计算迭代误差的。顶点间的约束正是由误差计算函数构建的。优化时误差项将逐步趋近于0。_error的维度和类型通常由构建的模型决定。比如该问题中误差为距离误差。

TEB的速度边构建的误差项为线速度与最大线速度的差值和角速度与最大角速度的差值。所以这里的误差项是二维的。

  void computeError()
{
    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
    const VertexPose* conf1=static_cast<const VertexPose*>(_vertices[0]);
    const VertexPose* conf2=static_cast<const VertexPose*>(_vertices[1]);
    const VertexTimeDiff* deltaT=static_cast<const VertexTimeDiff*>(_vertices[2]);
    
    const Eigen::Vector2d deltaS=conf2->estimate().position() - conf1->estimate().position();
    
    double dist=deltaS.norm();
    const double angle_diff=g2o::normalize_theta(conf2->theta() - conf1->theta());
    if (cfg_->trajectory.exact_arc_length && angle_diff !=0)
{
        double radius=dist/(2*sin(angle_diff/2));
        dist=fabs( angle_diff * radius ); // actual arg length!
    }
    double vel=dist / deltaT->estimate();
    
//     vel *=g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
    vel *=fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
    
    const double omega=angle_diff / deltaT->estimate();
  
    _error[0]=penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
    _error[1]=penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);

    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\
",_error[0],_error[1]);
  }

linearizeOplus()函数里主要是配置雅克比矩阵。当然，G2O是支持自动求导的，该函数可以不实现。优化时由G2O自动处理。但准确的实现可加快优化计算的速度。下面介绍雅克比矩阵该如何计算。

雅克比矩阵存储了误差项的每一维相对于顶点各优化成员的偏导数。

一元边只有一个顶点所以只定义了_jacobianOplusXi，其对应顶点类型VertexXi。注意，_jacobianOplus变量才完整地描述雅克比矩阵，_jacobianOplusXi只是_jacobianOplus中相关部分的引用，以方便配置雅克比矩阵。

// This could be a simple using statement, but in multiple places
// _jacobianOplusXi is used.
template <int D, typename E, typename VertexXi>
class BaseUnaryEdge : public BaseFixedSizedEdge<D, E, VertexXi>{
 public:
  using VertexXiType=VertexXi;
  BaseUnaryEdge() : BaseFixedSizedEdge<D, E, VertexXi>(){};

 protected:
  typename BaseFixedSizedEdge<D, E, VertexXi>::template JacobianType<
      D, VertexXi::Dimension>& _jacobianOplusXi=std::get<0>(this->_jacobianOplus);
};

本优化问题的一元边按下面代码的方式计算雅克比矩阵。

// 误差模型 模板参数：误差项维度，测量值类型，连接顶点类型
class SimpleUnaryEdge : public g2o::BaseUnaryEdge<1, double, SimpleVertex>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SimpleUnaryEdge() : BaseUnaryEdge(){}

  // 计算曲线模型误差
  virtual void computeError() override{
    const SimpleVertex *v=static_cast<const SimpleVertex *> (_vertices[0]);
    const double abc=v->estimate();
    _error(0, 0)=_measurement - abc;
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override{
    _jacobianOplusXi[0]=-1;//一元边只有一个顶点，所以只有_jacobianOplusXi。_jacobianOplusXi的维度和顶点需优化的维度是一致的
  }//此处就是求误差相对于abc变量的导数

  virtual bool read(istream &in){}

  virtual bool write(ostream &out) const{}

};

二元边有两个顶点，所以定义了_jacobianOplusXi和_jacobianOplusXj。

// This could be a simple using statement, but in multiple places
// _jacobianOplusXi and _jacobianOplusYi are used.
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseBinaryEdge : public BaseFixedSizedEdge<D, E, VertexXi, VertexXj>{
 public:
  using VertexXiType=VertexXi;
  using VertexXjType=VertexXj;
  BaseBinaryEdge() : BaseFixedSizedEdge<D, E, VertexXi, VertexXj>(){};

 protected:
  typename BaseFixedSizedEdge<D, E, VertexXi, VertexXj>::template JacobianType<
      D, VertexXi::Dimension>& _jacobianOplusXi=std::get<0>(this->_jacobianOplus);
  typename BaseFixedSizedEdge<D, E, VertexXi, VertexXj>::template JacobianType<
      D, VertexXj::Dimension>& _jacobianOplusXj=std::get<1>(this->_jacobianOplus);
};

本优化问题的二元边按下面代码的方式计算雅克比矩阵。

// 误差模型 模板参数：误差项维度，测量值类型，连接顶点类型
class SimpleBinaryEdge : public g2o::BaseBinaryEdge<1, double, SimpleVertex, SimpleVertex>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SimpleBinaryEdge(){}

  // 计算曲线模型误差
  virtual void computeError() override{
    const SimpleVertex *v1=static_cast<const SimpleVertex *> (_vertices[0]);
    const SimpleVertex *v2=static_cast<const SimpleVertex *> (_vertices[1]);
    const double abc1=v1->estimate();
    const double abc2=v2->estimate();
    _error[0]=_measurement - (abc1 - abc2);
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override{
    _jacobianOplusXi(0,0)=-1;//二元边有两个顶点，偏差_error对每个顶点的偏导数都需要求解;如果_error是多维的，则每一维的偏导都需要求解。即会出现_jacobianOplusXi(1,0)

    _jacobianOplusXj(0,0)=1;//因为误差_error只有一维，所以_jacobianOplusXi只有_jacobianOplusXi(0,0)项。此时_jacobianOplusXi[0]与_jacobianOplusXi(0,0)效果等价。
  }

  virtual bool read(istream &in){}

  virtual bool write(ostream &out) const{}

};

多元边直接配置_jacobianOplus变量。

下面的示例代码是teb中的速度约束，来源于edge_velocity.h文件。

class EdgeVelocity : public BaseTebMultiEdge<2, double>
{
public:
  
  /**
   * @brief Construct edge.
   */       
  EdgeVelocity()
{
    this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
  }
  
  /**
   * @brief Actual cost function
   */  
  void computeError()
{
    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
    const VertexPose* conf1=static_cast<const VertexPose*>(_vertices[0]);
    const VertexPose* conf2=static_cast<const VertexPose*>(_vertices[1]);
    const VertexTimeDiff* deltaT=static_cast<const VertexTimeDiff*>(_vertices[2]);
    
    const Eigen::Vector2d deltaS=conf2->estimate().position() - conf1->estimate().position();
    
    double dist=deltaS.norm();
    const double angle_diff=g2o::normalize_theta(conf2->theta() - conf1->theta());
    if (cfg_->trajectory.exact_arc_length && angle_diff !=0)
{
        double radius=dist/(2*sin(angle_diff/2));
        dist=fabs( angle_diff * radius ); // actual arg length!
    }
    double vel=dist / deltaT->estimate();
    
//     vel *=g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
    vel *=fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
    
    const double omega=angle_diff / deltaT->estimate();
  
    _error[0]=penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
    _error[1]=penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);

    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\
",_error[0],_error[1]);
  }

#ifdef USE_ANALYTIC_JACOBI
#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value)
      // Change accordingly...

  /**
   * @brief Jacobi matrix of the cost function specified in computeError().
   */
  void linearizeOplus()
{
    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
    const VertexPose* conf1=static_cast<const VertexPose*>(_vertices[0]);
    const VertexPose* conf2=static_cast<const VertexPose*>(_vertices[1]);
    const VertexTimeDiff* deltaT=static_cast<const VertexTimeDiff*>(_vertices[2]);
    
    Eigen::Vector2d deltaS=conf2->position() - conf1->position();
    double dist=deltaS.norm();
    double aux1=dist*deltaT->estimate();
    double aux2=1/deltaT->estimate();
    
    double vel=dist * aux2;
    double omega=g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2;
    
    double dev_border_vel=penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
    double dev_border_omega=penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
    
    _jacobianOplus[0].resize(2,3); // conf1
    _jacobianOplus[1].resize(2,3); // conf2
    _jacobianOplus[2].resize(2,1); // deltaT
    
//  if (aux1==0) aux1=1e-6;
//  if (aux2==0) aux2=1e-6;
    
    if (dev_border_vel!=0)
{
      double aux3=dev_border_vel / aux1;
      _jacobianOplus[0](0,0)=-deltaS[0]* aux3; // vel x1
      _jacobianOplus[0](0,1)=-deltaS[1]* aux3; // vel y1 
      _jacobianOplus[1](0,0)=deltaS[0]* aux3; // vel x2
      _jacobianOplus[1](0,1)=deltaS[1]* aux3; // vel y2
      _jacobianOplus[2](0,0)=-vel * aux2 * dev_border_vel; // vel deltaT
    }
    else
{
      _jacobianOplus[0](0,0)=0; // vel x1
      _jacobianOplus[0](0,1)=0; // vel y1 
      _jacobianOplus[1](0,0)=0; // vel x2
      _jacobianOplus[1](0,1)=0; // vel y2 
      _jacobianOplus[2](0,0)=0; // vel deltaT
    }
    
    if (dev_border_omega!=0)
{
      double aux4=aux2 * dev_border_omega;
      _jacobianOplus[2](1,0)=-omega * aux4; // omega deltaT
      _jacobianOplus[0](1,2)=-aux4; // omega angle1
      _jacobianOplus[1](1,2)=aux4; // omega angle2
    }
    else
{
      _jacobianOplus[2](1,0)=0; // omega deltaT
      _jacobianOplus[0](1,2)=0; // omega angle1
      _jacobianOplus[1](1,2)=0; // omega angle2   
    }

    _jacobianOplus[0](1,0)=0; // omega x1
    _jacobianOplus[0](1,1)=0; // omega y1
    _jacobianOplus[1](1,0)=0; // omega x2
    _jacobianOplus[1](1,1)=0; // omega y2
    _jacobianOplus[0](0,2)=0; // vel angle1
    _jacobianOplus[1](0,2)=0; // vel angle2
  }
#endif
#endif
 
  
public:
  
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

上面的代码中_error[0]对应线速度vel误差，_error[1]对应角速度omega误差。所以需要求解vel和omega分别相对于顶点变量x,y,theta的导数。

_jacobianOplus[0].resize(2,3);对应第一个顶点，其中误差项有两维，顶点优化变量有3维，所以雅克比矩阵是一个2x3的矩阵。

_jacobianOplus[1].resize(2,3);对应第二个顶点，其中误差项有两维，顶点优化变量有3维，所以雅克比矩阵是一个2x3的矩阵。

_jacobianOplus[2].resize(2,1);对应第三个顶点，其中误差项有两维，顶点是时间差，只有一维，所以雅克比矩阵是一个2x1的矩阵。

// 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<1, 1>> BlockSolverType;  // 每个误差项优化变量维度为1，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver=new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

teb中的优化器设置

//! Typedef for the block solver utilized for optimization
typedef g2o::BlockSolverX TEBBlockSolver;

//! Typedef for the linear solver utilized for optimization
typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
//typedef g2o::LinearSolverCholmod<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;

/*
 * initialize g2o optimizer. Set solver settings here.
 * Return: pointer to new SparseOptimizer Object.
 */
std::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer()
{
  // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
  static std::once_flag flag;
  std::call_once(flag,[this](){this->registerG2OTypes();});

  // allocating the optimizer
  std::shared_ptr<g2o::SparseOptimizer> optimizer=std::make_shared<g2o::SparseOptimizer>();
  auto linearSolver=std::make_unique<TEBLinearSolver>(); // see typedef in optimization.h
  linearSolver->setBlockOrdering(true);
  auto blockSolver=std::make_unique<TEBBlockSolver>(std::move(linearSolver));
  g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));

  optimizer->setAlgorithm(solver);
  
  optimizer->initMultiThreading(); // required for >Eigen 3.1
  
  return optimizer;
}

从上面的代码对比可以看出，BlockSolver可设成固定的，也可以设成动态的。

下面是本人对于BlockSolver设置的理解，不一定正确。仅为参考。

优化变量的维度通常是明确的，但误差项的维度可能是变化的。每条边的误差项维度可能都不一样。这时应该使用g2o::BlockSolverX，以便能动态适应误差项的维度。

linear solver也是可选的。主要有下面几种可选：

g2o::LinearSolverCSparse
g2o::LinearSolverCholmod
g2o::LinearSolverEigen
g2o::LinearSolverDense
g2o::LinearSolverPCG

不同的linear solver有不同的依赖，需要注意是否已经安装了相应的依赖。

// 往图中增加顶点
  std::vector<SimpleVertex *> vertexs;

  SimpleVertex *v=new SimpleVertex();
  v->setEstimate(0);
  v->setId(0);
  v->setFixed(true);
  optimizer.addVertex(v);
  vertexs.push_back(v);

  SimpleVertex *v1=new SimpleVertex();
  v1->setEstimate(1);
  v1->setId(1);
  optimizer.addVertex(v1);
  vertexs.push_back(v1);

  SimpleVertex *v2=new SimpleVertex();
  v2->setEstimate(0.1);
  v2->setId(2);
  optimizer.addVertex(v2);
  vertexs.push_back(v2);

顶点的id需要确保不能重复。

顶点可使用setEstimate接口设置初始值。

// 往图中增加边
  SimpleUnaryEdge *edge=new SimpleUnaryEdge();
  // edge->setId(i);
  edge->setVertex(0, vertexs[0]);                // 设置连接的顶点
  edge->setMeasurement(0);      // 观测数值
  edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity()); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge(edge);

  SimpleBinaryEdge * edge1=new SimpleBinaryEdge();
  // edge->setId(i);//id 不设置似乎没有关系，如果设置需要每条边设置成不一样的
  edge1->setVertex(0, vertexs[1]);//这里设置的序号对应的顶点要和边的computeError函数里设定的顶点是一一对应的
  edge1->setVertex(1, vertexs[0]);                // 设置连接的顶点
  edge1->setMeasurement(1);      // 观测数值
  edge1->setInformation(Eigen::Matrix<double, 1, 1>::Identity()*1.0); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge(edge1);

  SimpleBinaryEdge * edge2=new SimpleBinaryEdge();
  // edge->setId(i);
  edge2->setVertex(0, vertexs[2]);                // 设置连接的顶点
  edge2->setVertex(1, vertexs[1]);                // 设置连接的顶点
  edge2->setMeasurement(-0.8);      // 观测数值
  edge2->setInformation(Eigen::Matrix<double, 1, 1>::Identity()); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge(edge2);

  SimpleBinaryEdge * edge3=new SimpleBinaryEdge();
  // edge->setId(i);
  edge3->setVertex(0, vertexs[2]);                // 设置连接的顶点
  edge3->setVertex(1, vertexs[0]);                // 设置连接的顶点
  edge3->setMeasurement(0);      // 观测数值
  edge3->setInformation(Eigen::Matrix<double, 1, 1>::Identity()); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge(edge3);

边也有setId接口，但不设置好像也可以。如果设置也需确保不重复。但id号的顺序似乎并没有要求。

使用setVertex接口设置顶点时是有顺序的。这个顺序与边的computeError函数中使用顶点的顺序要对应起来。

setMeasurement接口用于设置内部的_measurement值。

setInformation接口用于设置信息矩阵。信息矩阵是方正矩阵，其行列数由误差项的维度决定。

void TebOptimalPlanner::AddEdgesVelocity()
{
  if (cfg_->robot.max_vel_y==0) // non-holonomic robot
{
    if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0)
      return; // if weight equals zero skip adding edges!

    int n=teb_.sizePoses();
    Eigen::Matrix<double,2,2> information;
    information(0,0)=cfg_->optim.weight_max_vel_x;
    information(1,1)=cfg_->optim.weight_max_vel_theta;
    information(0,1)=0.0;
    information(1,0)=0.0;

    for (int i=0; i < n - 1; ++i)
{
      EdgeVelocity* velocity_edge=new EdgeVelocity;
      velocity_edge->setVertex(0,teb_.PoseVertex(i));
      velocity_edge->setVertex(1,teb_.PoseVertex(i+1));
      velocity_edge->setVertex(2,teb_.TimeDiffVertex(i));
      velocity_edge->setInformation(information);
      velocity_edge->setTebConfig(*cfg_);
      optimizer_->addEdge(velocity_edge);
    }
  }
  ...
}

最终优化求解的结果为estimated model: 0 0.933333 0.0666667 。基本和标准结果一致。

如果将edge1的信息矩阵如下设置：

edge1->setInformation(Eigen::Matrix<double, 1, 1>::Identity()*10.0);

优化求解的结果为：estimated model: 0 0.990476 0.0952381

可以发现x1更接近1了。说明此时我们更相信编码器测量的从x0到x1的距离值。

完整的测试代码可查看下面的链接：

https://github.com/shoufei403/g2o_learning.git

觉得有用就点赞吧！

我是首飞，一个帮大家填坑的机器人开发攻城狮。

另外在公众号《首飞》内回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2等机器人行业常用技术资料。

The school focuses on two themes. In the morning sessions, we will look at various graph theoretic problems that also model real-world affairs. We begin with problems that can be solved in polynomial time. Later on, we will see that not all the problems of our interest can be solved in polynomial time and fall under the category of NP-hard problems. Since many of these problems are essential and need to be solved, we will introduce various algorithmic tools to get around NP-hard problems in this school. Specifically, we will cover exact exponential algorithms, approximation algorithms, and parameterized algorithms.

In contemporary times, we need to handle extensive data and design algorithms that can process and work on a massive amount of data even when the whole data is unavailable to us in one go. This school will also introduce streaming algorithms that are useful when the algorithm has limited access to the memory, and the data is available only as streams and can be examined in only a few passes.

In the evening sessions, we will introduce various problems in game theory. Game theory is a vast field, and in this school, we will focus on Voting Theory, Stable Matching, and Fair Allocation. We will introduce various rules, axiomatic properties, algorithms, and research directions in these fields.

There will also be several group activities to demonstrate the applicability of some of these concepts in real-world scenarios using various fun games and puzzles.

We are also organizing a poster session at the end of the school. You are welcome to submit posters of your work in the related domain.
Best two papers will be awarded.

Registration Deadline: October 30, 2022

School: December 05 - 11, 2022

Poster Session: December 11, 2022.

The school is intended for graduate students working or who want to work in the area of Algorithms and related areas, and for motivated undergraduate students who are enthusiastic regarding these areas. People from the industry looking for exposure or those working or planning to work in this area will also find the school beneficial.

详情请见官网：https://cse.iitj.ac.in/index.php/events/winter-school/algorithms-graphs-games

欢迎关注（公众号） @运筹OR帷幄 ，了解更多运筹学、人工智能及相关学科的干货资讯。

知乎专栏： 『运筹OR帷幄』大数据时代的运筹学『运筹AI帷幄』大数据时代的人工智能

获得最新运筹学及其相关学科的干货资料、行业前沿信息、学术动态、硕博offer信息等。特别适合你~

欢迎大家交流。

也欢迎大佬们投稿和商业合作，学术信息、会议通讯、征稿启事、硕博招生信息等免费推广。

参考：

Sky Shaw：多传感器融合｜各类滤波器方法整理

SLAM中的EKF，UKF，PF原理简介 - 半闲居士 - 博客园

SLAM 中的 Kalman Filter 推导

边缘概率是相对于联合概率而言的的，虽然你有两个变量（x,y）但是你可以只考虑x或者y的分部，好像另外一个不存在一样，写作 $p(x)$ 或者 $p(y)$ 。

$p(X=a)=\\sum_{k=1}^{n}p(X=a)p(Y=k)\\\\ p(Y=b)=\\sum_{k=1}^{n}p(X=k)p(Y=b)\\\\$

离散概率和为1，即： $\\sum_{x}^{}{p(X=x)}=1\\\\$

为了简化符号，在可能时通常省略随机变量的明确表示，而是使用常见的缩写 $p(x)$ 代替 $p(X=x)$ 。

两个随机变量X和Y的联合概率表示两件事都发生的概率，由下式给出：

$p(x,y)=p(X=x ,Y=y)=p(x|y)p(y)=p(y|x)p(x)\\\\$

如果X和Y相互独立，有 $p(x,y)=p(x)p(y)$ .

$相互独立：p(x,y)=p(x)p(y)\\\\ 条件独立：p(x,y|z)=p(x|z)p(y|z)\\\\$

左边是独立的定义，右边是条件独立的定义。可以看出：条件独立是指在某个条件已知的前提下独立。

注意：独立与条件独立不能互推。

菠萝头：独立与条件独立

如果 A,B 关于 C 条件独立：
C 的发生，使本来不独立的 A 和 B 变得独立起来，也即事件 C 的发生，解开了 A 和 B 的依赖关系；

假定已经知道Y=y，想知道基于以上事实条件X=x的概率。这样的概率表示为 $p(x|y)=p(X=x | Y=y)$

称为条件概率。

如果p(y)>0，则条件概率定义为 $p(x|y)=\\frac{p(x,y)}{p(y)}$ .

通过统计方法分析事件发生关系的占比，以做到已知结果倒推原因。

如果X和Y相互独立，则有 $p(x|y)=\\frac{p(x)p(y)}{p(y)}=p(x)$ .

$P(X=a|Y=b)=\\frac{P(X=a,Y=b)}{P(Y=b)}$

将概率转化为面积来理解：

联合概率P(X=a,Y=b)满足X=a且Y=b的面积
边缘概率P(X=a)不考虑Y的取值，所有满足X=a的区域的总面积
条件概率P(X=a|Y=b)在Y=b的前提下，满足X=a的面积（比例）

从条件概率和概率测量公理得出的一个有趣事实经常被称为全概率定理：

$p(x)=\\sum_{y}^{}{p(x|y)p(y)}(离散情况)\\\\ p(x)=\\int_{}^{}p(x|y)p(y)dy (连续情况)\\\\$

贝叶斯定理推导（Bayes's Theorem）

该定理将条件概率p(x|y)与其“逆”概率p(ylx)联系起来。如此处所阐述的，准则要求p(y)>0:

$p(x|y)=\\frac{p(y|x)p(x)}{p(y)}=\\frac{p(y|x)p(x)}{\\sum_{x^{'}}^{}{p(y|x^{'})p(x^{'})}}(离散) \\\\ p(x|y)=\\frac{p(y|x)p(x)}{p(y)}=\\frac{p(y|x)p(x)}{\\int_{}^{}{p(y|x^{'})p(x^{'})}dx^{'}}(连续) \\\\$

如果x是一个希望由y推测出来的数值，其中y称为数据(data)（结果），也就是传感器测量值。 $p(x)$ 是先验概率（prior probability）， $p(y|x)$ 是条件概率（conditional probability）（原因）， $p(x|y)$ 是后验概率（posterior probability）。

SLAM中很多理论，诸如滤波方法、BA和图优化等，都是基于贝叶斯定理的思想而来的。

考虑贝叶斯定理： $p(dyn|meas)=\\frac{p(meas|dyn)p(dyn)}{p(meas)}$ .

因为 $p(meas)$ 是固定的常数，从而有： $p(dyn | meas) \\sim p(dyn)p(meas|dyn)$ .

在进行涉及概率的数值计算时使用对数形式 。那么有：

$L(dyn|meas)=log(p(dyn)) + log(p(meas|dyn)).\\\\$

贝叶斯滤波基于马尔可夫假设。

马尔可夫假设：系统当前时刻的状态只与上一个时刻有关，与之前任意时刻的状态都没有关系。当然，马尔可夫假设的成立是有条件的，它要求状态变量具有完整性，换句话说，系统的全部信息都包含在了中，不存在其它随机变量对系统状态产生影响。

$控制更新：\\bar{bel}(x_t)=p(x_t|z_{1:t-1},u_{1:t}),\\\\ 测量更新：bel(x_t)=p(x_t|z_{1:t},u_{1:t}).\\\\$

证明来自《概率机器人》p23~25页，用到了马尔可夫假设、全概率定理、条件独立、贝叶斯定理等。

Will：EKF公式推导

所以EKF面临的一个重要问题是，当一个高斯分布经过非线性变换后，如何用另一个高斯分布近似它？按照它现在的做法，存在以下的局限性：（注意是滤波器自己的局限性，还没谈在SLAM问题里的局限性）。

即使是高斯分布，经过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能很差）。
系统本身线性化过程中，丢掉了高阶项。
线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值。于是，在这个工作点下计算的 $F,G$ ，也不是最好的。
在估计非线性输出的均值时，EKF算的是 $\\mu_y=f(\\mu_x)$ 的形式。这个结果几乎不会是输出分布的真正期望值。协方差也是同理。

那么，怎么克服以上的缺点呢？途径很多，主要看我们想不想维持EKF的假设。如果我们只是希望维持高斯分布假设，可以这样子改：

为了克服第2条，可以使用ESKF。它的线性化是总是在0附近，并且雅可比矩阵的形式简单，计算迅速。因此线性化比EKF更准确。虽然，ESKF主要用于有imu的系统。
为了克服第3条工作点的问题，我们以EKF估计的结果为工作点，重新计算一遍EKF，直到这个工作点变化够小。是为迭代EKF（Iterated EKF, IEKF）。
为了克服第4条，我们除了计算 $\\mu_y=f(\\mu_x)$ ，再计算其他几个精心挑选的采样点，然后用这几个点估计输出的高斯分布。是为Sigma Point KF（SPKF，或UKF）。
如果我们不要高斯分布假设，凭什么要用高斯去近似一个长得根本不高斯的分布呢？于是问题变为，丢掉高斯假设后，怎么描述输出函数的分布就成了一个问题。一种比较暴力的方式是：用足够多的采样点，来表达输出的分布。这种蒙特卡洛的方式，也就是粒子滤波的思路。
如果再进一步，可以丢弃滤波器思路，说：为什么要用前一个时刻的值来估计下一个时刻呢？我们可以把所有状态看成变量，把运动方程和观测方程看成变量间的约束，构造误差函数，然后最小化这个误差的二次型。这样就会得到非线性优化的方法，在SLAM里就走向图优化那条路上去了。不过，非线性优化也需要对误差函数不断地求梯度，并根据梯度方向迭代，因而局部线性化是不可避免的。

无迹卡尔曼滤波算法（UKF）详细推倒及其仿真(matlab)_钢蛋儿（顺利毕业）的博客-CSDN博客_ukf算法

该算法的核心思想是：采用无损变换（Unscented Transform，UT），利用一组Sigma采样点来描述随机变量的高斯分布，然后通过非线性函数的传递，再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。

相比于EKF，UKF的估计精度能够达到泰勒级数展开的二阶精度。其本质是：近似非线性函数的均值和方差远比近似非线性函数本身更容易。

y=f(x)是一个非线性变换, x为 n 维随机变量，UT变换根据一定的采样策略，获得一组Sigma采样点，并设定均值权值W和方差权值，来近似非线性函数的后验均值和方差。利用选取的Sigma采样点集进行非线性函数传递可得：

其中，y_i为Sigma采样经过非线性函数传递后对应的点。根据加权统计线性回归技术，可以近似得到y的统计特性：

下面介绍两种经常使用的采样策略：比例采样和比例修正对称采样。

根据Sigma点采样策略不同，相应的Sigma点以及均值权值和方差权值也不尽相同，因此UT变换的估计精度也会有差异。但总体来说，其估计精度能够达到泰勒级数展开的二阶精度。

使用参数beta对高斯表示的附加的分布信息进行编码。如果分布是精确的高斯分布，则beta=2是最佳的选择。

其中11行，参考 Will：EKF公式推导中的证明的协方差的其他形式。

为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性，所以采样点的选取应满足：

式中{}内的符号分别为Sigma采样点，均值权值和方差权值组成的集合；L为采样点个数，P为随机变量x的密度函数，g[~]确定x的相关信息。如果密度函数P(x)只有一、二阶矩时，可以写成：

UKF相比于EKF的精度更高一些，其精度相当于二阶泰勒展开，但速度会略慢一点。UKF另一个巨大优势是不需要计算雅克比矩阵，而有些时候雅克比矩阵也确实的我们无法获得的。

另外UKF与PF(粒子滤波)也有相似之处，只是无迹变换中选择的粒子是明确的，而粒子滤波中的粒子是随机的。随机的好处是可以用于任意分布，但也具有其局限性。因此对于分布近似为高斯分布的，采用UKF将更有效。

卡尔曼滤波(KF)的对偶滤波算法就是信息滤波(Information Filter，IF)，形式和KF差不多。

其中，用信息向量 $\\varepsilon$ 和信息矩阵 $\\Omega$ 表示KF中的 $\\mu$ 和 $\\Sigma$ 。转换关系为： $\\varepsilon=\\Sigma^{-1}\\mu\\\\ \\Omega=\\Sigma^{-1}\\\\$

ESKF(error state Kalman filter)是Kalman滤波的一种特殊形式，采用ESKF的原因是由于对误差的线性化要比直接对函数进行线性化更加符合实际情况。如下图：

状态误差卡尔曼(ESKF)的应用，它是卡尔曼滤波器的变种中应用最为广泛的一种，与EKF一样，它也是一种针对时变系统的非线性滤波器。但是与EKF不同的是，它的线性化是总是在0附近，因此线性化更准确。绝大部分的场景，ESKF就足够使用了。如果对于滤波有更高的要求，可以选择UKF，甚至PF。

Error-state Kalman Filter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：

true state：实际状态，系统实际的运行状态
nominal state：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）
error state：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）

基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominal state和error state，然后再进行二者叠加。

对IMU数据进行积分，获得名义状态X，注意这个X并没有考虑噪声，所以必然引入了累计误差；
利用KF算法估计Error State，包括状态更新和测量更新，这个过程是考虑了噪声的，而且由于这个误差状态的方程式近似线性的，直接使用KF就可以；
将Error State加到Nominal State上，获取“真值”；
重置Error State，等待下一次更新；

Error State中的参数数量与运动自由度是相等的，避免了过参数化引起的协方差矩阵奇异的风险；
Error State总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性；
Error State总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速；
Error State的变化平缓，因此KF修正的频率不需要太高。

Will：ESKF公式推导

算法

Caliber：迭代扩展卡尔曼滤波(IEKF)

IEKF与高斯牛顿的等价性

IEKF将一个高斯牛顿问题，转换为EKF的方式，并通过迭代更新。
IEKF改进的是KF的后验更新公式，实际上就是就是高斯牛顿法，其中包含来预测和观测的约束。

游振兴：IEKF 和 Gaussian-Newton method 等价性证明

马氏范数和2-范数：Derek Lee：SLAM中的问题及个人思考汇总

AMCL理解（一）_wanghua609的博客-CSDN博客_amcl

卡尔曼滤波是递归的线性高斯系统最优估计。
EKF将NLNG系统在工作点附近近似为LG进行处理。
IEKF对工作点进行迭代。
UKF没有线性化近似，而是把sigma point进行非线性变换后再用高斯近似。
PF去掉高斯假设，以粒子作为采样点来描述分布。
优化方式同时考虑所有帧间约束，迭代线性化求解。

滤波方法（EKF）：

（1）滤波方法具有一阶马尔可夫假设，k时刻的状态只与k-1时刻相关。

（2）可以粗略认为EKF仅是优化中的一次迭代，除非是使用IKF/IEKF

（3）计算量小，实时性较好，容易积累误差。

优化方法（BA）：

（1）没有马尔可夫假设，更倾向于使用所有的历史数据。称为全体时间上的SLAM（Full-SLAM），计算量变大

（2）没有迭代次数的限制

（3）累计误差少。最理想的情况是回环检测之后，可以消除整个轨迹的误差。

（4）计算量大。不过目前基于雅各比和海森矩阵稀疏性的研究，使得计算资源的消耗大大降低。

下文采自：Sky Shaw：多传感器融合｜各类滤波器方法整理

卡尔曼滤波器：从k-1时刻后验推k时刻先验，从k时刻先验推k时刻后验（马尔可夫假设）；
扩展卡尔曼滤波器：对卡尔曼滤波器进行修正，针对不是线性的情况，采用一阶泰勒展开近似线性（马尔可夫假设）；
BA优化：把一路上的所有坐标点（像素坐标对应的空间点等）与位姿整体放在一起作为自变量进行非线性优化
PoseGraph优化：先通过一路递推方式算出的各点位姿，通过数学方式计算得到一个位姿的变换A，再通过单独拿出两张图像来算出一个位姿变换B，争取让B=A
增量因子图优化：保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，需要重新计算的再去计算，从而避免冗余计算。iSAM是增量的处理后端优化。由于机器人是运动的，不同的边和节点会被不断加入图中。每加入一个点，普通图优化是对整个图进行优化，所以比较麻烦和耗时。iSAM的话相当于是保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。以这种方式加速计算，避免冗余计算。

从全文的整理来看，特别是在IKF章节，我们可以发现滤波算法其实就是将Sliding Window的大小设置为1的优化算法，不论是优化算法还是滤波算法都是期望求解出问题概率模型的最大似然估计（MLE），本质上滤波就是基于马尔可夫假设将优化问题的建模进行了“特征化”的处理。再进一步分析，正因为滤波器进行了范围上维度的“简化”、模型的近似处理，所以其计算消耗较于优化算法更低，但由此引发的代价就是精度上的损失。另一个需要考虑的方面是，滤波器是由先验信息+运动模型+观测信息三个方面点顺序执行，以实现位姿状及其协方差的估计和更新，也正因为滤波器的框架如此，若是先验（上一时刻）状态出现了问题，比如位姿跟丢、计算错误等，那么在该时刻之后的状态都会出现问题以致纠正不回来了，而基于优化方法的位姿求解则会考虑更多时刻（关键帧）下的状态信息和观测信息，即使有某一时刻的状态量和协方差是outlier，系统也有一定的能力维稳。

如果在处理器算力充足且精度为第一需求的前提下，那么在位姿估算问题处理上是首推优化算法，但若是效率是第一前提条件，那么就需要根据实际的应用情况和机器人问题模型选择合适的滤波器了。

论文链接：https://lamor.fer.hr/images/50036607/2021-ajuric-comparison-mipro.pdf

作者：自动驾驶专栏 | 原文出处：公众号【自动驾驶专栏】

同时定位和建图（SLAM）是一种重要的工具，它使移动机器人能够在未知环境中自主地导航。正如SLAM这名称所示，其核心为获取环境的正确表示以及估计地图中机器人位姿的正确轨迹。主流的最先进的方法使用基于最小二乘法的图优化技术来求解位姿估计问题，其中最流行的方法为g2o、Ceres、GTSAM和SE-Sync等库。本文的目的是以统一的方式来描述这些方法，并且在一系列公开可用的合成位姿图数据集和真实世界的位姿图数据集上对它们进行评估。在评估实验中，对四个优化库的计算时间和目标函数值进行分析。

基于滤波的方法已经在SLAM场景中占据主导地位多年，其越来越多地被基于优化的方法所取代。位姿图优化（PGO）首次在Lu等人的工作（Globally Consistent Range Scan Alignment for Environment Mapping）中被引入，但是由于计算效率低的原因，它并不是非常流行。如今，随着计算能力的提高，PGO方法已经成为最先进的方法，并且能够快速且精确地求解SLAM优化和估计问题。

基于优化的SLAM方法通常由两部分组成。第一部分通过基于传感器数据的对应关系来识别新观测数据和地图之间的约束。第二部分计算给定约束情况下的机器人位姿和地图，这能够被分为图和平滑方法。当前最先进的基于优化方法的一个例子为g2o（A general framework for graph optimization），它是一种针对非线性最小二乘问题的通用优化框架。最初的平滑方法之一为 $\\sqrt{SAM}$ ，它由Dellaert等人提出（Square root SAM: Simultaneous localization and mapping via square root information smoothing）。一种对该方法的改进为增量式的平滑和建图（iSAM），它由Kaess等人引入（iSAM: Incremental smoothing and mapping）。iSAM对 $\\sqrt{SAM}$ 进行扩展以通过更新稀疏平滑信息矩阵的因子分解为整个SLAM问题提供一种有效的求解方式。iSAM的升级版本iSAM2由Kaess等人提出（ISAM2: Incremental smoothing and mapping using the Bayes tree）。这些平滑方法在GTSAM（https://gtsam. org）中实现，它是另一个最先进的优化库。

SLAM问题的一个很好的例子是所谓的位姿SLAM，它避免了构建环境的显式地图。位姿SLAM的目标是要在给定回环和里程计约束的情况下估计机器人的轨迹。这些相对的位姿测量通常使用自身运动估计、扫描匹配、迭代最近点（ICP）或者一些最小化视觉重投影误差的形式，从IMU、激光传感器、相机或者轮式里程计中获取。值得注意的是，Eustice等人和Lenac等人提出的方法为基于滤波的位姿SLAM算法，但是本文的关注点为实现基于优化的位姿SLAM的方法。

最流行的优化方法有g2o、Ceres Solver（http:// http://ceres-solver.org）、GTSAM和SE-Sync（特殊欧式群SE(n)上的同步）（SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group）。文献中很少有比较这些方法的工作。例如，Latif等人提供了视觉SLAM的概述，并且将g2o、GTSAM和HOG-Man作为后端进行比较。Carlone等人讨论位姿图估计中旋转估计的重要性，并且在基准数据集上将g2o和GTSAM与不同的旋转估计技术进行比较。Li等人在g2o框架下比较不同的优化算法。Zhao等人提出统一的SLAM框架——GLSLAM，它提供了各种SLAM算法的实现，还能够对不同的SLAM方法进行基准测试。然而，据我们所知，目前没有一种统一的方式来比较g2o、Ceres、GTSAM和SE-Sync。本文的目的为以统一的方式来描述这些方法，并且在一系列公开可用的合成位姿图数据集和真实世界位姿图数据集上对它们进行评估，如下图所示。

在未来，我们想要通过这种比较来促进PGO方法的选择。

本文组织如下。第二节描述通用的非线性图优化以及这四种方法中的每一种。实验是本文的主要部分，在第三节中进行描述，在该节的第一部分，描述了硬件、实验设备和基准数据集。在该节末尾讨论了实验的结果。最后，第四节对本文进行总结。

每个位姿图由节点和边组成。位姿图中的节点对应于环境中机器人的位姿，以及边表示它们之间的空间约束。相邻节点间的边为里程计约束，以及其余边表示回环约束。这在下图图（a）中可视化。

位姿图优化的目标是要寻找一种节点的配置，使得位姿图中所有约束的最小二乘误差最小。一般而言，非线性最小二乘优化问题能够被定义如下： $x^{*}=\緻set{x}{argmax}F(x) \ ag{1}$ 其中， $F(x)$ 为图中所有约束上的误差总和：

$F(x)=\\sum_{<i,j> \\in \\mathcal C}e^T_{ij}\\Omega_{ij}e_{ij}\ ag{2}$ 这里， $\\mathcal C$ 表示连接节点间索引对的集合， $\\Omega_{ij}$ 表示节点 $i$ 和节点 $j$ 之间的信息矩阵， $e_{ij}$ 为非线性误差函数，它建模位姿 $x_i$ 和 $x_j$ 满足由测量 $z_{ij}$ 施加的约束的程度。最后，每个约束用信息矩阵 $\\Omega_{ij}$ 和误差函数 $e_{ij}$ 进行建模，如下图图（b）所示。

传统上，公式（1）的解通过迭代优化技术（例如，高斯-牛顿或者莱文贝格－马夸特方法）获取。他们的思想是用误差函数在当前初始估计附近的一阶泰勒展开来近似该误差函数。一般而言，它们由四个主要步骤组成：

1）固定一个初始估计值；

2）将问题近似为凸问题；

3）求解步骤2）并且把求解结果作为新的初始估计值；

4）重复步骤2）直到收敛。

位姿SLAM更容易求解，因为它没有构建环境的地图。由图构建的问题具有稀疏的结构，所以计算速度更快，另一个优势在于它对糟糕的初始估计值具有鲁棒性。位姿SLAM的缺陷在于它通常对异常值不鲁棒，并且当存在很多错误回环时无法收敛。此外，旋转估计使得它成为一个困难的非凸优化问题，因此凸松弛导致问题具有局部极小解，并且无法保证全局最优。在本节中，我们简要地描述基于非线性最小二乘方法的优化框架，这些框架以位姿图的形式提供解决方案。

A.g2o

g2o是一个开源的通用框架，用于优化能够被定义为图的非线性函数。它的优势在于易于扩展、高效并且适用于广泛的问题。作者在他们的工作中指出，他们的系统与其它最先进的SLAM算法相比，同时具有高度通用性和可扩展性。他们通过利用图的稀疏连接和特殊结构、使用改进的方法来求解稀疏线性系统并且利用现代处理器的特性，从而实现高效性。该框架包含三种不同的方法来求解PGO：Gauss-Newton、Levenberg-Marquardt和Powell的Dogleg方法。框架主要用于求解机器人中的SLAM问题和计算机视觉中的bundle adjustment问题。ORB-SLAM使用g2o作为相机位姿优化的后端，SVO将它用于视觉里程计。

B.Ceres

Ceres Solver是一个开源的C++库，用于建模并且求解大规模、复杂的优化问题。它主要致力于求解非线性最小二乘问题（bundle adjustment和SLAM），但是也能够求解通用的无约束优化问题。该框架容易使用、移植并且被广泛优化以提供具有低计算时间的求解质量。Ceres被设计用于支持使用者定义并修改目标函数和优化求解器。实现的求解器包括置信域求解器（Levenberg-Marquardt，Powell的Dogleg）和线搜索求解器。由于Ceres具有很多优势，因此它被用于很多应用和领域。OKVIS和VINS使用Ceres来优化定义为图的非线性问题。

C.GTSAM

GTSAM是另一个开源的C++库，它实现了针对机器人和计算机视觉应用的传感器融合。GTSAM能够被用于求解SLAM、视觉里程计和由运动恢复结构（SfM）中的优化问题，它使用因子图来建模复杂的估计问题，并且利用它们的稀疏性来提高计算效率。GTSAM实现Levenberg-Marquardt和Gauss-Newton风格的优化器、共轭梯度优化器、Dogleg和iSAM（增量式平滑和建图）。GTSAM与学术界和工业界中各种传感器前端一起使用。例如，SVO的一种变体（On-Manifold Preintegration for Real-Time Visual-Inertial Odometry）使用GTSAM作为视觉里程计的后端。

D.SE-Sync

SE-Sync是一种被证实正确的算法，用于在特殊欧式群上执行同步。SE-Sync的目标是在给定节点间相对变化的噪声测量情况下估计一组未知的位姿（欧氏空间中位置和姿态）值，它的主要应用是在2D和3D几何估计，例如位姿图SLAM（机器人中）、相机运动估计（计算机视觉中）和传感器网络定位（分布式感知中）。作者在其工作中指出，SE-Sync通过利用一种新型的特殊欧式同步问题的（凸）半正定松弛来直接搜索全局最优的解以改进之前的方法，并且能够对于找到的解生成正确性的计算证明。他们应用截断的牛顿黎曼置信域方法来寻找位姿的高效估计。

本文的目标是要通过实验评估上一节中描述的优化框架，并且比较它们。出于这个目的，本文在总计算时间和目标函数的最终值（由公式（2）描述）方面考虑它们的性能。本文使用公开可用的合成基准数据集和真实世界的基准数据集。

A.实验设备

实验是在配备有八核Intel Core i7-6700HQ CPU、2.60GHz运行频率和16GB内存的Lenovo ThinkPad P50上进行的。该计算机运行Ubuntu 20.04系统。g2o、Ceres和GTSAM框架选择相同的求解器Levenberg-Marquardt，而SE-Sync使用黎曼置信域（RTR）方法。每种算法限制最大迭代100次。停止条件基于达到最大迭代次数和相对误差减少。SE-Sync使用一种稍微不同的方法，所以必须指定一种额外的基于黎曼梯度范数的条件。相对误差减少的公差范围被设置为 $10^{-5}$ ，以及梯度的范数被设置为 $10^{-2}$ 。根据SE-Sync工作中的建议来选择参数。Carlone等人研究姿态初始化对于寻找全局最优的影响（Initialization techniques for 3D SLAM: A survey on rotation estimation and its use in pose graph optimization）。受到该工作的启发，我们还在优化过程之前进行位姿图初始化。为了取得更好的结果，我们使用生成树方法来获取初始的位姿图。

B.基准数据集

Carlone等人获取六个两维的位姿图，包括两个真实世界的位姿图和四个在仿真中创建的位姿图。INTEL和MIT位姿图是真实世界的数据集，它们通过处理在西雅图Intel Research Lab和MIT Killian Court采集的原始轮式里程计和激光雷达传感器测量数据来创建。M3500位姿图是一个仿真的曼哈顿世界（Fast iterative alignment of pose graphs with poor initial estimates）。M3500a、M3500b和M3500c数据集是M3500数据集的变体，它们在相对姿态测量数据中加入高斯噪声。噪声的标准偏差分别为0.1、0.2和0.3弧度。Carlone等人还获取六个三维的数据集。位姿图Sphere-a、Torus和Cube在仿真中创建，其中Sphere-a数据集是由Stachniss等人发布的一个有挑战性的问题（http://www.openslam.org/）。其它三个位姿图数据集是真实世界的数据集。Kummerle等人引入Garage数据集（Autonomous driving in a multi-level parking structure），Cubicle和Rim数据集是通过在来自佐治亚理工学院RIM中心的3D激光传感器的点云上使用ICP方法来获取的。所有这些位姿图在本文第一张图中可视化，图中显示它们的里程计和回环约束。表格I中包含每个数据集的节点和边的数量。这些数量确定了优化过程中参数的数量和问题的复杂度。

C.结果

本文对于总计算时间和目标函数值（公式（2））方面在表格II中总结了所有的性能结果。对于每一个数据集，我们指出算法的终止原因。如果算法在最大迭代次数的限制内完成优化，则该算法收敛。

我们还给出SE-Sync的验证时间以证明全局最优。下面三张图展示针对基准测试问题的优化后的位姿图。

1）INTEL：INTEL是最易求解的问题之一。所有方法均已经成功对其进行求解。所有轨迹显示在下图图（a）中，并且由图可知它们均获得了相似的结果。GTSAM耗费最长的时间来完成优化，但是取得最低的目标函数值。在这种情况下，SE-Sync速度最快，并且其目标函数值略大于GTSAM。考虑到这一点，SE-Sync似乎是最好的求解方案。

2）MIT：MIT是最小的问题，但是仅具有一些回环约束。因此，从一个好的初始估计值开始优化是非常重要的。否则，GTSAM、Ceres和g2o无法通过Levenberg-Marquardt算法收敛到一个有意义的解。所有算法在最大迭代次数内收敛，并且它们获得几乎相同的目标函数值。优化问题在半秒内求解，并且Ceres和g2o速度最快。Ceres还获得最低的目标函数值，因此似乎是数据集MIT的最佳求解器。最终的轨迹如下图图（b）所示。

3）M3500：M3500的所有四种变体均在这里展示。GTSAM和Ceres在求解基础的M3500问题中表现最佳，因为它们均取得最低的目标函数值，但是Ceres速度稍微更快。所有方法都能成功求解该问题，并且它们优化后的位姿图如下图图（c）所示。M3500a是一个更为困难的问题，因为它将噪声加入到相对姿态中。尽管如此，所有方法均能求解它，但是如下图图（d）所示，g2o解决方案偏离了其它的解决方案。Ceres和GTSAM收敛到最低的目标函数值，但是Ceres再次速度更快。

M3500b和M3500c是非常有挑战性的问题，因为其噪声量很高。GTSAM在这两种情况下没有收敛，并且Ceres和g2o陷入局部极小值。SE-Sync是求解这个问题两种变体的最为成功的方法。考虑到缺少连接位姿图左边部分和右边部分的回环约束以及测量中的噪声量，SE-Sync已经实现相当好的解决方案。这些位姿图如下图图（a）和下图图（b）所示。

4）Sphere-a：Sphere-a问题也具有挑战性，因为大量噪声被加入到相对姿态测量数据中。即使给出一个初始估计值，仅SE-Sync能够求解它，它仅在0.15s内收敛到全局最优，比g2o速度快50倍并且比Ceres和GTSAM速度快100倍。由SE-Sync获得的最优解如上图图（c）所示，而其它三种方法获得局部最优解。

5）Cubicle和Rim：Cubicle数据集是Rim数据集的一个子集，所以本文一起讨论它们。这两个数据集具有挑战性，因为它们都包含大量的节点和边。此外，大量的边为错误的回环。仅GTSAM和SE-Sync在这种情况下收敛到一个解。优化后的位姿图如上图图（d）和上图图（e）所示。Ceres和g2o无法找到一个解。GTSAM求解Cubicle的速度比SE-Sync快两倍，但是SE-Sync收敛到全局最优。SE-Sync优化Rim的速度比GTSAM快五倍，并且收敛到全局最优。

6）Torus：Torus是最容易的3D问题之一。除了g2o之外的所有方法均收敛到全局最优，但是SE-Sync再次在速度方面获胜。虽然g2o解决了该问题，但是由下图图（a）所示，可以看出它优化后的轨迹与其它三种方法相比略有漂移。

7）Cube：Cube数据集更为复杂并且耗时，这是由于它包含大量的节点和边，但是所有方法均找到一个解。SE-Sync是迄今为止求解该问题速度最快的方法，它耗费8秒，而其它的方法耗费超过一分钟。最终的解如下图图（b）所示。

8）Garage：Garage数据集是最小且最容易求解的3D数据集，每种方法均能获得一个解，如下图图（c）所示。Ceres、GTSAM和SE-Sync收敛到相同的目标函数值，GTSAM在这种情况下速度最快。

在本文中，我们比较用于SLAM中位姿估计的图优化方法。我们考虑g2o和GTSAM（它们是当前最先进的方法）、一个用户友好的开源框架Ceres和一个用于位姿同步的新型且鲁棒的方法SE-Sync。评估过程考虑了耗费的优化时间和目标函数值，12个基准数据集的结果以表格的形式给出。

当与其它三种方法比较时，SE-Sync在大多数数据集上取得最少的总耗时。如果需要，它还能够验证全局最优，但是以额外的计算时间为代价。g2o具有最高的总耗时，但是在简单的2D数据集上表现良好。Ceres易于使用，提供很多的灵活性，并且速度相对较快。GTSAM表现与SE-Sync几乎一样好，除了在噪声非常多的2D数据集上。搭配一个正确的前端，这些方法在解决SLAM问题中能够非常强大。对于较差的数据关联、高噪声和一个表现较差的前端，似乎最好使用SE-Sync作为后端。GTSAM具有好的初始化方法，因此它似乎也表现相当不错。如果前端非常出色、数据集相对简单或者噪声较低，那么在这些后端中做出选择是个人偏好的问题。我们希望这种比较能够帮助其他研究者为他们的SLAM应用选择一种后端方法。

G2o在很多环境中都能使用， C++,python,android,java等。

由于orb_slam是用C++写的，所以使用了g2o的C++版本。

不过，无论哪种版本，基本使用逻辑都相同。

配置图的节点，配置图的边，配置所使用的优化算法
调用优化函数进行优化

https://github.com/RainerKuemmerle/g2o/tree/pymem/python

看一个例子：https://github.com/RainerKuemmerle/g2o/blob/pymem/python/examples/ba_demo.py

本例子中，直接用g2o的SparseOptimazer来解决BA问题：

95行，生成一条边edge，边的功能是将3维点映射到图像坐标

96行，将边一个顶点设置为机器人位姿，即待优化求解的第一个量。

97行，将边的另一个顶点设置为三维点，即待求解的第二个量。

98行，将边的测量值设置为z,这个测量值，就是关键点的坐标。

我们用边的一个顶点机器人位姿把边的另一个顶点：3维点坐标映射到图片上的p点，计算p距离点z的距离做为误差。

所以z,称为测量值。

我们的目标就是求取合适的顶点值，即求取合适的机器人位姿和3维点，使p距离z点的距离近量接近0。

104行，将边加入到优化器中。

118行，利用算法进行优化，此句运行完，将会得到合适位姿和三维点。

120行至123行，计算投影误差。

也可以把SparseOptimazer直接封装为一个BA类：

其中，add_pose函数：把机器人位姿做为一种顶点；add_point：把三维点做为另一种顶点；add_edge：定义pose顶点和point顶点之间的投影关系，并提供真实二维点的坐标，即用ORB特征求得的关键点的坐标，即measurement, projection的measurement.

C++版本：

这个版本我们直接看ORB_SLAM中的调用方式。

我们看这个源码https://gitee.com/anjiang2020_admin/ORB_SLAM/blob/master/src/Optimizer.cc的第154行，PoseOptimization函数

设置机器人位姿为一种类型的顶点：帧顶点:VertexSE3Expmap，setFixed(false),不固定，可以改变的量。setId(0),顶点编号为0.

设置三维点为另一种类型的顶点：三维点顶点VertexSBAPointXYZ, setFixed(true),固定，三维点坐标不变。setId(I+1),顶点编号为：1，2，3，。。。N

设置边，边的类型为：映射，即映射三维点的边， EdgeSE3ProjectXYZ

这里固定，就是已知量。不固定对应待优化的量。

213行，把三维点顶点设置为边的0位值顶点。

214行，把帧顶点设置为边的1位值顶点。

215行，把obs设置为边的测量值，即三维点的映射目标点。

Obs则是关键点去畸变后的坐标，如下：

配置完成后，就可以调用optimize(10),来进行优化了。词句写法与python写法一样。

下面，我们看一下PoseOptimizer函数的内是如何使用optimize的。

249行，for循环内使用了4次optimize,分别是optimize(10),optimize(10),optimize(7),optimize(5)

其实使用一次就能够得到机器人位姿了，为什么使用了4次呢？

因为第274行，我们希望优化的停止条件位为：误差小于某个值的边,这类边的数量，刚好小于10.

为了达到这个目的，我们进行了4次优化，并且这4次优化后，可允许误差从9.210变化到了 5.911.

可允许误差的减小，意味着对优化结果要求越来越严格，满足要求的边的数量会减少。

282行，将最终的优化结果复制给pFrame->mTcw.

最近在用ceres写图优化的时候，对信息矩阵这个东西简单粗暴的忽视掉了，这并不是一个很nice的操作。最近看vins-mono的时候，又想到了这个事情，干脆好好从头梳理一遍，把过去学习的坑给补上。

简单回顾一下最小二乘法，考虑简单的最小二乘问题： $\\min_{x}F(x)=\\frac{1}{2}\\left \\| f(x) \\right \\| ^2_2\\\\$

对 $F(x)$ 关于 $\ riangle x$ 做泰勒展开：

$F(x_k+\ riangle x_k) \\approx F(x_k)+J(X_k)^T\ riangle x_k +\\frac{1}{2}\ riangle x_k^TH(x_k)\ riangle x_K\\\\$ 一阶梯度法(最速下降法)：

先对一阶展开求导，然后取增量为反向的梯度

$\ riangle x^*=-\\lambda J(x_k) \\\\$ 二阶梯度(牛顿法) :

对二阶展开求导，然后令导数等于0，增量的最佳位置在极值点：

对 $f(x)$ 进行一阶泰勒展开，那么原式就变为：

$F(x+\ riangle x)=\\frac{1}{2}\\left \\| f(x)+J(x)^T\ riangle x\\right\\|^2\\\\$ 把函数二次项展开进行关于 $\ riangle x$ 求导，再令导数为零，求解得：

$J(x)f(x)+J(x)J^T(x)\ riangle x=0\\\\$ 然后左边一项移到右边，定义一下 $H$ 和 $g$ 那么就有了增量方程：

$H(x)\ riangle x=g(x)\\\\H(x)=J(x)J^T(x)\\\\g(x)=-J(x)f(x)\\\\$ 求解增量方程就可以得到增量的解。

先来看一个图优化问题：

当然我们可以很简单粗暴的构建最小二乘问题 $x=\\arg\\min \\left \\| e_{1,2}\\right \\|^2+ \\left \\| e_{2,3}\\right \\|^2+ \\left \\| e_{3,4}\\right \\|^2+ \\left \\| e_{1,4}\\right \\|^2\\\\$

上式中 $e_{i,j}$ 表示节点之间由测量和观测产生的误差。如果这样去构建最小二乘，会出现一个问题，所有的边都会被以同样的标准去优化，比如说： $x_1$ 和 $x_2$ 之间的观测，在测量时的噪声较大，精度不高，所以我们希望这个边在约束时能够适当减少约束强度。比较容易想的做法就是在各个误差前加入一个权重：

$x=\\arg\\min \\alpha_1\\left \\| e_{1,2}\\right \\|^2+ \\alpha_2\\left \\| e_{2,3}\\right \\|^2+ \\alpha_3\\left \\| e_{3,4}\\right \\|^2+ \\alpha_4\\left \\| e_{1,4}\\right \\|^2\\\\$ 这就稍微有了一些方差的雏形了，如何去确定各个误差的权重，甚至是各个误差的各个纬度的权重呢？当然是协方差矩阵，众所周知，测量的结果方差越大，测量就越不准。那么将协方差矩阵的逆，即信息矩阵乘到各个误差的各个纬度上，就可以得到一个比较nice的约束： $x=\\arg\\min \\left \\| e_{1,2}\\right \\|^2_{\\Sigma^{-1}_1}+ \\left \\| e_{2,3}\\right \\|^2_{\\Sigma^{-1}_2}+ \\left \\| e_{3,4}\\right \\|^2_{\\Sigma^{-1}_3}+ \\left \\| e_{1,4}\\right \\|^2_{\\Sigma^{-1}_4}\\\\$

那么稍作整理就可以写出(知乎的公式有点问题，加和符号的上下标就不写了)：

$x=\\arg\\min\\sum \\left \\| e_i\\right \\|^2_{\\Sigma^{-1}_i}\\\\$ 接下来从最大后验估计的角度再证明一遍这个公式，因为上面我们只是从经验的方面来思考，现在我们从更优雅的数学模型上去思考这个问题(细节方面，参见视觉SLAM十四讲123页)

假设任意高维高斯分布 $x \\sim N(\\mu ,\\Sigma )$ 那么可得它的概率密度展开形式： $P(\\boldsymbol{x})=\\frac{1}{\\sqrt{(2 \\pi)^{N}\\operatorname{det}(\\boldsymbol{\\Sigma})}}\\exp \\left(-\\frac{1}{2}(\\boldsymbol{x}-\\boldsymbol{\\mu})^{\\mathrm{T}}\\boldsymbol{\\Sigma}^{-1}(\\boldsymbol{x}-\\boldsymbol{\\mu})\\right)\\\\$ 取负对数： $-\\ln (P(\\boldsymbol{x}))=\\frac{1}{2}\\ln \\left((2 \\pi)^{N}\\operatorname{det}(\\boldsymbol{\\Sigma})\\right)+\\frac{1}{2}(\\boldsymbol{x}-\\boldsymbol{\\mu})^{\\mathrm{T}}\\boldsymbol{\\Sigma}^{-1}(\\boldsymbol{x}-\\boldsymbol{\\mu}) \\\\$ 可以发现，右边第一项与 $x$ 无关，所以要想概率更大，那么右边第二项就要更小，所以对于图优化中的二元边我们如下建模： $r_{i,j}=h(x_i,x_j)+v_{i,j}\\\\ v_{i,j}\\sim N(0,\\Sigma_{i,j})\\\\ P(r_{i,j}|x_i,x_j)=N(h,\\Sigma_{i,j})\\\\$ $r_{i,j}$ 表示边的约束，而 $x_i,x_j$ 应该满足这个约束，再根据前面的理论再求最大似然概率：

$x_i,x_j=\\arg\\min((r_{i,j}-h)^T\\Sigma^{-1}_{i,j}(r_{i,j}-h))=\\arg\\min\\left\\| r_{i,j}-h\\right\\|^2_{\\Sigma_{i,j}^{-1}}\\\\$

这就可以看出来，前文所说的 $e$ 实际上是 $r-h$ 。

imu，GNSS还是比较好获得的，imu姿态递推时，方差一并跟着递推就可以了。GNSS的每帧数据都会包含方差来表示这帧定位数据的好坏。

那么点云呢？(纯个人瞎扯淡环节，参考参考就可以了) 基于EKF的里里程计，方差也会跟随着递推，但是像Fast-lio这类激光里程计中，帧间匹配计算误差时，这个方差被设为了定值。可能会觉得激光还是比较准的吧。也许我们可以利用匹配的误差来作为方差？科学原理是怎样的呢？不知道，但是先去睡觉了，留着以后再说吧。

LIOSAM其中的两大模块imuPreintegration和mapOptimization都依赖了因子图优化框架——GTSAM。为了确保本项目的自洽性以及后面章节讲解的展开，不介绍一下因子图后端优化算法似乎说不过去。但是笔者目前对因子图的原理还没有进行非常详细的推导，因此这部分内容会偏向如何对GTSAM快速入门。当然，最快速的入门方式就是完整地阅读一遍GTSAM的官方教程。这部分章节只是拙劣地进行知识的搬迁，当然不是直接将 GTSAM的教程直接翻译过来。本章节会介绍GTSAM快速入门的知识，同时讲解GTSAM提供的examples中跟LIOSAM联系较为紧密的一两个，同时指明LIOSAM中的用法出处。至少阅读完本章节后应该能够对因子图有基本的认识，并且能够看懂LIOSAM中对GTSAM的使用

让我们先回归问题本身。在SLAM中，我们可以拥有多个输入，我们的最终目的是获取一条最准确的轨迹，然后使用这条准确的轨迹实时构建地图。在LIOSAM中，我们有以下输入： 1. IMU数据； 2. 点云数据； 3. GPS数据（可选）； 4. 回环检测；

我们的问题是如何使用这些数据计算出一条最准确的历史轨迹，使用该轨迹就可以将对应的点云投影到地图坐标系，以此来建立一个点云格式地图。 PS：这只是SLAM的一个形式，就是优化出一条光滑准确的轨迹，这也是在LIOSAM中使用的方法，因此本文以这种类型作为讲解。

在SLAM领域后端优化的一个分支是滤波器方法，主要以扩展卡尔曼滤波方法为主，采用隐马尔科夫假设，只保存上一次状态，并通过当前输入预测当前状态，偶尔通过观测修正状态。具体可见本博客的卡尔曼滤波相关文章。

目前的后端优化主要以各种非线性优化方法为主，把批量的位姿加入目标方程（projective function）一起优化，得到具有更好的全局性质的解。 $\\mathbf{x}^*=\\argmin_{\\mathbf{x}}\\mathbb{F}(\\mathbf{x})$ 其中，$\\mathbb{F}$是历史轨迹引入的各个误差之和 $\\mathbb{F}(\\mathbf{x})=\\sum\\mathbf{e}(x_i,c_{ij},x_j)$ $x_i,x_j$是历史时刻的位姿，$c_{ij}$是两者之间的约束（可以认为是观测或者输入）

PS：再重申一句，SLAM后端体系很庞大，不是一篇文章能说清楚的，笔者目前也还没有这个能力。目前只求能够稍微讲清楚GTSAM在LIOSAM中是怎么被使用的即可。

为了求解非线性方程在不同的SLAM任务中的特点，又发展出各种不同的优化方式，比如VIO中有Bundle Adjustment，有其他叫PoseGraph的，还有GTSAM中使用的FactorGraph。简单理解就是这些都是非线性方程和求解在不同的任务场景中的细分。后面有机会再单独列一篇文章讲解这些后端优化方法把。

以下内容大量引用自GTSAM-repo/doc/gtsam.pdf

为了批量优化历史位姿$\\mathbf{x}={x_0, x_1, x_2, ... x_i}$，研究者将图论引入SLAM后端优化。每一个机器的位姿$x_i$是图中的顶点；通过各种里程计计算，比如连续位姿间的IMU积分结果或者点云匹配结果，成为两个位姿$x_i,x_j$之间的边；对于一些额外的观测，比如GPS数据，成为连接顶点的一元边（只有一端连接到顶点）。比如下图(引用自：GTSAM-Hands-on Introduction)

GTSAM使用因子图（FactorGraph）作为通用的图问题描述框架，并为各种机器人任务提供实现好的优化算法API，使得研究者快速开发后端优化框架。

图优化（GraphOptimization）vs. GTSAM中使用的因子图优化（FactorGraph Optimization）区别 vs. BA

图优化似乎一般指位姿图（PoseGraph）的优化，图中的顶点都是机器人的位姿
BA(Bundle Adjustment)似乎一般指在视觉SLAM中对地标（Landmarks）和位姿同时优化的方法
GTSAM的因子图除了能够构建位姿图之外，还可以将速度、偏差、IMU预积分等当作顶点和边加入图中。另外，GTSAM框架实现了iSAM和iSAM2两个递增式优化器

变量（variables）：因子图中的每一个顶点是一个代求的变量。假设我们只需要求解每一个时刻的机器人姿态，那么每个顶点就是该时刻的机器人位姿，如上图中的 ${X_1,X_2,X_3}$

值（values）：值是每个变量数值。在调用优化器对因子图做优化时，我们要先为每个变量设置初始值，优化结束后再从优化器中拿出每个变量优化后的值。

因子（factors）：因子是因子图中的边，每条边都可以视为一个因子，每个因子又可以认为是一个约束。比如连续两个位姿之间可以由IMU计算出两个位姿的变换，该变换作为一条边加入因子图连接两个变量（二元因子：binary factor）；又或者在某个时刻有GPS数据输入，GPS数据是一个观测，也可以作为一个因子加入因子图，这时候GPS的边只有一端连接到变量（一元因子：unary factor）。

因子图（factor graph）：因子图由顶点（变量）和因子（边）构成。

优化器（Optimizer）：因子图只是建模了SLAM的历史位姿和输入与观测间的关系，如何求解这个因子图也就是如何设置变量使得整个图最符合所有的约束（误差最小）则需要用到优化器。除了最常见的求解非线性问题的Gaussian-Newton和Levenberg-Marquardt优化器之外，GTSAM还实现了两个增量式优化器iSAM,iSAM2

键（Keys）：往因子图添加因子需要指定该因子连接到哪些变量。GTSAM中使用键来指明。一元因子需要指明其连接到第几个变量；二元因子需要指明其连接到哪两个变量。因子图中每个变量的键都应该是唯一的。为了方便在多种变量类型的情况下指明键，GTSAM提供Symbols来让用户方便生成不同变量类型的键值。（键不一定是连续的，但必须是唯一的）

本小节引用自GTSAM-Hands-on Introduction—第2章以构建并求解下面因子图为例

图中$x_1,x_2,x_3$为机器人三个时刻的位姿；$f_0$为第一个时刻机器人位姿的观测；$f_1,f_2$为时刻1-2和时刻2-3之间的里程计估计。代码

// 构建一个空的非线性因子图
NonlinearFactorGraph graph;

// 构建先验因子，也就是图中的f_0
// 这里使用二维姿态（x,y,theta）简化问题
Pose2 priorMean(0.0, 0.0, 0.0);
// 高斯噪声，代表我们对该因子的不确定性
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));

// 将先验因子加入因子图
// 其中的1表示该因子连接到第1个变量
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));

// 构建里程计因子，也就是图中的f_1,f_2
// 往前移动2米，y轴不便，theta不变
Pose2 odometry(2.0, 0.0, 0.0);
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

// 将里程计因子加入因子图
// 1,2代表该里程计约束是从变量1到变量2
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
// 添加相同的因子到变量2和变量3之间
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));


// 设置各个变量的初始值
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));

// 调用优化器并使用设定好的初始值对因子图优化
Values result = LevenberMarquardtOptimizer(graph, initial).optimize();

在目前的SLAM，无论是VIO还是LIO，前端都需要IMU做积分得到一个位姿的初始估计。GTSAM中实现了一个IMU预积分算法，并可以将其直接加入因子图中做优化。这在LIOSAM中也被使用到。本例子引用自GTSAM-repo/examples/ImuFactorExample2.cpp，为了说明GTSAM中IMU预积分器及ImuFactor的用法，对该例子进行简化。最后构建出的因子图如下（为了可视化，人工调整了布局）：

// 伪代码，完整代码见下文
// ---------------------------------------------------------

// IMU 预积分器
// 设置预积分器的噪声参数，这些噪声参数一般来自IMU的离线标定结果
auto params = PreintegrationParams::MakeSharedU(kGravity);
params->setAccelerometerCovariance(I_3x3 * 0.1);
params->setGyroscopeCovariance(I_3x3 * 0.1);
params->setIntegrationCovariance(I_3x3 * 0.1);
params->setUse2ndOrderCoriolis(false);
params->setOmegaCoriolis(Vector3(0, 0, 0));
// 设置IMU预积分器
PreintegratedImuMeasurements accum(params);

// 设置一个非线性因子图
NonlinearFactorGraph newgraph;

// 声明因子图的初始值和优化结果
Values initialEstimate;

// 设置位姿的先验因子
// 对应到上图中连接到变量x0的一元因子
// 噪声设置为roll、pitch、yaw有0.1rad的标准差，x，y，z有30cm的标准差
auto noise = noiseModel::Diagonal::Sigmas(
    (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
// 将位姿的先验因子加入因子图中
newgraph.addPrior(X(0), pose_0, noise);
// 将位姿0的初始值加入初始估计集合
initialEstimate.insert(X(0), pose_0);

// 设置IMU偏差的先验因子
// 对应到上图中连接到变量b0的一元因子
Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
// 将IMU偏差b0的初始值加入初始估计集合
initialEstimate.insert(biasKey, imuBias::ConstantBias());

// 设置速度的先验因子
// 对应到上图中连接到变量v0的一元因子
// 这里设置初始速度为0：Vector3(0,0,0)
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
newgraph.addPrior(V(0), Vector3(0,0,0), velnoise);
// 将速度v0的初始值加入初始估计集合
initialEstimate.insert(V(0), Vector3(0,0,0));


// 对于机器人的所有位姿时刻（关键帧）
for (i in N_steps)
{
  biasKey++;
  Symbol b1 = biasKey - 1;
  Symbol b2 = biasKey;

  // 设置i时刻的位姿初始值
  initialEstimate.insert(X(i), pose_i);

  // 对于两个时刻之间的所有IMU数据进行积分
  for (j in N_imu_queue)
  {
    accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);

  // 使用预积分器构建IMU因子，并加入因子图中
  // IMU因子需要指定本次积分的过程连接了哪两个位姿X，速度V以及当前的偏差估计，因此也叫`5-way factor`
  ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
  newgraph.add(imufac);

  // 添加两个时刻之间IMU偏差因子
  auto f = std::make_shared<BetweenFactor<imuBias::ConstantBias> >(
            b1, b2, imuBias::ConstantBias(), cov);
  newgraph.add(f);
  initialEstimate.insert(biasKey, imuBias::ConstantBias());

  // 将速度的初始值加入初始值集合，注意，这里加入的是GroundTruth的速度，只是为了做示例
  initialEstimate.insert(V(i), linear_velocity_vector);

  // 重值IMU预积分器
  accum.resetIntegration();
  }
}
// 调用LM优化器对因子图进行优化并获取优化结果
result = LevenbergMarquardtOptimizer(newgraph, initialEstimate).optimize();

完整代码请见项目博客：

https://zeal-up.github.io/2023/03/27/lio_sam/gtsam-quickstart/

GTSAM不仅内置了Gaussian-Newton和Leverberg-Marquat两个常见的非线性优化器，还是实现了iSAM和iSAM2两个递增式的优化器。递增式优化器更符合SLAM中常见的使用方式，也就是持续加入新的位姿和约束，并持续进行优化。

这部分内容可以参考GTSAM-Hands-on Introduction——第7章

example/VisualISAM2Example.cpp

// 设置iSAM2优化器
ISAM2 isam(parameters);

// 设置因子图和初始值
NonlinearFactorGraph graph;
Values initialEstimate;

for (size_t i = 1; i < poses.size(); ++i)
{
  for (size_t j = 0; j < relations.size(); ++i)
  {
    // 添加因子
    graph.add(BetweenFactor<Pose3>(i, j))
  }

  // 添加初始值到初始估计集合
  initialEstimate.insert(Symbol('x',i), initial_x[i]);

  // 将因子图加入优化器
  isam.update(graph, initialEstimate);

  // 清空当前因子图和初始值
  // 因子图已经加入优化器，因此需要清空为下一次因子图做准备
  graph.resize(0);
  initialEstimate.clear();
}

GTSAM对整个SLAM后端问题都封装得很好，因此可以让我们在不了解优化细节的情况下也可以构建自己的优化算法。除了对后端优化和图优化这些知识掌握外，GTSAM的学习主要还是要学以致用，这里只是简单列出笔者觉得比较合适的资料阅读和学习顺序。

本文
GTSAM仓库下doc文件夹中的gtsam.pdf
GTSAM仓库下examples文件夹中的一些例子，主要是对照上面的文档以及本文出现的例子进行阅读运行
本项目博客中关于ImuPreintegration和MapOptimization的讲解
本项目代码spl中src/mapOptimization.cpp和src/imuPreintegration.cpp代码

对目前各个图优化框架（g2o,GTSAM,ceres,SE-Sync）等做统一分析的综述性论文（强烈建议）

A Comparison of Graph OptimizationApproaches for Pose Estimation in SLAM

2. Introduction to Bundle Adjustment, ppt

Bundle Adjustment

3. 对基于图的SLAM后端优化算法的讲解，对SLAM的后端问题做比较正式的描述，接近2000引用

A Tutorial on Graph-Based SLAM

@inproceedings{grisetti2011g2o,
  title={g2o: A general framework for (hyper) graph optimization},
  author={Grisetti, Giorgio and K{\\"u}mmerle, Rainer and Strasdat, Hauke and Konolige, Kurt},
  booktitle={Proceedings of the IEEE international conference on robotics and automation (ICRA), Shanghai, China},
  pages={9--13},
  year={2011}
}

https://github.com/RainerKuemmerle/g2o https://vincentqin.gitee.io/blogresource-4/slam/g2o-details.pdf

【注】也可以通过直接编译RainerKuemmerle/g2o/doc中的latex文件得到，

Ubuntu环境需要apt-get安装fig2dev工具，终端执行doc目录下的make编译将fig图片转换为pdf文件

可以本地安装texlive或者使用overleaf，本人采用后者网页版进行编译，上传doc下的所有文件以及转换好后的pdf图片。右侧点击编译，就会同步显示pdf版本论文，下载即可。

在本文中，我们描述了一个C++框架，用于执行非线性最小二乘问题的优化，这些问题可以嵌入到图(graph)或超图(hyper-graph)中。超图是图的扩展，其中一条边可以连接多个节点，而不仅仅是两个节点。机器人技术和计算机视觉中的几个问题需要根据一组参数找到误差函数的最优值。例如，类似SLAM和BA集束调整等流行应用。

在文献中，已经提出了许多方法来解决这类问题。当选择正确的参数化时，标准方法的简单实现，如列文伯格马夸尔特LM或高斯-牛顿GN法，可以为大多数应用程序带来可接受的结果。然而，为了达到最大限度的性能，可能需要付出大量的努力。

g2o的字面意思 general (hyper) graph optimization 表示一般（超）图优化。这个框架的目的是：

提供一个易于扩展(easy-to-extend)和易于使用(easy-to-use)的通用图优化库，可以很容易地应用于不同的问题
给想要理解SLAM或BA的人提供一种易于阅读的实现，使得他们专注于问题规范的相关细节。
实现了最先进的性能，同时是尽可能的普遍。

在本文档的其余部分中，我们将首先描述（超）图可嵌入问题 (hyper) graph-embeddable problems，并将通过在这个库中实现的流行的列文伯格-马夸尔特LM或高斯-牛顿GN算法来介绍它们的解决方案。随后，我们将描述g2o库的高级行为和基本结构。最后，作为一个简单的例子，我们将介绍如何实现二维SLAM。

此文档不能替代内置文档。相反，它是一个帮助用户/读者阅读/浏览和扩展代码的摘要。

一个最小二乘最小化问题（least squares minimization problem）可以通过下式来描述：

$\\begin{align}\\mathbf{F(x)}&=\\sum_{k\\in\\mathcal C}\緻brace{\\mathbf e_k(\\mathbf x_k,\\mathbf z_k)^\ op\\boldsymbol\\Omega_k\\mathbf e_k(\\mathbf x_k,\\mathbf z_k)}_{\\mathbf{F(x)}}\ ag{1}\\\\ \\mathbf x^* &=\\arg\\min_{\\mathbf x}\\mathbf{F(x)}\ ag{2}\\end{align}$ 这里：

$\\mathbf x=(\\mathbf x_1^\ op,\\cdots,\\mathbf x_n^\ op)^\ op$ 是参数向量，其中每个 $\\mathbf x_i$ 表示通用的参数块。
$\\mathbf x_k=(\\mathbf x_{k_1}^\ op,\\cdots,\\mathbf x_{k_n}^\ op)^\ op\\subset(\\mathbf x_1^\ op,\\cdots,\\mathbf x_n^\ op)^\ op$ 是第 $k$ 个约束中涉及到参数的子集。
$\\mathbf z_k$ 和 $\\boldsymbol\\Omega$ 分别表示与 $\\mathbf x_k$ 中参数相关联约束的均值和信息矩阵。
$\\mathbf e_k(\\mathbf x_k\\mathbf z_k)$ 是一个向量误差函数，测量 $\\mathbf x_k$ 中的这些参数块如何良好地满足约束 $\\mathbf z_k$ 。当 $\\mathbf x_k$ 和 $\\mathbf x_j$ 完美地匹配约束时，残差就为 $\\mathbf 0$ 。例如，如果存在一个观测函数 $\\hat{\\mathbf z_k}=\\mathbf h_k(\\mathbf x_k)$ ，给定 $\\mathbf x_k$ 中节点的一个实际配置，观测函数生成一个合成测量 $\\hat{\\mathbf z}_k$ 。一个简单的残差函数将是 $\\mathbf e(\\mathbf x_k,\\mathbf z_k)=\\mathbf h_k(\\mathbf x_k)-\\mathbf z_k$ 。

为了简化符号，本文的其余部分，将观测编码到误差函数的索引中： $\\mathbf e_k(\\mathbf x_k,\\mathbf z_k)\\overset{\\mathrm{def.}}{=}\\mathbf e_k(\\mathbf x_k)\\overset{\\mathrm{def.}}{=}\\mathbf e_k(\\mathbf x)\ ag{3}$ 请注意，每个参数块和每个误差函数都可以跨越不同的空间。这种形式的问题可以用有向超图（directed hyper-graph）有效地表示。图中的节点 $i$ 表示参数块 $\\mathbf x_i\\in\\mathbf x_k$ ，且在节点 $\\mathbf x_i\\in\\mathbf x_k$ 之间的超边（hyper-edge）表示涉及 $\\mathbf x_k$ 中所有节点的一个约束。如果超边的大小为2，那么超图就变成了普通的图。图1显示了一个超图和目标函数之间的映射示例。阐释了如何通过超图来表示一个目标函数。这里我们解释一个小型SLAM问题的一部分。

在这个例子中，我们假设，观测函数由一些未知的标定参数 $\\mathbf K$ 控制。机器人的位姿通过变量 $\\mathbf p_{1:n}$ 来表示。这些变量是通过正方形表示的约束 $\\mathbf z_{ij}$ 进行关联。例如，通过激光参考系中匹配附近的激光扫描而产生的约束。然而，激光匹配与机器人位姿之间的关系取决于机器人上传感器的位置，该位置由标定参数 $\\mathbf K$ 建模得到。相反，后续的机器人姿态是由里程计观测产生的二元约束 $\\mathbf u_k$ 连接的。这些测量是在机器人移动基地的坐标系下进行的。

如果已知参数有比较好的初始猜测 $\\hat{\\mathbf x}$ ，则可以通过流行的高斯牛顿GN法或者列文伯格马夸尔特LM算法得到公式（2）的数值解。这种思想是通过在当前初始猜测 $\\hat{\\mathbf x}$ 附近作一阶泰勒展开来近似误差函数。 $\\begin{align}\\mathbf e_k(\\hat{\\mathbf x}_k+\\Delta\\mathbf x_k)&=\\mathbf e_k(\\hat{\\mathbf x}+\\Delta\\mathbf x)\ ag{4}\\\\ (一阶泰勒近似)&\\simeq\\mathbf e_k+\\mathbf J_k\\Delta\\mathbf x\ ag{5}\\end{align}$ 这里 $\\mathbf J_k$ 是在 $\\hat{\\mathbf x}$ 中计算的误差 $\\mathbf e_k(\\mathbf x)$ 的雅可比矩阵，且 $\\mathbf e_k\\overset{\\mathrm{def}}{=}\\mathbf e_k(\\hat{\\mathbf x})$ 。用公式（5）替换公式（1）的误差项 $\\mathbf F_k$ ，我们可得： $\\begin{align}&\\mathbf F_k(\\hat{\\mathbf x}+\\boldsymbol\\Delta\\mathbf x) \ ag{6}\\\\&=\\mathbf e_k(\\hat{\\mathbf x}+\\boldsymbol\\Delta\\mathbf x)^\ op\\boldsymbol\\Omega_k\\mathbf e_k(\\hat{\\mathbf x}+\\boldsymbol\\Delta\\mathbf x)\ ag{7}\\\\ 公式（5）&\\approx (\\mathbf e_k+\\mathbf J_k\\boldsymbol\\Delta\\mathbf x)^\ op\\boldsymbol\\Omega_k(\\mathbf e_k+\\mathbf J_k\\boldsymbol\\Delta\\mathbf x)\ ag{8}\\\\ &=(\\mathbf e_k^\ op+\\boldsymbol\\Delta\\mathbf x^\ op\\mathbf J_k^\ op)\\boldsymbol\\Omega_k(\\mathbf e_k+\\mathbf J_k\\boldsymbol\\Delta\\mathbf x)\ otag\\\\ （展开）&=\\mathbf e_k^\ op\\boldsymbol\\Omega_k\\mathbf e_k+\\mathbf e_k^\ op\\boldsymbol\\Omega_k\\mathbf J_k\\boldsymbol\\Delta\\mathbf x+\\boldsymbol\\Delta\\mathbf x^\ op\\mathbf J_k^\ op\\boldsymbol\\Omega_k\\mathbf e_k+\\boldsymbol\\Delta\\mathbf x^\ op\\mathbf J_k^\ op\\boldsymbol\\Omega_k\\mathbf J_k\\boldsymbol\\Delta\\mathbf x\ otag\\\\ &=\緻brace{\\mathbf e_k^\ op\\boldsymbol\\Omega_k\\mathbf e_k}_{c_k}+2\緻brace{\\mathbf e_k^\ op\\boldsymbol\\Omega_k\\mathbf J_k}_{\\mathbf b_k}\\boldsymbol\\Delta\\mathbf x+\\boldsymbol\\Delta\\mathbf x^\ op\緻brace{\\mathbf J_k^\ op\\boldsymbol\\Omega_k\\mathbf J_k}_{\\mathbf H_k}\\boldsymbol\\Delta\\mathbf x\ ag{9}\\\\ &=c_k + 2\\mathbf b_k\\boldsymbol\\Delta\\mathbf x + \\boldsymbol\\Delta\\mathbf x^\ op\\mathbf H_k\\Delta\\mathbf x\ ag{10}\\end{align}$ 有了这个局部近似，我们可以重写公式（1）中的 $\\mathbf{F(x)}$ 为： $\\begin{align}\\mathbf F(\\hat{\\mathbf x}+\\boldsymbol\\Delta\\mathbf x) &=\\sum_{k\\in\\mathcal C}\\mathbf F_k(\\hat{\\mathbf x}+\\boldsymbol\\Delta\\mathbf x)\ ag{11}\\\\ &\\approx \\sum_{k\\in\\mathcal C}c_k + 2\\mathbf b_k\\boldsymbol\\Delta\\mathbf x + \\boldsymbol\\Delta\\mathbf x^\ op\\mathbf H_k\\boldsymbol\\Delta\\mathbf x\ ag{12}\\\\ &=c+2\\mathbf b^\ op\\boldsymbol\\Delta\\mathbf x+\\boldsymbol\\Delta\\mathbf x^\ op\\mathbf H\\boldsymbol\\Delta\\mathbf x\ ag{13}\\end{align}$ 公式（13）中的这个二次形式是通过在公式（12）中令 $c=\\sum c_k,\\mathbf b=\\sum \\mathbf b_k,\\mathbf H=\\sum\\mathbf H_k$ 得到。通过求解以下线性系统，可以在 $\\boldsymbol\\Delta\\mathbf x$ 中被最小化。 $\\mathbf H\\boldsymbol\\Delta\\mathbf x^*=-\\mathbf b\ ag{14}$ 矩阵 $\\mathbf H$ 是系统的信息矩阵，通过构造是一个稀疏矩阵，只有被约束连接着的块之间有非零项。它的非零块的个数是约束边数目加上节点数目。这允许使用类似稀疏Cholesky分解或者Preconditioned Conjugate Gradients，PCG的高效算法来解决公式（14）中的问题。稀疏乔尔斯基分解的一种高效实现可以在开源的CSparse或者Cholmod库中找到。然后，线性化解可以通过将初始猜测加入到计算得到的增量中： $\\mathbf x^*=\\hat{\\mathbf x}+\\boldsymbol\\Delta\\mathbf x^*\ ag{15}$ 流行的高斯牛顿GN算法迭代运行公式（13）中的线性化，公式（14）中的求解，以及公式（15）中的更新步骤。在每次迭代中，前一个解用于下一次的线性化点以及初始值。

列文伯格-马夸尔特LM算法是高斯牛顿GN算法的非线性变体，其引入了一个阻尼因子以及backup动作来控制收敛性。而不是直接求解公式（14），LM算法求解的是它的阻尼版本： $(\\mathbf H+\\lambda\\mathbf I)\\boldsymbol\\Delta\\mathbf x^*=-\\mathbf b\ ag{16}$ 这里， $\\lambda$ 是阻尼因子：值越大， $\\boldsymbol\\Delta\\mathbf x$ 越小。这在非线性表面的情况中控制步长大小是很有用的。LM算法背后的这个思想是动态控制阻尼因子。在每一次迭代中，都将监控新配置的误差。如果新的误差低于前一个误差，则下一次迭代减少 $\\lambda$ 值，否则，解将会回退并且 $\\lambda$ 值将会增加。对于g2o库中实现的LM算法更多详细解释，可以参考文献[5]。

上述步骤是多远函数最小化的一般方法。然而，一般方法假设参数 $\\mathbf x$ 是在欧几里得空间内的，这对于诸如SLAM或者BA等一些问题是无效的。这可能会导致次优解。在本节的其它部分中，将首先讨论参数空间为欧氏空间时的通解，然后将这个解推广到更一般的非欧空间中。

根据公式（13），矩阵 $\\mathbf H$ 以及向量 $\\mathbf b$ 是由一组矩阵和向量累加求和得到的，每个约束对应一个。如果令 $\\mathbf b_k=\\mathbf J_k^\ op\\boldsymbol\\Omega_k\\mathbf e_k$ 以及 $\\mathbf H_k=\\mathbf J_k^\ op\\boldsymbol\\Omega_k\\mathbf J_k$ ，则可以将 $\\mathbf H$ 和 $\\mathbf b$ 重写为： $\\begin{align}\\mathbf b &=\\sum_{k\\in\\mathcal C}\\mathbf b_{ij}\ ag{17}\\\\ \\mathbf H &=\\sum_{k\\in\\mathcal C}\\mathbf H_{ij}\ ag{18}\\end{align}$ 每个约束将会为这个系统贡献一个附加项。该附加项的结构依赖于误差函数的雅可比矩阵。因为一个约束的误差函数仅依赖于节点 $\\mathbf x_i\\in\\mathbf x_k$ 的值，公式（5）中的雅可比矩阵有下面的形式： $\\mathbf J_k=(\\mathbf 0\\cdots\\mathbf 0\\mathbf J_{k_1}\\cdots\\mathbf J_{k_i}\\cdots\\mathbf 0\\cdots\\mathbf J_{k_q}\\mathbf 0\\cdots\\mathbf 0)\ ag{19}$ 这里 $\\mathbf J_{k_i}=\\frac{\\partial\\mathbf e(\\mathbf x_k)}{\\partial\\mathbf x_{k_i}}$ 是误差函数相对于由第 $k$ 个超边相连的节点的导数，也就是相对于参数块 $\\mathbf x_{k_i}\\in\\mathbf x_k$ 。

从公式（9）中，我们得到下面块矩阵 $\\mathbf H_{ij}$ 的结构： $\\begin{align}\\mathbf H_k &=\\begin{pmatrix}\\ddots&&&&&&\\\\ &\\mathbf J_{k_1}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_1}&\\cdots&\\mathbf J_{k_1}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_i}&\\cdots&\\mathbf J_{k_1}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_q}&\\\\ &\\vdots&&\\vdots&&\\vdots&\\\\ &\\mathbf J_{k_i}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_1}&\\cdots&\\mathbf J_{k_i}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_i}&\\cdots&\\mathbf J_{k_i}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_q}&\\\\ &\\vdots&&\\vdots&&\\vdots&\\\\ &\\mathbf J_{k_q}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_1}&\\cdots&\\mathbf J_{k_q}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_i}&\\cdots&\\mathbf J_{k_q}^\ op\\boldsymbol\\Omega_k\\mathbf J_{k_q}&\\\\ &&&&&&\\ddots \\end{pmatrix}\ ag{20}\\\\ \\mathbf b_k &=\\begin{pmatrix}\\vdots\\\\ \\mathbf J_{k_1}^\ op\\boldsymbol\\Omega_k\\mathbf e_k\\\\ \\vdots\\\\ \\mathbf J_{k_i}^\ op\\boldsymbol\\Omega_k\\mathbf e_k\\\\ \\vdots\\\\ \\mathbf J_{k_q}^\ op\\boldsymbol\\Omega_k\\mathbf e_k\\\\ \\vdots \\end{pmatrix}\ ag{21}\\end{align}$ 为了简化符号，上式省略了零块。可以注意到，矩阵 $\\mathbf H$ 的块结构是超图的邻接矩阵（adjacency matrix）。此外，因为所有的 $\\mathbf H_k$ 都是对称的，所以海森矩阵 $\\mathbf H$ 是一个对称矩阵。一个单一的连接 $q$ 个顶点的超边将在海森矩阵中引入 $q^2$ 个非零块，对应所连接节点的每一对 $\\langle\\mathbf x_{k_i},\\mathbf x_{k_j}\\rangle$ 。

为了处理跨越非欧空间上的参数块，通常要在流形（manifold）上应用误差最小化（error minimization）。流形是一个数学空间，它在全局尺度上不一定是欧几里得的，但在局部尺度上可以看作是欧几里得的。

例如，在SLAM问题的上下文中，每个参数块 $\\mathbf x_i$ 由一个平移向量 $\\mathbf t_i$ 以及一个旋转分量 $\\alpha_i$ 组成。平移 $\\mathbf t_i$ 明显地形成了一个欧式空间。与此相反，旋转分量 $\\alpha_i$ 跨越了非欧几里得2D或3D旋转群 $SO(2)$ 或 $SO(3)$ 。为了避免奇点，这些空间通常以一种过度参数化地方式进行描述，例如，通过旋转矩阵或者四元数。直接应用公式（15）到这些过度参数化表示中将会打破由过度参数化引起的约束。过度参数化产生额外的自由度，因此在解中引入了误差。为了克服这个问题，一种是使用旋转的最小表示（类似3D中的欧拉角）。然而，这受限于万向节锁的奇异性。

另一种想法是将底层空间看作一个流形，并定义一个运算 $\\boxplus$ ，将欧氏空间中的局部变化量 $\\boldsymbol\\Delta x$ 映射为流形上的变化量， $\\boldsymbol\\Delta x\\mapsto\\mathbf x\\boxplus\\boldsymbol\\Delta x$ 。更多的数学细节可以参考文献[7]中的1.3小节。有了这个运算，新的误差函数可以定义为： $\\begin{align}\\hat{\\mathbf e}_k(\\boldsymbol\\Delta\ ilde{\\mathbf x}_k) &\\overset{\\mathrm{def.}}{=}\\mathbf e_k(\\hat{\\mathbf x}_k\\boxplus\\boldsymbol\\Delta\ ilde{\\mathbf x}_k)\ ag{22}\\\\ 一阶泰勒近似&\\approx \\hat{\\mathbf e}_k(\\mathbf x_k)+\ ilde{\\mathbf J}_k\\boldsymbol\\Delta\ ilde{\\mathbf x}_k\ ag{23}\\end{align}$

其中， $\\hat{\\mathbf x}$ 跨越了原始过度参数化空间，例如四元数。项 $\\boldsymbol\\Delta\ ilde{\\mathbf x}$ 是在原始位置 $\\hat{\\mathbf x}$ 附近的一个小增量，且以一种最小化表示来表达。一种常用的 $SO(3)$ 的选择是使用单位四元数的虚部向量部分。更具体的是，一个可以将增量 $\\boldsymbol\\Delta\ ilde{\\mathbf x}$ 表示为6维向量 $\\boldsymbol\\Delta\ ilde{\\mathbf x}^\ op=(\\boldsymbol\\Delta\ ilde{\\mathbf t}^\ op,\ ilde{\\mathbf q}^\ op)$ ，其中 $\\boldsymbol\\Delta\ ilde{\\mathbf t}$ 表示平移， $\ ilde{\\mathbf q}^\ op=(\\Delta q_x,\\Delta q_y, \\Delta q_z)^\ op$ 是表示3D旋转的四元数的虚部向量。相反， $\\hat{\\mathbf x}^\ op=(\\hat{\\mathbf t}^\ op,\\hat{\\mathbf q}^\ op)$ 使用四元数 $\\hat{\\mathbf q}$ 去编码旋转部分。因此，运算符 $\\boxplus$ 可以首先将 $\\boldsymbol\\Delta \ ilde{\\mathbf q}$ 转换为完整的四元数 $\\boldsymbol\\Delta\\mathbf q$ ，然后应用变换 $\\boldsymbol\\Delta\\mathbf x^\ op=(\\boldsymbol\\Delta\\mathbf t^\ op,\\boldsymbol\\Delta\\mathbf q^\ op)$ 到 $\ ilde{\\mathbf x}$ 上。在描述误差最小化的公式中，这些操作可以通过运算符 $\\boxplus$ 很好地进行封装。雅可比矩阵 $\ ilde{\\mathbf J}_k$ 可以表示为：

$\ ilde{\\mathbf J}_k=\\frac{\\partial\\mathbf e_k(\\hat{\\mathbf x\\boxplus\\boldsymbol\\Delta\ ilde{\\mathbf x}})}{\\partial\\boldsymbol\\Delta\ ilde{\\mathbf x}}\\bigg|_{\\boldsymbol\\Delta\ ilde{\\mathbf x}=\\mathbf 0}\ ag{24}$ 因为在之前的公式中， $\\hat{\\mathbf e}$ 仅依赖于 $\\boldsymbol\\Delta\ ilde{\\mathbf x}_{k_i}\\in\\boldsymbol\\Delta\ ilde{\\mathbf x}_k$ ，我们可以进一步将其进行如下扩展：

$\\begin{align}\ ilde{\\mathbf J}_k&=\\frac{\\partial\\mathbf e_k(\\hat{\\mathbf x\\boxplus\\boldsymbol\\Delta\ ilde{\\mathbf x}})}{\\partial\\boldsymbol\\Delta\ ilde{\\mathbf x}}\\bigg|_{\\boldsymbol\\Delta\ ilde{\\mathbf x}=\\mathbf 0}\ ag{25}\\\\ &=(\\mathbf 0\\cdots\\mathbf 0\ ilde{\\mathbf J}_{k_1}\\cdots\ ilde{\\mathbf J}_{k_i}\\cdots\\mathbf 0\\cdots\ ilde{\\mathbf J}_{k_q}\\mathbf 0\\cdots\\mathbf 0)\ ag{26}\\end{align}$

用一个简单的符号扩展，令：

$\ ilde{\\mathbf J}_{k_i}=\\frac{\\partial\\mathbf e_k(\\hat{\\mathbf x\\boxplus\\boldsymbol\\Delta\ ilde{\\mathbf x}})}{\\partial\\boldsymbol\\Delta\ ilde{\\mathbf x}_{k_i}}\\bigg|_{\\boldsymbol\\Delta\ ilde{\\mathbf x}=\\mathbf 0}\ ag{27}$ 可以将公式（23）插入到公式（8）和公式（11）中，可得： $\ ilde{\\mathbf H}\\boldsymbol\\Delta\ ilde{\\mathbf x}^*=-\ ilde{\\mathbf b}\ ag{28}$ 因为增量 $\\boldsymbol\\Delta\ ilde{\\mathbf x}^*$ 是在局部欧几里得环境中计算的，因此需要由 $\\boxplus$ 运算符将它们重新映射到原始的冗余空间中。因此，公式（15）的更新规则变为： $\\mathbf x^*=\\hat{\\mathbf x}\\boxplus\\boldsymbol\\Delta\ ilde{\\mathbf x}^*\ ag{29}$ 总之，将流形上的最小化问题形式化，包括首先围绕公式（28）的初始猜测，以局部欧几里得近似计算一组增量，然后通过公式（29）累积全局非欧几里得空间中的增量。请注意，在流形表示上计算的线性系统与在欧几里得空间上计算的线性系统具有相同的结构。只有通过定义一个 $\\boxplus$ 运算及其关于对应参数块的雅可比矩阵 $\ ilde{\\mathbf J}_{k_i}$ 。我们才可以很容易地从非流形版本中推导出图最小化的流形版本对应的参数块。在g2o中，我们提供了数值计算流形空间上的雅可比矩阵的工具。这就要求用户只实现误差函数和具体的 $\\boxplus$ 运算过程。作为一种设计选择，我们不处理非流形的情况，因为它已经包含在流形的情况中。然而，为了达到最大的性能和准确性，我们建议用户实现雅可比矩阵的解析解，一旦系统与数值系统一起运行。

可选地，最小二乘优化可以被鲁棒化。注意，公式（1）中的误差项有以下形式：

$\\mathbf F_k=\\mathbf e_k^\ op\\Omega_k\\mathbf e_k=\\rho_2\\left(\\sqrt{\\mathbf e_k^\ op\\Omega_k\\mathbf e_k}\\right),\\qquad \\mathrm{with}\\qquad \\rho_2(x)=x^2\ ag{30}$ 因此，误差向量 $\\mathbf e_k$ 对 $\\mathbf F$ 有二次影响，因此单个潜在的离群值会产生重大的负面影响。为了使其更具有离群值鲁棒性，二次误差函数 $\\rho_2$ 可以被一个更加鲁棒的代价函数代替，而权重越大误差越小。在g2o中，使用Huber代价函数 $\\rho_H$ ： $\\rho_H(x)=\\left\\{\\begin{array}{ll}x^2&\\mathrm{if}\\quad|x|<b\\\\ 2b|x|-b^2&\\mathrm{else}, \\end{array}\\right.\ ag{31}$ 上式对小的 $|x|$ 是二次的，但对于大的 $|x|$ 是线性的。与其它更加鲁棒的代价函数相比，Huber核的优点是它仍然是凸的，因此不会在 $\\mathbf F$ 中引入新的局部最小值。实际上，我们不需要修改公式（1）。相反，我们采用以下的方案。首先，误差 $\\mathbf e_k$ 像往常一样计算得到。然后， $\\mathbf e_k$ 被一个加权版本的 $w_k\\mathbf e_k$ 替代，例如： $(w_k\\mathbf e_k)^\ op\\Omega_k(w_k\\mathbf e_k)=\\rho_H\\left(\\sqrt{\\mathbf e_k^\ op\\Omega_k\\mathbf e_k}\\right)\ ag{32}$ 这里，权重 $w_k$ 计算为： $w_k=\\frac{\\sqrt{\\rho_H(\\|\\mathbf e_k\\|_\\Omega)}}{\\|\\mathbf e_k\\|_\\Omega},\\qquad \\mathrm{with}\\qquad \\|\\mathbf e_k\\|_\\Omega=\\sqrt{\\mathbf e_k^\ op\\Omega_k\\mathbf e_k}\ ag{33}$ 在g2o中，用户具有细粒度的控制，并且可以为每条边单独启用/禁用鲁棒的代价函数（见第6.2.2小节）。

从上面部分可以清楚看到，一个图优化问题完全被定义为：

图中顶点的类型（即参数块 $\\{\\boldsymbol x_i\\}$ ）。对于每个都必须指定：

内部参数的域 $\\mathrm{Dom}(\\boldsymbol x_i)$ ；
增量 $\\Delta\\boldsymbol x_i$ 的域 $\\mathrm{Dom}(\\Delta\\boldsymbol x_i)$ ；
操作 $\\boxplus:\\mathrm{Dom}(\\boldsymbol x_i)\ imes\\mathrm{Dom}(\\Delta\\boldsymbol x_i)\\rightarrow\\boxplus:\\mathrm{Dom}(\\boldsymbol x_i)$ 将增量 $\\Delta\\boldsymbol x_i$ 应用于上一个解 $\\boldsymbol x_i$ 。

当扰动后的估计量 $\\boldsymbol x_k\\boxplus\\Delta\\boldsymbol x_k$ 完美满足约束 $\\boldsymbol z_k$ 时，超边 $\\boldsymbol e_k$ 每种类型的误差函数： $\\mathrm{Dom}(\\Delta\\boldsymbol x_{k_1})\ imes\\mathrm{Dom}(\\Delta\\boldsymbol x_{k_2})\ imes\\cdots\ imes\\mathrm{Dom}(\\Delta\\boldsymbol x_{k_q})\\rightarrow\\mathrm{Dom}(\\Delta\\boldsymbol x_{k_z})$ 应该为零。

默认情况下，雅可比矩阵是由g2o框架通过数值求导计算得到。然而，为了在特定的实现中实现最大的性能，我们可以指定误差函数和流形算子的雅可比矩阵。

在其余部分，我们将简要讨论一些需要使用和扩展g2o的基本概念。此文档并不完整，但它旨在帮助用户浏览自动生成的文档。为了更好地可视化g2o的组件之间的相互作用，我们参考了图2中的类图。

总之，我们的系统使用了一个通用的超图结构来表示一个问题实例（在hyper_graph.h中定义）。这个泛型超图是专门由optimizable_graph.h中定义的类OptimizableGraph来表示一个优化问题的。在OptimizableGraph中，内部类OptimizableGraph::Vertex和OptimizableGraph::Edge用于表示一般的超边hyper edges和超顶点hyper vertices。虽然具体的实现可以通过直接扩展这些类来实现，但我们提供了一个模板，它可以自动实现为系统工作所必需的大多数方法。

这些类是BaseVertex、BaseUnaryEdge、BaseBinaryEdge以及BaseMultiEdge。

BaseVertex：对参数块 $\\boldsymbol x_i$ 和相应的流形 $\\Delta \\boldsymbol x_i$ 的维数进行模板化，从而可以使用在编译时布局已知的内存块（平均效率）。此外，它还实现了一些映射操作符来存储线性化问题的海森矩阵和参数块，以及可用于保存/恢复部分图的之前值的堆栈。方法oplusImpl(double* v)是需要实现的，它将由 $\\boldsymbol v$ 表示的扰动 $\\Delta \\boldsymbol x_i$ 应用于成员变量_estimate。这就是 $\\boxplus$ 操作符。此外，方法setToOriginImpl()必须指定将顶点内部状态设置为0。

BaseUnaryEdge：这是一个用于建模一元超边的模板类，可以用于表示先验。通过线性化运算方法实现，默认提供了雅可比矩阵的计算。需要指定（单个）顶点 $\\boldsymbol x_i$ 的类型，以及残差 $\\boldsymbol e(\\boldsymbol x_k)$ 的类型与维度作为模板参数。函数compueError()必须要实现，它在成员变量Eigen::Matrix _error中存储残差 $\\boldsymbol{e(x}_k)$ 的结果。

BaseBinaryEdge：这是一个构建二元约束（形式为 $\\boldsymbol e_k(\\boldsymbol x_{k_1},\\boldsymbol x_{k_2})$ 的误差函数）的模板类。它提供了跟一元边BaseUnaryEdge相同的功能，且需要指定下面的模板参数：节点 $\\boldsymbol x_{k_1}$ 和 $\\boldsymbol x_{k_2}$ 的类型，以及观测的类型与维度。同样地，它通过linearizeOplus()方法的默认数值求导方法来实现雅可比矩阵的计算。computeError()计算残差方法必须在派生类中具体实现。

BaseMultiEdge：这是建模形式如 $\\boldsymbol e_k(\\boldsymbol x_{k_1},\\boldsymbol x_{k_2},\\cdots,\\boldsymbol x_{k_q})$ 的多顶点约束的模板类。它提供了上面两种边类型相同的功能，且它只需要指定观测的类型与维度作为模板参数。派生的类需要关注调整连接顶点的正确大小 $q$ 。这个超类依赖于动态内存，因此太多参数是未知的，并且如果你想要高效实现某个特定的问题，必须自己实现。数值求导雅可比是默认可选的，但是跟上面一样，必须在派生类中实现残差计算方法computeError()。

简而言之，要定义一个新的问题实例，需要做的就是从上面列出的类中推导出一组类，每种类型的参数块，每种类型的（超）边。总要尝试从对你最有效的类中获得。如果你想看一个简单的例子，看看vertex_se2和edge_se2。这两种类型定义了一个简单的二维图SLAM问题，就像在许多SLAM论文中所描述的那样。

当然，对于构造的每一种类型，还应该定义read()和write()读写函数来将数据读写入流。最后，一旦定义了新类型，为了启用加载和保存新类型，应该将其“register注册”到工厂中。通过registerType类型函数，将字符串标记分配给新类型，可以很容易地做到这一点。这应该在加载所有文件之前调用一次。

为此，g2o提供了一个简单的宏来执行向工厂注册的类。例如，清单Listing 1，完整的示例可以在types_slam2d.cpp中找到。给宏G2O_REGISTER_TYPE的第一个参数指定已知某个顶点/边的标记。g2o将在加载文件和将当前图形保存到文件中时使用此信息。在Listing 1中给出的示例中，我们分别用类VertexSE2和EdgeSE2注册标记VERTEX_SE2和EDGE_SE2。

此外，宏G2O_REGISTER_TYPE_GROUP允许声明一个类型组。如果我们使用工厂来构造类型，并且我们必须强制我们将代码链接到特定的类型组，那么这是必要的。否则，链接器可能会删除我们的库，因为我们没有显式地使用包含我们的类型的库所提供的任何符号。声明特定类型库的使用并因此强制执行链接是由名为G2O_USE_TYPE_GROUP的宏来完成的。

构造和解决方案可以分为单独的迭代步骤：

优化的初始化（仅在第一次迭代之前）
计算每个约束的误差向量
线性化每个约束
构建现象系统
更新LM列文伯格-马夸尔特算法的阻尼因子

在下面小节中，我们将会描述这些步骤。

类SparseOptimizer提供了几种方法来初始化潜在的数据结构。函数initializeOptimization()要么接受一个顶点的子集或者一条边的子集，这将考虑为下一次优化运行。此外，还可以考虑所有的顶点和边来进行优化。我们将当前的顶点和边认为是活动的（active）顶点和边。

在初始化过程中，优化器分配内存索引给每个活动顶点。临时索引对应于海森矩阵中顶点的块列/块行。在优化过程中，一些顶点可能需要保持固定，以解决任意的自由度（测量自由度）。这可以通过设置顶点的_fixed属性来实现。

computeActiveErrors()函数取活动顶点的当前估计，并为每条活动边调用computeError()函数，用于计算当前的残差向量。使用6.1小节中描述的边的基类，残差应该缓存在内存变量_error中。

对于特定的活动边，如果robustKernel()函数设置为true，robustifyError()函数被调用，且_error被鲁棒化，如第5小节中描述的那样。

每条活动边通过调用linearizeOplus()函数被线性化。同样地，雅可比矩阵可以通过模板基类提供的成员变量缓存。如果linearizeOplus()函数没有被派生类重新实现，雅可比矩阵将会默认以及数值计算得到：

$\ ilde{\\mathbf J}_k^{\\bullet^l}=\\frac{1}{2\\delta}(\\mathbf e_k(\\mathbf x_k\\boxplus\\delta\\mathbf 1_l)-\\mathbf e_k(\\mathbf x_k\\boxplus-\\delta\\mathbf 1_l))\ ag{34}$ 其中 $\\delta>0$ 是小的常数（在我们的实现中为 $10^{-9}$ ）且 $\\mathbf 1_l$ 是沿着 $l$ 的单位向量。注意我们只存储并计算 $\ ilde{\\mathbf J}_k$ 的非零项，其在初始化过程中已经被固定了。

对于每条活动边，使用公式（18）中的附加项（addend term）是将雅可比矩阵的相应块与边的信息矩阵相乘计算得到的。在每条边中通过调用constructQuadraticForm()函数来计算附加项。

如公式（16）中所示列文伯格-马夸尔特LM算法需要更新线性系统。但是，只需要修改沿着主对角线的元素。为此，Solver求解器类的updateLevenbergSystem(double lambda)和recoverSystem(double lambda)的方法通过分别沿 $\\mathbf H$ 的主对角线添加或减去 $\\lambda$ 来应用修改。

这些最小二乘方法的一个中心组成部分是线性系统 $\ ilde{\\mathbf H}\\boldsymbol\\Delta\ ilde{\\mathbf x}=-\ ilde{\\mathbf b}$ 的解。为此，有几种可用的方法，其中一些利用某些问题的已知结构，并对系统进行中间缩减，例如通过对变量的子集应用舒尔补（Schur Complement）。在g2o中，我们不选择任何特定的求解器，但我们依赖于外部库。为此，我们将这些结构操作（如Schur舒尔补）从线性系统的解中解耦。

由雅可比矩阵和超图元素中的误差向量构造的线性问题由一个所谓的Solver求解器类来控制。要使用系统的特定分解算法，用户必须扩展Solver求解器类，并实现虚函数。也就是说，求解器应该能够从一个超图中提取线性系统，并返回一个解。这可以通过几个步骤来完成：在优化开始时，调用函数初始化结构，以分配将在后续操作中被覆盖的必要内存。这是可能的，因为系统的结构在迭代之间不会改变。然后，用户应该提供通过函数 $\\mathrm{b}()$ 和 $\\mathrm{x()}$ 来访问增量向量 $\\boldsymbol\\Delta\ ilde{\\mathbf x}$ 和 $\ ilde{\\mathbf b}$ 的方法。为了支持列文伯格-马夸尔特LM算法，我们还应该实现一个函数来用 $\\lambda\\mathbf I$ 项干扰海森矩阵。这个函数为setLambda(double lambda)，需要由特定的solver求解器来实现。

我们提供了一个求解器类的实例化实现，即BlockSolver<>，它将线性系统存储在SparseBlockMatrix<>类中。BlockSolver<>也实现了Schur舒尔补，并依赖于另一个抽象类，即LinearSolver来求解系统。线性求解器的实现实现了求解（简化的）线性系统的实际工作，并必须实现几种方法。在g2o的发布中，我们提供了分别使用预条件梯度下降（preconditioned gradient descent）、CSparse和CHOLMOD的线性求解器。

就g2o而言，存储在超图中的实体具有纯粹的数学意义。它们要么表示要优化的变量（顶点），要么编码优化约束。然而，一般来说，这些变量通常与更“具体concrete”的物体有关，如激光扫描、机器人姿态、相机参数等。某些变量类型可能只支持可行操作的一个子集。例如，可以“绘制draw”一个机器人的姿态，但不可能“绘制”标定参数。更一般的来说，我们无法预先知道g2o的用户类型将支持的操作类型。但是，我们希望设计一组依赖于某些操作的工具和函数。这些功能包括，例如，viewers，或以特定格式保存/加载图形的函数。

这样做的一种可能性是用许多虚拟函数来“超载overload”超图元素（顶点和边）的基类，我们想要支持的每个功能都有一个。这当然并不优雅，因为每次添加新内容时，我们都需要用新函数来修补基类。另一种可能性是利用C++的多重继承（multiple inheritance），并定义一个抽象的“可绘制drawable”对象，viewer对其进行操作。这个解决方案要好一些，但是我们不能为每个对象有超过一个的“绘图”函数。

在g2o中使用的解决方案包括创建一个函数对象库，它对图的元素（顶点或边）进行操作。其中一个函数对象通过函数名和其操作的类型来标识。这些函数对象可以注册到操作库中。一旦这些对象被加载到操作库中，就可以在图上调用它们。这些功能在hyper_graph_action.h中定义。在定义边和顶点的类型时，通常会注册和创建操作，可以在type_*/*.h中看到许多示例。

有缘再续。。。

未完待续。。。

博主作为一名测绘er（大地测量方向），早闻图优化大名，奈何一直没有搞懂图优化。对我而言，更熟悉的优化方法还是滤波或最小二乘。在学习图优化的过程中，总是感到教材上（《因子图在SLAM中的应用》、《视觉SLAM十四讲》）的内容不够直观，难以结合自己的专业背景进行理解。最近感觉对图优化有了一些体会，便想着尝试从测绘数据处理角度出发，谈谈自己对于图优化的理解，欢迎拍砖。先说结论：图优化/因子图本质上就是用图表达的整体平差。

假设这样一个应用场景，在室外采集了一段时间的GPS数据（n个时刻），现在想要求出每个时刻接收机的坐标及钟差。除了滤波外，另一种可行的方法就是将所有时刻的观测值都放在一起，将所有时刻的接收机坐标和钟差也放在一起，进行整体解算，本领域内也称为整体平差。通常做法是会写出

可记

$\\bm{X}=[\\bm{x}_1,\\bm{x}_2,…\\bm{x}_n ]，\\bm{Z}=[\\boldsymbol{p}_1,\\boldsymbol{p}_2,…\\boldsymbol{p}_n]\\\\$

使（1）取得最小值的即是目标参数。更恰当的做法应该是同时考虑先验信息的约束、运动状态的约束，即有

$‖\\boldsymbol{p}_1?h(\\boldsymbol{x}_1 )‖_{\\boldsymbol{R}_1}^2+ ‖\\boldsymbol{p}_2?h(\\boldsymbol{x}_2 )‖_{\\boldsymbol{R}_2}^2+…+‖\\boldsymbol{p}_n?h(\\boldsymbol{x}_n )‖_{\\boldsymbol{R}_n}^2 + ‖\\bm{x}_1 ?g(\\bm{x}_2)‖_{\\bm{Q}_2}^2+…+ ‖\\bm{x}_{(n-1)}?g(\\bm{x}_n )‖_{\\bm{Q}_n}^2 + ‖\\bm{x}_0?f(\\bm{x}_1 )‖_{\\bm{Q}_1}^2 (2)\\\\$

相比于式（1），式（2）后面部分多出来的项分别代表运动约束和先验信息约束，该式的求解借助最小二乘即可完成。

换成贝叶斯估计角度来看待该问题，可以描述为“在已知 $\\bm{ }$ 的先验信息和观测值 $\\bm{Z}$ 的情况下，如何使得 $?p(\\bm{X}|\\bm{Z})$ 最大？”。下面将 $p(\\bm{X}|\\bm{Z})$ 拆成一系列条件概率密度函数的乘积，后面可以看到，这些拆分出来的条件概率密度函数其实也就是因子图中的因子。

已知

$p(\\bm{X}|\\bm{Z})=\\frac{p(\\bm{X},\\bm{Z})}{?p(\\bm{Z})}\\\\$

由于观测值已知了，所以求 $p(\\bm{X}|\\bm{Z})$ 最大也就等价于求联合条件概率密度函数? $p(\\bm{X},\\bm{Z})$ 最大。由条件概率公式

$p(ABC)=p(A)p(B|A)p(C|AB)\\\\$

可知

$p(\\bm{X},\\bm{Z})\\\\=p(\\bm{x}_1,\\bm{x}_2,...,\\bm{x}_n,\\bm{p}_1,\\bm{p}_2,...,\\bm{p}_n)\\\\=p(\\bm{x}_1) \\\\ *p(\\bm{x}_2|\\bm{x}_1)...p(\\bm{x}_n|\\bm{x}_{n-1})\\\\ *p(\\bm{p}_1|\\bm{x}_1)....p(\\bm{p}_n|\\bm{x}_n)$

上式中最后三列，其实正好分别对应着先验信息、运动信息、观测信息。使 $p(\\bm{X},\\bm{Z})$ 最大也就意味着要使展开式中的每一项条件概率最大。以 $p(\\bm{x}_2|\\bm{x}_1), p(\\bm{p}_1|\\bm{x}_1)$ 为例，介绍如何使得这些条件概率密度函数取到最大值：

1）考虑到实际中常用高斯模型来描述运动模型和观测模型，即有

$p(\\bm{x}_2│\\bm{x}_1 )=1/√(2 \\bm{Q}_2 ) exp?(?1/2 ‖\\bm{x}_2? (\\bm{x}_1 )‖_{\\bm{Q}_2}^2)\\\\ p(\\bm{p}_1│\\bm{x}_1 )=1/√(2 \\bm{R}_1 ) exp?(?1/2 ‖\\bm{p}_1(\\bm{x}_1 )‖_{\\bm{R}_1}^2) \\\\$ 2） $p(\\bm{x}_2│\\bm{x}_1 ), p(\\bm{p}_1|\\bm{x}_1)$ 取到最大值，即等价于 $‖\\bm{x}_2? (\\bm{x}_1 )‖_{\\bm{Q}_2}^2， ‖\\bm{p}_1(\\bm{x}_1 )‖_{\\bm{R}_1}^2$ 要取到最小值。由此可知，? $p(\\bm{X}|\\bm{Z})$ 最大值的求解过程也就等价于（2）式的整体平差过程。

通俗的来讲，因子图只是换了一种形式来表达 $p(\\bm{X}|\\bm{Z})$ 的分解。用专业一点的语言来说，因子图是一种无向二分图，由因子节点和变量节点组成。变量节点就是参数，因子节点就是用于描述参数间函数关系的表达式。用因子图可以 $p(\\bm{X},\\bm{Z})$ 的分解过程改写为：

$\\phi (\\bm{X})\\\\=\\phi(\\bm{x}_1,\\bm{x}_2,…\\bm{x}_n)\\\\=\\phi_1 (\\bm{x}_1 ) \\\\ ?\\phi_2 (\\bm{x}_2,\\bm{x}_1) …\\phi_n (\\bm{x}_n,\\bm{x}_{n-1})\\\\ ?\\phi_{ +1}(\\bm{x}_1 ) \\phi_{ +2}(\\bm{x}_2) …\\phi_2 (\\bm{x}_n)$

其中 $\\phi_i$ 就是因子节点，在表示时只需要列出其相关的变量，不用列出已知值（如观测值、已知的变量节点）。每一个因子节点本质上都是其相关变量的代价函数。图1用因子图则可以表达为

[1]Li Z, Guo J, Zhao Q. POSGO: an open-source software for GNSS pseudorange positioning based on graph optimization[J]. GPS Solutions, 2023, 27(4): 187.

[2]Wen W, Hsu L T. Towards robust GNSS positioning and real-time kinematic using factor graph optimization[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 5884-5890.

[3]Niu X, Tang H, Zhang T, et al. IC-GVINS: A robust, real-time, INS-centric GNSS-visual-inertial navigation system[J]. IEEE Robotics and Automation Letters, 2022, 8(1): 216-223.

图5 factor graph-based GNSS/IMU/Visual integration

后续可能会更新自己对稀疏化、边缘化的理解。上述思考可能存在不足，期待不同的见解。

（平时知乎不一定会经常登录，有问题也可直接联系邮箱：guange_chen@tongji.edu.cn，备注知乎-图优化）

（知乎公式编辑好麻烦。。。Q_Q）

:-)

好久好久不见了^_^ !!!!!!!! 今天我们来说一说后端。

本篇代码 pose_graph：

git clone https://github.com/mengkai98/ieskf_slam.git -b pose_graph pose_graph

本篇将作为《动手写FAST-LIO》里IESKF-SLAM的后端部分，所以在以后的环节里我们都将以之前写的前端为示例。
不使用《动手写FAST-LIO》里的前端部分可以吗？当然是可以，你可以使用任意的激光LIO框架，但是要保证输出的结果里包含①点云；②这帧点云的位姿；③点云要是360°激光雷达的点云。
本篇侧重点在图优化以及地图管理的相关操作和代码编写，并不注重于回环检测的方法。

整体的思想是先了解各个工具是如何使用的？然后再去构建这样的一个系统。

第一部分：位姿图的基本知识，以及使用Ceres求解一个图优化问题；
第二部分：回环检测部分：我们这里就采用最经典的Scan-Context来进行回环检测；
第三部分：整合前面两个部分，加入到SLAM系统中。

假设这样一个场景：园区里有一个携带了激光雷达的小机器人，它要完成对整个园区的建图，所以它要绕园区走一周，将关键帧点云和它的位姿保存下来。这样你就有了一系列带有位姿的点云，通过将点云按各自的位姿变换，很容易就可以得到拼接后的点云地图。但是系统的误差会导致路径漂移，导致原本应该闭合的路径无法闭合，如下图所示：

起点A和终点B位于同一个区域，但是从建图的结果来看，出现了漂移。你告诉小机器人A和B位于同一个区域，让小机器人去修正。该怎么做呢？

先画出位姿图，把每个关键帧的位姿使用位姿图绘制出来 (假设你的小机器人在K4的时候，身上罢工的GPS突然吐了一条定位信息出来)：

可以发现位姿图上有圆圈和连线，我们称之为顶点(vertex)和边(edge)。顶点里面存储第K帧的位姿。感觉这个更像是记录着路径的信息，这和优化又有什么关系呢？

统一上来讲，顶点里面存的是待优化的变量，也就是每一关键帧点云的位姿。通过优化每一帧的位姿，来使得建图的结果更加美好。边里面存储的是约束，注意：约束是个常量。

在上面这张图里面存在两种约束：

二元边约束：红色(回环约束)、黑色(帧间约束)
一元边约束：绿色(比如GPS数据等 )

接下来我们就可以使用图优化来构建最小二乘问题了：

帧间约束：

对于关键帧K1，K2，他们的位姿为 $T_1,T_2$ ，那么他们边约束K1与K2之间的相对位姿为： $T_{c(1,2)}=T_1^{-1}T_2$ 所以这个边对于残差的贡献为：

$e_{1,2}=Log(T_{c(1,2)}^{-1}(T_1^{-1}T_2))\\\\$

这里Log表示SE(3)的指数映射，如果不了解指数映射，我们在下面会给出具体的形式，但还是建议去了解一下这部分的知识。

可能有疑问的是： $e_{1,2}$ 不应该恒定为0吗？当然不是的，在第一次迭代时，确实是0，但是由于K1同样还受回环约束，K2还受与K3的帧间约束。因此第一次迭代后，K1，K2被优化后就产生了误差。因为我们希望整体的误差最小，所以在微调顶点的时候产生会使某些边的误差增大，同样也会使得某些边的误差减小。但不可否认的是整体的误差一定会下降。

所以要清晰: 约束是个常量，但待优化的量即 $T_1,T_2$ 他们是个变量。

回环约束：

红色的边表示回环约束，表示K1与Kn之间的相对位姿约束为 $T_{c(1,n)}$ ,同样，这也是个常量，这一般都是由回环系统匹配给出。那么它的误差方程就很简单：

$e_{1,n}=Log( T_{c(1,n)}^{-1}(T_1^{-1}T_n))\\\\$

如果二元边表示相对位姿，那么一元边往往表示绝对位姿, 一元边产生一个姿态的约束 $T_{c4}$ ，那么我们要要求K4的位姿 $T_4$ 尽可能的接近 $T_{c4}$ ，那么一元边产生的误差为：

$e_{4}=Log(T_{c4}^{-1}T_4)\\\\$ 然后我们把所有的误差的平方项加和在一起就完成了这个整个位姿图的误差方程的建立：

$e=\\sum{e_n}^Te_n\\\\$ 接下来只要使误差最小就可以了。

在求解之前，我们要考虑一件事情？所有的误差是否都可信呢？

也就是说像K1这个顶点，如果回环给出的匹配结果误差很大(这里只是举例，实际中回环检测的要求十分苛刻)，这个边的约束可能会不太好，那么还要继续把回环约束和精度其实比较高的帧间约束一起优化？？？？？

有没有一种方式来描述一个边的约束强度？约束强的边，在优化过程中可以更多相信，约束差的边在优化过程中可以尽可能的削弱它对结果的影响？

对！很聪明，就是信息矩阵。我们都知道到的，方差表示了一个分布的离散程度，方差越小分布的可信程度越高，方差越大，就表示它的可信程度比较低。那么方差的逆，信息矩阵，刚好满足了，约束能力强，可信程度高的边，信息矩阵的值就大。因此我们可以在边的约束中引入信息矩阵：

具体可以看这个，我就不再从贝叶斯讲起来了> <

https://zhuanlan.zhihu.com/p/613769587

那么残差方程可以写为：

$e=\\sum{e_n}^T\\Sigma^{-1}e_n\\\\$

为了编程方便，我们可以对是对称矩阵的信息矩阵做LLT(LU)分解：

$\\Sigma ^{-1}=LL^T=L^TL\\\\$ 那么：

$e=\\sum{e_n}^T\\Sigma^{-1}e_n\\\\=\\sum{(L^Te_n})^TL^Te_n\\\\=\\sum{(Le_n})^TLe_n\\\\$

代码我是参考官方的位姿图优化例程，大家也可以去看原版

非常轻松的写代码，因为我们这次不依赖ROS，只用简单的CMAKE工程就可以了。另外使用Ceres，你要先安装，在安装的时候，你可以参照Ceres的官方文档，为了防止你从google上拉取代码遇到问题，可以使用国内这个镜像：

git clone https://gitee.com/mirrors/ceres-solver.git

先来看工程目录：

├── CMakeLists.txt               //cmake文件
├── plot_results.py              // 绘图脚本
├── pose_graph_3d.cpp            // 主入口文件
├── pose_graph_3d_error_term.h   // ceres二元边
├── read_g2o.hpp                 // 读取g2o数据集
├── sphere_data.g2o.txt          // g2o数据
└── types.h                      // 包括顶点和边的定义

先来看数据定义吧，非常的简单：

#include <Eigen/Dense>
struct Pose3D{
    Eigen::Quaterniond q;// 姿态
    Eigen::Vector3d p; // 位置
};

struct BinaryEdge
{
    size_t id_a; // 顶点A的索引
    size_t id_b; // 顶点B的索引
    Pose3D constraint_pose;
    Eigen::Matrix<double,6,6> information;
};

是不是很简单。

然后我么来说一下这个G2O文件，文件里包含顶点和边：

// 顶点的格式：
VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
// 二元边的格式：注意后面的信息矩阵只存了上三角阵，信息矩阵是对称阵
EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT

读取的方式就不多说了，大概就是文件IO的操作，这个就不多说了。主要还是来说说pose_graph_3d_error_term 。残差块(我习惯的翻译。。。。。) 的定义：由于这里只有二元边，所以我们只需要考虑二元边(相信我，会了二元边，一元边手到擒来。)

class PoseGraph3dErrorTerm
{
public:
// 构造函数：记录一个约束 input_sqrt_information对应我们LLT分解的L矩阵
    PoseGraph3dErrorTerm(Pose3D constraint,Eigen::Matrix<double,6,6>input_sqrt_information):
                        q_constraint(constraint.q),p_constraint(constraint.p),sqrt_information(input_sqrt_information){};
    /// @brief 优化过程中计算残差
    /// @tparam T ceres::double 特定类型用于自动求导
    /// @param p_a_ptr 
    /// @param q_a_ptr 
    /// @param p_b_ptr 这几个指针都是指向优化变量的指针，这个会在构建Problem的时候指定。
    /// @param q_b_ptr 
    /// @param residuals_ptr 残差 
    /// @return 
    template<typename T>
    bool operator()(const T* const p_a_ptr,
                    const T* const q_a_ptr,
                    const T* const p_b_ptr,
                    const T* const q_b_ptr,
                    T* residuals_ptr)const{// 注意const 一个都不能少，否则无法构建CostFunction
        // 将指针映射为Eigen的类型
        Eigen::Map<const Eigen::Matrix<T,3,1>> p_a(p_a_ptr);
        Eigen::Map<const Eigen::Matrix<T,3,1>> p_b(p_b_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);
        
        //求当前的相对位姿
        Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
        Eigen::Quaternion<T> q_ab_relative = q_a_inverse*q_b;
        Eigen::Matrix<T,3,1> p_ab_relative = q_a_inverse*(p_b-p_a);

        // 求和约束之前的误差
        Eigen::Quaternion<T> delta_q = q_constraint.cast<T>()*q_ab_relative.conjugate();
        Eigen::Map<Eigen::Matrix<T,6,1>> residuals(residuals_ptr);
        residuals.template block<3,1>(0,0)= p_ab_relative-p_constraint.cast<T>();
        residuals.template block<3,1>(3,0)= T(2.0)*delta_q.vec();
        sqrt_information.cast<T>();
        // 左乘L矩阵
        residuals = sqrt_information.cast<T>()*residuals;
        return true;
        // Eigen::Quaternion

    }  
    //写一个Create函数来帮助我们简洁的构建残差块    
    static ceres::CostFunction *Create(const Pose3D& constraint, const Eigen::Matrix<double,6,6>&input_sqrt_information){
        return new ceres::AutoDiffCostFunction              // 使用自动求导
                        <PoseGraph3dErrorTerm,6,3,4,3,4>    // 6：残差有6维 3：第一个对应p_a_ptr，表示该指针指向的待优化变量的大小为3维，第二个指向p_b_ptr 4:同理 指向四元数
                        (new PoseGraph3dErrorTerm(constraint,input_sqrt_information));
    }  
private:
    Eigen::Quaterniond q_constraint;
    Eigen::Vector3d p_constraint;
    Eigen::Matrix<double,6,6> sqrt_information;

};

这里说一下残差的构成：

残差是一个六维的向量，根据SE(3)的指数映射，前三维对应位移，后三维对应位姿。

$e_{0:2}=\\Delta p\\\\ e_{3:5}=Log(\\Delta q)\\\\$ 众所周知：delta_q.vec()是四元数的虚部，这个向量表示 $v \\bullet \\sin(\\frac{\ heta}{2})$ , $v$ 表示旋转轴， $\ heta 表示绕轴旋转的角度$ 。根据极限近似的理论，当角度足够小： $\\sin(\\frac{\ heta}{2})\\approx\\frac{\ heta}{2}$ ，那么按照代码里的再乘以2，刚好有：

$e_{3:5}=Log(\\Delta q)=v*\ heta\\approx 2 \ imes v \\sin(\\frac{\ heta}{2})\\\\$ （这是个人理解，有更权威的理解方法，可以一起讨论一下）。

最后一部分啦：Ceres:::Problem 的构建以及求解：

 //*  构建Problem

    ceres::Problem *problem = new ceres::Problem();
    // 核函数, 这里就不用核函数了，当然也可以试试其他的核函数
    ceres::LossFunction* loss_function = nullptr;
    // 四元数流形，指定四元数的求导方式
    ceres::Manifold *quaternion_manifold = new ceres::EigenQuaternionManifold();
    //添加二元边约束(添加二元边产生的误差)
    for( const auto &be:binary_edges){

        // 对信息矩阵开方
        Eigen::Matrix<double,6,6>sqrt_information =be.information.llt().matrixL();
        // 构建误差函数

        ceres::CostFunction *cost_function = PoseGraph3dErrorTerm::Create(be.constraint_pose,sqrt_information);
        // 添加残差块，指定优化变量，这里的顺序对应我们计算残差时传入的变量的顺序。
        // vertex存放在vector里，id就是下标
        problem->AddResidualBlock(  cost_function,
                                    loss_function,
                                    vertexs[be.id_a].p.data(),
                                    vertexs[be.id_a].q.coeffs().data(),
                                    vertexs[be.id_b].p.data(),
                                    vertexs[be.id_b].q.coeffs().data()
        );
        //对于四元数，要设定使用四元数流形的求导方式：
        problem->SetManifold(vertexs[be.id_a].q.coeffs().data(),quaternion_manifold);
        problem->SetManifold(vertexs[be.id_b].q.coeffs().data(),quaternion_manifold);
    }
    // 对于第一个顶点，我们希望它固定
    problem->SetParameterBlockConstant(vertexs.front().q.coeffs().data());
    problem->SetParameterBlockConstant(vertexs.front().p.data());

求解：

    //* 求解
    
    // 配置求解参数 
    ceres::Solver::Options options;
    // 最大迭代次数
    options.max_num_iterations = 200;
    // 线性求解方式：考虑一下这几种求解方式在应用场景和效率上各有什么不同？
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    ceres::Solver::Summary summary;
    // 求解
    ceres::Solve(options,problem,&summary);
    // 输出求解结果
    std::cout<<summary.FullReport()<<std::endl;

如果你看到了如下信息，就ok了，可以仔细看看都输出了啥：

READ G2O FILE COMPLETED!!
Vertexs Size: 2200
Constraints Size: 8647
WORK DIR: /home/xxxx/pose_graph/

Solver Summary (v 2.2.0-eigen-(3.4.0)-lapack-suitesparse-(5.10.1)-eigensparse)

                                     Original                  Reduced
Parameter blocks                         4400                     4398
Parameters                              15400                    15393
Effective parameters                    13200                    13194
Residual blocks                          8647                     8647
Residuals                               51882                    51882

Minimizer                        TRUST_REGION
Trust region strategy     LEVENBERG_MARQUARDT
Sparse linear algebra library    SUITE_SPARSE 

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                     4398

Cost:
Initial                          1.134837e+08
Final                            1.478348e+06
Change                           1.120054e+08

Minimizer iterations                       41
Successful steps                           38
Unsuccessful steps                          3

Time (in seconds):
Preprocessor                         0.004429

  Residual only evaluation           0.021868 (41)
  Jacobian & residual evaluation     0.346038 (38)
  Linear solver                      3.161391 (41)
Minimizer                            3.604819

Postprocessor                        0.000242
Total                                3.609490

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 2.968991e-07 <=1.000000e-06)

最后在工作目录执行：

https://www.zhihu.com/topic/plot_results.py --initial_poses init_pose.txt --optimized_poses opt_pose.txt

就可以看到绘图的结果了

图1：GPMP在七轴机械臂上的验证。(a)4个机械臂的启动状态和6个末端状态，共组成24组不同的规划问题。(b)GPMP对其中一组规划问题的规划结果示例。在(c)模拟环境和(d)真实环境的执行结果

作者：Mustafa Mukadam, Xinyan Yan, and Byron Boots

发表：2016年于ICRA

摘要：运动规划是机器人技术中的一个基本工具，用于生成无碰撞、平滑的轨迹，同时满足与任务相关的约束条件。本文提出了一种利用高斯过程（Gaussian processes）进行运动规划的新方法。与大多数现有的轨迹优化算法在实践中依赖于离散状态参数化（discrete state parameterization）相比，我们将连续时间轨迹表示为由线性时变随机微分方程（linear time-varying stochastic differential equation）生成的高斯过程（GP）的样本。然后，我们提供了一种基于梯度的优化技术，以优化关于成本函数（cost functional）的连续时间轨迹。通过利用GP插值（GP interpolation），我们开发了高斯过程运动规划器（GPMP），它可以找到由少量状态（states）参数化的最优轨迹。通过在仿真中解决7-DOF机械臂规划问题，我们将我们的算法与最近的轨迹优化算法进行了基准测试，并在一个真实的7-DOF WAM臂上验证了我们的方法。

运动规划是机器人的一项基本技能，它渴望在环境中移动或操纵实现目标的物体。我们从轨迹优化的角度考虑运动规划，其中我们寻求找到一个轨迹，最小化一个给定的代价函数，同时满足任何给定的约束。

以往的大量工作都集中在轨迹优化和相关问题上。Khatib[1]开创了势场（potential fifield）的想法，其中目标位置（ goal position）是一个吸引人的极点，障碍形成排斥场。人们由此扩展除了很多类似方法，用来解决局部最小[2]、空闲空间建模[3]等问题。协变运动规划哈密顿优化（ Covariant Hamiltonian Optimization for Motion Planning，CHOMP）[4][5]利用协变梯度下降来最小化障碍和光滑代价函数，以及一个预先计算的有符号距离场用于快速碰撞检查。STOMP[6]是一种随机轨迹优化方法，它可以处理不可微的约束条件（non-differentiable constraints），方法是通过对一系列有噪声的轨迹进行采样。CHOMP和STOMP的一个重要缺点是，当存在许多约束时，需要大量的轨迹状态来推理小的障碍或找到可行的解决方案。Multigrid CHOMP[7]试图从低分辨率轨迹开始，在每次迭代中逐渐提高分辨率，以减少 CHOMP使用大量状态而导致的大量计算时间。最后，TrajOpt[8]将运动规划制定为顺序二次规划（sequential quadratic programming）。Swept volumes被认为是为了确保连续时间的安全，使TrajOpt能够以更少的状态解决更多的问题。

本文提出了一种新的高斯过程（GP）方法来进行运动规划。虽然，使用高斯过程[9]来做函数近似（function approximation）是许多机器人领域的常用方法，例如监督学习[10][11]，逆动力学建模[12][13]，强化学习[14]，路径预测[15]，同时定位和映射[16][17]，状态估计[18–20]，和控制[21]，据我们所知他们没有用于运动规划。

GPs提供了一种非常自然（natural）的方法来建模运动规划问题，与以前的方法相比有几个优点。Vector-valued GPs提供了一种标准的方法（principled way），将“连续时间轨迹”表示为“将时间映射到机器人状态”的函数。平滑的轨迹可以用少量的状态紧凑地表示，高斯过程回归（Gaussian process regression）可以在任何时间查询机器人的状态。基于这一思路（Using this insight），我们开发了高斯过程运动规划器（GPMP），这是一种新的运动规划算法，利用高斯过程插值和基于梯度的优化来优化由少量状态参数化的轨迹。我们在仿真和一个7-DOF Barrett WAM臂上评估了我们的算法，并将我们的方法与几种最近的轨迹优化算法进行了基准测试。

符号定义【笔者注】：

$\\xi$ 连续时间上的机器人状态集合。始末状态为 $ξ_0$ 和 $ξ_{N+1}$
$\\mu$ 均值
$K$ 方差
$t_0,t_{N+1}$ 始末时间
$D,d$ 配置空间的维度
$R,r$ 配置空间中每个维度中的变量数，本文R=3，分别代表关节位置、速度和加速度

传统的运动规划包括寻找一个从起始状态到目标状态的平滑的、无碰撞的轨迹。与之前的方法[4]-[8]类似，我们将运动规划视为一个优化问题，并寻找一个使预定义的代价函数最小化的轨迹（详见章节III）。与之前的使用离散化离散时间轨迹的方法不同，我们使用的是连续时间轨迹。定义t时刻的机器人状态为

$\\xi(t)=\\left\\{\\xi^d(t)\\right\\}_{d=1}^D \\quad \\xi^d(t)=\\left\\{\\xi^{d r}(t)\\right\\}_{r=1}^R （1）$

其中，D为配置空间的维度（关节数），R为配置空间中每个维度中的变量数。这里我们选择R=3（分别代表关节位置、速度和加速度），确保了我们的状态是马尔可夫性的。利用公式6中定义的运动先验中状态的马尔可夫性质，我们建立了一个精确的稀疏逆核（sparse inverse kernel）（precision matrix，精度矩阵）[16][22]，用于高效的优化和快速的GP插值（详见章节II-B节）

假设连续时间轨迹是从向量值化的（vector-valued）高斯过程（GP）中采样的

$\\xi(t) \\sim \\mathcal{G}\\mathcal{P}\\left(\\mu(t), \\mathcal{K}\\left(t, t^{\\prime}\\right)\\right) \\quad t_0<t, t^{\\prime}<t_{N+1}（2）$

其中：

$\\mu(t)$ 代表向量值化的均值函数，写作 ${\\{u^{dr}(t)\\}_{d=1,r=1}^{R,r}}$
${K}(t, t^{\\prime})$ 代表矩阵值化（matrix-valued）的协方差函数，它的组成单元 ${K}(t, t^{\\prime})_{dr,d'r'}$ 代表于 $\\xi^{dr}(t)$ 和 $\\xi^{d'r'}(t')$ 之间的协方差。

基于向量值化的高斯过程[23]的定义，轨迹上的任何N个状态的集合都具有联合高斯分布（ joint Gaussian distribution），

$\\begin{array}{r}\\xi \\sim \\mathcal{N}(\\mu, \\mathcal{K}), \\quad \\xi=\\xi_{1: N}, \\quad \\mu=\\mu_{1: N}\\\\ \\mathcal{K}_{i j}=\\mathcal{K}\\left(t_i, t_j\\right), \\quad t_1 \\leq t_i, t_j \\leq t_N \\end{array}$ （3）

起始状态和目标状态 $ξ_0$ 和 $ξ_{N+1}$ 被排除在外，因为它们是固定的。 $ξ_ i$ 表示 $t_i$ 时刻的状态。

用GPs来推理运动轨迹有几个优点。高斯过程对轨迹空间施加了一个先验分布。针对一组固定的时间参数化状态（ $t_1 \\leq t_i, t_j \\leq t_N$ ），我们可以将这些状态添加给GP，从而计算出在任意时间上的后验均值（posterior mean） $\\bar{\\xi}(\ au)$ ：

$\\begin{aligned}\\bar{\\xi}(\ au) &=\\mu(\ au)+\\mathcal{K}(\ au) \\mathcal{K}^{-1}(\\xi-\\mu) \\\\ \\mathcal{K}(\ au) &=\\left[\\begin{array}{lll}\\mathcal{K}\\left(\ au, t_1\\right) & \\ldots & \\mathcal{K}\\left(\ au, t_N\\right) \\end{array}\\right]\\end{aligned}$

GP插值指的是：轨迹不需要在高分辨率下离散，轨迹状态也不需要在时间上均匀间隔。此外，后验均值是轨迹的最大后验插值（maximum a posteriori interpolation），高频插值（high-frequency interpolation）可以用来生成一个可执行的轨迹或解释整个轨迹的成本，而不是仅仅在给定的状态下。

在使用高斯过程时，先验的选择（choice of prior）是一个重要的设计考虑因素。在本工作中，我们考虑了由线性时变（LTV）随机微分方程（stochastic differential equations，SDEs）[16]所产生的高斯过程

$\\begin{aligned}\\xi^{\\prime}(t) &=A(t) \\xi(t)+F(t) w(t) \\\\ w(t) & \\sim \\mathcal{G}\\mathcal{P}\\left(0, Q_c \\delta\\left(t-t^{\\prime}\\right)\\right), \\quad t_0<t, t^{\\prime}<t_{N+1}\\end{aligned}$ （6）

其中：

A (t)和F (t)是时变系统矩阵（time-varying system matrices），
δ（·）是Dirac delta function，
w (t)是由一个均值为零的高斯过程建模的过程噪声，
Qc是一个功率谱密度矩阵（powerspectral density matrix），是一个根据在系统[16]上收集的数据来设定的超参数。

将固定的起始状态ξ0作为初始条件， $Φ(t，s)$ 作为将状态从时间s（笔者注：start时间）传播到时间t的状态转移矩阵。求解SDE（公式6），可得t时刻状态的后验：

$\\widetilde{\\xi}(t)=\\Phi\\left(t, t_0\\right) \\xi_0+\\int_{t_0}^t \\Phi(t, s) F(s) w(s) d s$ (7)

$\\widetilde{\\xi}(t)$ 对应于一个高斯过程。通过计算“数据的”一阶和二阶矩（即均值和协方差），可以获得均值函数和协方差函数：

均值： $\\widetilde{\\mu}(t)=\\Phi\\left(t, t_0\\right) \\xi_0$ （8）

协方差： $\\begin{aligned}\\widetilde{\\mathcal{K}}\\left(t, t^{\\prime}\\right) &=E\\left[(\\widetilde{\\xi}(t)-\\widetilde{\\mu}(t))\\left(\\widetilde{\\xi}\\left(t^{\\prime}\\right)-\\widetilde{\\mu}\\left(t^{\\prime}\\right)\\right)^T\\right]\\\\ &=\\int_{t_0}^{\\min \\left(t, t^{\\prime}\\right)}\\Phi(t, s) F(s) Q_c F(s)^T \\Phi\\left(t^{\\prime}, s\\right)^T d s \\end{aligned}$ （9）

在考虑将结束状态 $ξ_{N+1}$ 作为一个无噪声的“测量”，并对 $\\widetilde{\\xi}(t)$ 在此测量上进行条件化之后，我们得到了具有固定起始和终止状态的轨迹的高斯过程。均值和协方差函数为：

$\\mu(t)=\\widetilde{\\mu}_t+\\widetilde{\\mathcal{K}}_{N+1, t}\\widetilde{\\mathcal{K}}_{N+1, N+1}^{-1}\\left(\\xi_{N+1}-\\widetilde{\\mu}_{N+1}\\right)\\\\=\\Phi_{t, 0}\\xi_0+W_t \\Phi_{N+1, t}^{\ op}W_{N+1}^{-1}\\left(\\xi_{N+1}-\\Phi_{N+1,0}\\xi_0\\right) (10)$

$\\mathcal{K}\\left(t, t^{\\prime}\\right)=\\widetilde{\\mathcal{K}}_{t, t^{\\prime}}-\\widetilde{\\mathcal{K}}_{N+1, t}^{\ op}\\widetilde{\\mathcal{K}}_{N+1, N+1}^{-1}\\widetilde{\\mathcal{K}}_{N+1, t^{\\prime}}, \\quad t<t^{\\prime}\\\\=W_t \\Phi_{t^{\\prime}, t}^{\ op}-W_t \\Phi_{N+1, t}^{\ op}W_{N+1}^{-1}\\Phi_{N+1, t^{\\prime}}W_{t^{\\prime}}, \\quad t<t^{\\prime}(11)$

其中

$W_t=\\int_t^t \\Phi(t, s) F(s) Q_c F(s)^T \\Phi\\left(t^{\\prime}, s\\right)^T d s (13)$

逆核矩阵 $K^{-1}$ 对于 N 个状态来说是完全稀疏的（block-tridiagonal form,块三对角形式），并且可以分解为：

$\\mathcal{K}^{-1}=B^T Q^{-1}B$ (13)

其中

$B=\\left[\\begin{array}{cccc}1 & 0 & \\cdots & 0 \\\\ -\\Phi\\left(t_1, t_0\\right) & 1 & \\cdots & 0 \\\\ 0 & -\\Phi\\left(t_2, t_1\\right) & \\ddots & \\vdots \\\\ \\vdots & \\vdots & \\ddots & 1 \\\\ 0 & 0 & \\cdots & -\\Phi\\left(t_N, t_{N-1}\\right) \\end{array}\\right]$ （14）

and

$\\begin{gathered}Q=\\operatorname{diag}\\left(Q_1, \\ldots, Q_{N+1}\\right), \\\\ Q_i=\\int_{t_{i-1}}^{t_i}\\Phi\\left(t_i, s\\right) F(s) Q_c F(s)^T \\Phi\\left(t_i, s\\right)^T d s \\end{gathered}$

这里Q?1是块对角线，B是带对角线。

逆核矩阵 $K^{-1}$ ，编码了状态转移矩阵 $Φ\\{t, s\\}$ 施加在连续状态上的约束。如果状态 $ξ\\{t\\}$ 包括关节角度、速度和加速度等变量，则这些约束适用于所有这些变量。如果我们将状态转移矩阵 $Φ\\{t, s\\}$ 和 $Q^{-1}$ 设置为单位标量，那么 $K^{-1}$ 将简化为 CHOMP 中通过有限差分形成的矩阵 A。

符号定义【笔者注】：

从s时刻到t时刻的状态转移矩阵 $Φ\\{t, s\\}$ 。【问】怎么计算？

高斯过程轨迹估计的一个优点是可以通过计算后验均值（公式4）从其他点对轨迹上的任意点进行插值。例如第 II-A 节中由 LTV-SDE（线性时变随机微分方程）生成的高斯过程，插值非常快速，仅涉及两个相邻状态，并且在 $O(1)$ 的时间内完成[16]。

【笔者注】 $t_{i}$ 到 $t_{i+1}$ 中间的任意时刻 $\ au$ 的插值状态的计算方法如下：

$\\bar{\\xi}(\ au)=\\mu(\ au)+[\\Lambda(\ au) \\Psi(\ au)]\\left(\\left[\\begin{array}{c}\\xi_i \\\\ \\xi_{i+1}\\end{array}\\right]-\\left[\\begin{array}{c}\\mu_i \\\\ \\mu_{i+1}\\end{array}\\right]\\right)$ （15）

其中 $t_i<\ au<t_{i+1}, i=0, \\ldots, N$

【问】 $\\mu(\ au)$ 怎么计算，是任意时刻都相同吗？

插值非常快速是因为 $K(τ)K^{?1}$ 只有的第i和第i+1个块列是非零的：

$K(τ)K^{?1}=[0，... ，0，\\Lambda(\ au)， \\Psi(\ au)， 0， ... ，0]$ （16）

其中 $\\Lambda(\ au)=\\Phi\\left(\ au, t_i\\right)-\\Psi(\ au) \\Phi\\left(t_{i+1}, t_i\\right)$ ，

其中 $\\Psi(\ au)=Q_\ au \\Phi\\left(\ au, t_i\\right)^T Q_{i+1}^{-1}$

【笔者注】其中 $Q_\ au$ 和 $Q_{i+1}$ 于公式14中定义

这种快速插值方法被应用在了我们的优化过程中（详见章节三-d）。

运动规划可以视为以下问题：

$\\begin{array}{ll}\\operatorname{minimize}& \\mathcal{U}[\\xi]\\\\ \ ext{ subject to }& f_i[\\xi]\\leq 0, i=1, \\ldots, m \\end{array}$ （17）

其中，轨迹 ξ 是一个连续时间函数，其起始状态和结束状态是固定的。 $\\mathcal{U}[\\xi]$ 是一个目标或成本函数，用于评估轨迹的质量， $f_i[\\xi]$ 是约束函数，如关节限制和任务相关的约束。为了简化表示，我们使用 ξ 来表示连续时间函数或参数化该函数的 N 个状态（公式3）。根据上下文，其含义应该是清楚的。函数式以方括号中的函数参数表示，例如 $\\mathcal{U}[\\xi]$ 。

轨迹优化的通常目标是找到平滑和无碰撞的轨迹。因此，我们将目标函数定义为两个目标的组合：

一个先验函数Fgp，由先验高斯过程决定，它惩罚轨迹从先验均值的位移以保持轨迹平滑；
另一个障碍代价函数Fobs，它惩罚与障碍物的碰撞。

因此，目标函数定义为

$\\mathcal{U}[\\xi]=\\mathcal{F}_{o b s}[\\xi]+\\lambda \\mathcal{F}_{g p}[\\xi]$ （18）

其中，两个之间的权衡（tradeoff）是由λ控制。这个目标函数看起来与CHOMP[4]中使用的类似，然而，在我们的例子中，轨迹也包含速度和加速度。另一个重要的区别是，CHOMP中的平滑代价是由有限动力学(fifinite dynamics)计算出来的，而在这里，它是由GP先验生成的。

先验函数 Fgp 是由公式 2 中的高斯过程公式引出的。

$\\mathcal{F}_{g p}[\\xi]=\\frac{1}{2}\\|\\xi-\\mu\\|_{\\mathcal{K}}^2$ （19）

其中，范数是马氏距离范数： $\\|e\\|_{\\Sigma}^2=e^{\ op}\\Sigma^{-1}e$

我们使用一个类似于CHOMP[4]，[5]中使用的障碍代价函数，它计算每个身体点通过工作空间时的工作空间障碍代价的弧长参数化线积分（arc-length parameterized line integral），并对所有身体点进行积分：

$\\begin{aligned}\\mathcal{F}_{\ ext{obs }}[\\xi]&=\\int_{\\mathcal{B}}\\int_{t_0}^{t_{N+1}}c(x)\\left\\|x^{\\prime}\\right\\| d t d u \\\\ &=\\int_{t_0}^{t_{N+1}}\\int_{\\mathcal{B}}c(x)\\left\\|x^{\\prime}\\right\\| d u d t \\end{aligned}$ （20）

其中， $B ? R^3$ 是机器人身体上的点集， $x(·) : C × B → R^3$ 是正向运动学映射，将机器人的配置 $\\mathcal{q}$ 映射到工作空间， $c(·) : R^3 → R$ 是工作空间成本函数，用于在身体点位于障碍物内部或附近时进行惩罚。在实践中，这通过预先计算的有符号距离场（precomputed signed distance fifield）来计算。 $x’$ 是工作空间中机器人山体上的一个点的速度。将速度的范数 $\\|x^{\\prime}\\|$ 与上述线积分中的代价值 $c(x)$ 相乘，可以得到弧长参数化的结果，这个结果对轨迹的重新定时具有不变性（invariant to re-timing the trajectory）。直观地说， $\\mathcal{F}_{\ ext{obs }}[\\xi]$ 鼓励机器人的轨迹绕过障碍物来最小化成本，而不是试图通过障碍物快速通过它们。

优化公式18中的目标函数U[ξ]可能非常困难，因为代价函数Fobs是非凸的。我们采用一种迭代的基于梯度的方法来最小化U[ξ]。在每次迭代中，我们通过对当前轨迹 $ξˉ$ 进行泰勒级数展开（Taylor series expansion ）来形成对成本函数的近似：

$\\mathcal{U}[\\xi]\\approx \\mathcal{U}[\\bar{\\xi}]+\\bar{\ abla}\\mathcal{U}[\\bar{\\xi}](\\xi-\\bar{\\xi})$ （21）

【笔者注】：当前轨迹ξˉ，新轨迹 $\\xi$ 。 $\\mathcal{U}[\\xi]$ 是机器人中最佳的

然后，在约束轨迹接近先前轨迹的条件下最小化近似成本。轨迹的最优扰动δξ?为：

$\\begin{aligned}\\delta \\xi^* &=\緻set{\\delta \\xi}{\\operatorname{argmin}}\\left\\{\\mathcal{U}[\\bar{\\xi}]+\\bar{\ abla}\\mathcal{U}[\\bar{\\xi}](\\xi-\\bar{\\xi})+\\frac{\\eta}{2}\\|\\xi-\\bar{\\xi}\\|_{\\mathcal{K}}^2\\right\\}\\\\ &=\緻set{\\delta \\xi}{\\operatorname{argmin}}\\left\\{\\mathcal{U}[\\bar{\\xi}]+\\bar{\ abla}\\mathcal{U}[\\bar{\\xi}]\\delta \\xi+\\frac{\\eta}{2}\\|\\delta \\xi\\|_{\\mathcal{K}}^2\\right\\}\\end{aligned}$ （22）

其中η是正则化常数。公式22可以通过对右侧进行微分（对 $\\delta \\xi$ 微分）并将结果设为零来求解：

$\\bar{\ abla}\\mathcal{U}[\\bar{\\xi}]+\\eta \\mathcal{K}^{-1}\\delta \\xi^*=0 \\quad \\Longrightarrow \\quad \\delta \\xi^*=-\\frac{1}{\\eta}\\mathcal{K}\\bar{\ abla}\\mathcal{U}[\\bar{\\xi}]$ （23）

【问】 $\\frac{\\eta}{2}\\|\\xi-\\bar{\\xi}\\|_{\\mathcal{K}}^2$ 是什么？这个方程可以通过求解导数为零的条件来得到最优的扰动 $\\delta \\xi^*$ 。具体推导和求解过程可能需要进行符号计算和优化技巧的应用。

因此，在每次迭代中，更新规则都是

$\\bar{\\xi}\\leftarrow \\bar{\\xi}+\\delta \\xi^*=\\bar{\\xi}-\\frac{1}{\\eta}\\mathcal{K}\\bar{\ abla}\\mathcal{U}[\\bar{\\xi}]$ （24）

为了完成更新，我们需要计算当前轨迹 $\\bar{\\xi}$ 下的代价函数 $\\bar{\ abla}\\mathcal{U}[\\xi]$ 的梯度，这需要找到 $\\bar{\ abla}\\mathcal{F}_{o b s}$ 和 $\\bar{\ abla}\\mathcal{F}_{g p}$ ：

$\\bar{\ abla}\\mathcal{U}[\\xi]=\\bar{\ abla}\\mathcal{F}_{o b s}+\\lambda \\bar{\ abla}\\mathcal{F}_{g p}$ （25）

下面我们将讨论这两个分量的梯度。

先验代价的梯度：成本函数Fgp可以看作是偏离规定的运动模型的惩罚轨迹。在等式中给出了一个例子37.在实验结果中（第四节）。为了计算先验代价的梯度，我们首先展开了等式19:

$\\begin{aligned}\\mathcal{F}_{g p}[\\xi]&=\\frac{1}{2}\\left(\\xi^T-\\mu^T\\right) \\mathcal{K}^{-1}(\\xi-\\mu) \\\\ &=\\frac{1}{2}\\xi^T \\mathcal{K}^{-1}\\xi-\\mu^T \\mathcal{K}^{-1}\\xi+\\frac{1}{2}\\mu^T \\mathcal{K}^{-1}\\mu \\end{aligned}$ （26）

然后对轨迹ξ的取导数

$\\bar{\ abla}\\mathcal{F}_{g p}[\\xi]=\\mathcal{K}^{-1}\\xi-\\mathcal{K}^{-1}\\mu=\\mathcal{K}^{-1}(\\xi-\\mu)$ （27）

障碍代价的梯度：对公式20障碍代价Fobs的函数求梯度，可以通过欧拉-拉格朗日方程[24]计算得到，形式为 $F[ξ]=\\int R v(ξ)dt$ ，产生梯度

$\\bar{\ abla}\\mathcal{F}[\\xi]=\\frac{\\partial v}{\\partial \\xi}-\\frac{d}{d t}\\frac{\\partial v}{\\partial \\xi^{\\prime}}$ （28）

应用公式28来找到公式20在工作空间中的梯度，然后通过运动学雅可比矩阵[25]将其映射回配置空间，并按照[26]中的证明，我们计算相对于位置、速度和加速度的梯度。

$\\bar{\ abla}\\mathcal{F}_{o b s}[\\xi]=\\left[\\begin{array}{c}\\int_{\\mathcal{B}}J^T\\left\\|x^{\\prime}\\right\\|\\left[\\left(I-\\hat{x}^{\\prime}\\hat{x}^{\\prime T}\\right) \ abla c-c \\kappa\\right]d u \\\\ \\int_{\\mathcal{B}}J^T c \\hat{x}^{\\prime}d u \\\\ 0 \\end{array}\\right]$ （29）

其中 $\\kappa=\\left\\|x^{\\prime}\\right\\|^{-2}\\left(I-\\hat{x}^{\\prime}\\hat{x}^{\\prime T}\\right) x^{\\prime \\prime}$ 是沿工作空间轨迹的曲率向量，x'和x''是由正向运动学和黑森矩阵所确定的该body point的速度和加速度， $\\hat{x}^{\\prime}=x^{\\prime}/\\left\\|x^{\\prime}\\right\\|$ 是归一化的速度向量，J是该body point的运动学雅可比矩阵。CHOMP使用有限差分来计算x'和x''，但是使用我们的状态表示（公式1），x'和x''可以分别直接从雅可比矩阵和黑森矩阵得到，因为配置空间中的速度和加速度是状态的一部分。

为了计算先验泛函和障碍泛函的梯度，我们将连续时间轨迹参数化为N个小状态， $ξ=ξ_{1:N}$ 。如果我们考虑由LTV-SDEs生成的GPs，如Sec。II-A，则根据等式中逆核矩阵的表达式，可以从观测状态中计算出GP先验代价Fgp(ξ) 13.在等式中使用B14，和等式中的μ(t) 10:

$\\begin{aligned}\\mathcal{F}_{g p}(\\xi) &=\\frac{1}{2}\\|\\xi-\\mu\\|_{\\mathcal{K}}^2=\\frac{1}{2}\\|B(\\xi-\\mu)\\|_Q^2 \\\\ &=\\frac{1}{2}\\sum_{i=1}^N\\left\\|\\Phi\\left(t_i, t_{i-1}\\right) \\xi_{i-1}-\\xi_i\\right\\|_{Q_i}^2 \\end{aligned}$ (30)

$\\bar{\ abla}\\mathcal{F}_{g p}(\\xi)=\\mathcal{K}^{-1}(\\xi-\\mu)=B^T Q^{-1}B(\\xi-\\mu)$ (31)

在N个状态下计算了障碍物成本的梯度。在实践中，ˉ?Fobs(ξ)通过对有限数量的机器人b0?b的身体点的梯度相加来进一步近似，因此障碍梯度变为：

$\\bar{\ abla}_{\\xi_i}\\mathcal{F}_{\ ext{obs }}(\\xi)=\\left[\\begin{array}{c}\\sum_{\\mathcal{B}}^{\\prime}J_i^T\\left\\|x_i^{\\prime}\\right\\|\\left[\\left(I-\\hat{x}_i^{\\prime}\\hat{x}_i^{\\prime T}\\right) \ abla c_i-c_i \\kappa_i\\right]d u \\\\ \\sum_{\\mathcal{B}}^{\\prime}J_i^T c_i \\hat{x}_i^{\\prime}d u \\\\ 0 \\end{array}\\right]$ (32)

其中，i表示该符号在ti时刻的值。

状态的更新规则可以从公式24、25和27中总结出来：

$\\bar{\\xi}\\leftarrow \\bar{\\xi}-\\frac{1}{\\eta}\\mathcal{K}\\left[\\lambda \\mathcal{K}^{-1}(\\bar{\\xi}-\\mu)+g\\right]$ (33)

其中，g是每个状态下的障碍梯度向量（公式32）。请注意，这个更新规则可以被解释为对CHOMP的更新规则的泛化，但其状态轨迹已被增强到包括速度和加速度。因此，我们将此算法称为 Augmented CHOMP

（AugCHOMP）。在下一小节中，我们充分利用GP先验和GP插值机制来进一步推广运动规划算法。

与轨迹的固定、离散状态参数化相比，高斯过程运动规划的一个关键好处是，我们可以使用GP在任何感兴趣的时间查询轨迹函数（见第II-B节）。为了在两个状态 $i$ 和 $i + 1$ 之间插值p个点，我们定义了两个聚合矩阵（aggregated matrices）：

$\\begin{aligned}\\Lambda_i &=\\left[\\begin{array}{lllll}\\Lambda_{i, 1}^T & \\ldots & \\Lambda_{i, j}^T & \\ldots & \\Lambda_{i, p}^T \\end{array}\\right]^T \\\\ \\Psi_i &=\\left[\\begin{array}{lllll}\\Psi_{i, 1}^T & \\ldots & \\Psi_{i, j}^T & \\ldots & \\Psi_{i, p}^T \\end{array}\\right]^T \\end{aligned}$ （34）

例如，如果我们想通过在每个两个轨迹状态之间插值p个点来对一个轨迹进行上采样，我们可以快速地计算出新的轨迹为

$\\begin{gathered}\\bar{\\xi}=M\\left(\\bar{\\xi}_w-\\mu_w\\right)+\\mu & （35）\\\\ \ ext{ where, }\\quad M=\\left[\\begin{array}{cccccccc}\\Psi_0 & 0 & \\ldots & \\ldots & \\ldots & \\ldots & 0 & 0 \\\\ I & 0 & \\ldots & \\ldots & \\ldots & \\ldots & 0 & 0 \\\\ \\Lambda_1 & \\Psi_1 & \\ldots & \\ldots & \\ldots & \\ldots & 0 & 0 \\\\ 0 & I & \\ldots & \\ldots & \\ldots & \\ldots & 0 & 0 \\\\ \\vdots & \\vdots & \\ddots & & & & \\vdots & \\vdots \\\\ 0 & 0 & \\ldots & I & 0 & \\ldots & 0 & 0 \\\\ 0 & 0 & \\ldots & \\Lambda_i & \\Psi_i & \\ldots & 0 & 0 \\\\ 0 & 0 & \\ldots & 0 & I & \\ldots & 0 & 0 \\\\ \\vdots & \\vdots & & & & \\ddots & \\vdots & \\vdots \\\\ 0 & 0 & \\ldots & \\ldots & \\ldots & \\ldots & 0 & I \\\\ 0 & 0 & \\ldots & \\ldots & \\ldots & \\ldots & 0 & \\Lambda_N \\end{array}\\right]\\end{gathered}$

$\\bar{ξ}_w$ 是具有稀疏状态集合的轨迹。如果我们想将计划的轨迹输入到一个控制器中，或者如果我们想在优化过程中仔细检查状态之间的碰撞，那么快速、高时间分辨率的插值在实践中是有用的。

此外，与CHOMP的离散状态公式相比，GP插值还允许我们减少表示高保真连续时间轨迹所需的状态数，从而在优化过程中实现加速，而对最终的规划几乎没有影响。其想法是修改公式33只更新稀疏状态集 $\\bar{ξ}_w$ ，但通过整个轨迹进行扫描，通过插值大量的中间点来计算障碍代价。计算了所有点（插值点和状态点）的障碍物梯度，然后使用矩阵 $M^T$ 投影回所有状态（公式36）。该方法的更新规则来源于公式33，写作如下：

$\\begin{aligned}\\bar{\\xi}& \\leftarrow \\bar{\\xi}-\\frac{1}{\\eta}\\mathcal{K}\\left[\\lambda \\mathcal{K}^{-1}(\\bar{\\xi}-\\mu)+g\\right]\\\\ \\Longrightarrow M \\bar{\\xi}_w & \\leftarrow M \\bar{\\xi}_w- \\frac{1}{\\eta}M \\mathcal{K}_w M^T\\left[\\lambda M^{-T}\\mathcal{K}_w^{-1}M^{-1}M\\left(\\bar{\\xi}_w-\\mu_w\\right)+g\\right]\\\\ \\Longrightarrow \\quad \\bar{\\xi}_w & \\leftarrow \\bar{\\xi}_w-\\frac{1}{\\eta}\\mathcal{K}_w\\left[\\lambda \\mathcal{K}_w^{-1}\\left(\\bar{\\xi}_w-\\mu_w\\right)+M^T g\\right]\\end{aligned}$ (36)

其中Kw为核， $μ_w$ 为由稀疏状态集组成的轨迹 $\\bar{ξ}_w$ 的平均值。我们将该算法（公式36）称为高斯过程运动规划器（GPMP）。

略

从实验结果（表I）中可以看出，GPMP与基准上最近的轨迹优化算法相比是有利的。GPMP能够在合理的时间内解决所有24组问题（图1），同时在状态空间中求解所有其他算法（AugCHOMP除外）的3倍于状态大小的轨迹。GPMP通过在优化过程中利用GP插值，提供了比AugCHOMP更快的速度，使得每次迭代更快，而在优化结束时轨迹质量没有任何显著损失。这在从GPMP和AugCHOMP获得的关于同一问题的示例优化轨迹的比较中得到了说明（图2）。

GPMP和AugCHOMP能够收敛到更好的解决轨迹（解决更多的问题），并且比CHOMP做得更快。这些算法的一个优点是，轨迹随速度和加速度而增强。与CHOMP相比，速度和加速度是由有限的差分计算出来的，而GPMP和AugCHOMP中的速度和加速度可以在优化过程中直接使用。这影响了目标代价及其梯度的计算，从而导致更好的梯度步长和在更少的迭代中收敛。

基准测试结果表明，TrajOpt比我们的方法更快，并且只在其中一个问题上失败了。通过将优化问题表示为序列二次规划（SQP），TrajOpt以较少的迭代次数实现了更快的收敛速度。然而，与TrajOpt相比，GPMP有几个优点：

连续表示允许GPMP只使用几个状态来参数化轨迹，以确保平滑性。
在优化过程中，GP插值可以将障碍代价从插值点传播到状态，并对输出轨迹进行上采样（保证平滑并提供无碰撞保证），从而在实际系统上执行。

我们提出了一种新的运动规划方法，利用高斯过程通过优化少量状态来推理连续时间轨迹。我们考虑了由关节位置、速度和从LTV-SDE生成的GP中采样的加速度组成的轨迹，并且我们提供了一种基于梯度的优化技术来解决运动规划问题。高斯过程机制使我们能够在任何感兴趣的时间点查询轨迹，这允许我们生成可执行的轨迹或推理，关于整个轨迹的成本，而不是仅仅在状态下。我们在一组7-DOF机械臂规划问题上对我们的算法与最新的各种轨迹优化算法进行了基准测试，并通过在一个7-DOF Barrett WAM臂上运行它们来验证我们的算法。我们的经验结果表明，GPMP在速度和解决问题的数量方面优于竞争算法。

图1. 轨迹优化问题的因子图。图中显示了优化后的状态（白色圆圈）以及三种类型的因子（黑色点），分别是1)起始和目标状态的先验因子，2)每个状态的障碍成本因子，以及3)连接相邻状态的高斯过程（GP）先验因子。

作者：Jing Dong, Mustafa Mukadam, Frank Dellaert, and Byron Boots

发表：2016年于 Robotics: Science and Systems

创新点：相比于GPMP，GPMP2在运动规划中实现了基于iSAM的重规划问题

疑问：各因子的雅可比怎么推导？

摘要：随着必须实时执行任务的高自由度机器人的使用不断增加，因此需要快速的运动规划算法。在这项工作中，我们从概率的角度来看待运动规划。我们将光滑的连续时间轨迹视为来自高斯过程（GP）的样本，并将规划问题表述为概率推理。我们使用因子图和数值优化来快速执行推理，并展示了GP插值如何进一步提高算法的速度。我们的框架还允许我们逐步更新规划问题的解决方案，以应对不断变化的条件。我们将我们的算法与几个最近的轨迹优化算法的规划问题在多个环境中。我们的评估显示，我们的方法比以前的算法快几倍，同时保持了鲁棒性。最后，我们演示了我们的算法对重新规划问题的增量版本，并表明它通常可以在从头开始重新规划所需的一小部分时间内找到成功的解决方案

运动规划是机器人技术中的一个基本工具。目标是在机器人的配置空间中找到一个既可行又最优的轨迹。在这项工作中，可行的轨迹使机器人从一个开始到目标配置，同时保持无碰撞和服从机器人的物理限制。最优性与轨迹平滑度有关，换句话说，使速度或加速度等动力学标准最小化的轨迹被认为比没有[12,26,30]的轨迹更最优。以往的工作都集中在基于采样的算法和轨迹优化方法的运动规划。

基于采样的算法，如概率道路图( Probabilistic Road Maps)[13]和快速探索随机树(Rapidly exploring Random Trees)[16,17]，可以在复杂的高维配置空间中生成可行的轨迹。然而，由于随机抽样的性质，基于采样的算法产生的轨迹往往缺乏质量，可能导致不稳定和冗余的运动。当构造空间很少有障碍物或对导航有严格的限制时，问题进一步复杂。

轨迹优化(Trajectory optimization)试图找到平滑的、无碰撞的轨迹，以优化一个成本函数。例如，CHOMP和相关方法[2,8,24,30]使用协变梯度下降来优化成本函数，而STOMP[12]通过对噪声轨迹的随机抽样来优化不可微约束。这些方法的缺点之一是在实践中轨迹被精细地离散，因此优化器可以对环境中的障碍进行推理，并保证求解的平滑性。不幸的是，这导致了很高的计算成本。TrajOpt[25,26]试图通过将轨迹优化定义为序列二次规划和执行连续时间碰撞检查来克服这一问题并加快求解时间。这使得TrajOpt在理论上可以使用更少的状态来表示轨迹，但在更复杂的环境中以及需要在输出轨迹中平滑时，仍然需要密集参数化的轨迹。

连续时间轨迹表示(Continuous-time trajectory representations)可以克服使用大量状态所产生的计算代价。b样条[6]和核方法[19]被用于表示较少状态的轨迹。高斯过程运动规划器（GPMP）[20博客链接]只参数化少数状态的轨迹，然后使用高斯过程（GP）插值在任何感兴趣的时间查询轨迹。

在过去的工作（即高斯过程运动规划器，GPMP）中，我们从概率推理的角度将运动规划视为轨迹优化，[27,28]。我们将连续时间轨迹表示为来自GP[20]的样本，然后通过一个概率图形模型(probabilistic graphical model)来制定规划问题。我们使用解决非线性最小二乘优化问题的数值工具对图进行推理，并利用底层线性系统的稀疏性生成快速解。

通过将运动规划视为推理问题（inference），我们就能够从机器人技术的其他领域借用工具。同步定位和建图（SLAM）多年来一直专注于高效的优化算法，其中一个更成功的方法是平滑和映射（SAM）算法家族[4]。通过将SLAM构造为因子图[15]中的推理，并利用底层大规模线性系统的稀疏性，SAM可以执行有效的推理。我们将这些方法应用到我们的运动规划算法中，实现了比以往方法速度的显著提高。

重新规划问题需要对轨迹进行修改，如改变目标位置。早期的重新规划工作包括D*[14]和Anytime A*[18]，但这些算法都是为有限维数的离散状态空间制定的。最近，ITOMP[21]能够通过调度程序完成流畅的重新计划来强制执行硬定时的最后期限，但不能保证最优甚至可行的解决方案。Park等人[22]建议使用gpu进行并行计算，以显著加快重新规划，但这种方法对降低实际计算成本作用甚微。

我们提供了一种来自SLAM启发的重新规划的方法。增量平滑和映射（Incremental Smoothing and Mapping，iSAM）[9,11]对增量和实时的因子图进行推理。通过使用增量求解器[11]，我们避免了从头开始重新解决整个规划问题，并且只在需要的地方更新轨迹。这大大降低了我们的方法的总体计算成本，从而实现了快速的重新规划。

【笔者注】iSAM是gtsam的主要创新点之一

根据 Toussaint等人[27,28]的惯例，我们将轨迹优化问题视为概率推理问题（probabilistic inference）。在这个框架中，目标是找到最大后验轨迹（maximum a posteriori trajectory，MAP trajectory），基于给定的先验分布和似然函数。其中，先验分布（prior distribution）鼓励轨迹的平滑性，似然函数（likelihood function）鼓励轨迹是无碰撞的。

我们将轨迹表示为一个连续值函数，映射时间 $t$ 到机器人状态 $\\boldsymbol{\ heta}(t)$ 。我们通过概率推理找到了MAP函数 $θ^?$ 。这涉及到几个步骤，我们先在这里简单介绍，然后在下面详细描述。

首先，在轨迹的函数空间中直接放置一个鼓励平滑性的高斯过程先验（Gaussian process prior）（第II-A节）。接下来，我们指定一个似然函数（likelihood function），鼓励无碰撞的轨迹（第II-B节）。然后，MAP估计值（MAP estimate）从后验分布中计算得出（第II-C节）。最后，在后验分布的拉普拉斯近似下，可以在任何感兴趣的时间计算出平均轨迹来查询（第II-D节）

向量值化的（vector-valued）的高斯过程（GP）是一组随机变量（random variables），其中任意有限数量的变量具有联合高斯分布(joint Gaussian distribution)。我们定义了一个先验分布，描述了轨迹 $\\boldsymbol{\ heta}(t)$ 的分布，其中 $\\boldsymbol{\ heta}(t) \\sim \\mathcal{G}\\mathcal{P}\\left(\\boldsymbol{\\mu}(t), \\mathcal{K}\\left(t, t^{\\prime}\\right)\\right)$ ，其中 $\\mu (t)$ 是向量值化的均值函数， $\\mathcal{K}\\left(t, t^{\\prime}\\right)$ 是矩阵值化的协方差函数。

【问】t'是什么？

利用高斯过程的框架，我们可以说对于任意“一组”时间点 $t=\\{t_0, . . . , t_N\\}$ ，轨迹 $\ heta$ 具有联合高斯分布：

$\\boldsymbol{\ heta}\\doteq\\left[\\begin{array}{lll}\\boldsymbol{\ heta}_0 & \\ldots & \\boldsymbol{\ heta}_N \\end{array}\\right]^{\ op}\\sim \\mathcal{N}(\\boldsymbol{\\mu}, \\mathcal{K})$ （1）

将均值向量μ和协方差核 $\\mathcal{K}$ （covariance kernel）定义为

$\\boldsymbol{\\mu}\\doteq\\left[\\begin{array}{lll}\\boldsymbol{\\mu}\\left(t_0\\right) & \\ldots & \\boldsymbol{\\mu}\\left(t_N\\right) \\end{array}\\right]^{\ op},\\left.\\mathcal{K}\\doteq\\left[\\begin{array}{l}\\mathcal{K}\\left(t_i, t_j\\right) \\end{array}\\right]\\right|_{i j, 0 \\leq i, j \\leq N}$ （2）

高斯过程的光滑性和泛化性质（smoothness and generalization properties）是由协方差核 $\\mathcal{K}$ 编码的。与先前关于高斯过程运动规划的工作类似[20博客链接]，我们采用由线性时变随机微分方程（LTV-SDE）[1]生成的结构化协方差函数：

$\\dot{\\boldsymbol{\ heta}}(t)=\\mathbf{A}(t) \\boldsymbol{\ heta}(t)+\\mathbf{u}(t)+\\mathbf{F}(t) \\mathbf{w}(t)$ （3）

其中，u(t)是已知的系统控制输入，A(t)和F(t)是系统的时变矩阵，w(t)是表示为白噪声的随机过程，可以表示为

【问】gpmp2开源程序中有没有对这个有定义？怎么计算u(t)，A(t)和F(t)。怎么起作用？

$\\mathbf{w}(t) \\sim \\mathcal{G}\\mathcal{P}\\left(\\mathbf{0}, \\mathbf{Q}_C \\delta\\left(t-t^{\\prime}\\right)\\right)$ （4）

其中， $Q_C$ 是功率谱密度矩阵(power-spectral density matrix)， $δ(t?t')$ 是Dirac delta函数。

公式（1）高斯过程的均值和协方差是通过求解方程（3）中的LTV-SDE得到的：

$\\boldsymbol{\\mu}(t)=\\boldsymbol{\\Phi}\\left(t, t_0\\right) \\boldsymbol{\\mu}_0+\\int_{t_0}^t \\boldsymbol{\\Phi}(t, s) \\mathbf{u}(s) d s$ （5）

【问】这里的s什么意思？

$\\begin{aligned}& \\mathcal{K}\\left(t, t^{\\prime}\\right)=\\mathbf{\\Phi}\\left(t, t_0\\right) \\mathcal{K}_0 \\mathbf{\\Phi}\\left(t^{\\prime}, t_0\\right)^{\ op}\\\\ & \\quad+\\int_{t_0}^{\\min \\left(t, t^{\\prime}\\right)}\\boldsymbol{\\Phi}(t, s) \\mathbf{F}(s) \\mathbf{Q}_C \\mathbf{F}(s)^{\ op}\\boldsymbol{\\Phi}\\left(t^{\\prime}, s\\right)^{\ op}d s \\end{aligned}$ （6）

其中， $\\boldsymbol{\\mu}_0$ 是第一个状态的初始均值， $\\mathcal{K}_0$ 是第一个状态的协方差， $Φ(t, s)$ 是从 $s$ 时刻到 $t$ 时刻的状态转移矩阵[1]。

然后，高斯过程的先验分布可以根据其均值μ和协方差K来定义：

$P(\\boldsymbol{\ heta}) \\propto \\exp \\left\\{-\\frac{1}{2}\\|\\boldsymbol{\ heta}-\\boldsymbol{\\mu}\\|_{\\mathcal{K}}^2\\right\\}$ （7）

【笔者注】（1）二维正态分布 $\ heta \\sim N\\left(\\mu, \\sigma^2\\right)$ 的概率密度函数为 $f(\ heta)=\\frac{1}{\\sqrt{2 \\pi}\\sigma}\\exp \\left(-\\frac{(\ heta-\\mu)^2}{2 \\sigma^2}\\right)$
范数是马氏距离范数： $\\|e\\|_{\\mathcal{K}}^2=e^{\ op}\\mathcal{K}^{-1}e$

这个先验的好处在于，由于LTV-SDE的马尔可夫性质，逆核矩阵 $K^{-1}$ 被证明是精确稀疏的（块三对角形式）。正如我们将在第III-A节中看到的那样，这个核函数允许进行快速的、基于结构的推断。

似然函数由条件分布给出，记作 $L_{obs}(θ_i | c_i=0)=P(c_i=0 | θ_i)$ ，它指定了在给定当前配置 $θ_i$ 的情况下不发生碰撞的概率。

【问】为什么 $L_{obs}(θ_i | c_i=0)=P(c_i=0 | θ_i)$
按照贝叶斯定理里面说的，后验概率=似然函数×先验概率/证据。其中，似然函数表示观测数据在给定参数（例如slam中传感器的位姿）下的偏差概率，先验概率表示参数的先验分布，证据是归一化因子，确保后验概率的总和为1。

我们将似然函数定义为指数族(exponential family)中的一个分布： $L_{o b s}\\left(\\boldsymbol{\ heta}_i \\mid c_i=0\\right) \\propto \\exp \\left\\{-\\frac{1}{2}\\left\\|\\mathbf{h}\\left(\\boldsymbol{\ heta}_i\\right)\\right\\|_{\\boldsymbol{\\Sigma}_{o b s_i}}^2\\right\\}$ （8）

其中， $\\mathbf{h}(\\boldsymbol{\ heta}_i)$ 代表向量值化的障碍成本函数， $\\boldsymbol{\\Sigma}_{obs_i}^{-1}$ 则是分布的超参数。我们在实现中使用的具体似然函数和障碍成本函数在第IV-B节中有详细说明。

【问】 $\\mathbf{h}(\\boldsymbol{\ heta}_i)$ 在没有障碍物时等于0，应该是发生碰撞的概率，怎么用来表示 $P(c_i=0 | θ_i)$
以上两个代价项在图优化中的雅可比怎么计算？

给定一个高斯过程先验和指数族似然函数，目标是计算MAP后验轨迹

$\\begin{aligned}\\boldsymbol{\ heta}^* &=\緻set{\\boldsymbol{\ heta}}{\\operatorname{argmax}}\\left\\{P(\\boldsymbol{\ heta}) \\prod_i P\\left(c_i \\mid \\boldsymbol{\ heta}_i\\right)\\right\\}, & & & (9)\\\\ &=\緻set{\\boldsymbol{\ heta}}{\\operatorname{argmin}}\\left\\{-\\log \\left(P(\\boldsymbol{\ heta}) \\prod_i P\\left(c_i \\mid \\boldsymbol{\ heta}_i\\right)\\right)\\right\\}, & & & (10)\\\\ &=\緻set{\\boldsymbol{\ heta}}{\\operatorname{argmin}}\\left\\{\\frac{1}{2}\\|\\boldsymbol{\ heta}-\\boldsymbol{\\mu}\\|_{\\mathcal{K}}^2+\\frac{1}{2}\\|\\boldsymbol{h}(\\boldsymbol{\ heta})\\|_{\\boldsymbol{\\Sigma}_{o b s}}^2\\right\\}, & & & (11) \\end{aligned}$

【笔者注】其中 $P(\\boldsymbol{\ heta})$ 是高斯先验。 $P(c_i \\mid \\boldsymbol{\ heta}_i)$ 代表配置空间 $\ heta_i$ 不发生碰撞的概率，用 $\\prod_i$ 表示整体不发生碰撞的概率。
是不是应该是 $\緻set{\\boldsymbol{\ heta}}{\\operatorname{argmin}}\\left\\{\\frac{1}{2}\\|\\boldsymbol{\ heta}-\\boldsymbol{\\mu}\\|_{\\mathcal{K}}^2+\\frac{1}{2}\\sum_i \\|\\boldsymbol{h}(\\boldsymbol{\ heta_i})\\|_{\\boldsymbol{\\Sigma}_{o b s_i}}^2\\right\\}$

其中 $\\boldsymbol{h}(\\boldsymbol{\ heta}) \\doteq\\left[\\mathbf{h}\\left(\\boldsymbol{\ heta}_0\\right) \\ldots \\mathbf{h}\\left(\\boldsymbol{\ heta}_N\\right)\\right]^{\ op}$ 和等式（11），来自于等式（7）和等式（8）。

方程（11）展示了概率推断和优化之间的对偶性（duality），它们是对运动规划问题的两种不同视角。方程（11）中的各项也可以被视为信息（information）或需要减少的“成本”（cost）。因此，MAP估计问题可以转化为一个（可能是非线性的）最小二乘问题，这个问题已经得到了广泛研究，并且有许多数值工具可供使用。

迭代方法，如Gauss-Newton或Levenberg-Marquardt，反复解决方程（11）的线性化近似，直到收敛。线性化的过程涉及到对 $h(θ_i)$ 进行线性化处理。

$\\mathbf{h}\\left(\\boldsymbol{\ heta}_i\\right) \\approx \\mathbf{h}_i\\left(\\overline{\\boldsymbol{\ heta}_i}\\right)+\\mathbf{H}_i \\delta \\boldsymbol{\ heta}_i,\\left.\\mathbf{H}_i \\doteq \\frac{\\partial \\mathbf{h}_i}{\\partial \\boldsymbol{\ heta}_i}\\right|_{\\overline{\\boldsymbol{\ heta}_i}\\left(t_i\\right)},$ (12)

【问】 $t_i$ 是什么？

在这里， $H_i$ 是 $h(θ_i)$ 的雅可比矩阵。

Hi怎么表达？

我们将方程（11）的非线性最小二乘问题转化为了线性化点(linearization point) $\\overline{\\boldsymbol{\ heta}}$ 附近的线性问题。

$\\delta \\boldsymbol{\ heta}^*=\緻set{\\delta \\boldsymbol{\ heta}}{\\operatorname{argmin}}\\left\\{\\frac{1}{2}\\|\\overline{\\boldsymbol{\ heta}}+\\delta \\boldsymbol{\ heta}-\\boldsymbol{\\mu}\\|_{\\mathcal{K}}^2+\\frac{1}{2}\\|\\boldsymbol{h}(\\overline{\\boldsymbol{\ heta}})+\\mathbf{H}\\delta \\boldsymbol{\ heta}\\|_{\\boldsymbol{\\Sigma}_{o b s}}^2\\right\\},$ （13）

在这里， $\\mathbf{H}\\doteq \\operatorname{diag}\\left(\\mathbf{H}_0, \\ldots, \\mathbf{H}_N\\right)$ 对角矩阵。最优扰动 $\\delta \\boldsymbol{\ heta}^*$ 可以通过解下面的线性系统获得：

$\\left(\\mathcal{K}^{-1}+\\mathbf{H}^{\ op}\\boldsymbol{\\Sigma}_{o b s}^{-1}\\mathbf{H}\\right) \\delta \\boldsymbol{\ heta}^*=\\mathcal{K}^{-1}(\\boldsymbol{\\mu}-\\overline{\\boldsymbol{\ heta}})-\\mathbf{H}^{\ op}\\boldsymbol{\\Sigma}_{\ ext{obs }}^{-1}\\boldsymbol{h}(\\overline{\\boldsymbol{\ heta}})$ （14）

一旦线性系统被求解，就会应用迭代更新 $\\overlineθ ← \\overlineθ + δθ^?$ ，直到满足收敛条件。

在[1,20,29]之后，任意时间τ下的轨迹后验均值可以用拉普拉斯的方法来近似，并用时间点t[23]处的轨迹 $\\overlineθ$ 表示：

$\\overline{\\boldsymbol{\ heta}}(\ au)=\\mathcal{K}(\ au, \\boldsymbol{t}) \\mathcal{K}^{-1}\\overline{\\boldsymbol{\ heta}}$ (15)

尽管方程（15）中的插值需要 $O(N)$ 的操作，但通过利用第II-A节中介绍的稀疏高斯过程（GP）先验的结构，可以在 $O(1)$ 的时间复杂度内计算 $\\overline θ(τ)$ [1]。由于LTV-SDE是马尔可夫过程，τ时刻（处于ti和ti+1之间）的θ(τ)是仅由相邻的函数值θi和θi+1的线性组合，所以可以高效地计算，方法如下：

【笔者注】这种方法利用了稀疏高斯过程先验的结构，通过仅考虑相邻的函数值，避免了对所有函数值进行插值的计算复杂度，并提高了计算效率。

$\\begin{aligned}\\overline{\\boldsymbol{\ heta}}(\ au) &=\\boldsymbol{\\mu}(\ au)+\\boldsymbol{\\Lambda}(\ au)\\left(\\bar{\ heta}_i-\\boldsymbol{\\mu}_i\\right)+\\boldsymbol{\\Psi}(\ au)\\left(\\bar{\ heta}_{i+1}-\\boldsymbol{\\mu}_{i+1}\\right) &（16） \\\\ \\boldsymbol{\\Lambda}(\ au) &=\\boldsymbol{\\Phi}\\left(\ au, t_i\\right)-\\mathbf{Q}_\ au \\boldsymbol{\\Phi}\\left(\ au, t_i\\right)^{\ op}\\mathbf{Q}_{i+1}^{-1}\\boldsymbol{\\Phi}\\left(t_{i+1}, t_i\\right) &（17）\\\\ \\boldsymbol{\\Psi}(\ au) &=\\mathbf{Q}_\ au \\boldsymbol{\\Phi}\\left(\ au, t_i\\right)^{\ op}\\mathbf{Q}_{i+1}^{-1}&（18） \\end{aligned}$

其中

$\\mathbf{Q}_i=\\int_{t_{i-1}}^{t_i}\\boldsymbol{\\Phi}\\left(t_i, s\\right) \\mathbf{F}(s) \\mathbf{Q}_c \\mathbf{F}(s)^{\ op}\\boldsymbol{\\Phi}\\left(t_i, s\\right)^{\ op}d s$ （19）

在第三节-B中，我们将描述如何在轨迹优化过程中使用GP插值，以显著加快推理

我们现在描述了如何使用因子图和有效的轨迹插值来执行快速推理，以及如何使用增量更新来解决诸如快速重新规划等问题。

如果方程（14）中的线性系统是稀疏的，那么可以通过利用稀疏的Cholesky分解后跟前向-后向传递来高效地求解 δθ?[7]。幸运的是，这正是我们的情况（方程（14）中的线性系统确实是稀疏的）：H是块对角矩阵， $H^T Σ^{-1}_{obs}H$ 的块对角性质是trivial的，并且我们选择了一个具有块三对角 $K^{-1}$ 的高斯过程先验（见第II-A节）。

通过保持方程（14）中的稀疏线性系统，MAP轨迹优化问题可以等效地看作对因子图进行推理的问题[15]。因子图G={Θ，F，E}是一个二部图(bipartite graph)，表示函数的因子分解(factorization)（参见方程（20））。Θ={θ0，...，θN}是变量集合，F={f0，...，fM}是因子集合，其中fi是变量子集Θi上的函数，E是连接到两种类型节点的边。

$f(\\boldsymbol{\\Theta})=\\prod_i f_i\\left(\\boldsymbol{\\Theta}_i\\right)$ （20）

在这项工作中，因子代表先验函数和似然函数。

定义GP的先验因子：

$f_{g p}\\left(\\boldsymbol{\ heta}_{i-1}, \\boldsymbol{\ heta}_i\\right) \\doteq \\exp \\left\\{-\\frac{1}{2}\\left\\|\\mathbf{u}_i-\\boldsymbol{\ heta}_i+\\boldsymbol{\\Phi}\\left(t_i, t_{i-1}\\right) \\boldsymbol{\ heta}_{i-1}\\right\\|_{\\mathbf{Q}_i}^2\\right\\}$ （21.1）

【问】为什么于公式（7）不一样。 $\\boldsymbol{\ heta}_{i-1}$ 为什么也进入了？

其中 $\\mathbf{u}_i=\\int_{t_{i-1}}^{t_i}\\mathbf{\\Phi}\\left(t_i, s\\right) \\mathbf{u}(s) d s$ ， $Φ(t_i, t_{i-1})$ 表示 $t_{i-1}$ 时刻到 $t_i$ 时刻的状态转移矩阵[1]。

和公式（5）很像。起什么作用？s表示start时刻？ds中的s什么含义？

障碍因素的定义为

$f_{o b s}\\left(\\boldsymbol{\ heta}_i\\right)=L_{o b s}\\left(\\boldsymbol{\ heta}_i \\mid c_i=0\\right)$ （21.2）

$f_{o b s}\\left(\\boldsymbol{\ heta}_i\\right)=exp\\{ -\\frac{1}{2}\\|h({\\boldsymbol{\ heta}}_i) \\|_{\\boldsymbol{\\Sigma}_{o b s}}^2\\}$

图1展示了一个示例因子图。在因子图上进行推理等效于解决方程（9）中的MAP估计问题。这种等价性在SLAM社区[4]已经被利用了多年，导致了一系列可以用于高效推理的工具[9,10,11,29]。这一见解（即前面两句）在第III-C节中至关重要，我们在III-C节中描述了如何根据新的条件逐步更新MAP轨迹。

B. 通过轨迹插值进行更快的推断 Even Faster Inference through Trajectory Interpolation

在实际的轨迹优化中，通常会将轨迹表示为一组离散的时间点[12,21,24,25]。通常需要对时间点进行密集采样，以便考虑碰撞等因素，这使得优化器需要处理大量的参数，增加了计算成本。

图2：一个不同分辨率的轨迹。系统状态将轨迹参数化（parameterize the trajectory），在优化过程中以更高的分辨率进行碰撞检查，输出轨迹可以进一步上采样以便执行。

在运动规划中使用高斯过程的主要好处之一是，轨迹被表示为可以通过高斯过程插值（第II-D节）在任意感兴趣的时间查询的函数[20]。关键的洞察力是，平滑的轨迹可以用非常少的状态来表示，但通过在状态之间进行密集插值，仍然可以考虑到丰富的障碍物信息。这在图2中对于两个时间点之间的轨迹部分进行了说明。

为了在优化过程中考虑状态之间的碰撞情况，实现了一种新版本的障碍物因子。与图1中的障碍物因子不同，后者是一元的，仅计算与连接状态的碰撞成本，新的障碍物因子是二元的，并与其两个相邻状态θ_i和θ_i+1相连接。对于任意时间τ，ti < τ < ti+1，状态θ(τ)通过方程（16）进行插值，并计算障碍物成本 $h( \\overline{\\boldsymbol{\ heta}}(\ au))$ 。插值状态的成本然后被纳入二元因子中，以 $f_{o b s}\\left(\\boldsymbol{\ heta}_i, \\boldsymbol{\ heta}_{i+1}\\right)$ 表达：

$f_{o b s}\\left(\\boldsymbol{\ heta}_i, \\boldsymbol{\ heta}_{i+1}\\right) \\doteq exp\\{ -\\frac{1}{2}\\|h(\\overline{\\boldsymbol{\ heta}}(\ au)) \\|_{\\boldsymbol{\\Sigma}_{o b s}}^2\\}\\\\=\\exp \\{ -\\frac{1}{2}\\| \\mathbf{h}\\left[\\boldsymbol{\\mu}(\ au)+ \\boldsymbol{\\Lambda}(\ au)\\left(\\boldsymbol{\ heta}_i-\\boldsymbol{\\mu}_i\\right)\\right. \\left.\\left.+\\boldsymbol{\\Psi}(\ au)\\left(\\boldsymbol{\ heta}_{i+1}-\\boldsymbol{\\mu}_{i+1}\\right)\\right]\\|_{\\boldsymbol{\\Sigma}_{o b s}}^2\\right\\}$ （22）

这种表示方式允许在轨迹优化过程中考虑状态之间的碰撞情况。通过在状态之间进行密集的高斯过程插值，可以充分利用轨迹的平滑性，并根据插值状态的障碍物成本对优化进行约束。这使得轨迹规划可以在保持轨迹平滑性的同时，考虑到丰富的障碍物信息，提高规划的质量和安全性。

综上所述，插值的障碍因子将时间τ时的障碍信息纳入因子图中，并以此更新离散状态θi和θi+1。一旦优化收敛，就可以通过密集插值轨迹生成执行轨迹，如图2所示。

除了运动规划，我们还考虑再规划问题：给定一个已解决的运动规划问题和新的条件，解决新的问题。再规划问题在现实世界中经常遇到，例如：

末端执行器的目标位置已移动或
机器人接收到有关其当前配置的更新信息。由于再规划是在机器人运行过程中进行的，可能在动态环境中，实时再规划对于确保安全至关重要。

解决该问题的一种朴素方法是从头开始运行轨迹优化来进行重新规划。然而，如果问题的大部分内容保持不变，重新解决整个问题会产生重复工作。我们建议采用增量方法根据新信息更新当前解决方案。

为了完成这个任务，我们再次借鉴了SLAM（同时定位与地图构建）社区提供的在因子图上执行增量推断的高效工具[9, 11, 29]。

具体而言，我们使用Bayes Tree[10, 11]对因子图[29]进行增量推断。Bayes Tree 是一种类似于团树但有向的数据结构。图3的上部显示了一个简单运动规划问题的因子图：给定起始状态、目标状态和障碍成本因子，解决最大后验（MAP）推断问题以找到最优轨迹。图3中的Bayes Tree是由相应的因子图和从第一个状态到最后一个状态的消除顺序生成的。如果在图中添加或删除任何因子，只有部分Bayes Tree 会根据因子的添加或删除位置进行更新。详细信息请参阅[10, 11]。

图3：用贝叶斯树重新规划的例子。中间和下面示例中的虚线框表示在执行重新规划时和贝叶斯树的因子图中受影响和更改的部分。

图3中展示了两个再规划的示例。中间的示例显示了当目标状态改变时的再规划。当Bayes Tree 根据新的目标进行更新时，只有树的根节点会改变。下方的示例展示了给定当前执行器配置的观测（例如来自执行过程中的感知），该观测被添加到轨迹的中间位置θ2。当Bayes Tree更新时，只有与剩余轨迹对应的树的部分会改变。

在我们的实现中，我们使用增量求解器 iSAM2[11]，并结合高斯过程插值因子[29]来解决再规划问题：首先，我们收集额外的信息。其次，我们形成需要添加或替换因子图中的因子。最后，我们运行算法1来更新iSAM2中的Bayes Tree，以获得新的最优解。

在这里，我们提供了关于我们为解决运动规划问题的实际实施的技术细节。

公式(3) $\\dot{\\boldsymbol{\ heta}}(t)=\\mathbf{A}(t) \\boldsymbol{\ heta}(t)+\\mathbf{u}(t)+\\mathbf{F}(t) \\mathbf{w}(t)$ ，其中， $\\dot{\\boldsymbol{\ heta}}(t)$ 代表速度，u(t)是已知的系统控制输入，A(t)和F(t)是系统的时变矩阵，w(t)是表示为白噪声的随机过程

在我们的实现中，我们使用“恒速”GP先验[1]，它应用了白噪声模型

$\\ddot{\\boldsymbol{\\xi}}=\\mathbf{w}(t),$ （23）

其中， $\\boldsymbol{\\xi}\\in \\mathbb{R}^D$ 是D维向量空间中的系统配置（例如，D=机械手臂的关节数）。对于等式（3）中的模型，ξ不是一个有效的马尔可夫状态，但是

$\\boldsymbol{\ heta}(t)=\\left[\\begin{array}{l}\\boldsymbol{\\xi}(t) \\\\ \\dot{\\boldsymbol{\\xi}}(t) \\end{array}\\right]$ （24）

在等式（3）的LVT-SDE模型中是马尔可夫。我们通过等式（3）中的SDE来表示先验的恒定速度，其中

$\\mathbf{A}(t)=\\left[\\begin{array}{ll}\\mathbf{0}& \\mathbf{1}\\\\ \\mathbf{0}& \\mathbf{0}\\end{array}\\right], \\mathbf{u}(t)=\\mathbf{0}, \\mathbf{F}(t)=\\left[\\begin{array}{l}\\mathbf{0}\\\\ \\mathbf{1}\\end{array}\\right]$ （25）

说明对速度的系统控制输入为0，
$\\begin{aligned}\\dot{\\boldsymbol{\ heta}}(t) &=\\left[\\begin{array}{l}\\dot{\\boldsymbol{\\xi}}(t) \\\\ \\ddot{\\boldsymbol{\\xi}}(t) \\end{array}\\right]=\\mathbf{A}(t) \\boldsymbol{\ heta}(t)+\\mathbf{u}(t)+\\mathbf{F}(t) \\mathbf{w}(t)\\\\=&{\\left[\\begin{array}{ll}0 & 1 \\\\ 0 & 0 \\end{array}\\right]\\cdot\\left[\\begin{array}{l}\\xi(t) \\\\ \\dot{\\xi}(t) \\end{array}\\right]+0+\\left[\\begin{array}{l}0 \\\\ 1 \\end{array}\\right]\\cdot w(t) }\\\\=&{\\left[\\begin{array}{l}\\dot{\\xi}(t) \\\\ 0 \\end{array}\\right]+0+\\left[\\begin{array}{c}0 \\\\ w(t) \\end{array}\\right]}\\\\=&{\\left[\\begin{array}{l}\\dot{\\xi}(t) \\\\ w(t) \\end{array}\\right]}\\end{aligned}$

在这种情况下，给定 $\\Delta t_i=t_i-t_{i-1}$ ，我们得到了

状态转移矩阵 $\\mathbf{\\Phi}(t, s)=\\left[\\begin{array}{cc}\\mathbf{1}& (t-s) \\mathbf{1}\\\\ \\mathbf{0}& \\mathbf{1}\\end{array}\\right],$ ？？矩阵 $\\mathbf{Q}_i=\\left[\\begin{array}{cc}\\frac{1}{3}\\Delta t_i^3 \\mathbf{Q}_C & \\frac{1}{2}\\Delta t_i^2 \\mathbf{Q}_C \\\\ \\frac{1}{2}\\Delta t_i^2 \\mathbf{Q}_C & \\Delta t_i \\mathbf{Q}_C \\end{array}\\right]$ (26)

其中， $Q_C$ 是功率谱密度矩阵(power-spectral density matrix)

【问】这里的s什么意思？ $Q_i$ 用在了哪里？
根据公式（5）和公式（26），我们可以计算先验因子的均值和方差，从而获得先验因子。

“恒速”GP先验以零加速度轨迹为中心，因此应用该先验将使配置空间中的驱动器加速度最小化，并给出了我们的方法中平滑性的物理意义。

对于在等式中的可能性(8)我们首先定义铰链损失 $c(z)$ ：

$\\mathbf{c}(z)=\\left\\{\\begin{array}{cl}-d(z)+\\epsilon & \ ext{ if }d(z) \\leqslant \\epsilon \\\\ 0 & \ ext{ if }d(z)>\\epsilon \\end{array}\\right.$ （27）

其中，d (z)为从工作区中任意一个点z到最近的障碍物表面的有符号距离， $\\epsilon$ 为“安全距离”，表示障碍物表面附近的“危险区域”的边界。有符号距离d (z)是根据在优化[30]之前预先计算的符号距离场（SDF）计算出来的。

铰链损失在 $d(z)=\\epsilon$ 处是不可微的，所以在我们的实现中，我们在 $d(z)=\\epsilon$ 时设置了 $c(z)=-0.5$

根据公式（27），机器人在逐渐接近障碍物直至发生碰撞的过程中， $c(z)$ 从0增加到了 $\\epsilon$ .

为了对机器人的物理体进行快速碰撞检测，我们使用一组球体来表示机器人，就像Zucker等人所做的那样（如图4所示）。通过这种方式，我们将从机器人表面到任何障碍物的最小有符号距离的问题转化为从球心到任何障碍物的有符号距离减去球半径的问题。对于每个配置θi，障碍物成本函数通过计算每个球体sj（j=1, . . . , M）的有符号距离，并将它们收集到一个向量中来完成。

$\\mathbf{h}\\left(\\boldsymbol{\ heta}_i\\right)=\\left.\\left[\\mathbf{c}\\left(\\mathbf{x}\\left(\\boldsymbol{\ heta}_i, s_j\\right)\\right)\\right]\\right|_{1 \\leq j \\leq M}$ （28）这是一个向量

在上述公式中，x表示正向运动学，将任何配置θi（从配置空间）映射到工作空间中。为了完全实现公式（8）中的似然函数，还需要定义剩余的参数 $\\boldsymbol{\\Sigma}_{o b s_i}$ ，它被定义为 $\\boldsymbol{\\Sigma}_{o b s_i}=\\sigma_{o b s}\\mathbf{I}$ 。

$\\sigma_{o b s}$ 是什么？这里，σobs是一个标量，表示障碍物的观测误差或不确定性。I是一个单位矩阵，用于保持维度一致。
公式（8） $L_{o b s}\\left(\\boldsymbol{\ heta}_i \\mid c_i=0\\right) \\propto \\exp \\left\\{-\\frac{1}{2}\\left\\|\\mathbf{h}\\left(\\boldsymbol{\ heta}_i\\right)\\right\\|_{\\boldsymbol{\\Sigma}_{o b s_i}}^2\\right\\}$ ，机器人离障碍物越远， $c(z)$ 越小， $h(\\boldsymbol{\ heta}_i)$ 越小（越接近0）， $L_{obs}$ 越大，即免于碰撞的概率越大。

在图4中的示例可视化了，基于公式（28）中的“障碍物成本函数”定义的“似然函数”计算得到的，机器人免于碰撞的条件概率。较暗的区域显示的是免于碰撞的配置空间，不发生碰撞的概率较高。在障碍物边界之外的小区域较浅，表示由ε定义的“安全距离”。

图4：左侧子图显示了在一个二维空间中具有两个障碍物和ε=0.1米时的似然函数L_obs。障碍物由黑色线条标记，较暗的区域表示无碰撞的概率更高。右侧子图显示了用于碰撞检测的WAM机械臂，用粉色的球体表示。

C.运动约束 Motion constraints

在实际的规划问题中存在运动约束，应该在轨迹优化过程中加以考虑。例如，约束的起始状态和目标状态以及沿着轨迹的其他状态的约束。在推理框架中，这些约束被视为对轨迹状态的先验知识，其具有非常小的不确定性。

额外的等式约束，例如末端执行器的旋转约束（例如保持装有水的杯子直立），可以表示为 $\\mathbf{f}\\left(\\boldsymbol{\ heta}_c\\right)=\\mathbf{0}$ 的形式，其中θc是涉及的状态集合。这些约束可以被纳入到似然函数中。

$L_{\ ext{constraint }}(\\boldsymbol{\ heta}) \\propto \\exp \\left\\{-\\frac{1}{2}\\left\\|\\mathbf{f}\\left(\\boldsymbol{\ heta}_c\\right)\\right\\|_{\\boldsymbol{\\Sigma}_c}^2\\right\\}$

其中， $\\boldsymbol{\\Sigma}_c=\\sigma_c \\mathbf{I}$ ， $\\sigma_c$ 是该约束的任意方差(arbitrary variance)，，表示约束的"紧度"程度。

是不是可以理解为，设置的方差越小，对 $\\mathbf{f}\\left(\\boldsymbol{\ heta}_c\\right)=\\mathbf{0}$ 的要求越高。

为了防止关节限制的违规，我们会在每次迭代中检测违规情况，并通过添加一个等式约束因子，在该时间戳上将关节的最大值限制在合理范围内，然后继续优化过程。这种方法的效果类似于CHOMP（优化与碰撞处理）中防止关节限制违规的过程。

【问】限制关节角度的等式约束因子在源代码的哪里定义的？

我们使用GTSAM[3]C++库来实现我们的算法和OpenRAVE[5]进行模拟。我们的算法实现了两个版本，一个批处理版本（GPMP2）和一个增量版本（iGPMP2）。GPMP2使用LM算法求解非线性最小二乘优化问题，初始λ=为0.01，如果最大迭代100次，或误差相对减少小于10?4，则停止优化。iGPMP2使用具有默认设置的iSAM2[11]增量优化器。

我们在两个具有许多不同的起始配置和目标配置的数据集上进行了实验。起始和目标由高斯先验因子（具有一个非常小的协方差）作为一个软约束指定。虽然理论上解在开始和目标时是不精确的，但在我们的实验中误差是微不足道的，可以忽略。我们： (1)使用GPMP[20]中使用的7-DOF WAM臂数据集（图5（左）），包括24个独特的规划问题；(2)使用TrajOpt[25,26]中使用的PR2的7-DOF右臂数据集，包括书架（图5（中心））和工业（图5（右））场景中的90个独特的规划问题。最后，我们还验证了在一个真实的7-DOF WAM臂上的成功轨迹，设置在一个与模拟相同的环境中。2

在我们的实验中，我们使用了两个数据集，这些数据集包含了许多不同的起始和目标配置。起始和目标配置通过具有非常小协方差的高斯先验因子来指定，作为软约束。尽管在起始和目标处的解理论上是不精确的，但在我们的实验中误差很小，可以忽略不计。我们进行了以下实验：

使用了GPMP[20]中使用的包含24个独特规划问题的7自由度WAM机械臂数据集（图5左侧）；
使用了TrajOpt[25, 26]中使用的PR2机器人的7自由度右臂数据集，其中包含了在书架场景（图5中间）和工业场景（图5右侧）中的共计90个独特规划问题。
我们还在与模拟环境完全相同的实际7自由度WAM机械臂上验证了成功的轨迹。

图5：用于评估的机器人启动和目标配置，显示WAM数据集（左），以及书架（中心）和工业场景（右）中的PR2数据集。

1）设置

在设置中，我们对我们的算法GPMP2进行了基准测试。在优化过程中，我们将使用插值的GPMP2算法（GPMP2 inter）与不使用插值的GPMP2算法（GPMP2 no-inter）以及其他轨迹优化算法（TrajOpt[25, 26]、GPMP[20]、CHOMP[24, 30]和STOMP[12]）进行对比。这些基准测试都在一台3.4GHz英特尔Core i7 CPU的单个线程上运行。

所有算法都使用配置空间中的恒定速度直线轨迹进行初始化，除了GPMP需要使用加速度平滑的直线轨迹进行初始化[20]。对于WAM数据集，所有算法的初始化轨迹都由101个等距状态参数化。由于GPMP2 inter和GPMP使用了插值，我们使用11个等距状态进行初始化，其中每两个状态之间插值出9个点（实际上相当于101个状态）。由于PR2数据集中的轨迹任务较短，我们对所有算法使用了51个状态，并使用11个状态和4个插值点（实际上相当于51个状态）对GPMP2 inter和GPMP进行初始化。

GPMP、CHOMP和STOMP被允许最多优化250次迭代，或者找到无碰撞轨迹（在至少优化10次迭代后开始进行碰撞检测）为止。

为了公平比较，我们还将TrajOpt在两个数据集上使用仅11个状态（TrajOpt-11）进行了比较，因为它使用连续时间碰撞检测，通常可以找到较少状态的成功轨迹。尽管TrajOpt在使用较少状态时更快，但仍需要对生成的轨迹进行后处理，使其可执行并保持平滑。值得注意的是，由于连续时间碰撞检测只在线性方式下进行，因此在轨迹经过后处理之后，它不能提供任何无碰撞保证。我们的方法（以及GPMP）通过使用GP插值对轨迹进行上采样，并在插值点处进行碰撞检测，避免了使用较少状态时遇到的这个问题。这样得到的上采样轨迹保持平滑，并可以直接在执行过程中使用。

2）参数：

在我们的方法中，参数包括"安全距离"（safety distance） $\\epsilon$ 和"障碍物成本权重"（obstacle cost weight） $σ_{obs}$ 。通常情况下， $\\epsilon$ 被选择为场景中允许的任何障碍物的最小距离的两倍（在基准测试中，我们选择 $\\epsilon$ =0.2m用于WAM数据集， $\\epsilon$ =0.08m用于PR2数据集）。 $σ_{obs}$ 起到了平衡优化轨迹上的平滑性和无碰撞要求的权重项，并根据应用程序的轨迹要求进行设置。较大的σobs将更多的权重放在平滑性上，而较小的 $σ_{obs}$ 则更注重避开障碍物。图6显示了使用不同 $σ_{obs}$ 设置的PR2优化轨迹的示例。在我们的实验中，我们发现[0.001, 0.02]的范围适用于 $σ_{obs}$ ，并且较大的机器人臂应使用较大的σobs（在基准测试中，我们选择 $σ_{obs}$ =0.02m用于WAM数据集， $σ_{obs}$ =0.005用于PR2数据集）。

源码中哪里设置的 $σ_{obs}$ ？
较大的σobs将更多的权重放在平滑性上，而较小的 $σ_{obs}$ 则更注重避开障碍物。

3）分析

WAM数据集的基准测试结果总结在表I.A中，而PR2数据集的结果总结在表I.B中。平均成功时间和最长成功时间仅包括成功运行的结果。

评估运动规划算法是一项困难的任务。这些算法使用不同的技术来制定和解决运动规划问题，并且其性能取决于初始条件以及一系列可能根据规划问题的性质而变化的参数设置。因此，在我们的实验中，我们对每个算法进行了调整，使其接近默认设置，并且对每个数据集都找到了最佳设置。然而，我们仍然观察到TrajOpt-11在WAM数据集上表现不佳（可能是由于在轨迹上使用了太少的状态），而GPMP在PR2数据集上表现不佳（可能是由于轨迹的不同初始化以及数据集中起始和结束配置非常靠近障碍物）。

从表I.A和I.B的结果中，我们可以看到我们的算法在这些数据集上表现一致优秀，相比其他算法而言。在优化过程中使用插值（GPMP2 inter）相比不使用插值（GPMP2 no-inter）可以将平均和最长运行时间减少30%到50%。在WAM数据集上，TrajOpt-11具有最低的运行时间，但只能解决20%的问题，而GPMP2 inter和GPMP2 no-inter具有第二和第三低的运行时间。在PR2数据集上，GPMP2 inter具有最低的运行时间，GPMP2 no-inter排名第二。在我们的所有实验中，相对误差条件的减小总是在达到最大迭代次数之前满足，这可能是由于我们的算法具有二次收敛速度。因此，所有故障案例都是由于不可行的局部最小值。像随机重启这样的解决方案可以解决这个问题，但目前尚未实现。

C.增量规划 The Incremental Planner

我们通过将增量运动规划器（iGPMP2）与批处理规划器（GPMP2）在WAM和PR2数据集的每个场景的重新规划问题上进行基准测试来评估我们的增量运动规划器。重新规划问题涉及从起始配置计划到最初分配的目标配置的轨迹。然后，在中间时间步骤，轨迹被分配一个新的目标配置。这需要对因子图进行两个更改：

在轨迹末端添加一个新的目标因子，以确保轨迹到达目标更改的配置位置，
并在中间时间步骤添加一个固定状态因子以强制当前状态的约束。

GPMP2将轨迹重新初始化为从中间状态到新目标的恒定速度直线，然而，iGPMP2可以使用旧目标的解和更新的贝叶斯树作为初始化，以增量方式更新轨迹，从而更快地找到解决方案。我们的实验证实了这个说法。

为WAM数据集准备了32个重新规划问题，为PR2数据集准备了28个重新规划问题（18个在书架上，10个在工业环境中）。使用GP插值，并且所有参数与批处理基准测试相同。基准测试结果显示在表II.A和表II.B中。从结果中我们可以看到，与从头重新规划轨迹的GPMP2相比，iGPMP2提供了显著的加速，只有少量的成功率下降。iGPMP2的成功率下降与GPMP2相比有两个可能的原因：（1）iGPMP2使用原始轨迹作为初始化，如果目标移动较大，这可能是一个不好的选择；（2）GPMP2使用Levenberg-Marquardt进行优化，它提供适当的步长阻尼来帮助改善结果，但是iGPMP2在实现中没有使用类似的步长阻尼。

图7展示了使用iGPMP2成功重新规划的轨迹的示例。由于在机器人状态上添加了配置和速度约束，重新规划器在原始轨迹和重新规划轨迹之间实现了平滑过渡，这对于在实际机器人上执行轨迹至关重要。

图7：WAM和PR2工业的iGPMP2结果。红线表示最初计划的末端执行器轨迹，绿线表示重新计划的末端执行器轨迹。最好的颜色。

尽管我们的工作受到了[28]中概率运动规划的启发，但我们没有使用消息传递，而是将推理问题转化为非线性最小二乘问题。这使我们能够使用利用问题的稀疏结构的求解器，从而导致更快的算法。GPMP[20]和我们的算法都使用稀疏高斯过程（GP）来表示连续时间轨迹，使用GP插值来减少要优化的系统状态，并使用符号距离场进行碰撞检测。然而，我们的方法与GPMP不同之处在于，我们完全接受了运动规划的概率视角，并将问题形式化为非线性最小二乘问题。我们的算法使用具有二次收敛速度的优化方法，相比使用仅具有线性收敛速度的梯度下降技术的GPMP（以及CHOMP和STOMP的实现），可以得到更快的解决方案。

TrajOpt[25, 26]将运动规划问题形式化为约束优化问题，这允许在障碍物上使用硬约束，但也使优化问题变得更加困难和更慢。第V-B节的基准测试结果显示，即使使用少量状态来表示轨迹，我们的方法也比TrajOpt更快。TrajOpt执行连续时间的碰撞检测，理论上只能解决具有少量状态的问题。然而，轨迹没有连续时间的表示，因此必须通过近似障碍物的凸包和状态之间的直线来执行碰撞检测。这在实践中可能行不通，因为少量状态的轨迹需要进行后处理才能使其可执行。此外，根据后处理方法的不同，最终轨迹可能无法保证无碰撞。使用高斯过程将轨迹表示为连续时间，并使用GP插值对其进行上采样，使我们的算法能够避免这个问题。

最后，我们的框架使我们能够非常快速地解决重新规划问题，而上述的轨迹优化方法都无法提供这种能力。在实时真实世界应用中，快速解决这些类型的问题非常有用。通过将运动规划问题形式化为因子图上的概率推理，我们能够实现这一点，并且这是本文的一个重大贡献之一。

解决非线性最小二乘问题的迭代方法的一个缺点是它们无法提供全局最优性保证。然而，考虑到我们的目标是满足平滑性和无碰撞性，全局最优解并不是严格必要的。许多先前的运动规划方法面临类似的问题，容易陷入局部最小值。先前的工作[30]中讨论了几种解决方案，包括随机重启。由于我们的方法在单次运行中速度很快，随机重启是克服局部最小值问题的一个有希望的解决方案。

我们提出的方法的主要缺点是它在处理非线性不等式约束等运动约束方面的能力有限。顺序二次规划（SQP）可用于解决具有此类约束的问题，并且先前在运动规划中使用过[25, 26]。我们相信SQP可以集成到我们的轨迹优化器中，尽管这仍然是未来的工作。

我们将运动规划问题形式化为使用高斯过程进行概率推理，以推理连续时间轨迹。利用我们的表达式的稀疏结构，我们通过因子图上的非线性最小二乘优化，在因子图上进行快速推理。使用GP插值，我们可以在任意感兴趣的时间查询轨迹，使得初始轨迹可以由仅几个状态参数化，然后在优化过程中进行上采样，以检查并传播碰撞成本信息到正在优化的状态。

我们在两个数据集的三个不同环境中，使用7个自由度机械臂规划问题，将我们的算法与几种最先进的轨迹优化算法进行了基准测试，并且结果显示，我们的方法始终比最接近的竞争对手快，通常快几倍。

最后，使用Bayes Tree数据结构和iSAM2优化器，我们能够在几毫秒内逐步解决重新规划问题，这是我们运动规划算法的独特之处，并且在实时真实世界应用中非常有用。

SLAM的优化问题主要分为滤波和图优化两大类。以扩展卡尔曼滤波为例，滤波就是通过当前时刻状态+输入+运动模型来估计下一时刻的状态，然后通过观测模型来纠正，因为其计算量小，只考虑相邻帧，因此被广泛应用到嵌入式中。在图优化（graph-based slam）的方法中，处理数据的方式就和滤波不同了，它不是在线的纠正位姿，而是把所有的数据记录下来，最后一次算总账。在图中，以顶点表示优化变量，以边表示观测方程。例如：

机器人两个Pose之间的变换；——一条Binary Edge（二元边），顶点为两个pose，边的方程为： $T1=ΔT?T2$
机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标；——Binary Edge，顶点为一个3D Pose：T和一个空间点x=[x,y,z]T，观测数据为像素坐标z=[u,v]T。那么观测方程为： $z=C(Rx+t)$

C为相机内参，R,t为旋转和平移。

图优化详细理论见高翔博士文章：

深入理解图优化与g2o：图优化篇 - 半闲居士 - 博客园

总结一下做图优化的流程：

选择你想要的图里的节点与边的类型，确定它们的参数化形式；
往图里加入实际的节点和边；
选择初值，开始迭代；
每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
求解稀疏线性方程HkΔx=?bk，得到梯度方向；
继续用GN或LM进行迭代。如果迭代结束，返回优化值。

实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。

g2o（General Graphic Optimization)，是一个通用图优化算法库。由于目前主流的SLAM研究基本都是基于图优化的，因此十分有必要掌握一下g2o方法。

先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点（都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。g2o的整个代码框架就是按照图中标的这个顺序来写的。

顺着整体结构图往下看，可以看到这部分其实算是整个g2o里面比较隐晦的部分，设计到优化的算法，块求解器，线性求解器等等部分，在程序中，这部分通常位于g2o算法的开头配置部分，一般情况下我们可以随着一个例程进行配置即可，这里对这部分进行了稍微浅显的理解，特意写在这里（下面的东西纯属个人理解了，如有不妥还请大神指出）。

我们知道在求解增量方程HdeltaX=-b的时候，通常情况下想到线性求解，很简单嘛，deltaX=-H.inv*b，的确，当H的维度较小的时候，上述问题变得简单，只需要矩阵的求逆就能解决问题，但是当H的维度较大时，问题变得复杂，此时我们就需要一些特殊的方法对矩阵进行求逆，g2o中主要有图中所示的三种方法，PCG，CSparse和Cholmod方法。

注意，这里说再多，线性求解器仅仅只是完成了一个求解的功能，可以说是整个优化中比较靠后的计算部分了。

块求解器是包含线性求解器的存在，之所以是包含，是因为块求解器会构建好线性求解器所需要的矩阵块（也就是H和b），之后给线性求解器让它进行运算，边的jacobian也就是在这个时候发挥了自己的光和热。

这里再记录下一个比较容易混淆的问题，也就是在初始化块求解器的时候的参数问题。

大部分的例程在初始化块求解器的时候都会使用如下的程序代码：

std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver=g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();

其中的BlockSolver_6_3有两个参数，分别是6和3，在定义的时候可以看到这是一个模板的重命名（模板类的重命名只能用using）

template<int p, int l>  
using BlockSolverPL=BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度，l表示landmark的维度，且这里都表示的是增量的维度（从后续的程序中可以看出是增量的维度而并非是状态变量的维度）。

因此（后面的话以SLAM情况为例），对于仅仅优化位姿的应用而言，这里l的值是没有太大影响的，因为H矩阵中并没有Hll的块，因此这里的维度也就没有用武之地了。

这里就不多讲了，从图中可以看到主要有三种方法：GN，LM，PSD，不同的方法主要表现在最终的H矩阵构造不同。

g2o详细知识见文章：

深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园 g2o学习--g2o整体框架 - feifanren - 博客园

在添加顶点的时候需要知道自己的定点类型,看g2o里面是否已有,如果有的话则不需要自己再定义了.下面设计了两个代码实例，第一个直接使用g2o里面的顶点类型，第二个使用自己定义的顶点类型。

在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。在高博代码基础上增加了部分注释，并且其图优化问题可以画成以下形式（个人感觉抽象出图优化结构是很重要的一步！！！，结构抽象出来后就能像套模板一样利用g2o构建顶点和边了）：

代码中针对顶点首先定义了两个相机位姿，然后定义了特征点顶点；针对边首先定义了相机1与各特征点的边（即图中虚线），然后定义了相机2与各特征点的边（即图中虚线）

/**
 * BA Example
 * Author: Xiang Gao
 * Date: 2016.3
 * Email: gaoxiang12@mails.tsinghua.edu.cn
 *
 * 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。
 */

// for std
#include <iostream>
// for opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <boost/concept_check.hpp>
// for g2o
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>


using namespace std;

// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );

// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main( int argc, char** argv )
{
    // 调用格式：命令[第一个图][第二个图]
    if (argc != 3)
    {
        cout<<"Usage: ba_example img1, img2"<<endl;
        exit(1);
    }

    // 读取图像
    cv::Mat img1 = cv::imread( argv[1] );
    cv::Mat img2 = cv::imread( argv[2] );

    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    {
        cout<<"匹配点不够！"<<endl;
        return 0;
    }
    cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    //BlockSolver_6_3 ：表示pose 是6维，观测点是3维。
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );

    // 添加节点
    // 两个位姿节点：这里是相机的两个位姿，将第一个位姿假设为单位矩阵，类似于T1=ΔT?T2
    //VertexSE3Expmap()即李群位姿，对于三维的 x y z pitch roll yaw 组成的
    //变换矩阵T(R,t)属于SE3,注意应该使用VertexSE3Expmap的类型而不是VertexSE3
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    //VertexSBAPointXYZ()即三维空间点
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx;
        double y = ( pts1[i].y - cy ) * z / fy;
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }

    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );

    // 准备边
    // 第一帧：也就是相机1对应的边，即相机1与各特征点的边
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        //setVertex 定义顶点，有两个：
        //一个是 0：VertexSBAPointXYZ 类型的顶点    
        //一个是1：pose类型
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧：也就是相机2对应的边，即相机2与各特征点的边
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;

    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;

    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos=";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }

    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            cout<<"error="<<e->chi2()<<endl;
        }
        else
        {
            inliers++;
        }
    }

    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}


int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 )
{
    cv::ORB orb;
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb( img1, cv::Mat(), kp1, desp1 );
    orb( img2, cv::Mat(), kp2, desp2 );
    cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;

    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");

    double knn_match_ratio=0.8;
    vector< vector<cv::DMatch> > matches_knn;
    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
    vector< cv::DMatch > matches;
    for ( size_t i=0; i<matches_knn.size(); i++ )
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
            matches.push_back( matches_knn[i][0] );
    }

    if (matches.size() <= 20) //匹配点太少
        return false;

    for ( auto m:matches )
    {
        points1.push_back( kp1[m.queryIdx].pt );
        points2.push_back( kp2[m.trainIdx].pt );
    }

    return true;
}

自定义顶点时，需要重写4个函数:

 virtual bool read(std::istream& is);                           // 读操作
 virtual bool write(std::ostream& os) const;                    // 写操作
 virtual void oplusImpl(const number_t* update);                // 顶点更新函数
 virtual void setToOriginImpl();                                // 顶点重置函数

其中read，write 函数可以不进行覆写，仅仅声明一下就可以，setToOriginImpl 设定被优化变量的原始值，oplusImpl 比较重要，我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要写对，否则会造成一直优化却得不到好的优化结果的现象。

自定义边时，需要重写4个函数:

 virtual bool read(std::istream& is);                           // 读操作
 virtual bool write(std::ostream& os) const;                    // 写操作
 virtual void computeError();                                   // 误差计算函数
 virtual void linearizeOplus();                                 // 计算雅克比矩阵

read 和 write 函数同上，computeError 函数是使用当前顶点的值计算的测量值与真实的测量值之间的误差，linearizeOplus 函数是在当前顶点的值下，该误差对优化变量的偏导数，即jacobian矩阵。

实例：

对曲线 $y=exp({ax}^{2}+bx+c)$ 进行优化，待估计参数为 a,b,c , 观测数据为 y, 误差函数为 e=y - $exp({ax}^{2}+bx+c)$ 。其中构成g2o的顶点为 a,b,c ，自定义顶点时， setToOriginImpl 函数设定被优化变量的原始值，为0,0,0，oplusImpl 函数对估计值进行更新调整。

其图优化问题可以画成以下形式

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

//todo 1.定义顶点和边的类型
//todo 2.构建图
//todo 3.选择优化算法
//todo 4.调用g2o进行优化，返回结果

// 顶点为优化变量，边为误差项
// 待估计的参数构成顶点，观测数据构成了边（位姿R,t构成顶点，像素点p构成了边）
// 误差定义在边内，边附着在顶点上（e=x - f(x)）
// 误差关于顶点的偏导数定义在边里的雅克比矩阵J上，而顶点的更新通过内置的oplusImpl函数实现更新
// 在VSLAM里，相机的位姿与观测的路标构成图优化的顶点，相机的运动和路标的观测构成图优化的边
// ******************************************************************** /
//  |      问题    |   y=exp(a*x*x+b*x+c)    |        VSLAM        |
//  |  待估计参数  |         a,b,c             |    位姿R,t 路标点P  |
//  |    观测数据  |            y              |       像素点p       |
//  |      误差    |  e=y-exp(a*x*x+b*x+c)   |    e=x - f(x)     |  
// ******************************************************************** / 

// 拟合曲线模型的顶点，模板参数：优化变量（待估计参数）维度和数据类型
// 顶点需要继承BaseVertex，维度为3，数据类型为Vector3d

// 曲线模型的顶点，模板参数：优化变量维度和数据类型(此处为自定义定点，具体可以参考下方的点定义)
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 字节对齐
    virtual void setToOriginImpl() // 重置，设定被优化变量的原始值 
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);//update强制类型转换为Vector3d
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据
    
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器。第一步：创建一个线性求解器LinearSolver。
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器。第二步：创建BlockSolver，并用上面定义的线性求解器初始化。
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//第三步：创建BlockSolver，并用上面定义的线性求解器初始化。
    // g2o::OptimizationAlgorithmGaussNewton* solver=new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver=new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型。第四步：创建图优化的核心：稀疏优化器（SparseOptimizer）。
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点。第五步：定义图的顶点和边HyperGraph，并添加到SparseOptimizer中。
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
    
    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点,因为只有一个顶点即构成一元边
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }
    
    // 执行优化。第六步：设置优化参数，开始执行优化。
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost="<<time_used.count()<<" seconds. "<<endl;
    
    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}

【SLAM模块】g2o 曲线拟合实例 https://www.guyuehome.com/37292 https://www.guyuehome.com/37120 SLAM中的图优化---基本概念及Rviz显示位姿图code - 古月居 SLAM中的图优化---基本概念及Rviz显示位姿图code - 古月居 SLAM图优化g2o - 吴建明wujianming - 博客园 https://zhuanlan.zhihu.com/p/121628349 https://zhuanlan.zhihu.com/p/362600811 g2o学习记录（5）g2o例子-曲线拟合（新旧版本g2o实现）_settooriginimpl-CSDN博客

上一篇：什么是CPS广告？一文带你了解CPS广告的优缺点
下一篇：英国预科最全解析（英国本科预科）返回列表

华体会动态

个人学习总结--图优化与g2o