DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた

DJI Tello等ドローンの自律飛行を実現するには、3D環境でナビゲーション経路を計画としてOMPL（Open Motion Planning Library）を試してみます。

# 目次
1. [OMPLのインストール](https://qiita.com/k-koh/items/4f7e441d1225e6580f82)
2. [OMPLの基礎](https://qiita.com/k-koh/items/9933cd29599c859cac45)
3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)
4. [OMPLの使用](https://qiita.com/k-koh/items/3a8b372537aa29e17863)
5. DJI Telloの使用
    1. [DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
    2. [Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)
    3. [DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた](https://qiita.com/k-koh/items/98b5f079f784b22dcd01)　← いまココ
6. navigationスタックの使い方
    1. [Turtlebot3 NavigationをGazebo Simulationで動かしてみた](https://qiita.com/k-koh/items/1a4fbc175ca37be6206b)
    2. [Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用](https://qiita.com/k-koh/items/1ec9097e4afedd0b43ca)
    3. [ARdroneのシミュレーターをGazeboで動かす](https://qiita.com/k-koh/items/feccbaee7c11c43c3ab4)
    4. [AirSimのシミュレーターを動かす](https://qiita.com/k-koh/items/52122fb5e96336ac8d33)
    5. [octomapのインストール](https://qiita.com/k-koh/items/c9a3a87e043934456842)
    6. [単眼カメラでORB_SLAM2から3D point clouodの生成](https://qiita.com/k-koh/items/1e069c9dda43b85e2fa0)
    7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)
7. OMPLで3D 経路計画
    1. [状態空間環境のセットアップ](https://qiita.com/k-koh/items/e19ac08bb0a16975081e)


## 動作環境
- Ubuntu 18.04
- ROS Melodic
- Tello EDU

# DJI Telloのカメラ でVisual-SLAMのORB-SLAM2
DJI Telloの単眼カメラでVisual-SLAM自己位置推定検証で、ORB-SLAM2 を動かしてみた。

# 環境
Ubuntu 18.04
ROS Melodic
Tello EDU
tello_driver
Visual-SLAMのORB-SLAM2

# tello_driverのインストール
[tello_driver](http://wiki.ros.org/tello_driver)をcatkin_wsのsrcにgit cloneします。

```console
git clone --recursive https://github.com/appie-17/tello_driver.git
```

# 必要なライブラリのインストール
[camera_info_manager_py](http://wiki.ros.org/camera_info_manager_py?distro=kinetic)をcatkin_wsのsrcにgit cloneします。

```console
git clone https://github.com/ros-perception/camera_info_manager_py.git
```

image_transportをインストールします。

```console
sudo apt-get install ros-melodic-image-transport
```

# H264動画ストリームのデコード
tello_driverの「tello_node.launch」にimage_transportのrepublishを追加します。
「/tello/image_raw/h264」から「/tello/image_raw」にデコード

```xml
  <!-- <node pkg="image_transport" name="image_compressed" type="republish" args="raw in:=image_raw compressed out:=image_raw" />-->
  <node pkg="image_transport" name="image_decompressed" type="republish" args="compressed in:=image_raw raw out:=image_raw">
    <param name="image_transport" value="h264"/>
  </node>
```

# ORB-SLAM2のインストール
## 必要なライブラリのインストール
```console
sudo apt install libglew-dev cmake libjpeg-dev libtiff5-dev libopenexr-dev libpng-dev
sudo apt install ros-melodic-tf
```

## Pangolinのインストール
```console
$ git clone https://github.com/stevenlovegrove/Pangolin.git
$ cd Pangolin
$ mkdir build
$ cd build
$ cmake ..
$ make -j4
$ sudo make install
```

## ORB_SLAM2のインストール
```console
$ git clone https://github.com/ILab-01/ORB_SLAM2.git
$ cd ORB_SLAM2
$ chmod +x build.sh

$ ./build.sh
```

ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.ccにincludeを追記します。

```
#include <unistd.h>
```

```
$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/XXX_YOUR_PATH_XXX/ORB_SLAM2/Examples/ROS
$ chmod +x build_ros.sh
$ ./build_ros.sh
```

# ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成
[DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
[ORB_SLAM2用カメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)


# tello_driverの起動
```console
$ roslaunch tello_driver tello_node.launch
```

# Visual-SLAMのORB-SLAM2の起動
```
$ rosrun ORB_SLAM2 Mono 'ORB_SLAM2/Vocabulary/ORBvoc.txt' 'ORB_SLAM2/Examples/ROS/ORB_SLAM2/calibration_tello.yaml'
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/da891114-30a0-dce7-a3df-4c47092e1bf2.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/d5e329b2-42ec-ca59-6d42-b3d9077927e5.png)

ORB-SLAM2のYouTube動画
[![Visual-SLAMのORB-SLAM2を起動](http://img.youtube.com/vi/FRLjdZ0rzLI/0.jpg)](http://www.youtube.com/watch?v=FRLjdZ0rzLI)

Next: 6.1. [Turtlebot3 NavigationをGazebo Simulationで動かしてみた](https://qiita.com/k-koh/items/1a4fbc175ca37be6206b)

Prev: 5.2. [Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)