深入AMCL(四):相机如何辅助AMCL自动全局定位

问题的引出：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 <iostream>
#include <vector>
#include <string>/*功能：利用图片构建Dbow词典输入：环境下所有图像输出：词典（详细说明查找dbow库说明）*/int main( int argc, char** argv )
{// read the image std::cout<<"reading images... "<<std::endl;std::vector<cv::Mat> 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 featuresstd::cout<<"detecting ORB features ... "<<std::endl;cv::Ptr< cv::Feature2D > detector = cv::ORB::create();std::vector<cv::Mat> descriptors;for ( cv::Mat& image:images ){std::vector<cv::KeyPoint> 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 deepthvocab.create( descriptors );// create words of imagesDBoW3::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）