问题的引出：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）
