evoを使用してORB-SLAM3のSLAM精度を評価する

このブログは、株式会社フィックスターズのエンジニアが、あらゆるテーマについて自由に書いているブログです。

2022年10月4日 ryo.suzuki

はじめに

こんにちは、Fixstars Autonomous Technologiesの鈴木です。

自動運転やロボットなどの分野でSLAMと呼ばれる技術があります。
SLAMとは「Simultaneous Localization and Mapping」の略で、自動車やロボットに搭載されたカメラやLiDARなどのセンサーから得られた情報を元に、自己位置推定と地図の作成を行う技術を指します。
本稿では、代表的なSLAM評価ツールであるevoを使用したSLAM精度評価についてORB-SLAM3を題材に解説を行います。

代表的なSLAMにはLiDAR SLAMとVisual SLAMの2種類があります。

LiDAR SLAMとは、LiDARと呼ばれるレーザーを使った測距センサーを利用したSLAMのことを指します。
Visual SLAMとは、カメラなどのイメージセンサーを利用したSLAMのことを指します。
ORB-SLAM3はカメラから取得した画像データを使用してSLAMを行うため、Visual SLAMに該当します。

ORB-SLAM3の詳細については論文をご参考ください。

Groundtruth（真値データ）との比較によるSLAM精度の評価には以下の作業が必要です。

Groundtruthファイルを含むデータセットの準備
SLAMプログラムの実行結果である軌跡ファイルの取得
Groundtruthファイルと軌跡ファイルのフォーマットの統一（必要に応じて）
evoによる評価結果の取得

上記の一連の作業について、本稿で順に解説します。

動作確認環境

本稿の作業は以下の開発環境で動作確認されています。

OS : Ubuntu 18.04.5 LTS
CPU : Intel(R) Core(TM) i7-8700 CPU @ 3.20GHz
メモリ : 48GB

本稿ではDocker環境を構築し、Dockerコンテナ上で作業を行います。
Dockerを使用しない場合には ORB-SLAM3についてはORB-SLAM3のREADMEを、 evoについては Installation / Upgradeを参照して環境構築を行ってください。

データセットの準備

ORB-SLAM3は実機のカメラを使用しなくとも実行することができます。
実機のカメラを使用しない実行方法には、以下の2つが考えられます。

Gazebo などのシミュレーターを使用する
時系列で連続する画像データと、軌跡情報のGroundtruthが同梱されたデータセットを使用する

今回は準備と実行が比較的簡単な2を採用します。ここではデータセットとしてミュンヘン工科大学のサイトからダウンロードできる Sequence 'freiburg1_xyz' を使用します。表の fr1/xyz の行にある tgz と書かれたリンクからrgbd_dataset_freiburg1_xyz.tgz をダウンロードします。なお、rgbd_dataset_freiburg1_xyz.tgz のサイズは 428 MBです。

以下のコマンドでダウンロードした圧縮ファイルを解凍し、 /opt以下に配置してください。なお、展開後の rgbd_dataset_freiburg1_xyzディレクトリの合計サイズは 461 MBです。

tar xzvf rgbd_dataset_freiburg1_xyz.tgz
sudo mkdir -p /opt/dataset/tum/
sudo mv rgbd_dataset_freiburg1_xyz /opt/dataset/tum/

ORB-SLAM3の実行

以下のコマンドを実行し、ビルドしたDockerイメージから、Dockerコンテナを作成します。
なお、画面を表示するためにX11転送の設定を行っています。

XSOCK=/tmp/.X11-unix
XAUTH=/tmp/.docker.xauth
touch $XAUTH
xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -

docker run -it --rm --privileged \
  --env "XAUTHORITY=${XAUTH}" \
  --env "DISPLAY=${DISPLAY}" \
  --env TERM=xterm-256color \
  --env QT_X11_NO_MITSHM=1 \
  --volume $XSOCK:$XSOCK:rw \
  --volume $XAUTH:$XAUTH:rw \
  --volume /opt/dataset:/opt/dataset \
  --net host \
  --name orb-slam3 \
  sample/orb-slam3 \
  bash

正常に実行できた場合、ターミナルのユーザー名が fatとなっているはずです。

Dockerコンテナの中で以下のコマンドを実行し、ORB-SLAM3を実行します。
ここでは rgbd_dataset_freiburg1_xyzのRGB画像とDepth画像を使用してSLAMを行うように実行します。

