ORB_SLAM3 やり方

最近リリースされたORB_SLAM3をやってみました。Ubuntu16.04(ROS kinetic)で、すでにORB_SLAM2をインストール済みの環境で構築しました。

$ cd ~
$ git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git
$ cd ORB_SLAM3
$ chmod +x build.sh
$ ./build.sh

主にEigenからwarningがかなり出ましたが、これでスタンドアローン版はビルドできました。次にROS上でのビルドです。

$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/ORB_SLAM3/Examples/ROS
$ chmod +x build_ros.sh
$ ./build_ros.sh

ここでエラーが発生しました。

 fatal error: Eigen/Core: そのようなファイルやディレクトリはありません

このコマンドでテキストエディタを開きinclude_directoriesにEigenを追加します。(同じ名前のファイルがORB_SLAM3/CMakeLists.txtにもありますが、以下のファイルです。)

$ gedit ./Examples/ROS/ORB_SLAM3/CMakeLists.txt

書き換え後はこのようになります。

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)

再度ビルドを実行したらうまくいきました。次は実行していきます。

$ source ~/ORB_SLAM3/Examples/ROS/ORB_SLAM3/build/devel/setup.bash
$ roscore
$ rosrun ORB_SLAM3 Mono ~/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/ORB_SLAM3/Examples/Monocular/TUM1.yaml
$ rosbag play LSD_room.bag /image_raw:=/camera/image_raw

とりあえずこれで動きました。rosbagでLSD_SLAMのサンプル動画を動かしていますがファイルを持っていない方は以下のコマンドでダウンロードしてください。

$ wget http://vmcremers8.informatik.tu-muenchen.de/lsd/LSD_room.bag.zip

デジカメで撮影した映像で試して見ました。デジカメのキャリブレーションファイル(.yaml)は既にORB_SLAM2で作成していました。しかし、そのyamlファイルをORB_SLAM3でも使うには一行追加が必要です。
Camera.type: "PinHole"
もしくは
Camera.type: "KannalaBrandt8"
を5行目あたりに追加します。

ソースコード(src/Tracking.cc)を確認するとその2種類で分岐されていました。KannalaBrandt8が何を表すかよく分かりませんが、Kannala-Brandt Camera modelという言葉が魚眼レンズに関する論文に書いてありました。yamlファイル書き換え後は以下のようになります(冒頭部分の抜粋)。

%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"
# Camera calibration and distortion parameters (OpenCV) 

ORB_SLAM2とORB_SLAM3の比較ですどちらも大学内で撮影した同じ映像を同じ設定で入力しています。ORB_SLAM2だとカメラの位置の初期化が行えていません。一応、特徴点の抽出の設定を少し緩くするとORB_SLAM2でももう少しマシにはなります。一方でORB_SLAM3だと最初から設定を変えなくてもうまく行っています。
youtu.be
youtu.be

ちょっと前まではもっとエラーが出て、いろいろ書き換えないと動かなかったのですがバグ修正が入ったようで簡単に動いてくれました。



ノートPCにもORB_SLAM3をインストールしてみました。スペック不足で./build.shを実行中にフリーズしてなかなかうまくいきませんでした。

gedit ./build.sh

で、make -j とある箇所をmake -j3 としました。それでもフリーズします。terminalには

virtual memory exhausted: Cannot allocate memory

と表示されていました。仮想メモリが足りないと言うことで、まずは仮想メモリの容量を確認します。

free -h

でサイズを確認すると1GB程度しかありませんでした。以下のサイトを参考にして合計10GBに増やしました。
tyablog.net
一度~/ORB_SLAM3/のディレクトリを削除して、

sudo rm -rf ~/ORB_SLAM3/

再度Gitからクローンしました。さらにメモリを軽くするためPCを再起動してからビルドしました。フリーズしてるのか処理中なのかよく分からない状態が何度かありましたが、最後までビルドできました。
./build_ros.shを実行する際も同様に make -j を make -j3 に書き換えて、再起動をしてから実行しました。無事にインストールが完了して動作確認を行えました。

ORB_SLAM3のオドメトリのモーションキャプチャを使った精度の評価

ORB_SLAM3で出力したカメラのオドメトリ(キーフレームの移動軌跡)と、モーションキャプチャで出力した軌跡を比べて、オドメトリの精度を見ていきたいと思います。それぞれから出力された軌跡をUnity上で読み込んで比較しました。

