OSOYOO PI CARキット ROS2化 #8 – 自己位置推定の検討 ORB-SLAM3 搭載検討

OSOYOO PI CAR

前回までYOLOをノードとして搭載したのですが、とある方から「自律制御なら、まずSLAMだろ」と御指導をいただきました。当然、私はど素人なので、素直に助言に従います。
SLAMも沢山種類があり、どれを選んでよいのか正直わからなかったのですが、ROS1で搭載実績のある「単眼 Visual SLAM」が可能な「ORB-SLAM3」を使用しようと思いました。
Visual SLAMの方式は大きく２種類あるらしいのですが、「ORB-SLAM3」は、演算量の比較的少ない特徴点ベース方式に分類されるようです。
リソースは、単眼、ステレオ、LiDAR等への対応が可能となっていますが、カメラが１丁あるだけですので、単眼のみ対応しようと思います。
ROS２ノードへの対応は、自身の環境に合わせたいので、YOLO同様に適当に独自で用意しようと思います。

※ ORB-SLAM3のROS2(humble)対応は、やられている方が少なく、わたくしは正解と思われる方法は見つけられませんでした。本対応方式は、あくまで独自のど素人解決方式です。
参考程度にみていただければと思います。

■ ORB-SLAM3の準備/ビルド

下記にORB-SLAM3のgithubがあり説明等記載されています。
ORB-SLAM3
ただ、あまり親切な説明はなく、色々と調べるとビルドするには、自身の環境に合わせてCMakeLists.txtの修正が必要な模様です。
また、依存モジュールがあるようなので、それらもインストールしていきます。
必要なモジュールは、下記の２つです。ORB-SLAM3をビルドする前に用意しておきます。

  1. Pangolin
    ユーザーIF用のライブラリのようです。
    下記のようにビルドして、インストールを行います。
    Pangolin 0.91から、0.91をダウンロードしてソースを「Pangolin」に展開します。
    git cloneで取得してもよいかもしれませんが、私は念のためタグ付けされているコードを使用しました。

    % cd Pangolin
    % ./scripts/install_prerequisites.sh recommended
    % cmake -B build
    % cmake –build build
  2. Eigene3
    C++用の行列演算ライブラリのようです。
    Eigen3 3.4.0から、ソースコードをダウンロードして展開します。
    ダウンロードするファイル形式は、都合の良いものを使用します。ここでは、tar.gz形式のファイルを取り扱います。

    % tar zxvf eigen-3.4.0.tar.gz
    % cd eigen-3.4.0
    % mkdir build
    % cd build
    % cmake ..
    % sudo make install

ORB-SLAM3のビルド

まずは、下記のようにORB_SLAM3を取得します。
% git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git

ビルドする前にUbuntu 22.04.4 LTS ServerようにORB_SLAM3直下のCMakeLists.txtを修正します。

〇 OpenCVバージョンの修正(CMakeLists.txt)

ORB-SLAMが動作確認を取ってるOpenCVは4.4.0ですが、Ubuntu 22.04.4 LTS Serverには、4.5.4dが導入されています。
IF自体は4.4.0と4.5.4dでおおきく相違することはないので、対象バージョンを書き換えます。
CMakeLists.txt内の下記記述を変更します。

find_package(OpenCV 4.4 -> 4.5)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR “OpenCV > 4.5 not found.”)
endif()

〇 コンパイラ言語仕様の修正(CMakeLists.txt)

先にビルドしたPangolinとORB-SLAMの言語スペックが合わないので、ORB-SLAMの言語スペックをC++11からC++14に変更します。また、ROS2のデフォルト言語スペックもC++14であるので、整合性を保つためにC++14に設定します。
CMakeLists.txtの「-std=c++11」の記載を「-std=c++14」に置き換えます。

＃Check C++14 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG(“-std=c++14” COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG(“-std=c++0x” COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS “${CMAKE_CXX_FLAGS} -std=c++14“)
add_definitions(-DCOMPILEDWITHC11)
message(STATUS “Using flag -std=c++14.”)
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS “${CMAKE_CXX_FLAGS} -std=c++0x”)
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS “Using flag -std=c++0x.”)
else()
message(FATAL_ERROR “The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.”)
endif()

次に、Raspberry PI3の場合ですが、build.shを変更してジョブ数に制約を加えます。
Raspberry PI4以降でしたら、不要かもしれません

〇 ジョブ数の変更(build.sh)

「make -j」は、ジョブ数設定なのですが、これをすべて「make -j1」に変更します。
c++のコンパイラは、メモリ使用量を見ているとメモリ空き容量のある限り使うような設定になっているらしく、１本のコンパイルでメモリを80%以上消費します。
これを無条件、または、4本使用されてしまうと、全タスクがスワップだらけになってしまい、ほぼデッドロック状態に陥ります。