cd $HOME/ORB_SLAM3/
./Examples/RGB-D/rgbd_tum \
  Vocabulary/ORBvoc.txt \
  Examples/RGB-D/TUM1.yaml \
  /opt/dataset/tum/rgbd_dataset_freiburg1_xyz \
  $HOME/associations.txt

データセットの最後のデータが読み込まれてSLAMが終了するまで待ちます。
SLAMが正常に実行終了した場合、 $HOME/ORB_SLAM3/CameraTrajectory.txtが生成されます。
これは、SLAM実行時の軌跡のデータを格納したファイルです。
このファイルを使用してSLAM精度の評価を行います。

SLAM精度の評価

evo とは、SLAM評価を行うことができるPythonパッケージです。
evoの使用例についてはワークフローにも記載されています。

ORB-SLAM3を実行して得られた軌跡ファイルをevoを使用してSLAM精度を評価します。 SLAM精度の評価はrgbd_dataset_freiburg1_xyzに同梱されているGroundtruthとの比較で行います。

Dockerコンテナの中で以下のコマンドを実行し、GroundtruthファイルとSLAM実行結果の軌跡ファイルのAPE(Absolute Pose Error : 絶対位置誤差)の評価を行います。^1
ここで、以下のコマンドログの各項目の意味を以下に示します。

max : 誤差の最大値
mean : 誤差の平均値
median : 誤差の中央値
min : 誤差の最小値
rmse(root mean squared error) : 二乗平均平方根誤差
sse(sum of squared errors) : 平方根誤差の合計値
std : 標準誤差

evo_ape tum $HOME/groundtruth.txt $HOME/ORB_SLAM3/CameraTrajectory.txt --align -p

得られる出力の例を示します。

APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

      max      0.033069
      mean      0.009058
    median      0.007842
      min      0.000433
      rmse      0.010556
      sse      0.088024
      std      0.005421

上記から、最大誤差は約 3.3 cm、平均誤差は 0.9 cmであることがわかります。

描画結果は以下のようになります。

上の図は縦軸がAPE、横軸が時間です。
グラフが縦に大きい箇所ほど、自己位置推定の誤差が大きいことがわかります。

上の図は軌跡全体のプロット図と、その誤差を示しています。
画像右上の軌跡が特に赤色に近いことから、自己位置推定の誤差が比較的大きいことがわかります。
マウスカーソルで操作することで視点を変えることができます。

次に、Dockerコンテナの中で以下のコマンドを実行し、GroundtruthファイルとSLAM実行結果の軌跡ファイルの評価結果のプロットを行います。

evo_traj tum --ref=$HOME/groundtruth.txt $HOME/ORB_SLAM3/CameraTrajectory.txt --align -p

得られる出力の例を示します。

--------------------------------------------------------------------------------
name:   CameraTrajectory
infos:  790 poses, 8.738m path length, 26.572s duration
--------------------------------------------------------------------------------
name:   groundtruth
infos:  3000 poses, 9.159m path length, 30.090s duration

描画結果は以下のようになります。

上の図は軌跡全体のプロット図です。
マウスカーソルで操作することで視点を変えることができます。

上の図は縦軸が距離、横軸が時間です。
点線のGroundtruthと実線のSLAM軌跡がずれている箇所ほど、自己位置推定の誤差が大きいことがわかります。

上の図は縦軸が角度、横軸が時間です。
点線のGroundtruthと実線のSLAM軌跡がずれている箇所ほど、自己位置推定の誤差が大きいことがわかります。

`evo` のオプションについて

evo の主要なオプションについて記載します。

`-p` オプション

evo_ape tum --help には以下のように説明が記載されています。

-p, --plot            show plot window

このオプションを使用した場合には、評価結果のプロットが行われます。

`--align` オプション

evo_ape tum --help には以下のように説明が記載されています。

-a, --align           alignment with Umeyama's method (no scale)

このオプションを使用した場合には、 Umeyama’s method でアラインを行います。
Groundtruthファイルと軌跡ファイルとの間で、開始時点と終了時点のタイムスタンプが異なる場合には、このオプションでアラインを行うことができます。

`--correct_scale` オプション

evo_ape tum --help には以下のように説明が記載されています。

-s, --correct_scale   correct scale with Umeyama's method

単眼SLAMの場合は、スケール不定の問題の回避のために、 Umeyama’s method でスケール補正を行い評価する必要があります。
このオプションを使用することでスケールを補正して評価を行うことができます。