緑の線がORB_SLAM3で、白っぽい線がMotiveで出力した軌跡です。以下のように座標上で方向が異なっていて綺麗に重ねて表示は出来てませんが、それぞれ視点を変えてスクショした画像を貼ります。
f:id:shibuya_shogo:20200901122041p:plain

f:id:shibuya_shogo:20200901122413p:plainf:id:shibuya_shogo:20200901122409p:plain
途中でORB_SLAMが位置をロストしたり、逆にモーションキャプチャでうまく取れていなかったりで多少誤差があります。
f:id:shibuya_shogo:20200901122555p:plainf:id:shibuya_shogo:20200901122551p:plain
f:id:shibuya_shogo:20200901123000p:plainf:id:shibuya_shogo:20200901123004p:plain

以下はMotiveから出力されたファイル(.csv)をUnity上で読み込むスクリプトです。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO;

public class csvImport : MonoBehaviour
{
    TextAsset csvFile;
    List<string[]> csvDatas = new List<string[]>();
    LineRenderer lineRenderComp;
    int lineVertexCount=0;

    void Start()
    {
        csvFile = Resources.Load("motive_csv2") as TextAsset; // Resouces下のCSV読み込み
        StringReader reader = new StringReader(csvFile.text);
        int csvLineCount=0;
        int loadColumnNum=7; //何行目から読み込むか
        int timeRowNum=1;
        int xPosRowNum=6;
        int yPosRowNum=7;
        int zPosRowNum=8;

        // リストに追加していく
        while (reader.Peek() != -1)
        {
            string line = reader.ReadLine();
            csvDatas.Add(line.Split(','));
            csvLineCount++;
        }

        if (!GetComponent<Animation>())
        {
            gameObject.AddComponent<Animation>();
        }
        if(!GetComponent<LineRenderer>()){
            gameObject.AddComponent<LineRenderer>();
        }
        lineRenderComp=GetComponent<LineRenderer>();

        AnimationClip clip = new AnimationClip();
        clip.legacy = true;

        Keyframe[] xKfArray;
        Keyframe[] yKfArray;
        Keyframe[] zKfArray;
        xKfArray = new Keyframe[csvLineCount-loadColumnNum];
        yKfArray = new Keyframe[csvLineCount-loadColumnNum];
        zKfArray = new Keyframe[csvLineCount-loadColumnNum];

        Vector3 startPosVec=new Vector3 (
                                    float.Parse(csvDatas[loadColumnNum][xPosRowNum])*10.0f,
                                    float.Parse(csvDatas[loadColumnNum][yPosRowNum])*10.0f,
                                    float.Parse(csvDatas[loadColumnNum][zPosRowNum])*10.0f);

        //this.gameObject.transform.position = startPosVec;

        for(int i=0;loadColumnNum<csvLineCount;loadColumnNum++){
        if(!csvDatas[loadColumnNum][xPosRowNum].Equals("")&&!csvDatas[loadColumnNum][yPosRowNum].Equals("")&&!csvDatas[loadColumnNum][zPosRowNum].Equals("")){
        Vector3 positionVec=new Vector3(
        float.Parse(csvDatas[loadColumnNum][xPosRowNum])*10.0f,
        float.Parse(csvDatas[loadColumnNum][yPosRowNum])*10.0f,
        float.Parse(csvDatas[loadColumnNum][zPosRowNum])*10.0f
        );
        xKfArray[i] = new Keyframe(
        float.Parse(csvDatas[loadColumnNum][timeRowNum]),
        positionVec.x-startPosVec.x);
        yKfArray[i] = new Keyframe(
        float.Parse(csvDatas[loadColumnNum][timeRowNum]),
        positionVec.y-startPosVec.y);
        zKfArray[i] = new Keyframe(
        float.Parse(csvDatas[loadColumnNum][timeRowNum]),
        positionVec.z-startPosVec.z);
        i++;
        }
        }

        AnimationCurve xCurve = new AnimationCurve(xKfArray);
        AnimationCurve yCurve = new AnimationCurve(yKfArray);
        AnimationCurve zCurve = new AnimationCurve(zKfArray);
        clip.SetCurve("", typeof(Transform), "localPosition.x", xCurve);
        clip.SetCurve("", typeof(Transform), "localPosition.y", yCurve);
        clip.SetCurve("", typeof(Transform), "localPosition.z", zCurve);
        GetComponent<Animation>().AddClip(clip, "move");

        GetComponent<Animation>().Play("move");
    }
    void FixedUpdate()
    {
        lineVertexCount += 1;
        lineRenderComp.positionCount = lineVertexCount;
        lineRenderComp.SetPosition(lineVertexCount - 1, transform.position);
    }
}

