博客

1. 什么是回环？
   
   机器人在其雷达/相机中，辨认出之前访问的路标点。（后端优化）
   
   下图来自于官网,
   
   
   
   x

   5

   ,

   x

   2

   x_5,x_2
   
   
   
   
   
   
   
   x
   
   
   
   
   
   
   
   
   
   
   5
   
   
   
   
   
   ​
   
   
   
   
   
   
   
   
   
   
   
   
   ,
   
   
   
   
   
   x
   
   
   
   
   
   
   
   
   
   
   2
   
   
   
   
   
   ​
   
   
   
   
   
   
   
   
   
   
   
   
   
   
   
   为相同路标点，添加回环之后，重新优化位姿，可以有效降低不确定度（协防差）。
   
   
   绿色包围的大小表示不确定度（matlab画出）。
   
   

2. 代码1：官方例程
   
   .g2o文件：

VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.774115 1.183389 1.576173
VERTEX_SE2 2 -0.262420 2.047059 -3.127594
VERTEX_SE2 3 -1.605649 0.993891 -1.561134
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

使用pcl_viewer查看：（左图为矫正前，右图为矫正后（原理详见slam14讲第10讲））



代码：

#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>

using namespace std;
using namespace gtsam;

int main(int argc,char** argv){
    //read data
    string g2oFile="/home/n1/notes/gtsam/loopclosure/noisyToyGraph.g2o";
    NonlinearFactorGraph::shared_ptr graph;
    Values::shared_ptr initial;
//boost::tie:将两个内部参数赋值（最多可以有10个），并将其合并为一个类型范围
//  template<class T0, class T1>
//  inline typename detail::tie_mapper<T0, T1>::type
//  tie(T0& t0, T1& t1) {
//  typedef typename detail::tie_mapper<T0, T1>::type t;
//  return t(t0, t1);
// }
    boost::tie(graph,initial)=readG2o(g2oFile);
    //readG2o(file,is3D,kernelFuntionType)
    //  file:文件路径
    //  is3D：默认false 是否为3d问题
    //  kernelFuntionType：采用何种核函数
    NonlinearFactorGraph graphWithPrior=*graph;
    noiseModel::Diagonal::shared_ptr priorModel=noiseModel::Diagonal::Variances(Vector3(1e-6,1e-6,1e-8));
    graphWithPrior.add(PriorFactor<Pose2>(0,Pose2(),priorModel));//添加起始点
    std::cout << "Computing LAGO estimate ..." << std::endl;
    //使用lago 快速初始化因子图
    Values estimate=lago::initialize(graphWithPrior);//返回因子图中的值
    std::cout << "done" << std::endl;
    estimate.print("estimateLago");
    string outputfile="/home/n1/notes/gtsam/loopclosure/corretnoisy.g2o";
    std::cout << "Writing results to file: " << outputfile << std::endl;
    writeG2o(*graph,estimate,outputfile);
    //graph:图结构
    //estimate：估计值
    //outputfile：保存路径
    std::cout << "done! " << std::endl;
}

3. 例子2：

原始数据.g2o

#include  <gtsam/slam/InitializePose3.h>
#include  <gtsam/slam/dataset.h>
#include  <gtsam/slam/BetweenFactor.h>
#include  <gtsam/slam/PriorFactor.h>
#include  <fstream>
#include  <string>
using namespace std;
using namespace gtsam;
int main(int argc,char** argv){
    string g2ofile="/home/n1/notes/gtsam/loopclosuresphere3d/sphere.g2o";
    NonlinearFactorGraph::shared_ptr graph;
    Values::shared_ptr initial;
    bool is3D=true;
    boost::tie(graph,initial)=readG2o(g2ofile,is3D);
    NonlinearFactorGraph graphPrior=*graph;
    noiseModel::Diagonal::shared_ptr priormpdel=noiseModel::Diagonal::Variances(Vector6(1e-6,1e-6,1e-6,1e-4,1e-4,1e-4));
    Key firstKey=0;
    for(const Values::ConstKeyValuePair& key_value:*initial){
        std::cout<<"添加 起始因子 到g2o"<<std::endl;
        firstKey=key_value.key;//获取其第一个因子键值(因为不一定起始为0)
        graphPrior.add(PriorFactor<Pose3>(firstKey,Pose3(),priormpdel));
        break;
    }
    std::cout<<"Initialzing Pose3"<<std::endl;
    Values initialzation=InitializePose3::initialize(graphPrior);//初始化
    std::cout<<"done"<<endl;
    initialzation.print("initialzatoin");
    string output="/home/n1/notes/gtsam/loopclosuresphere3d/correctsphere.g2o";
    std::cout<<"wirte results to file:"<<output<<std::endl;
    writeG2o(*graph,initialzation,output);
    std::cout<<"done"<<std::endl;
    return 0;
}