単眼SLAMのスケール不定の問題の詳細については単眼カメラによるVisual SLAMの原理と3次元再構成の実装例をご参考ください。

`--save_as_tum` `--save_as_kitti` `--save_as_bag` オプション

evo_traj tum --help には以下のように説明が記載されています。

--save_as_tum         save trajectories in TUM format (as *.tum)
--save_as_kitti       save poses in KITTI format (as *.kitti)
--save_as_bag         save trajectories in ROS bag as <date>.bag

また各フォーマットとオプションの対応表は Saving / Exporting to other formats に記載されています。

このオプションを使用することで、Groundtruthファイルや軌跡ファイルの形式を他の形式から変換することができます。

本稿ではTUM形式のデータセットを使用し、軌跡ファイルもTUM形式で取得したためこのオプションは必要ありませんでした。
一方で、例えば The EuRoC MAV Dataset のEuRoC形式のデータセットを使用して、TUM形式の軌跡ファイルを取得した場合には、いずれかのファイルの形式を変換してもう一方の形式に合わせる必要があります。
例として The EuRoC MAV Dataset のデータセットに含まれているGroundtruthファイルをTUM形式に変換する場合のコマンドを示します。
data.csv はGroundtruthファイルです。

evo_traj euroc data.csv --save_as_tum

上記のコマンドを実行すると、TUM形式のGroundtruthファイルである data.tum が生成されます。

おわりに

Dockerコンテナ上で、 rgbd_dataset_freiburg1_xyz.tgz データセットとして、ORB-SLAM3を実行することができるようになりました。
また、ORB-SLAM3の実行後に得られる軌跡ファイルを使用して、そのSLAM精度を評価できるようになりました。
evo が対応している形式で軌跡ファイルを出力するSLAMソフトウェアであれば、そのSLAM精度を相互に評価・比較することもできます。

Appendix

Docker環境の準備

本稿では、ORB-SLAM3の実行と評価をDocker環境の中で行います。
使用するマシンにDockerがインストールされていない場合には、 Install Docker Engine on Ubuntu を参考に以下のコマンドを実行しDockerをインストールします。

sudo apt update
sudo apt install apt-transport-https ca-certificates curl gnupg lsb-release
curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /usr/share/keyrings/docker-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/docker-archive-keyring.gpg] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
sudo apt update
sudo apt install docker-ce=5:20.10.6~3-0~ubuntu-bionic docker-ce-cli=5:20.10.6~3-0~ubuntu-bionic containerd.io

作業を行うディレクトリを今回は ${HOME}/evo_ws とします。
使用するDockerイメージの元となる Dockerfile 本稿の末尾に記載しています。使用するマシン上に ${HOME}/evo_ws/Dockerfile にファイルを作成し、記載されている内容をコピーアンドペーストしてください。

作成した Dockerfile を元にDockerイメージをビルドします。
まず、以下のコマンドを実行し、Dockerイメージのビルドに必要なファイルを ${HOME}/evo_ws にコピーします。

cd ${HOME}/evo_ws/
cp /opt/dataset/tum/rgbd_dataset_freiburg1_xyz/depth.txt .
cp /opt/dataset/tum/rgbd_dataset_freiburg1_xyz/rgb.txt .
cp /opt/dataset/tum/rgbd_dataset_freiburg1_xyz/groundtruth.txt .

次に、以下のコマンドを実行し、Dockerイメージをビルドします。

docker build \
  --build-arg USER_NAME=fat \
  --build-arg USER_ID=$(id -u) \
  --build-arg GROUP_ID=$(id -g) \
  --build-arg NUM_THREADS=$(nproc) \
  -t sample/orb-slam3 \
  -f Dockerfile \
  .

正常にDockerイメージがビルドできた場合には、以下のログが表示されます。

Successfully tagged sample/orb-slam3:latest

Dockerfile

FROM ubuntu:20.04

ENV DEBIAN_FRONTEND=noninteractive