以下はORB_SLAM3から出力されたファイル(.txt)をUnity上で読み込むスクリプトです。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO;

public class OrbTxtImport : MonoBehaviour
{
    TextAsset txtFile;
    List<string[]> txtDatas = new List<string[]>();
    LineRenderer lineRenderComp;
    int lineVertexCount=0;

    void Start()
    {
        txtFile = Resources.Load("orb3_txt5") as TextAsset; // Resouces下のTXT読み込み
        StringReader reader = new StringReader(txtFile.text);
        int txtLineCount=0;
        int loadColumnNum=0; //何行目から読み込むか
        int timeRowNum=0;
        int xPosRowNum=1;
        int yPosRowNum=2;
        int zPosRowNum=3;
        float initTime;

        // リストに追加していく
        while (reader.Peek() != -1)
        {
            string line = reader.ReadLine();
            txtDatas.Add(line.Split(' '));
            txtLineCount++;
        }

        if (!GetComponent<Animation>())
        {
            gameObject.AddComponent<Animation>();
        }
        if(!GetComponent<LineRenderer>()){
            gameObject.AddComponent<LineRenderer>();
        }
        lineRenderComp=GetComponent<LineRenderer>();

        AnimationClip clip = new AnimationClip();
        clip.legacy = true;

        Keyframe[] xKfArray;
        Keyframe[] yKfArray;
        Keyframe[] zKfArray;
        xKfArray = new Keyframe[txtLineCount-loadColumnNum];
        yKfArray = new Keyframe[txtLineCount-loadColumnNum];
        zKfArray = new Keyframe[txtLineCount-loadColumnNum];

        Vector3 startPosVec= new Vector3 (
                                  float.Parse(txtDatas[loadColumnNum][xPosRowNum])*50.0f,
                                  float.Parse(txtDatas[loadColumnNum][yPosRowNum])*50.0f,
                                  float.Parse(txtDatas[loadColumnNum][zPosRowNum])*50.0f
                                  );

        //this.gameObject.transform.position = startPosVec;

        initTime=float.Parse(txtDatas[loadColumnNum][timeRowNum]);

        for(int i=0;loadColumnNum<txtLineCount;loadColumnNum++){
        Vector3 positionVec=new Vector3(
        float.Parse(txtDatas[loadColumnNum][xPosRowNum])*50.0f,
        float.Parse(txtDatas[loadColumnNum][yPosRowNum])*50.0f,
        float.Parse(txtDatas[loadColumnNum][zPosRowNum])*50.0f
        );
        xKfArray[i] = new Keyframe(i*0.1f,positionVec.x-startPosVec.x);
        yKfArray[i] = new Keyframe(i*0.1f,positionVec.y-startPosVec.y);
        zKfArray[i] = new Keyframe(i*0.1f,positionVec.z-startPosVec.z);
        i++;
        }

        AnimationCurve xCurve = new AnimationCurve(xKfArray);
        AnimationCurve yCurve = new AnimationCurve(yKfArray);
        AnimationCurve zCurve = new AnimationCurve(zKfArray);
        clip.SetCurve("", typeof(Transform), "localPosition.x", xCurve);
        clip.SetCurve("", typeof(Transform), "localPosition.y", yCurve);
        clip.SetCurve("", typeof(Transform), "localPosition.z", zCurve);
        GetComponent<Animation>().AddClip(clip, "move");

        GetComponent<Animation>().Play("move");
    }
    void FixedUpdate()
    {
        lineVertexCount += 1;
        lineRenderComp.positionCount = lineVertexCount;
        lineRenderComp.SetPosition(lineVertexCount - 1, transform.position);
    }
}

モーションキャプチャー(Motive)の使い方

大学の研究室のモーションキャプチャー(OptiTrack)で使用するソフト(Motive)の使い方です。このサイトと内容が多少かぶってます。参考にしてください↓
qiita.com

Motiveではモーションキャプチャのカメラを使って、反射マーカーの立体的な動きを観察、記録することができます。研究室にあるモーションキャプチャはOptiTrackのFlex3という種類みたいです。そのカメラが研究室内に、現在は5つ設置済みで未使用のものを4つ確認しています。www.mocap.jp

環境構築

