[Slam] Vins-Fusion

0. 사용할 토픽들

 

 

1. 적당한 디렉토리를 만들어서, 아래의 Dockerfile을 만든다.

# 기반 이미지는 ros2 humble로 설정 
FROM osrf/ros:humble-desktop

# apt설치중 지역/시간대 선택과 같은 대화형 입력이 뜨지 않게 한다.
# 왜냐 docker build중에는 사용자가 입력할 수 없기 때문.
ENV DEBIAN_FRONTEND=noninteractive

# 기본 shell을 /bin/bash로 변경 
SHELL ["/bin/bash", "-c"]

# VINS-Fusion-ROS2를 빌드하고 실행하는 데 필요한 의존성 패키지들을 설치한다.
RUN apt-get update && apt-get install -y \
    git \
    cmake \
    build-essential \
    python3-colcon-common-extensions \
    python3-rosdep \
    libopencv-dev \
    libeigen3-dev \
    libceres-dev \
    libgoogle-glog-dev \
    libsuitesparse-dev \
    ros-humble-cv-bridge \
    ros-humble-image-transport \
    ros-humble-message-filters \
    ros-humble-tf2 \
    ros-humble-tf2-ros \
    ros-humble-tf2-eigen \
    ros-humble-rviz2 \
    ros-humble-rqt-image-view \
    && rm -rf /var/lib/apt/lists/*

# ros2 워크스페이스의 src디렉토리로 이동
WORKDIR /root/vins_ws/src

# vins-fusion ros2 humble을 clone한다.
# clone한 디렉토리 이름은 vins-fusion-ros2로 지정 
RUN git clone https://github.com/JanekDev/VINS-Fusion-ROS2-humble-arm.git VINS-Fusion-ROS2

RUN sed -i 's/options.dense_linear_algebra_library_type = ceres::CUDA;/options.dense_linear_algebra_library_type = ceres::EIGEN;/' \
    /root/vins_ws/src/VINS-Fusion-ROS2/vins/src/estimator/estimator.cpp

# ros2 워크스페이스 루트로 이동한다 
WORKDIR /root/vins_ws

RUN source /opt/ros/humble/setup.bash && \
    colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

RUN echo "source /opt/ros/humble/setup.bash" >> /root/.bashrc && \
    echo "source /root/vins_ws/install/setup.bash" >> /root/.bashrc

CMD ["/bin/bash"]

 

 

2. 해당 디렉토리로 가서 build한다.

•  docker build -t vins-fusion-humble .

3. 실행

docker run -it --rm   --name vins_humble   --network host   --ipc host   --pid host   --privileged   -e ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0}   -e ROS_LOCALHOST_ONLY=0   -e RMW_IMPLEMENTATION=rmw_fastrtps_cpp   -e FASTDDS_BUILTIN_TRANSPORTS=UDPv4   -v ~/vins_config:/root/vins_config   vins-fusion-humble

 

 

4. docker안에서 다름과 같이 입력하여 py파일 생성

• 1. mkdir -p /root/vins_config/carter
• 2. cd /root/vins_config/carter
• 3. nano /root/vins_config/carter/make_vins_config.py
• 4. python3 /root/vins_config/carter/make_vins_config.py

make_vins_config.txt

0.01MB

 

 

5. 생성된 파일 확인

• carter_front_stero.yaml은 위쪽의 cam_0_calib , cam_1_calib를 절대경로가 아니라 상대 경로로 수정한다.

cat /root/vins_config/carter/carter_front_left.yaml
cat /root/vins_config/carter/carter_front_right.yaml
cat /root/vins_config/carter/carter_front_stereo_imu.yaml

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CameraInfo
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
from pathlib import Path
import math

OUT = Path("/root/vins_config/carter")

LEFT_TOPIC = "/carter1/front_stereo_camera/left/camera_info"
RIGHT_TOPIC = "/carter1/front_stereo_camera/right/camera_info"

IMAGE0_TOPIC = "/carter1/front_stereo_camera/left/image_raw"
IMAGE1_TOPIC = "/carter1/front_stereo_camera/right/image_raw"
IMU_TOPIC = "/carter1/front_stereo_imu/imu"

class Grabber(Node):
    def __init__(self):
        super().__init__("make_vins_config")
        self.left = None
        self.right = None

        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE,
        )

        self.create_subscription(CameraInfo, LEFT_TOPIC, self.left_cb, qos)
        self.create_subscription(CameraInfo, RIGHT_TOPIC, self.right_cb, qos)

    def left_cb(self, msg):
        self.left = msg

    def right_cb(self, msg):
        self.right = msg

def distortion_params(msg):
    d = list(msg.d)
    while len(d) < 4:
        d.append(0.0)

    # ROS plumb_bob usually: k1, k2, t1/p1, t2/p2, k3
    return d[0], d[1], d[2], d[3]

def write_cam_yaml(path, name, msg):
    k = list(msg.k)
    k1, k2, p1, p2 = distortion_params(msg)

    text = f"""%YAML:1.0
---
model_type: PINHOLE
camera_name: {name}

image_width: {msg.width}
image_height: {msg.height}

distortion_parameters:
   k1: {k1}
   k2: {k2}
   p1: {p1}
   p2: {p2}

projection_parameters:
   fx: {k[0]}
   fy: {k[4]}
   cx: {k[2]}
   cy: {k[5]}
"""
    path.write_text(text)

def estimate_baseline(left, right):
    # Stereo CameraInfo의 projection matrix P에서 baseline 추정:
    # right.P[3] = -fx * baseline 인 경우가 많음
    p = list(right.p)
    if len(p) >= 4 and abs(p[0]) > 1e-9 and abs(p[3]) > 1e-9:
        return abs(-p[3] / p[0])

    # P가 안 채워져 있으면 임시값
    return 0.12

def write_main_yaml(path, left, right, baseline):
    text = f"""%YAML:1.0

imu: 1
num_of_cam: 2

imu_topic: "{IMU_TOPIC}"
image0_topic: "{IMAGE0_TOPIC}"
image1_topic: "{IMAGE1_TOPIC}"

output_path: "/tmp/vins_carter/"

cam0_calib: "/root/vins_config/carter/carter_front_left.yaml"
cam1_calib: "/root/vins_config/carter/carter_front_right.yaml"

image_width: {left.width}
image_height: {left.height}

# 처음에는 online extrinsic estimation 사용
estimate_extrinsic: 1

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0,
          0, 0, 0, 1]

# 임시 stereo baseline. CameraInfo P에서 추정됨.
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [1, 0, 0, {baseline},
          0, 1, 0, 0,
          0, 0, 1, 0,
          0, 0, 0, 1]

multiple_thread: 1

max_cnt: 150
min_dist: 30
freq: 10
F_threshold: 1.0
show_track: 1
flow_back: 1

max_solver_time: 0.04
max_num_iterations: 8
keyframe_parallax: 10.0

# Isaac Sim에서 우선 안정적으로 시작하기 위한 임시 IMU noise
acc_n: 0.1
gyr_n: 0.01
acc_w: 0.001
gyr_w: 0.0001
g_norm: 9.81

estimate_td: 0
td: 0.0

load_previous_pose_graph: 0
pose_graph_save_path: "/tmp/vins_carter/pose_graph/"
save_image: 0
"""
    path.write_text(text)

def main():
    rclpy.init()
    node = Grabber()

    print("Waiting for camera_info...")
    while rclpy.ok() and (node.left is None or node.right is None):
        rclpy.spin_once(node, timeout_sec=0.2)

    OUT.mkdir(parents=True, exist_ok=True)

    baseline = estimate_baseline(node.left, node.right)

    write_cam_yaml(OUT / "carter_front_left.yaml", "carter_front_left", node.left)
    write_cam_yaml(OUT / "carter_front_right.yaml", "carter_front_right", node.right)
    write_main_yaml(OUT / "carter_front_stereo_imu.yaml", node.left, node.right, baseline)

    print("Generated:")
    print(OUT / "carter_front_left.yaml")
    print(OUT / "carter_front_right.yaml")
    print(OUT / "carter_front_stereo_imu.yaml")
    print(f"Estimated stereo baseline: {baseline} m")

    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

 

 

 

6. 도커 컨테이너에서 이렇게 입력하면, stero-only 버전이 실행된다.

• ros2 run vins vins_node /root/vins_config/carter/carter_front_stereo_only_stable.yaml

 

7. 이번에는 imu까지 같이 하는것을 실행할 것이다.

• ros2 run vins vins_node /root/vins_config/carter/carter_front_stereo_imu.yaml

 

8. 하지만 위의 방법대로 하면 아래와 같이 될거임

• 여기에서 average acc를 보면, x축 방향으로 중력이 들어오고 있음.
•  

해당되는 average acc를 보면, x축으로 중력이 작용중

 

 

 

9. carter_front_stereo_imu.yaml을 보면, boty_T_cam0이 identity matrix다.

• 위와 같은 것은, imu좌표계와 카메라 좌표계가 같다고 가정하는 것이다.
• 하지만 실제로는 다르다.
• 그래서 imu-camera extrinsic을 실제 tf를 찾아서 config에 넣어야 함.
• 9-1. 우선 camera의 imu와 left,right의 image_raw에 대해서 frame_id를 보자.

이런 식으로 나올거임

• 9-2. 아래와 같이 입력
  • ros2 run tf2_ros tf2_echo   front_stereo_camera_imu   front_stereo_camera_left_rgb   --ros-args -r /tf:=/carter1/tf
  • ros2 run tf2_ros tf2_echo   front_stereo_camera_imu   front_stereo_camera_right_rgb   --ros-args -r /tf:=/carter1/tf

이렇게 뜰거임. 이제 이를 반영해야함.

• 9-3. 저거를 기반으로 carter_front_stereo_imu.yaml을 수정

왼쪽이 수정 후 , 오른쪽이 수정 전.

• 9-4. 다시 실행해본다.

rm -rf /tmp/vins_carter
mkdir -p /tmp/vins_carter

ros2 run vins vins_node /root/vins_config/carter/carter_front_stereo_imu.yaml