RUN apt-get update && \
    : "install bootstrap tools" && \
    apt-get install --no-install-recommends -y \
        build-essential \
        ca-certificates \
        cmake \
        curl \
        git \
        libboost-serialization-dev \
        libssl-dev \
        python2 \
        python3 \
        python3-pip \
        python3-tk \
        sudo \
        unzip \
        usbutils \
        v4l-utils \
        vim \
        wget && \
    : "install packages for pangolin" && \
    apt-get install --no-install-recommends -y \
        libgl1-mesa-dev \
        libglew-dev \
        libjpeg-dev \
        libpng-dev \
        libtiff5-dev \
        libopenexr-dev \
        libgtk2.0-dev && \
    : "install eigen" && \
    apt-get install --no-install-recommends -y \
        libeigen3-dev && \    
    rm -rf /var/lib/apt/lists/*

# install python-pip
RUN curl https://bootstrap.pypa.io/pip/2.7/get-pip.py --output get-pip.py && \
    python2 get-pip.py

ARG USER_NAME
ARG USER_ID
ARG GROUP_ID

RUN groupadd -g ${USER_ID} ${USER_NAME}
RUN useradd --uid ${USER_ID} --gid ${GROUP_ID} -d /home/${USER_NAME} ${USER_NAME} -m -s /bin/bash
RUN gpasswd -a ${USER_NAME} sudo && gpasswd -a ${USER_NAME} dialout && gpasswd -a ${USER_NAME} video

# Set password
# RUN echo "${USER_NAME}:passw0rd" | chpasswd

# Do not set a password
RUN sed -i -e 's/%sudo\tALL=(ALL:ALL) ALL/%sudo   ALL=(ALL:ALL) NOPASSWD:ALL/' /etc/sudoers

ENV HOME=/home/${USER_NAME}
USER ${USER_NAME}

# install evo
RUN python3 -m pip install --user evo==1.19.0 matplotlib==3.5.3

ARG NUM_THREADS

# build opencv
ENV OPENCV_VERSION=4.5.5

WORKDIR $HOME
RUN wget -q https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.zip && \
    mkdir -p $HOME/lib/$OPENCV_VERSION && \
    unzip -q ${OPENCV_VERSION}.zip && \
    cd opencv-${OPENCV_VERSION} && \
    mkdir -p build && \
    cd build && \
    cmake -DCMAKE_BUILD_TYPE=Release \
        -DCMAKE_INSTALL_PREFIX=$HOME/lib/$OPENCV_VERSION \
        -DBUILD_DOCS=OFF \
        -DBUILD_EXAMPLES=OFF \
        -DBUILD_JASPER=OFF \
        -DBUILD_OPENEXR=OFF \
        -DBUILD_PERF_TESTS=OFF \
        -DBUILD_TESTS=OFF \
        -DBUILD_opencv_apps=OFF \
        -DBUILD_opencv_dnn=OFF \
        -DBUILD_opencv_ml=ON \
        -DBUILD_opencv_objdetect=ON \
        -DBUILD_opencv_python_bindings_generator=OFF \
        -DENABLE_FAST_MATH=OFF \
        -DWITH_EIGEN=ON \
        -DWITH_FFMPEG=OFF \
        -DWITH_OPENMP=ON \
        -DENABLE_PRECOMPILED_HEADERS=OFF \
        -DBUILD_opencv_python2=OFF \
        -DBUILD_opencv_python3=OFF \
        .. && \
    make -j${NUM_THREADS:-''} && \
    make install

# build pangolin
RUN git clone https://github.com/stevenlovegrove/Pangolin.git && \
    cd $HOME/Pangolin && \
    git checkout dd801d244db3a8e27b7fe8020cd751404aa818fd && \
    mkdir -p $HOME/Pangolin/build && \
    mkdir -p $HOME/lib/Pangolin && \
    cd build && \
    cmake \
        -DCMAKE_BUILD_TYPE=Release \
        -DCMAKE_INSTALL_PREFIX=$HOME/lib/Pangolin \
        .. && \
    make -j${NUM_THREADS:-''} && \
    make install

ENV OpenCV_DIR=$HOME/lib/${OPENCV_VERSION}/lib/cmake/opencv4

# build ORB_SLAM3
RUN git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git && \
    cd ORB_SLAM3/ && \
    git checkout 4452a3c4ab75b1cde34e5505a36ec3f9edcdc4c4 && \
    : "avoid bug of https://github.com/raulmur/ORB_SLAM2/pull/628" && \
    sed -i -e "s/#include<string>/#include <unistd.h>\n#include<string>/g" include/System.h && \
    : "avoid bug of https://github.com/raulmur/ORB_SLAM2/pull/585" && \
    sed -i -e "s/std::pair<const KeyFrame\*, g2o::Sim3>/std::pair<KeyFrame\* const, g2o::Sim3>/g" include/LoopClosing.h && \
    /bin/bash -c "source ~/.bashrc; \
        export LD_LIBRARY_PATH=$HOME/Pangolin/build/src/:${LD_LIBRARY_PATH}; \
        "

RUN cd ORB_SLAM3/ && \
    /bin/bash -c "NUM_THREADS=$NUM_THREADS ./build.sh"

# download associate.py for rgbd_tum
RUN wget -q https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/associate.py

# install numpy for associate.py
RUN python2 -m pip install --user numpy==1.16.6

COPY --chown=${USER_NAME} rgb.txt $HOME/rgb.txt
COPY --chown=${USER_NAME} depth.txt $HOME/depth.txt
COPY --chown=${USER_NAME} groundtruth.txt $HOME/groundtruth.txt

# generate associations.txt
RUN python2 associate.py rgb.txt depth.txt > associations.txt

# set path and parallel number of build
RUN echo "export PATH=\$PATH:\$HOME/.local/bin" >> ~/.bashrc && \
    echo "export NUM_THREADS=$NUM_THREADS" >> ~/.bashrc && \
    echo "echo NUM_THREADS=\$NUM_THREADS" >> ~/.bashrc

About Author

ryo.suzuki

Favorite Post

「OpenFOAMスレッド並列化のための基礎検討」を投稿＆発表してきました
2018年2月6日
FFmpeg API の使い方(1): デコードしてみる
2017年8月22日
ディリクレ過程混合モデルによるクラスタリングの振舞い方
2017年10月31日

evoを使用してORB-SLAM3のSLAM精度を評価する

はじめに

動作確認環境

データセットの準備

ORB-SLAM3の実行

SLAM精度の評価

`evo` のオプションについて

`-p` オプション

`--align` オプション

`--correct_scale` オプション

`--save_as_tum` `--save_as_kitti` `--save_as_bag` オプション

おわりに

Appendix

Docker環境の準備

Dockerfile

Tags

About Author

ryo.suzuki

Leave a Comment コメントをキャンセル

Tags

Favorite Post

Archives

Categories

コンピュータビジョンセミナーvol.2 開催のお知らせ - ニュース一覧 - 株式会社フィックスターズ in Realizing Self-Driving Cars with General-Purpose Processors 日本語版

【Docker】NVIDIA SDK Managerでエラー無く環境構築する【Jetson】 | マサキノート in NVIDIA SDK Manager on Dockerで快適なJetsonライフ

Windowsカーネルドライバを自作してWinDbgで解析してみる① - かえるのほんだな in Windowsデバイスドライバの基本動作を確認する (1)

2021年版G検定チートシート | エビワークス in ニューラルネットの共通フォーマット対決！ NNEF vs ONNX

YOSHIFUJI Naoki in CUDAデバイスメモリもスマートポインタで管理したい

Social Media

evoを使用してORB-SLAM3のSLAM精度を評価する

はじめに

動作確認環境

データセットの準備

ORB-SLAM3の実行

SLAM精度の評価

evo のオプションについて

-p オプション

--align オプション

--correct_scale オプション

--save_as_tum --save_as_kitti --save_as_bag オプション

おわりに

Appendix

Docker環境の準備

Dockerfile

Tags

About Author

ryo.suzuki

Leave a Comment コメントをキャンセル

Tags

Favorite Post

Archives

Categories

コンピュータビジョンセミナーvol.2 開催のお知らせ - ニュース一覧 - 株式会社フィックスターズ in Realizing Self-Driving Cars with General-Purpose Processors 日本語版

【Docker】NVIDIA SDK Managerでエラー無く環境構築する【Jetson】 | マサキノート in NVIDIA SDK Manager on Dockerで快適なJetsonライフ

Windowsカーネルドライバを自作してWinDbgで解析してみる① - かえるのほんだな in Windowsデバイスドライバの基本動作を確認する (1)

2021年版G検定チートシート | エビワークス in ニューラルネットの共通フォーマット対決！ NNEF vs ONNX

YOSHIFUJI Naoki in CUDAデバイスメモリもスマートポインタで管理したい

Social Media

`evo` のオプションについて

`-p` オプション

`--align` オプション

`--correct_scale` オプション

`--save_as_tum` `--save_as_kitti` `--save_as_bag` オプション