echo "Configuring and building Thirdparty/DBoW2 ..."

cd Thirdparty/DBoW2
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j1

cd ../../g2o

echo "Configuring and building Thirdparty/g2o ..."

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j1

cd ../../Sophus

echo "Configuring and building Thirdparty/Sophus ..."

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j1

cd ../../../

echo "Uncompress vocabulary ..."

cd Vocabulary
tar -xf ORBvoc.txt.tar.gz
cd ..

echo "Configuring and building ORB_SLAM3 ..."

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j1

ここまで変更がおわりましたら、下記のようにビルドを実行します。
Raspberry PI3の場合、かるく丸１ほどビルドに時間がかると思いますので、別のことをやった方がよいと思います。（私は、限界を感じてしまい、この間にラズパイ５を物色してました。）

% cd ORB_SLAM3
% ./build.sh

■ ROS2上でのビルド環境構築

次にROS2ノードのビルド環境を構築します。

〇 パッケージ生成

パッケージ生成は、ROS2の手順通りに下記のように行います。
以前、決めた命名規則通りにc++用のパッケージを生成します。

$ cd ros2_ws/src
$ ros2 pkg create picar_cpp_pkg

〇 ORB_SLAM3のパッケージ内への導入

ビルドするには、ORB_SLAM3や関連パッケージに含まれるインクルードファイル、ライブラリを参照する必要があります。どこに置くのか悩んだのですが、下記のような構成としました。

〇 スケルトンコード(picar_map_mono.cpp)

ノードがないとビルド環境のチェックができないので、Monocular(単眼 Visual Slam)のスケルトンをpicar_cpp_pkgに追加しておきます。
カメラ画像(/image_raw)をsubscriptionして、トラッキングし、終了時に軌道データを保存します。

#include <opencv2/core/core.hpp>

#include "rclcpp/rclcpp.hpp"
#include "cv_bridge/cv_bridge.hpp"

#include "System.h"

/*
 *  here's the definition of messages.
 */
#include "sensor_msgs/msg/image.hpp"


/*
 * here's the configuration of this file.
 */
#define CNF_VOCAVULARY_PATH "orb_slam3/Vocabulary/ORBvoc.txt"
#define CNF_SETTING_PATH    "orb_slam3/Setting/TUM.yaml"


using std::placeholders::_1;
using Image = sensor_msgs::msg::Image;


class PicarMapMonoNode : public rclcpp::Node
{   
public:     
    PicarMapMonoNode(ORB_SLAM3::System *pslam) : Node("picar_map_mono")
    {
        slam_ = pslam;
        img_subscriber_ = this->create_subscription<Image>(
            "/image_raw", 10,
            std::bind(&PicarMapMonoNode::trackImage, this, _1));
    
        RCLCPP_INFO(this->get_logger(), "monocular slam has been started.");
    }
    
    ~PicarMapMonoNode()
    {   
        slam_->Shutdown();
       slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    }

private:
    ORB_SLAM3::System*      slam_;
    cv_bridge::CvImagePtr   cvimg_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_subscriber_;

    /*
     *  purpose :   entry to track image to build map.
     */
    void trackImage(const Image::SharedPtr msg)
    {
        /* this converts ros raw image to cv:Mat.   */
        try 
        {
            cvimg_ = cv_bridge::toCvCopy(msg);
        }
        catch (cv_bridge::Exception &e)
        {   
            RCLCPP_ERROR(this->get_logger(), "cv_bridge error %s", e.what());
            return;
        }
        slam_->TrackMonocular(cvimg_->image, msg->header.stamp.sec);
        updateInfo();
    }

    /*
     *  purpose :   entry to update slam information.
     */
    void updateInfo()
    {
    }
};

int main(int argc, char **argv)
{
    /* this creates SLAM object.    */
    ORB_SLAM3::System   slam(CNF_VOCAVULARY_PATH, CNF_SETTING_PATH,
                            ORB_SLAM3::System::MONOCULAR, true);

    rclcpp::init(argc, argv);
    auto node = std::make_shared<PicarMapMonoNode>(&slam);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

〇 CMakeLists.txtの変更

ORB_SLAM3は、色々と関連パッケージがあるので少々設定が面倒でした。
ORB_SLAM3のCMakeLists.txtを手掛かりにして、必要なインクルード、ライブラリを追加しました。また、ワーニングの量もすごくて、みていると情報が錯乱しますので、確認したワーニングを表示しないように変更しました。
他にもっとよい方法があったかもしれないですが、力業の対応を行っています。
おそらくここが今回の記事の目玉となります。

赤字部：ワーニング対応
青字部：パッケージ、インクルード、ライブラリ対応部

cmake_minimum_required(VERSION 3.8)
project(picar_cpp_pkg)

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
    set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic
    -Wno-deprecated -Wno-reorder -Wno-unused-parameter
    -Wno-deprecated-declarations -Wno-sign-compare -Wno-pedantic
    -Wno-overloaded-virtual
    )
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(Pangolin REQUIRED)
find_package(Eigen3 REQUIRED)

