search

jianhengLiu / VINS-RGBD-FAST

VINS-RGBD-FAST is a SLAM system based on VINS-RGBD. We do some refinements to accelerate the system's performance in resource-constrained embedded platforms, like HUAWEI Atlas 200DK, Raspberry Pi.
133 stars 32 forks source link

The Localization result looks strange. #5

Closed demul closed 2 years ago

demul commented 2 years ago

1. How to Reproduce

1.1. Build Docker iamge

make Dockerfile like below

FROM ros:melodic-ros-core-bionic
# apt-get update
RUN apt-get update
# install essentials
RUN apt install -y gcc
RUN apt install -y g++
RUN apt-get install -y cmake
RUN apt-get install -y wget
RUN apt install -y git
# install ceres
WORKDIR /home
RUN apt-get install -y libgoogle-glog-dev libgflags-dev
RUN apt-get install -y libatlas-base-dev
RUN apt-get install -y libeigen3-dev
RUN apt-get install -y libsuitesparse-dev
RUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz
RUN tar zxf ceres-solver-2.1.0.tar.gz
WORKDIR /home/ceres-solver-2.1.0
RUN mkdir build
WORKDIR /home/ceres-solver-2.1.0/build
RUN cmake ..
RUN make
RUN make install
# install sophus
WORKDIR /home
RUN git clone https://github.com/demul/Sophus.git
WORKDIR /home/Sophus
RUN git checkout fix/unit_complex_eror
RUN mkdir build
WORKDIR /home/Sophus/build
RUN cmake ..
RUN make
RUN make install
# install ros dependencies
WORKDIR /home
RUN mkdir ros_ws
WORKDIR /home/ros_ws
RUN apt-get -y install ros-melodic-cv-bridge
RUN apt-get -y install ros-melodic-nodelet
RUN apt-get -y install ros-melodic-tf
RUN apt-get -y install ros-melodic-image-transport
RUN apt-get -y install ros-melodic-rviz
# build vins-rgbd-fast
RUN mkdir src
WORKDIR /home/ros_ws/src
RUN git clone https://github.com/jianhengLiu/VINS-RGBD-FAST
WORKDIR /home/ros_ws
RUN /bin/bash -c ". /opt/ros/melodic/setup.bash; cd /home/ros_ws; catkin_make"
RUN echo "source /home/ros_ws/devel/setup.bash" >> ~/.bashrc

docker build . --tag vins_rgbd_fast:1.0

1.2. Run Docker

docker run -it --name vins_rgbd_fast -v /tmp/.X11-unix:/tmp/.X11-unix -e DISPLAY=$DISPLAY -p 9999:9999 -p 9999:9999 vins_rgbd_fast:1.0

1.3. Inside the Docker

Change imu topic name in vio.yaml, to "/camera/imu"

then

roslaunch vins_estimator vins_rviz.launch roslaunch vins_estimator realsense_vio.launch rosbag play Normal.bag from [https://star-center.shanghaitech.edu.cn/seafile/d/0ea45d1878914077ade5/]

2. Result

https://user-images.githubusercontent.com/38728285/177267397-e86b67de-5964-415a-8463-943b78accdbf.mp4

Help me what's wrong?

jianhengLiu commented 2 years ago

Sorry for late response. I missed this message. The config files in this repo are modified with my own settings. So, to run datasets from [https://star-center.shanghaitech.edu.cn/seafile/d/0ea45d1878914077ade5/], you need to modify the config file in config/realsense/vio.yaml with the config in https://github.com/STAR-Center/VINS-RGBD/blob/master/config/realsense/realsense_color_config.yaml

For specific, from your video, it seems that the extrinsic and intrinsic are not set well. The following params should work for datasets from VINS-RGBD

%YAML:1.0

num_threads: 0  # 0  Use the max number of threads of your device.
                #    For some devices, like HUAWEI Atlas200, the auto detected max number of threads might not equivalent to the usable numble of threads. (Some cores(threads) might be reserved for other usage(NPU) by system.)
                # x  It is proper that 1 < x < MAX_THREAD.
                # For now, this parameter is relevant to grid-detector to run in parallel. 

#common parameters
imu: 1
static_init: 1
imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"
depth_topic: "/camera/aligned_depth_to_color/image_raw"
output_path: "/home/chrisliu/FASTLAB_ws/src/realflight_modules/VINS-RGBD-FAST/output"

#RGBD camera Ideal Range
depth_min_dist: 0.3
depth_max_dist: 6
fix_depth: 1    # 1: feature in ideal range will be set as constant 

frontend_freq: 20 # It should be raised in VO mode(without IMU).
num_grid_rows: 7
num_grid_cols: 8

#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480

distortion_parameters:
   k1: 1.25323e-01
   k2: -2.51452e-01
   p1: 7.12e-04
   p2: 6.217e-03
projection_parameters:
   fx: 6.165911254882812e+02
   fy: 6.166796264648438e+02
   cx: 3.242193603515625e+02
   cy: 2.3942701721191406e+02

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [  0.0008748, -0.0047406,  0.9999884,
           -0.9999681, -0.0079460,  0.0008371,
            0.0079419, -0.9999572, -0.0047474 ]
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [1.30441e-05, 0.0149225, 0.000316835]

#feature traker paprameters
max_cnt: 130           # max feature number in feature tracking. It is suggested to be raised in VO mode.
min_dist: 30           # min distance between two features
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 0             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel); if system fails frequently, please try to reduce the "keyframe_parallax"  

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. 
gyr_n: 0.5         # gyroscope measurement noise standard deviation.   
acc_w: 0.001         # accelerometer bias random work noise standard deviation. 
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.  

g_norm: 9.805       # gravity magnitude

#unsynchronization parameters
estimate_td: 1    ##########                  # online estimate time offset between camera and imu
td: -0.0229402219449                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#rolling shutter parameters
rolling_shutter: 1                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0.033               # unit: s. rolling shutter read out time per frame (from data sheet).

#loop closure parameters
loop_closure: 0                    # start loop closure
fast_relocalization: 0            # useful in real-time and large project
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/chrisliu/FASTLAB_ws/src/realflight_modules/VINS-RGBD-FAST/output/pose_graph" # save and load path

#visualization parameters
save_image: 0                   # enable this might cause crash; save image in pose graph for visualization prupose; you can close this function by setting 0
visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4      # size of camera marker in RVIZ
demul commented 2 years ago

It works. Thanks!