# Run ORB-SLAM3 in real time using Astra Pro camera on ROS environment


<!--more-->

# 搭建ORB-SLAM3在Ubuntu下的编译和运行环境

参考 [Build ORB-SLAM3 compile and run environment under Ubuntu](https://shangzg.top/2022-10-18-build-orb-slam3-compile-and-run-environment-under-ubuntu/)



# ROS环境上使用Astra Pro相机实时运行ORB-SLAM3

## ROS版本编译

添加ROS路径到`ROS_PACKAGE_PATH`环境变量中，一般在ORB-SLAM3源码下的Examples文件夹或者**新的版本在Examples_old**文件夹

```bash
# 打开 .bashrc file 添加路径：
vim ~/.bashrc
# 其中PATH是本机ORB_SLAM3的具体路径(如/home/xxx)
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples_old/ROS
# 保存更新环境变量
source ~/.bashrc
```

用以下命令测试

```bash
echo $ROS_PACKAGE_PATH
```

添加成功之后进行编译

```bash
chmod +x build_ros.sh
./build_ros.sh
```

**注：最新版的ORBSLAM3中Examples没有ROS目录，需要在`build_ros.sh`脚本中将Examples改成Examples_old**

## ROS编译问题

1. OpenCV版本问题

   直接在`CMakeLists.txt`中修改自己所装对应版本

   ![image.png](/images/2022-11-25-ROS环境上使用AstraPro相机实时运行ORB-SLAM3/1)

2. C++ 标准问题

   直接在`CMakeLists.txt`中添加`set(CMAKE_CXX_STANDARD 14)`

   ![image.png](/images/2022-11-25-ROS环境上使用AstraPro相机实时运行ORB-SLAM3/2)

3. 提示找不到sophus库

   进入`ORB_SLAM3/Thirdparty/Sophus`文件夹下，若未编译请先编译，在编译完成后执行`sudo make install`，加入Sophus依赖到系统环境中

**以下解决方法会引入cv中的eigen2cv方法，需要在每个修改文件的顶部包含以下文件头文件**

```c++
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
```

4. 解决报错`cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); `

   引入上述两个头文件，将`ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc`代码中这一行修改为：

   ```c++
   cv::Mat Tcw;
   Sophus::SE3f Tcw_SE3f = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
   Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();
   cv::eigen2cv(Tcw_Matrix, Tcw);
   ```

   ![image.png](/images/2022-11-25-ROS环境上使用AstraPro相机实时运行ORB-SLAM3/3)

5. 解决报错`vPoints.push_back(pMP->GetWorldPos());`

   引入上述两个头文件，将`ROS/ORB_SLAM3/src/AR/ViewerAR.cc`代码中这一行修改为：

   ```c++
   cv::Mat WorldPos;
   cv::eigen2cv(pMP->GetWorldPos(), WorldPos);
   vPoints.push_back(WorldPos);
   ```
   
   ![image.png](/images/2022-11-25-ROS环境上使用AstraPro相机实时运行ORB-SLAM3/4)

6. 解决报错`cv::Mat Xw = pMP->GetWorldPos();`

   引入上述两个头文件，将`ROS/ORB_SLAM3/src/AR/ViewerAR.cc`代码中这一行修改为：

   ```c++
   cv::Mat Xw;
   cv::eigen2cv(pMP->GetWorldPos(), Xw);
   ```
   
   ![image.png](/images/2022-11-25-ROS环境上使用AstraPro相机实时运行ORB-SLAM3/5)

## 实时运行

运行Astra Pro相机启动节点：

```bash
roslaunch astra_camera astrapro.launch
```

Astra Pro相机在ROS环境下的配置使用可参照：[Astra Pro camera configuration and use](https://shangzg.top/2022-11-23-astra-pro-camera-configuration-and-use/#%E5%9F%BA%E4%BA%8Eros%E7%9A%84%E5%BC%80%E5%8F%91%E7%8E%AF%E5%A2%83%E9%85%8D%E7%BD%AE)

运行ORB-SLAM3

```
rosrun ORB_SLAM3 RGBD Vocabulary/ORBvoc.txt Examples_old/ROS/ORB_SLAM3/Asus.yaml
```

运行截图如下：

![image.png](/images/2022-11-25-ROS环境上使用AstraPro相机实时运行ORB-SLAM3/6)