ROS 2でstella_vslamを動かすメモ

ROS 2でVisial SLAMを動かしたい！！！

`stalle_vslam`というものを見つけたのでROS 2でネイティブ環境で動かしてみる。
docsを参考にしたけどよくわからない内容が多くてよくわからない

~~Dockerでやった方がはやい~~

# メモ
```bash
mkdir stella_vslam
cd stella_vslam
mkdir src
cd src
sudo apt install -y libglm-dev libglfw3-dev libpng-dev libjpeg-dev libeigen3-dev libboost-filesystem-dev libboost-program-options-dev
git clone --recursive https://github.com/koide3/iridescence.git
cd iridescence
mkdir -p build
cd build
cmake \
    -DCMAKE_BUILD_TYPE=RelWithDebInfo \
    ..
make -j$(($(nproc) / 2))
sudo make install
```

ここでは`SocketViewer`てのを使ってSLAMの結果を可視化する
```bash:SocketViewerの前準備？
sudo apt install -y autogen autoconf libtool
git clone https://github.com/shinsumicco/socket.io-client-cpp.git
cd socket.io-client-cpp
git submodule init
git submodule update
mkdir build && cd build
cmake \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=/usr/local \
    -DBUILD_UNIT_TESTS=OFF \
    ..
make -j4
sudo make install
sudo apt install -y libprotobuf-dev protobuf-compiler
wget -q https://github.com/google/protobuf/archive/v3.6.1.tar.gz
tar xf v3.6.1.tar.gz
cd protobuf-3.6.1
./autogen.sh
./configure \
    --prefix=/usr/local \
    --enable-static=no
make -j4 # なんかあやしい
sudo make install # なんかあやしい
```

```bash:stella_vslamのビルド
rosdep update
sudo apt update
git clone --recursive --depth 1 https://github.com/stella-cv/stella_vslam.git
rosdep install -y -i --from-paths stella_vslam
cd stella_vslam/
mkdir -p build
cd build
cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo ..
make -j4
sudo make install
```

```bash:SocketViewerのstella_vslamプラグイン？のビルド
git clone --recursive https://github.com/stella-cv/socket_publisher.git
cd socket_publisher/
mkdir -p build
cd build
cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo ..
make -j
sudo make install
```

```bash:stella_vslam_ros
git clone --recursive -b ros2 --depth 1 https://github.com/stella-cv/stella_vslam_ros.git
rosdep install -y -i --from-paths stella_vslam_ros --skip-keys=stella_vslam
cd ..
colcon build --symlink-install --packages-up-to stella_vslam_ros
```

ここまでで必要なパッケージのビルドは完了。
こっから詰まってる
```bash
# ボキャブラリファイルを取得
curl -sL "https://github.com/stella-cv/FBoW_orb_vocab/raw/main/orb_vocab.fbow" -o orb_vocab.fbow
```

```bash:カメラキャリブレーション
sudo apt install ros-humble-camera-calibration ros-humble-image-pipeline

# ターミナル1
ros2 run v4l2_camera v4l2_camera_node

# ターミナル2 チェスボードの設定は変えてね
ros2 run camera_calibration cameracalibrator --size 6x9 --square 0.029 image:=/image_raw

# /tmpに保存されるキャリブレーションデータを解凍して持ってくる
mkdir calib_data
tar -xzvf /tmp/calibrationdata.tar.gz -C calib_data/
```

キャリブレーションデータをstella_vslamで使えるように変換（ChatGPT君にやってもらった）
- `"perspective"`: 標準的なピンホールカメラモデル
- `"fisheye"`: 魚眼レンズモデル
- `"equirectangular"`: 全天球カメラモデル

```yaml:ost.yaml
image_width: 640
image_height: 480
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [634.65355,   0.     , 330.26394,
           0.     , 637.15023, 245.406  ,
           0.     ,   0.     ,   1.     ]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.044028, -0.171853, -0.008163, 0.012192, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1., 0., 0.,
         0., 1., 0.,
         0., 0., 1.]
projection_matrix:
  rows: 3
  cols: 4
  data: [633.47095,   0.     , 336.30829,   0.     ,
           0.     , 641.51245, 242.23006,   0.     ,
           0.     ,   0.     ,   1.     ,   0.     ]
```

```bash:c922_pro_stream_webcam.yaml
Camera:
  name: "C922 PRO Stream WebCam"
  setup: "monocular"
  model: "perspective"
  fx: 634.65355
  fy: 637.15023
  cx: 330.26394
  cy: 245.406
  k1: 0.044028
  k2: -0.171853
  p1: -0.008163
  p2: 0.012192
  k3: 0.0
  fps: 30.0  # 必要に応じて設定
  cols: 640
  rows: 480
  color_order: "RGB"  # カメラの設定に応じて変更
```

```bash:socket_viewerの起動（nodejs）
sudo apt update
sudo apt install -y nodejs npm
npm install express
git clone --recursive https://github.com/stella-cv/socket_viewer.git
cd socket_viewer/

# ターミナル1
node app.js
```

```bash:実行
# ターミナル2
ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video0

# ターミナル3
ros2 run image_transport republish raw in:=/image_raw out:=/camera/image_raw

# ターミナル4（カメラ確認用）
ros2 run rqt_image_view rqt_image_view

# ターミナル5（stella_vslamの実行）
ros2 run stella_vslam_ros run_slam -v orb_vocab.fbow -c calib_data/c922_pro_stream_webcam.yaml --viewer socket_publisher
```

**でぎだ！！！！！**

これで、`localhost:3001`をブラウザで開くとslamの結果が表示されるはず！！


# 参考文献
[stella_vslam docs](https://stella-cv.readthedocs.io/en/latest/ros2_package.html)