Motiveの環境構築です。デスクトップPCで行わないと研究室のノートPCでは重たくてフリーズしました。おそらくWindows版しか無いみたいです。
以下のサイトからMotiveをダウンロードしてインストールします。↓
www.optitrack.jp
インストール完了後、起動時にライセンスキーを求められます。研究室内のサーバー(himeji)の以下の場所にライセンスキーのファイルがあるのでMotiveで読み込んでください。
himeji/robo/BackUp/2019/ohgitani/Optitrack/License_rigid_body_initial_93303_RBL1184_2009-02-11.dat

準備

起動すると、Choose a Starting Taskと出てきますがバツを押して消します。設置してある全てのカメラがMotive上に表示されているかを確認します。これが配線が悪いのか、なかなか認識しなかったりします。線を抜き差ししたりMotiveを起動し直したりして認識させます。うまく認識できると、このような画面になります。その際、カメラには1~5までの番号が表示された状態になるはずです。

f:id:shibuya_shogo:20200821204459p:plainf:id:shibuya_shogo:20200821205409j:plain

カメラを認識したら反射マーカーのついたものを全てカメラの外に移動させます。この状態ではCameraPreviewの中は真っ黒になっているのが理想的ですが、ノイズがあると白く表示されます。CameraPreviewのプラスボタンを押すとノイズが赤色に塗りつぶされます。うまく塗り潰されない場合はその他のボタンから手動で塗りつぶすこともできます。f:id:shibuya_shogo:20200822000828p:plain

白いノイズを全て赤く塗りつぶせたら、カメラのキャリブレーションを行なっていきます。CameraCalibrationの中にあるStartWandingというボタンを押します。ここでファイルの保存を求められるので適当な場所に保存します。その後キャリブレーションワンドを持ってカメラの視野に入りワンドを振り回します。

f:id:shibuya_shogo:20200822003318p:plainf:id:shibuya_shogo:20200822003335j:plain

以下の画像のようにSufficient for QualityがVery Highになるまで続けてCalculateのボタンを押します。Samplesの数値が低くなっているカメラの前を特に重点的に振り回します。f:id:shibuya_shogo:20200822011626p:plain
Calibration Result Reportのダイアログが表示されるためApplyを選択します。次に以下の画像に示すキャリブレーションスクエアを、全てのカメラの中心あたりにおきます。これが空間座標上で原点となります。配置後にSet Ground Planeを押します。

f:id:shibuya_shogo:20200822012328j:plainf:id:shibuya_shogo:20200822014609p:plain
ファイルの保存を求められるので適当な場所に保存します。反射マーカーのついたものをカメラの前に移すとPerspective Viewに表示されます。以下の画像では見やすいようにPerspective Viewの領域を拡大してます。Camera Previewはここからは使わないのでバツを押して消しています。一つのRigidBodyとして繋げたいマーカーをドラッグアンドドロップで選択します。選択後に右クリックしてRigit Body>Create From selected Markersの順に選択すると一つのRigidBodyとなります。
f:id:shibuya_shogo:20200822101806p:plainf:id:shibuya_shogo:20200822102813p:plain

録画

上部のツールバーからView>Timelineを開きます。2Dのボタンの左にある丸いボタンを押すと録画が始まります。

f:id:shibuya_shogo:20200822112428p:plainf:id:shibuya_shogo:20200822112940p:plain
録画後、再生します。Motiveを閉じなくてもできる方法があるかもしれませんが一度Motiveを起動し直します。File>Openからさっき保存したファイルを開いて、画像に示す再生ボタンを押します。録画したファイルのファイル名は変更していなければTake 2020-08-12 07.49.33 PM.takのようなファイル名です。
f:id:shibuya_shogo:20200822123959p:plainf:id:shibuya_shogo:20200822124525p:plain
File>Export Tracking Data ...を選択するとデータが出力されて保存できます。csvファイルとして保存するとExcelからデータを確認できます。青い線で示しているのがマーカーの名前です。赤い線で示している列がクウォータ二オンでRigidBodyの角度(姿勢)を表しています。黄色い線で示している列が位置を表します。
f:id:shibuya_shogo:20200822125039p:plainf:id:shibuya_shogo:20200822130258p:plain
各マーカーの名前は以下のようにPerspective Viewでマーカーをドラッグアンドドロップで選択することで確認できます。
f:id:shibuya_shogo:20200822131407p:plain

ORB_SLAM2 環境構築中に出たエラーとその対処

OpenCV

  • OpenCVをcmakeする際に発生
