• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 yuanguobin01 1720次浏览 0个评论

问题的引出: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)

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

保存词典和单词

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)


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明深入AMCL(四):相机如何辅助AMCL自动全局定位
喜欢 (0)

您必须 登录 才能发表评论!

加载中……