direct_visual_lidar_calibrationのサンプルの実行方法


# 概要

このページは以下のdirect_visual_lidar_calibrationのLIVOXのサンプルの実行方法を

まとめたものです。

[https://github.com/koide3/direct_visual_lidar_calibration](https://github.com/koide3/direct_visual_lidar_calibration)

# 動作確認環境

| OS | ubuntu22.04LTS |
| --- | --- |
| ROS | ROS2 Humble |
| GPU | RTX3060 |
| NVIDIA Driver | 525.60 |

# 事前準備

rosbagを入れるディレクトリと結果を格納するディレクトリを作成する

ここではrosbagを入れるディレクトリをbags/ , 結果を入れるディレクトリをresult/とする

```bash
mkdir bags/ result/
```

以下からrosbagデータをダウンロードして解凍し、bags/ディレクトリに入れる

• [livox.tar.gz (ROS2)](https://zenodo.org/record/7779880/files/livox.tar.gz?download=1)

上のリンクからダウンロードすると遅いので、またはこちらからダウンロード

[https://drive.google.com/drive/folders/18A7K5TC4pDpQHmdl_FXJklQxgOycBmbD](https://drive.google.com/drive/folders/18A7K5TC4pDpQHmdl_FXJklQxgOycBmbD)

# インストール

ほぼこちらのページの通り

[https://koide3.github.io/direct_visual_lidar_calibration/installation/](https://koide3.github.io/direct_visual_lidar_calibration/installation/)

## 依存パッケージのインストール

- [ROS1/ROS2](https://www.ros.org/)
- [PCL](https://pointclouds.org/)
- [OpenCV](https://opencv.org/)
- [GTSAM](https://gtsam.org/)
- [Ceres](http://ceres-solver.org/)
- [Iridescence](https://github.com/koide3/iridescence)
- [SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) [自動でInitial guessする際に必要]

※ ROS2, PCL, OpenCVはすでにインストール済みとする　(ROS 2をインストールするとPCLとOpenCVも入る)

```bash
sudo apt install libomp-dev libboost-all-dev libglm-dev libglfw3-dev libpng-dev libjpeg-dev
```

### GTSAM

ubuntu22.04の場合 cmake に　-DGTSAM_USE_SYSTEM_EIGEN=ON　をつける

```bash
git clone https://github.com/borglab/gtsam
cd gtsam && git checkout 4.2a9
mkdir build && cd build
# For Ubuntu 22.04, add -DGTSAM_USE_SYSTEM_EIGEN=ON
cmake .. -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
         -DGTSAM_BUILD_TESTS=OFF \
         -DGTSAM_WITH_TBB=OFF \
         -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF
make -j$(nproc)
sudo make install
```

### Ceres

```bash

git clone https://github.com/ceres-solver/ceres-solver
mkdir ceres-solver/build && cd ceres-solver/build
cmake .. -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DUSE_CUDA=OFF
make -j$(nproc)
sudo make install
```

### Iridescence

```bash
git clone https://github.com/koide3/iridescence --recursive
mkdir iridescence/build && cd iridescence/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install
```

### SuperGlue

```bash
pip3 install numpy opencv-python torch matplotlib
git clone https://github.com/magicleap/SuperGluePretrainedNetwork.git

echo 'export PYTHONPATH=$PYTHONPATH:/path/to/SuperGluePretrainedNetwork' >> ~/.bashrc
source ~/.bashrc
```

## direct_visual_lidar_calibration ROS2パッケージのインストール

```bash
mkdir -p ~/ros2_ws/src 
cd ~/ros2_ws/src
git clone https://github.com/koide3/direct_visual_lidar_calibration.git --recursive
cd .. && colcon build --symlink-install
```

# キャリブレーションの実行

プログラムの詳細はこちら

[https://koide3.github.io/direct_visual_lidar_calibration/programs/](https://koide3.github.io/direct_visual_lidar_calibration/programs/)

キャリブレーションは以下の4つのステップで行う

1. **Preprocessing**
事前処理
****rosbag データから点群 .plyと画像 .pngの ペアを抽出する
2. **Initial guess (Manual or Auto)**
事前の位置推定
手動または自動で点群と画像の対応する特徴点をマッチングしておおまかな位置合わせをする
3. **Calibration**
キャリブレーション
4. **Result inspection**
GUIツールでキャリブレーション結果の確認

## 1. **Preprocessing**

実行 

```bash
ros2 run direct_visual_lidar_calibration preprocess bags result -av
```

![Screenshot from 2023-04-30 10-20-30.png](https://s3-us-west-2.amazonaws.com/secure.notion-static.com/0cb957e5-9dd7-4199-8b95-5534654d34b9/Screenshot_from_2023-04-30_10-20-30.png)

result ディレクトリに

各 rosbagデータの ply とpng データおよび、結果を格納するcalib.json ファイルができていることを確認

```bash
porizou@porizou-RM7C-R35T:~/lidar_camera_calibration/result$ ls
calib.json                                         rosbag2_2023_03_09-13_44_10_lidar_indices.png      rosbag2_2023_03_09-13_46_10.png
rosbag2_2023_03_09-13_42_46.ply                    rosbag2_2023_03_09-13_44_10_lidar_intensities.png  rosbag2_2023_03_09-13_46_10_lidar_indices.png
rosbag2_2023_03_09-13_42_46.png                    rosbag2_2023_03_09-13_44_54.ply                    rosbag2_2023_03_09-13_46_10_lidar_intensities.png
rosbag2_2023_03_09-13_42_46_lidar_indices.png      rosbag2_2023_03_09-13_44_54.png                    rosbag2_2023_03_09-13_46_54.ply
rosbag2_2023_03_09-13_42_46_lidar_intensities.png  rosbag2_2023_03_09-13_44_54_lidar_indices.png      rosbag2_2023_03_09-13_46_54.png
rosbag2_2023_03_09-13_44_10.ply                    rosbag2_2023_03_09-13_44_54_lidar_intensities.png  rosbag2_2023_03_09-13_46_54_lidar_indices.png
rosbag2_2023_03_09-13_44_10.png                    rosbag2_2023_03_09-13_46_10.ply                    rosbag2_2023_03_09-13_46_54_lidar_intensities.png
```

## 2. **Initial guess**

### 手動の場合

```bash
ros2 run direct_visual_lidar_calibration initial_guess_manual result
```

1. `点群の3Dポイントと画像の対応する2Dポイントを右クリックします。`
2. `[ポイントを追加] ボタンをクリックします。`
3. `1と2を数回繰り返します（3点以上が望ましい）。`
4. `[推定] ボタンをクリックして、LiDARカメラ変換の初期推定値を取得します。`
5. `[blend_weight] を変更して、画像の投影結果が良好かどうかを確認します。`
6. `[保存] をクリックして、最初の推定値を保存します。`


### 自動の場合

- SuperGlue を実行して、LiDAR のカメラ画像間の対応を見つける。

```bash
ros2 run direct_visual_lidar_calibration find_matches_superglue.py result --rotate_camera 90
```

実行後 result ディレクトリに以下のような画像ができる

![rosbag2_2023_03_09-13_42_46_superglue.png](https://s3-us-west-2.amazonaws.com/secure.notion-static.com/0e10dfea-d728-4e7e-8f12-3f3de3d252c2/Screenshot_from_2023-04-30_10-33-05.png)

rosbag2_2023_03_09-13_42_46_superglue.png

```bash
torch.cuda.OutOfMemoryError: CUDA out of memory. Tried to allocate 1.20 GiB (GPU 0; 3.81 GiB total capacity; 1.30 GiB already allocated; 147.00 MiB free; 2.51 GiB reserved in total by PyTorch) If reserved memory is >> allocated memory try setting max_split_size_mb to avoid fragmentation.  See documentation for Memory Management and PYTORCH_CUDA_ALLOC_CONF
[ros2run]: Process exited with failure 1
```

もし上のようなエラーが出てプログラムが落ちる場合　--force_cpu オプションをつけてCPUのみで実行する

```bash
ros2 run direct_visual_lidar_calibration find_matches_superglue.py result --rotate_camera 90 --force_cpu
```

- RANSAC ベースの初期推定を実行

```bash
ros2 run direct_visual_lidar_calibration initial_guess_auto result
```

## 3. **Calibration**

NID ベースの LiDAR カメラ レジストレーションを実行して、LiDAR カメラの変換推定値をキャリブレーションする

```bash
ros2 run direct_visual_lidar_calibration calibrate result
```

![Screenshot from 2023-04-30 10-39-13.png](https://s3-us-west-2.amazonaws.com/secure.notion-static.com/ff27eb25-0196-40f8-bb00-2cb04e141d47/Screenshot_from_2023-04-30_10-39-13.png)

## 4. **Result inspection**

GUIツールでキャリブレーション結果を確認する

visualizerのblend_weightのバーを動かしながら点群と画像が一致していることを確認する