fatal error: stdlib.h: No such file or directory

cmakeにオプションをつけて

cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local -DENABLE_PRECOMPILED_HEADERS=OFF ..

とすることで解決。


  • opencvのアンインストール
cd build
sudo make uninstall

(openCVのアンインストールを行う場合は先にDBoW2をアンインストールする。)



g2o

  • g2oをcmakeするときに発生
target_compile_features specified unknown feature "cxx_std_14" for target "core".

g2o/g2o/core/CMakeLists.txtをgeditで開き、46行目の

target_compile_features(core PUBLIC cxx_std_14)

を、

set (CMAKE_CXX_STANDARD 14)

とすることで解決。



DBoW2

  • DBoW2をcmakeする際に発生
CMake Error at CMakeLists.txt:32 (find_package):

DBoW2/CMakeLists.txtの32行目に、

find_package(OpenCV REQUIRED)

とあるので、その上の行に、

set(OpenCV_DIR "~/opencv-3.1.0/build")

を追加して解決


  • DBoW2の最新バージョンで発生
error: ‘class cv::ORB’ has no member named ‘detectAndCompute’;
cv::KeyPoint is not a member of 'cv'

opencvのバージョンが古いことが原因のためopencv3.xを入れるか、DBoW2の古いバージョンを入れることで解決しました。


  • DBoW2のアンインストール

makeからはアンインストールできなかったので、手動でアンインストールしました。
sudo make install
一度再インストールを行い、インストール箇所を表示してくれるため、それを見て手動で削除。



ROS

  • 全てインストール完了後、ORB-SLAMを実行する際に発生
OpenCV Error: Bad argument (Invalid pointer to file storage) in cvGetFileNodeByName, file /build/opencv-_msWgW/opencv-2.4.8+dfsg1/modules/core/src/persistence.cpp, line 740
terminate called after throwing an instance of 'cv::Exception'

自分でインストールしたOpenCVとROSに対応するOpenCVのバージョンが異なることが原因。
ROSをインストールする際に自動でROS内部にOpenCVがインストールされます。Ubuntu14.04 ROS IndigoはOpenCV2.4.8に正式対応していてUbuntu16.04 ROS KineticはOpenCV3.1.0に正式対応しています。ROSのバージョンに対応しているOpenCVを入れることで解決します。


ORB-SLAM

  • ./build.shを実行する際に発生
