VINS-Fusion을 Intel Realsense D435i 카메라를 사용하여 실행

* 목표

Realsense D435i 카메라에서 나오는 IMU와 이미지를 바탕으로 VINS 돌리기

 

* 환경

Ubuntu 20.04

ROS1 Noetic

 

* 구현

 

0. Dependencies 설치

* Ceres Solver

sudo apt-get install -y cmake libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar zxf ceres-solver-1.14.0.tar.gz
cd ceres-solver-1.14.0
mkdir build && cd build
cmake -DEXPORT_BUILD_DIR=ON \
        -DCMAKE_INSTALL_PREFIX=/usr/local \
        ../
make -j 4 # number of cores
make test -j 4
sudo make install

* OpenCV

 

 

1. VINS-fusion 설치

기존 VINS-Fusion은 우분투 18.04 버전에 구현되어 있다.

하지만 나는 20.04에서 돌려야 하는 상황이었다.

20.04에서 설치할 경우 opencv 버전 문제로 인해 빌드가 잘 되지 않는다.

(참고)

https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/issues/167

VINS-fusion on Ubuntu 20.04 · Issue #167 · HKUST-Aerial-Robotics/VINS-Fusion

이를 해결해 놓은 깃을 찾았고, 기존 VINS-Fusion 대신 이 버전으로 설치하였다.

링크는 아래에 있다.

 

https://github.com/rkuo2000/VINS-Fusion/

GitHub - rkuo2000/VINS-Fusion: An optimization-based multi-sensor state estimator

mkdir -p ~/vins_ws/src
cd ~/vins_ws/src

git clone https://github.com/rkuo2000/VINS-Fusion.git
cd ~/vins_ws
catkin build

2. Realsense package 설치

 

sudo apt-get install ros-noetic-realsense2-camera

 

3. Realsense SDK 설치 및 세팅

 

https://yongee.tistory.com/49

[Ubuntu] Realsense SDK(realsense-viewer) 설치

 

- 이 부분은 실제로 필요한지 검증되지 않음, 추후 생략해도 문제 없는지 확인 필요하나, 일단 설정했을 때 잘 작동했기에 추가

============================================================================================

 

SDK 실행

realsense-viewer

 

Emitter Enable 설정을 Off로 변경

 

이후 설정 파일 저장. 파일명은 vins_setting.json으로 저장

============================================================================================

 

4. kalibr 설치

 Calibration 하기 위한 패키지

 

아래 명령어를 통해 dependencies 설치

sudo apt-get install -y \
    git wget autoconf automake nano \
    libeigen3-dev libboost-all-dev libsuitesparse-dev \
    doxygen libopencv-dev \
    libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev
    
    sudo apt-get install -y python3-dev python3-pip python3-scipy \
    python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph python3-pyx

기존 워크스페이스 설정 변경

cd ~/vins_ws
catkin init
catkin config --extend /opt/ros/$ROS1_DISTRO
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

cd ~/vins_ws
catkin init
catkin config --extend /opt/ros/$ROS1_DISTRO
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

*** 아래의 링크에는 추가되어있으나 vins를 설치한 워크스페이스에 kalibr 설치할 경우 아래 명령어 입력하면 빌드 할 때 에러가 발생하므로 생략함.

#catkin config --merge-devel

클론 후 빌드

cd ~/vins_ws/src
git clone https://github.com/ethz-asl/kalibr.git
cd ~/vins_ws
catkin build -DCMAKE_BUILD_TYPE=Release

 

* 설치 방법 안내

https://github.com/ethz-asl/kalibr/wiki/installation

Installation

 

* 깃허브

https://github.com/ethz-asl/kalibr

GitHub - ethz-asl/kalibr: The Kalibr visual-inertial calibration toolbox

 

* 5. D435i 자체 imu calibration

 

https://www.intel.com/content/dam/support/us/en/documents/emerging-technologies/intel-realsense-technology/RealSense_Depth_D435i_IMU_Calib.pdf

 

6. Camera Calibration

 

 

(참고 사이트)

https://support.stereolabs.com/hc/en-us/articles/360012749113-How-can-I-use-Kalibr-with-the-ZED-Mini-camera-in-ROS-

 

https://github.com/engcang/vins-application/tree/Intel-D435i

 

 

 

 

아래 링크를 눌러 6x6 Apriltag 다운로드

https://drive.google.com/file/d/10zw3LvCDvXYyTQje4Gt4vzJMMYOuGsma/view

april_6x6.yaml

이후 출력.  원본은 A0이나 여건상 A4로 출력하여 진행함.

 

7. Calibration 진행하기 위한 설정 파일 작성

 

* Apriltag

cd ~/data
vi april_6x6.yaml

april_6x6.yaml

target_type: 'aprilgrid'   #gridtype
tagCols: 6                 #number of apriltags
tagRows: 6                 #number of apriltags
tagSize: 0.022             #size of apriltag, edge to edge [m]
tagSpacing: 0.272727            #ratio of space between tags to tagSize
                           #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

- tag size와 spacing은 직접 자로 측정하였고, 각 22mm와 6mm이었음.

 

* IMU

vi imu-params.yaml

imu-params.yaml

accelerometer_noise_density: 0.001865   #Noise density (continuous-time)
accelerometer_random_walk:   0.0002   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     0.0018685   #Noise density (continuous-time)
gyroscope_random_walk:       0.000004   #Bias random walk

rostopic:                    /camera/imu      #the IMU ROS topic
update_rate:                 400.0     #Hz (for discretization of the values above)

출처
https://github.com/engcang/vins-application/tree/Intel-D435i

GitHub - engcang/vins-application: VINS-Fusion, VINS-Fisheye, OpenVINS, EnVIO, ROVIO, ORB-SLAM2, NVIDIA Elbrus application of di

 

 

8. realsense launch파일 작성

roscd realsense2_camera/
cp rs_camera.launch vins_d435i.launch
vi vins_d435i.launch

열어서 일부분만 수정

 

vins_d45i.launch의 일부

  <arg name="json_file_path"      default="[path-to-jetson]/vins_setting.json"/>
  <arg name="enable_infra"        default="true"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>
  
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>
  
  <arg name="unite_imu_method"          default="linear_interpolation"/>

 

9. rosbag 파일 생성

(6)에서 출력한 파일을 평평한 면에 잘 펴놓은 다음 카메라로 녹화 진행

 

위와 같이 책상에 고정하여 진행

 

rosbag record /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /camera/imu

 

10. Calibration 진행

 

* Camera Calibration

rosrun kalibr kalibr_calibrate_cameras \
--bag ./data/calib_d435i.bag  \ 
--target ./data/april_6x6.yaml  \
--models pinhole-radtan pinhole-radtan \
--topics /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw

 

* Camera-IMU Calibration

rosrun kalibr kalibr_calibrate_imu_camera \ 
--bag ./data/calib_d435i.bag \ 
--target ./data/april_6x6.yaml \ 
--cam ./data/calib_d435i-camchain.yaml \ 
--imu ./data/imu-params.yaml

 

11. Calibration으로 얻은 행렬을 vins 설정 파일에 복사

 

cd [PATH-TO-VINS-FUSION]/config/realsense_d435i
vi realsense_stereo_imu_config.yaml

수정할 행렬

 

 

12. 실행

# terminal 1
roscore
# terminal 2
rosrun vins vins_node ~/vins_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config
# terminal 3
roslaunch vins vins_rviz.launch
# terminal 4
roslaunch realsense2_camera vins_d435i.launch

 

 

결과