RGB-D SLAM——回环检测篇(四)

一.回环检测的流程

1.关键帧提取：把每一帧都拼到地图中是不明智的选择。因为帧与帧之间距离很近，导致地图需要频繁更新，浪费时间，所以当机器人的运动超过一个”关键帧”，将关键帧拼到地图里就行了。

2.回环的检测。回环的本质是识别曾经到过的地方。最简单的回环策略，是将新来的关键帧与之前所有的关键帧进行比较，不过这样会导致越往后，需要比较的帧就越多。所以，稍微快点的方法就是在过去的帧里面随机挑选一些，与之进行比较。

伪代码如下：
1.初始化关键帧序列：F，并将第一帧 f0 f 0 放入F中。
2.对于新来的一帧I，计算F中最后一帧与I的运动，并估计该运动的大小e。有以下集中可能性：
a.若e>error，说明运动太大，可能计算错误，丢弃该帧。
b.若没有匹配上(match)太少，说明该帧图像质量不高，丢弃。
c. 若e < Ekey,说明离前一帧很近，舍弃。
3.近距离回环：匹配I与F末尾m个关键帧。匹配成功时，在图里增加一条边。
4.随机回环：随机在F里取n个帧，与I进行匹配。若匹配上，在图里增加一条边。
5.若有新的数据，则回2，没有则进行优化与地图拼接。

主要代码如下：

int main(int argc, char** argv)
{
    vector keyframe;
    vector frame;
    vector<int> keyframe_count;
    keyframe_count.push_back(0);
    int frameid=0;
    for(int i=1;i<600;i++) //将所有的帧导入进来，存储在一个容器中
    {
        FRAME frame_single;
        string path_rgb("/home/hansry/Pose_part/data/rgb_png/");
        string path_depth("/home/hansry/Pose_part/data/depth_png/");
        string suffix(".png");
        stringstream str_rgb,str_depth;
        str_rgb<1);
        frame_single.frameid=frameid;
        frame.push_back(frame_single);
        frameid++;
    }
    cout<//g2o,initialize the computor
    SlamLinearSolver* linearSolver = new SlamLinearSolver();  //初始化求解器
    linearSolver->setBlockOrdering(false);
    SlamBlockSolver* blockSolver=new SlamBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver=new  g2o::OptimizationAlgorithmLevenberg(blockSolver); //选择求解策略


    g2o::SparseOptimizer globalOptimizer; //最后用的就是这个东东
    globalOptimizer.setAlgorithm(solver);
    // 不要输出调试信息
    globalOptimizer.setVerbose(false);

    // 向globalOptimizer增加第一个顶点
    g2o::VertexSE3* v=new g2o::VertexSE3();
    v->setId(frame[0].frameid);
    v->setEstimate(Eigen::Isometry3d::Identity()); //估计为单位矩阵t
    v->setFixed(true); //第一个顶点固定，不用优化
    globalOptimizer.addVertex(v);
    keyframe.push_back(frame[0]);

    bool check_closure=true;
    for(size_t k=1;kfalse);
        //check_result（）函数见下
        switch(result)
        {
        case NOT_MATCHED:
            cout<< (keyframe.back()).frameid<<" "<" "<<"this frame not matched"<break;
        case TOO_FAR_AWAY:
            cout<<(keyframe.back()).frameid<<" "<" "<<"this frame is toofar"<break;
        case TOO_CLOSE:
            cout<<(keyframe.back()).frameid<<" "<" "<<"this frame is tooclose"<break;
        case KEYFRAME:
            cout<<"save"<" "<<"frame"<cout<if(check_closure)//这里是回环检测，检测方式有俩个：1是与后面几帧进行检测，2是随意几帧进行检测
            {
                cout<<"check the closure"<//新帧与关键帧的后几帧进行比较，回环检测，成功回环则加边，函数如下所示
                checkrandomloop(keyframe,frame[k],globalOptimizer);  //新帧与关键帧的任意几帧进行回环检测，成功则回环加边，函数如下所示
                cout<<"the closure is ending"<break;
        default:
            break;
        }
    }

     cout<<"optimizing pose graph,vertices:"<"/home/hansry/Pose_part/data/result_befor.g2o");
     globalOptimizer.initializeOptimization(); //求解初始化
     globalOptimizer.optimize(100); //迭代次数为100次
     globalOptimizer.save("/home/hansry/Pose_part/data/result_after.g2o"); 
     cout<<"Optimization done."<new PointCloud());  //new一个新的内存
     PointCloud::Ptr cloud_output(new PointCloud());
     cout<<"start to generate the PointCloud"< voxel;// 网格滤波器，调整地图分辨率
     pcl::PassThrough pass;// z方向区间滤波器，由于rgbd相机的有效深度区间有限，把太远的去掉
     pass.setFilterFieldName("z");
     pass.setFilterLimits(0.0,4.0); //4m以上就不要了
     double gridsize=0.01;
     voxel.setLeafSize(gridsize,gridsize,gridsize);
     for(size_t z=0;zdynamic_cast(globalOptimizer.vertex(keyframe[z].frameid));
         cout<<"estimate frameid"<<" "<estimate();
         PointCloud::Ptr newcloud=image2PointCloud(keyframe[z].rgb,keyframe[z].depth);
         voxel.setInputCloud(newcloud);
         voxel.filter(*temp);
         pass.setInputCloud(temp);
         pass.filter(*newcloud);
         pcl::transformPointCloud( *newcloud, *temp, pose.matrix() );
         *cloud_output += *temp;
         temp->clear();
         newcloud->clear();
     }
     voxel.setInputCloud(cloud_output);
     voxel.filter(*temp);
     pcl::io::savePCDFile( "/home/hansry/Pose_part/data/myself_result.pcd", *cloud_output );
      globalOptimizer.clear();
      return  0;
}