# adds external library header.
include_directories(
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/include
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/include/CameraModels
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/Thirdparty/Sophus
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3
    ${EIGEN3_INCLUDE_DIR}
    ${Pangolin_INCLUDE_DIRS}
)

# adds executable nodes.
add_executable(picar_map_mono src/picar_map_mono.cpp)
ament_target_dependencies(picar_map_mono
    rclcpp
    sensor_msgs
    cv_bridge
    Pangolin
    Eigen3
)
target_link_libraries(picar_map_mono
    ${OpenCV_LIBS}
    ${EIGEN3_LIBS}
    ${Pangolin_LIBRARIES}
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/lib/libORB_SLAM3.so
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/Thirdparty/DBoW2/lib/libDBoW2.so
    ${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/Thirdparty/g2o/lib/libg2o.so
    -lboost_serialization
    -lcrypto
)

install(TARGETS
    picar_map_mono
    DESTINATION lib/${PROJECT_NAME}
)

ament_package()

〇 package.xmlの変更

package_xmlは、いつも通りにつくりました。こちらには、ORB_SLAM3に関する対応はおこないませんでした。cv_bridge、sensor_msg、rclcppのいつもの対応をいれておきました。（緑字部）

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>picar_cpp_pkg</name>
  <version>0.0.0</version> 
  <description>TODO: Package description</description>
  <maintainer email="ed@todo.todo">ed</maintainer>
  <license>TODO: License declaration</license>
  
  <buildtool_depend>ament_cmake</buildtool_depend>
    
  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>cv_bridge</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

〇 shared libraryパス設定

これまでの設定でビルドは可能なのですが、shared libraryが多数あるので、パスを設定しておく必要があります。Pangolin/ORB_SLAM3のshared libraryのパスを.bashrc等に記載すればよいのです。
ですが、バラバラに配置されていますので、結構パス設定が面倒です。
そこで、これらのバラバラに存在するshared libraryを特定ディレクトリ以下でリンクを作ることで、パス設定を単純化することにしました。
私は「ORB_SLAM3の準備/ビルド」でビルドしたPangolin/Eigene3/ORB_SLAM3を~/workspace/orb_slam3ディレクトリ以下でビルドしましたので、下記のようなスクリプトをまとめるディレクトリで実行することで一か所にまとめました。

#!/bin/bash
rm -f ./*.so

find ~/workspace/orb_slam3 -name "*.so" -print | xargs -I{} ln -s {}

下記のようように、必要なshared libraryへのリンクを作成することができました。

build_link.sh           libpango_packetstream.so
libDBoW2.so             libpango_plot.so
libORB_SLAM3.so         libpango_python.so
libg2o.so               libpango_scene.so
libpango_core.so        libpango_tools.so
libpango_display.so     libpango_vars.so
libpango_geometry.so    libpango_video.so
libpango_glgeometry.so  libpango_windowing.so
libpango_image.so       libtinyobj.so
libpango_opengl.so      pypangolin.cpython-310-aarch64-linux-gnu.so

あとは、下記の一文を.bashrcに追加することで、実行環境もできあがりまいた。（緑部は私がリンクを集めたディレクトリ）

export LD_LIBRARY_PATH=~/workspace/ros2_ws/orb_slam3/lib:${LD_LIBRARY_PATH}

〇 ビルド

あとは、ビルドするだけですが、ROS2のルールに従ってビルドするだけです。
緑部は、自分が今回作成したパッケージになります。

$ colcon build --packages-select picar_cpp_pkg

■ ORB_SLAM3の起動

使い方はよくわからないのですが、早速、ORB_SLAM3を起動してみました。
机の上で起動したところ、下記のような感じで何かをとらえているようです。
動き始めてから、CPUは約220～250%、メモリ使用量は動作時間に応じて徐々に増加してきますが、放置していると約2.6GBほど使用しているようです。
表示をやめた場合は、CPUは約150%程度、メモリは約2.4GBほどになるようです。
この状態では、CPU的にもメモリ的にもRaspberry PI3では少々厳しい感じがします。
最低でも、Raspberry PI4かそれ以上が欲しいところです。
画像変化量がおおいと、画像更新までに時間がかかりすぎてしまうのか、「xxx Frames set to lost」とメッセージが表示されます。

■ 次回

今回は、ORB_SLAM3をビルドして、とりあえず起動させてみました。
次回は、ORB_SLAM3の実用化に向けた設計を行ってみようと思います。
また、よろしければ見ていただけるとありがたいです。

タイトルとURLをコピーしました