基于ORB特征的三维定位与地图构建算法

这文章相当于被追屁股后面写的东西了，拿到地平线的板子一直准备做点什么东西出来。

不辜负是不辜负，有个雷达能干嘛，装头上吗？

自到我看见了这个尘封已久的小飞机

dji专属教育小飞机，啧，可以编程哪种

写过无数了，感兴趣的大家自己搜索一下

x3很牛，TT也牛，但是我不牛，我得让他牛，所以我决定搞个SLAM使用。TT有一个单目的相机，还有一个IMU，以及开放的接口。所以有这个潜力，其次也能学一手ROS2.事实上是我太年轻了，后面再说。

在SLAM框架上面我肯定自己写不了，这里研究了一下午，使用了ORB-SLAM。

ORB-SLAM是一种基于ORB特征的三维定位与地图构建算法（SLAM）[1]。该算法由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年发表在IEEE Transactions on Robotics。ORB-SLAM基于PTAM架构，增加了地图初始化和闭环检测的功能，优化了关键帧选取和地图构建的方法，在处理速度、追踪效果和地图精度上都取得了不错的效果。要注意ORB-SLAM构建的地图是稀疏的。

ORB-SLAM一开始基于monocular camera，后来扩展到Stereo和RGB-D sensor上。作者好像还会做Semi-dense mapping的扩展。作者的开源代码都在GIT上。

ORB-SLAM算法的一大特点是在所有步骤统一使用图像的ORB特征。ORB特征是一种非常快速的特征提取方法，具有旋转不变性，并可以利用金字塔构建出尺度不变性。使用统一的ORB特征有助于SLAM算法在特征提取与追踪、关键帧选取、三维重建、闭环检测等步骤具有内生的一致性。

架构图

ORB-SLAM利用三个线程分别进行追踪、地图构建和闭环检测。

一、追踪

ORB特征提取

初始姿态估计（速度估计）

姿态优化（Track local map，利用邻近的地图点寻找更多的特征匹配，优化姿态）

选取关键帧

二、地图构建

加入关键帧（更新各种图）

验证最近加入的地图点（去除Outlier）

生成新的地图点（三角法）

局部Bundle adjustment（该关键帧和邻近关键帧，去除Outlier）

验证关键帧（去除重复帧）

三、闭环检测

选取相似帧（bag of words）

检测闭环（计算相似变换（3D<->3D，存在尺度漂移，因此是相似变换），RANSAC计算内点数）

融合三维点，更新各种图

图优化（传导变换矩阵），更新地图所有点。

先别看这么高大上，需要做修改的。这个ORB_SLAM2 库不用Pangolin可视化，而是通过 ROS2 topcis 发布所有数据。

而且这里也需要对TT飞机来一套完整的ROS封装。下篇文章讲封装，在这之前还需要对X3上面的ROS2进行一个熟悉。

这里使用的编辑器是VSCode，记得密码是root

提前看好自己的IP

ssh root@ip

官方的安装文档很好了，这里不写，记得运行前必须加一下，当然了我这里建议是可以一劳永逸的写好

就这样，下面是我的SLAM库

但是我也不理解，写了就好像没有写一样

等等我找到了！

tros。。。地平线自己改了，不过注意后面录还是不对，没有foxy了

对了在VSCode里面推荐安装这些插件

Python的话是安装这些

而且可以使用Jupyter，记得pip3 install jupyter

运行的都可以的

PIP也是可以的

自动补全的速度还行

第一个就是我们的tros了

不过这个好像就不管用了

这些说的都对

但是这里指出的是源应该是墙外的，所以这里需要全局的安排

接下来试试摄像头：

应该是插入一个摄像头了，但是看源文件

在tos的文件夹里面

/opt/tros/share/hobot_usb_cam/launch/hobot_usb_cam.launch.py

源码的位置

参数是dev下面的

应该是要这个

这里启动失败，是不是是因为我的权限不够

报错是没有？？？

再开一个terimal，看看列表

也知道，节点都是Python的程序，ros的命令是脚本。

在tros下面的share是存放公开节点的地方

就像这样

包括我们的demo也是在这里的

工作温度：-25°Cto65°C稳定工作温度：20°Cto60°C生产工艺：SMT(ROSH)物距：60CM-150CM解析度：800LW/PH(Center)芯片型号：JX_F37芯片尺寸：1/3最大分辨率：1920*1080像素点大小：2.7um*2.7um最大图像传输速率：30fps(1920*1080)信噪比：36db动态范围：79db芯片型号：JX_F37芯片尺寸：1/3最大分辨率：1920*1080像素点大小：2.7um*2.7um最大图像传输速率：30fps(1920*1080)信噪比：36db动态范围：79db

这个就是我们的MIPI的相机

https://developer.horizon.ai/sunrise

售卖的位置，感谢大佬还给了个摄像头：

害，白嫖笑嘻嘻

source /opt/tros/setup.bash  ros2 topic list

我个人是习惯看看话题的情况的

# 配置 TogetherROS 环境：source /opt/tros/local_setup.bash# launch 方式启动ros2 launch mipi_cam mipi_cam.launch.py

开启一个新的终端，开始把摄像头开启

摄像头开启了一个新的线程

这个就是发布的信息

可以对比的看

# 一个终端编码source /opt/tros/local_setup.bashros2 run hobot_codec hobot_codec_republish --ros-args -p channel:=1 -p in_mode:=shared_mem -p in_format:=nv12 -p out_mode:=ros -p out_format:=jpeg -p sub_topic:=/hbmem_img -p pub_topic:=/image_jpeg

我们的数据在发送给屏幕显示的时候，需要编码压缩，所以一个新的终端

这个就是mjpeg的压缩节点

命令

# 再起一个终端source /opt/tros/local_setup.bash# 启动nginx，nginx只需启动一次，如前面已启动过nginx，则无需再次启动cd /opt/tros/lib/websocket/webservice && chmod +x ./sbin/nginx &&./sbin/nginx -p .# 启动websocketros2 run websocket websocket --ros-args -p image_topic:=/image_jpeg -p image_type:=mjpeg -p only_show_image:=true

在起一个节点来把我们的mjpeg的信息通过服务器发给浏览器

然后再浏览器打开就行

结果

我们可以看到各个节点的情况

先把硬件打开

把原生的raw数据来发送，并且解码成mjpeg

接着就是把数据转发到浏览器

事已至此我们在继续玩

把一个Mac连入我的热点，打开IP就可以播放（我在吃西红柿）

这里充分的展示了ROS的方便之处。

打开我的DELL，里面也有ROS，来个全节点之间的使用

首先是可以读取发送的节点信息

但是rviz2打不开

定位到Cmake,是有这个库的

在这里‘

修了半天也玩不来。

把ROS2卸载了重装看看怎么样？（好了）

卸载了重装了一次，记得最后加一个--fixmissing的东西

板子上面运行这种东西也没有什么毛病

之后我会给安装脚本，但sudo就可以的

对了在VSCode里面打开新的目录是使用这个命令

打开后你记得选择信任

抱歉了，本来想无梯子的，不地不挂了，有的文件有点难搞

这个是编译的TT ROS包，大概一分钟吧，DELL 几秒钟

救命。。。连个git也没有

这个是噩梦的开始，慢不说，内存还爆了

看不懂？

就是板子编译东西的时候，没内存维持正常运行就宕机了

两位兄台的建议很好，但是太慢了，我不准备在X3上面编译了，我试了脚本为1，但是好慢。

目前TT ROS搞好了，就差SLAM了

审核编辑：彭静