/tmp/cc7KAqxb.s: Assembler messages:
/tmp/cc7KAqxb.s:1008: Error: operand type mismatch for `vxorps'

gcc,g++のバージョンを4.8に戻して解決。


  • ./build.shを実行する際に発生
error: ‘usleep’ was not declared in this scope

https://ei0124.blog.fc2.com/blog-entry-21.html
を参考にして以下のように対処しました。

grep -ril "usleep" ~/ORB_SLAM2/Examples/ | xargs  sed -i 1i#include\ \"unistd.h\"
grep -ril "usleep" ~/ORB_SLAM2/src/ | xargs  sed -i 1i#include\ \"unistd.h\"

src/と.Examples/の中にあるファイルのうちusleepの文字が含まれるものの先頭行に#include "unistd.h"を付加しています。


  • ./build_rosを実行した際に発生
fatal error: Eigen/Core: そのようなファイルやディレクトリはありません

次のように対処します。

gedit ./Examples/ROS/ORB_SLAM2/CMakeLists.txt

テキストエディタでCtrl+Fで検索機能を使い、「include_directories」と入力。以下の箇所に

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
)

一行追加して、このようにします。

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)


  • ubuntu 16.04で./build_rosを実行した際に発生
/usr/bin/ld: CMakeFiles/RGBD.dir/src/ros_rgbd.cc.o: シンボル '_ZN5boost6system15system_categoryEv' への未定義参照です
/usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status

gedit ./Examples/ROS/ORB_SLAM2/CMakeLists.txt
テキストエディタでCtrl+Fで検索機能を使い、「include_directories」と入力。その下にあるsetの部分を変更して以下のように-lboost_systemを追加します。

set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
-lboost_system
)


  • いろんなところで発生
warning: ‘Eigen::AlignedBit’ is deprecated (declared at /usr/local/include/eigen3/Eigen/src/Core/util/Constants.h:162) [-Wdeprecated-declarations]

errorではなくwarningなので出てきても一応ビルドは行えました。今回は無視してます。

RTAB MAP やり方

研究室のPCでKinect v2を使ってRTAB-MAPをやってみました。

 Kinect v2はMicrosoftから2013年に発売されたもので、本来はモーションキャプチャのように人間の動きをゲームセンサーとして利用するデバイスです。深度センサが搭載されており、それをUbuntuでRGBDカメラとして利用し、SLAMを行いました。
 RTAB-MAPはRGBDカメラ,ステレオカメラ,Lidarを使ってSLAMが行えます。単眼カメラには対応していません。

今回はUbuntu16.04 + ROS kineticの環境で試してみました。kinect v2とROS kineticは綴りが似ていますが全然違うものです。
RTAB-MAPはROS上で動かさなくてもスタンドアローンで動かすこともできます。しかし、自分のPCには既にROS kineticが入っていて、ROS上で動かした方がインストールが簡単だったのでROS上でやっています。それとロボット制御などに活用する場合はROS上で動かした方が良いです。

ROS kineticのインストールは以下のサイトに従って行いました。ROSのバージョンは基本的にUbuntuのバージョンによって決まるので各環境に合ったROSを入れてください。
http://wiki.ros.org/ja/kinetic/Installation/Ubuntu

以下はRTAB-MAPをインストールしたときのコマンドです。この一行で入ってくれます。

sudo apt-get install ros-kinetic-rtabmap-ros

次にUbuntuKinect v2を認識するためにlibfreenect2の導入します。その後KinectをROS上で認識するためにiai_kinect2を導入します。このサイトを参考に導入しました。↓
qiita.com

上記のサイトでは以下のコマンドでKinect v2の接続確認を行っています。その際Kinect v2の線をUSBポートに接続しますがUSB3.0に接続してください。USB2.0に接続した場合には認識できませんでした。

$ ./bin/Protonect

Kinect v2がフリーズしてうまく行かないなどがあれば試しにNvidiaのドライバーを入れてみてください。詳細は以前こちらのページにまとめています。
sh090.hatenablog.com

RTAB-MAPをROS上でKinect v2を使って実行する際のコマンドです。

$ source ~/ros_ws/devel/setup.bash
$ roslaunch rtabmap_ros rgbd_mapping_kinect2.launch
$ rosrun kinect2_bridge kinect2_bridge  _publish_tf:=true

実際に研究室を撮影してみました。研究室の様子を見てもらうため通常の写真を載せときます。

f:id:shibuya_shogo:20200817172339j:plainf:id:shibuya_shogo:20200817172327j:plainf:id:shibuya_shogo:20200817172317j:plain

RTAB-MAP実行中の動画です。
youtu.be

3Dマップが赤くなっている時がありますが、自己位置をロストしているときです。懐中電灯を照らしているみたいにマップが出来上がっていきますね。
以下の画像はRTAB-MAPで出力した2Dの地図です。

f:id:shibuya_shogo:20200817014420p:plain
RTAB-MAP 2D
 これまで単眼SLAMを中心にやってきたので、それに比べるともちろん精度も良く(自己位置をロストしにくい)、また計算コストも少ないため小型のコンピュータでもRTAB-MAPは実行できます。深度センサがあるとスケールがわかるので、点郡間の距離を測ることで実際の距離やサイズも計測することができます。
以下のサイトでは、ドローンでステレオカメラ×RTAB-MAPを使って実際に対象物のサイズの測定を行なっているようです。誤差は7cm(計測値174.6cm,実測値182cm)となっています。

dronebiz.net

Ubuntuにグラフィックボードのドライバーをインストール

大学のPCはグラフィックボードを積んでいますがUbuntu上で適応されていませんでした。適応されている場合は以下のコマンドを実行したときにGPUの情報が表示されます。

$ nvidia-smi

Ubuntu16.04の場合

Ubuntuの「システム設定」を開き、「ソフトウェアとアップデート」を押し、「追加のドライバ」のタブを開きます。最初は「X.Org X server~」が選択されていますが「Nvidia binary driver~」を選択して変更の適用を押します。再起動すれば完了です。

Ubuntu14.04の場合

Ubuntu14.04では、普通の入れ方だとログインループに入ってしまいます。

  • Nouveauドライバの無効化

ドライバの干渉を避けるためデフォルトで入っているNouveauドライバを無効化します。

$ touch /etc/modprobe.d/blacklist-nouveau.conf
$ gedit /etc/modprobe.d/blacklist-nouveau.conf

テキストエディタで以下の内容を入力して保存することでカーネルモジュールをblacklistに追加します。

blacklist nouveau
options nouveau modeset=0

変更の適用と再起動

$ sudo update-initramfs -u
$ sudo reboot
  • NVIDIA公式サイトからドライバをダウンロード

NVIDIA公式サイトからドライバをダウンロードします。GPUの種類を入力しないといけないので、一度PCを開いて確認すると「STRIX-GTX1080-DC2O6G」と書いてありました。公式サイトでは以下のように選択をしました。

f:id:shibuya_shogo:20200812140217p:plain
nvidia driver
  • NVIDIAドライバのインストール

ドライバのインストールを行います。
Ctrl+Alt+F1を押してユーザ名、パスワードを入力してCUIでログインします。
ちなみにX Serverを停止する前ならばCtrl + Alt + F7でGUIに戻れます。

// X Serverを停止
sudo service lightdm stop
// 実行権限を付加
chmod a+xNVIDIA-Linux-x86_64-450.57.run
// openglなしでNVIDIAドライバをインストール
sudo ./NVIDIA-Linux-x86_64-450.57.run --no-opengl-files

no-opengl-files をつけないとログインループに入ってしまいます。
インストール後に再起動したら完了です。

                                                      • -

「失敗したときのやり方(備忘録)」
ログインループから脱出した方法も含むので書き残します。自分の環境では不具合が発生したやり方ですが、以下のサイトに従ってインストールを行いました。UbuntuGPUの相性によっては以下の方法で上手くいく場合もあります。ほとんど相性の問題なのでどの方法でインストールが上手くいくかは自分で試すしか無いようです。
note.com

//NVIDIAのドライバのリポジトリを追加
$ sudo add-apt-repository ppa:graphics-drivers/ppa
//パッケージの更新
$ sudo apt update
//推奨ドライバを確認
$ ubuntu-drivers devices
//ドライバのインストール
$ sudo apt install nvidia-driver-418

推奨ドライバを確認した際に recommended と表示されたドライバをインストール後、再起動を行いました。ここで問題が発生してログイン画面からログインを試みても一度画面が暗転して、またログイン画面に戻されてループします。

一度さっきインストールしたドライバを消すためログイン画面で
alt + control + f1
を押してCUIにします。ユーザ名とパスワードを入力しCUIでログインして以下のコマンドで削除しました。

$ sudo apt-get --purge remove nvidia-*
$ sudo apt-get --purge remove cuda-*
$ sudo reboot

ORB_SLAM2 環境構築

今回はORB-SLAM2をPCに導入していきます。
 ORB-SLAM2は、スペインのサラゴサ大学の研究機関が論文ソースコードを公開しているプロジェクトです。プロジェクトのウェブページもあります。単眼カメラ、ステレオカメラ、RGB-Dカメラのいずれかを使ってリアルタイムで特徴点ベースのSLAMが行えます。カメラ映像からオドメトリや疎な点郡で3D空間を再現します。LSD-SLAMよりもおそらくオドメトリの精度は上がります。
 導入はLSD-SLAMに比べるとかなり大変でした。terminalを使って導入します。自分はUbuntu14.04でもUbuntu16.04でも試しました。導入途中で出たエラーについては別のページにまとめました。
sh090.hatenablog.com

導入に必要なライブラリと、その依存関係は大体こんな感じかと思います。

ORB-SLAM2
	|-Pangolin
	|	 |-LIBUVC
	|	 |-OpenGL
	|-g2o
	|   |-Eigen3
	|   |-OpenGL
	|-DBoW2
	|     |-OpenCV
	|-(ROS)
  • Pangolinの依存ライブラリのインストール

OpenGLのインストール

sudo apt-get update
sudo apt-get install libglu1-mesa-dev 
sudo apt-get install libglew-dev

LIBUVCの依存ライブラリのインストール

// https://packages.ubuntu.com/ja/source/bionic/libuvc
sudo apt-get install libjpeg-dev
sudo apt-get install cmake
sudo apt-get install libusb-1.0-0-dev

LIBUVCのインストール

sudo apt-get install git
cd ~
git clone git://github.com/ktossell/libuvc.git
cd libuvc
mkdir build
cd build
cmake ..
make
sudo make install

その他のPangolinの依存ライブラリのインストール

sudo apt-get install libpython2.7-dev
sudo apt-get install libavcodec-dev libavutil-dev libavformat-dev libswscale-dev
sudo apt-get install libdc1394-22-dev libraw1394-dev
sudo apt-get install libpng12-dev libtiff5-dev libopenexr-dev
  • Pangolin(描画ライブラリ)のインストール
cd ~
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
cmake --build .
sudo make install
  • Eigen3(行列計算ライブラリ)のインストール

自分は一番新しい3.3.7のリリース版を入れました。以下のサイトからEigenの3.3.7のtarファイルのリンクを探しました。make checkでいくつかエラーが発生しましたが、構わずにインストールしました。それでも問題なくORB_SLAM2は動きました。make checkはなくてもインストールできました。異様に長いので飛ばしても大丈夫かと思います。
http://eigen.tuxfamily.org/index.php?title=Main_Page

cd ~
wget https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.tar.gz
tar xvf eigen-3.3.7.tar.gz
mkdir build
cd build
cmake ..
make check
sudo make install
  • OpenCV(コンピュータビジョンのライブラリ)のインストール

OpenCVの依存ライブラリのインストール

sudo apt-get install build-essential
sudo apt-get install libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libpng-dev libtiff-dev libjasper-dev

Ubuntu 14.04の場合はOpenCV 2.4.8~2.4.x
Ubuntu 16.04の場合はOpenCV 3.x
を使用しないとROS実行時にエラーが出るかもです。
自分は3.1.0を入れました。以下のサイトからOpenCVのリリース版のtarファイルのリンクを探しました。
https://opencv.org/releases/

cd ~
wget https://github.com/opencv/opencv/archive/3.1.0.tar.gz
tar xvf 3.1.0.tar.gz
cd opencv-3.1.0
mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
make -j4
sudo make install
  • g2oのインストール

g2o依存ライブラリのインストール

sudo apt install libsuitesparse-dev - qtdeclarative5-dev - qt5-qmake - libqglviewer-dev

g2oのインストール

cd ~
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build
cd build
cmake ..
make
sudo make install
  • DBoW2のインストール

「Ubuntu14.04の場合」
Ubuntu14.04だと少し厄介です。16.04では大丈夫です。
OpenCV 3.xのバージョンだとDBoW2の最新バージョンのインストールは行えます。しかし、それがUbuntu14.04+ROS indigoの環境だと、結局ROS実行時にエラーが出て動きませんでした。なのでOpenCV2.4.xを使うと今度はDBoW2の最新バージョンが入りません。DBoW2の古いバージョンを使うことになります。その場合は以下のDLibとDLoopDetectorをインストールしてから、DBoW2 v1.0を入れてください。
https://github.com/dorian3d/DLib/releases/tag/v1.0
https://github.com/dorian3d/DLoopDetector/releases/tag/v1.0
https://github.com/dorian3d/DBoW2/releases/tag/v1.0

cd ~
wget https://github.com/dorian3d/DBoW2/archive/v1.0.tar.gz
cd DBoW2-1.0
mkdir build
cd build
cmake ..
make
sudo make install

「Ubuntu16.06の場合」
DBoW2の最新バージョンならインストールする際にDLibなども一緒に入ってくれますので以下のコマンドだけでいけます。

cd ~
git clone https://github.com/dorian3d/DBoW2.git
cd DBoW2
mkdir build
cd build
cmake ..
make
sudo make install
  • ROSのインストール

Ubuntu14.04の場合はROS indigo(http://wiki.ros.org/ja/indigo/Installation/Ubuntu)
Ubuntu16.04の場合はROS kinetic(http://wiki.ros.org/ja/kinetic/Installation/Ubuntu)
に従ってインストールを行なってください。以下のコマンドはROS indigoをインストールした際のコマンドです。

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
sudo apt-get update
sudo apt-get install ros-indigo-desktop-full
sudo rosdep init
rosdep update
echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt-get install python-rosinstall
  • ORB_SLAM2のインストール
cd ~
git clone https://github.com/raulmur/ORB_SLAM2.git
cd ORB_SLAM2
chmod +x build.sh
./build.sh

ここまででも一応ROSを利用せず、単独でなら動かせるので動作確認してもみてもいいかもしれません。
次にROS上でのビルドです。

cd ~/ORB_SLAM2
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/ORB_SLAM2/Examples/ROS
chmod +x build_ros.sh
./build_ros.sh

cd ~/ORB_SLAM2/Examples
rosws init . /opt/ros/indigo
rosws set ./ROS -t . 
source ./setup.bash
source ~/ORB_SLAM2/Examples/setup.bash
cd ./ROS
rosmake ORB_SLAM2

ようやくインストール完了です。