CHECK_RESULT check_result(FRAME &frame1,FRAME &frame2,g2o::SparseOptimizer &opti,bool is_loop)
{
    static double min_inliers=5;
    static double max_norm=0.2;
    static double max_norm_lp=2.0;
    static double keyframe_threshold=0.1;
    RESULT_OF_PNP result=estimateMotion(frame1,frame2);
    cout<<"result inliers"<<" "<<result.inliers<<endl;
    if(result.inliers<min_inliers)
        return NOT_MATCHED;
     // 计算运动范围是否太大
    double norm=normofTransform(result.rvec,result.tvec); //算出俩帧之间的运动大小，有||t||+min(||2pi-r||,||r||)
    cout<<"norm"<<" "<<norm<<endl;
    if(norm<keyframe_threshold)
    {
        return TOO_CLOSE; //返回的为自定义枚举类型，下面的TOO_FAR_AWAY也是自定义的枚举类型。
    }
    if(is_loop==false)
    {
        if(norm>max_norm)
        {
            return TOO_FAR_AWAY;
        }
    }
    else
    {
         if(norm>max_norm_lp)
         {
           return TOO_FAR_AWAY;
         }
    }
    if(is_loop==false)
    {
        g2o::VertexSE3 *v = new g2o::VertexSE3(); //顶点，增加顶点
        v->setId( frame2.frameid );//给每个顶点打上标签
        v->setEstimate( Eigen::Isometry3d::Identity() );
        opti.addVertex(v);
        cout<<"add vertices"<<endl;
    }
      g2o::EdgeSE3* edge = new g2o::EdgeSE3();
       // 连接此边的两个顶点id
       edge->setVertex( 0, opti.vertex(frame1.frameid));//根据frameid连接俩个顶点形成边
       edge->setVertex( 1, opti.vertex(frame2.frameid));
       edge->setRobustKernel( new g2o::RobustKernelHuber());
       // 信息矩阵
       Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
       // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
       // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
       // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
       information(0,0) = information(1,1) = information(2,2) = 100;
       information(3,3) = information(4,4) = information(5,5) = 100;
       // 也可以将角度设大一些，表示对角度的估计更加准确
       edge->setInformation( information );
       // 边的估计即是pnp求解之结果
       Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
       // edge->setMeasurement( T );
       edge->setMeasurement( T.inverse() );//优化的东西，即所谓的决策变量
       // 将此边加入图中
       opti.addEdge(edge);
       cout<<"add edge ending"<<endl;
       return KEYFRAME;

void checknearbyloop(vector &frame,FRAME ¤t_frame,g2o::SparseOptimizer &globalOptimizer)
{
    static int nearby_loops=5;
    if(frame.size()<=5)
    {
        for(size_t i=0;itrue);
        }
    }
    else
    {
        for(size_t j=frame.size()-nearby_loops;jtrue);
        }
    }
}

void checkrandomloop(vector &frame,FRAME ¤t_frame,g2o::SparseOptimizer &globalOptimizer)
{
    static double random_loop=5;
    srand((unsigned)time(NULL));
    if(frame.size()for(size_t j=0;jtrue);
        }
    }
    else
    {
        for(size_t k=0;kint index=rand()%frame.size();
            check_result(frame[index],current_frame,globalOptimizer,true);
        }
    }
}

程序运行如下：

轨迹路径如下：

参考：http://www.cnblogs.com/gaoxiang12/p/4719156.html