问题的引出：AMCL的初始位姿通常要求用户手动输入，或者机器加载上一次保存的位姿。AMCL中自带的全局定位服务（globalLocalizationCallback）实际中基本很难用得上。

问题解决思路：相机在回环检测中具有比较大的优势，那么是不是可以通过搭载一个相机来辅助AMCL实现自动全局定位呢？

 

～～～～～～～～～～～～下面说说如何实现（相机回环辅助AMCL全局定位）～～～～～～～～～～～～

 

1.采集图像和位姿


移动机器人激光SLAM（激光同步定位和建图）过程中，检测轮式里程计输出，大概相对位移发生≥0.5m或相对角度发生≥10度，则同时采集保存一张当前图像和当前时刻机器人的位姿，直到整个室内环境行走一遍，则建立图像（images + pose.txt）结束（尽量多走不同的点，这样拍到的照片覆盖多，有利于增加回环的高检测率）。

在这里插入图片描述

2.构建DBOW3词典和图像单词

通过上一步骤采集的images，调用DBOW3构建词典和images对应单词

#include "DBoW3/DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include 
#include 
#include 

/*功能：利用图片构建Dbow词典
  输入：环境下所有图像
  输出：词典（详细说明查找dbow库说明）*/

int main( int argc, char** argv )
{
    // read the image 
    std::cout<<"reading images... "<<std::endl;
    std::vector images; 
    for ( int i=0; i<15; i++ )
    {
        std::string path = "/home/yuan/gosuncn/vscode/VLocation/images_map/"+std::to_string(i)+".png";
        images.push_back( cv::imread(path) );
    }
    // detect ORB features
    std::cout<<"detecting ORB features ... "<<std::endl;
    cv::Ptr< cv::Feature2D > detector = cv::ORB::create();
    std::vector descriptors;
    for ( cv::Mat& image:images )
    {
        std::vector keypoints; 
        cv::Mat descriptor;
        detector->detectAndCompute( image, cv::Mat(), keypoints, descriptor );
        descriptors.push_back( descriptor );
    }
    
    // create vocabulary 
    std::cout<<"creating vocabulary ... "<<std::endl;
    DBoW3::Vocabulary vocab(10, 5);//设定 tree branchs deepth
    vocab.create( descriptors );

    // create words of images
    DBoW3::Database db(vocab);
    for (size_t i = 0; i < descriptors.size() ; i++)
    {
        db.add(descriptors[i]);
    }
    
    //save vocabulary and words 
    std::cout<<"vocabulary info: "<<vocab<<std::endl;
    vocab.save( "vocabulary.dbow3" );
    db.save("database.db");
    std::cout<<"done"<<std::endl;
    
    return 0;
}

主要使用到库DBOW3（相机回环）(关于该库的具体使用和原理可以参考https://github.com/rmsalinas/DBow3)

在这里插入图片描述

保存词典和单词

3.相机回环检测和AMCL全局定位

3.1.移动机器人开机启动加载步骤2中保存的词典（11ocabulary.dbow3）和全部图像对应的单词（11database.db）；

3.2.接收当前相机拍到的图像（currentImage），调用DBOW3库在images进行回环检测；

3.3.回环检测成功的图像（loopImage），调取loopImage在images中的索引号index；

3.4.读取索引号index在步骤1中保存的pose.txt对应的位姿poseLoop；

3.5.计算currentImage与loopImage相对位姿（orb特征检测，最好使用深度相机或双目，这样的话求depth没那么累-）；

3.6.获取currentImage位姿pose，直接发送给initialpose topic（AMCL中的）；

注：实验中不计算3.5步骤，直接以loopImage对应的Pose 发送给AMCL的initialpose ，似乎效果也不错。
～～～～～～～～这步骤的代码由于一些原因就不贴了，反正也简单，大家理一理就会啦～～～～～～～

欢迎各位爱好者留言和加微信讨论（15112119047）