vins 实机测试 rs_d435 + imu

vins 实机测试

1. imu标定

git clone https://github.com/gaowenliang/code_utils.git
git clone https://github.com/gaowenliang/imu_utils.git

编译运行，
roslaunch imu_node imu_node.launch 

录rosbag
rosbag record /imu -o imu.bag

回放数据并进行标定
rosbag play -r 200 imu.bag
roslaunch imu_utils calb_imu.launch

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>
        <param name="imu_name" type="string" value= "demo"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "92"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

result

%YAML:1.0
---
type: IMU
name: demo
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.1579014862814587e-03
      gyr_w: 1.2080473993001349e-05
   x-axis:
      gyr_n: 7.8848718937629160e-04
      gyr_w: 1.1330695111173287e-05
   y-axis:
      gyr_n: 1.6161269692829495e-03
      gyr_w: 7.6665713225286887e-06
   z-axis:
      gyr_n: 1.0690903001851347e-03
      gyr_w: 1.7244155545302071e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.9601230975888886e-02
      acc_w: 7.9342809029857330e-04
   x-axis:
      acc_n: 2.4602981097020603e-02
      acc_w: 7.1949651700994606e-04
   y-axis:
      acc_n: 2.6079587164852428e-02
      acc_w: 7.3517233974109439e-04
   z-axis:
      acc_n: 3.8121124665793614e-02
      acc_w: 9.2561541414467935e-04

2. camera内参标定

ref: https://wiki.ros.org/camera_calibration

sudo apt install ros-noetic-usb-cam   ros-noetic-camera-calibration

usb_cam 源码： git clone  git@github.com:ros-drivers/usb_cam.git

运行

标定板棋盘格：

https://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFile&do=view&target=check-108.pdf

usb_cam.launch

<launch>
  <arg name="image_view" default="false" />

  <node name="cam0" pkg="usb_cam" type="usb_cam_node" output="screen" >
      <rosparam command="load" file="$(find usb_cam)/config/usb_cam.yml"/>
  </node>
  <node if="$(arg image_view)" name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/cam0/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

roslaunch usb_cam usb_cam.launch

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0244 image:=/usb_cam/image_raw  camera:=/usb_cam

棋盘格·标定结果

**** Calibrating ****
mono pinhole calibration...
D = [-0.0276431859616864, 0.05488145026340878, -0.0005282408469462047, -0.0067853851219709105, 0.0]
K = [524.1288091767894, 0.0, 333.5105867456761, 0.0, 523.1947915574598, 239.9741914015902, 0.0, 0.0, 1.0]
R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P = [525.1634521484375, 0.0, 328.12270221472863, 0.0, 0.0, 528.1163940429688, 239.20473370594664, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
524.128809 0.000000 333.510587
0.000000 523.194792 239.974191
0.000000 0.000000 1.000000

distortion
-0.027643 0.054881 -0.000528 -0.006785 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
525.163452 0.000000 328.122702 0.000000
0.000000 528.116394 239.204734 0.000000
0.000000 0.000000 1.000000 0.000000

('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')

rosrun kalibr kalibr_calibrate_cameras  --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml    --bag /home/kint/work/vi_ws/cam_imu_2024-01-04-15-39-36.bag  --models pinhole-radtan  --topics /cam0/image_raw  --show-extraction
 

rosrun kalibr kalibr_calibrate_cameras  --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml    --bag /home/kint/work/vi_ws/rs_d435_cam_imu_2024-01-04-17-23-03.bag   --models pinhole-radtan  --topics /camera/color/image_raw  --show-extraction

april·标定结果

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.1275672152449369, -0.295951445225514, -0.0005669310686147839, -0.00039780673670447125]
  distortion_model: radtan
  intrinsics: [580.6826631313598, 582.9348842345427, 333.60826920015415, 244.81883705086946]
  resolution: [640, 480]
  rostopic: /camera/color/image_raw

3. imu-cam 外参标定

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

sudo apt-get install python3-catkin-tools python3-osrf-pycommon # ubuntu 20.04
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

# Ubuntu 20.04
sudo apt-get install -y python3-dev python3-pip python3-scipy \
    python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph python3-pyx

https://github.com/ethz-asl/kalibr/wiki/downloads 下载标定板

Name	Target	Config
Aprilgrid 6x6 0.8x0.8 m (A0 page)	pdf	yaml

rosrun  kalibr kalibr_calibrate_imu_camera \
--target /home/kint/work/kalibr_workspace/data/april_6x6.yaml \
--cam /home/kint/work/kalibr_workspace/data/cam.yaml \
--imu /home/kint/work/kalibr_workspace/data/imu.yaml \
--bag /home/kint/work/vi_ws/cam_imu0.bag 

april_6x6.yaml

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.0211           #size of apriltag, edge to edge [m]
tagSpacing: 0.2986          #ratio of space between tags to tagSize
codeOffset: 0            #code offset for the first tag in the aprilboard

cam.yaml

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.1275672152449369, -0.295951445225514, -0.0005669310686147839, -0.00039780673670447125]
  distortion_model: radtan
  intrinsics: [580.6826631313598, 582.9348842345427, 333.60826920015415, 244.81883705086946]
  resolution: [640, 480]
  rostopic: /camera/color/image_raw

imu.yaml

#Accelerometers
accelerometer_noise_density: 3.3646456935574526e-02 #Noise density (continuous-time)
accelerometer_random_walk:   4.3950304954064600e-04    #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.1620361962149783e-03   #Noise density (continuous-time)
gyroscope_random_walk:       9.3617466820677679e-06   #Bias random walk

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

外参标定结果

kint@kint:~/work/kalibr_workspace$ rosrun  kalibr kalibr_calibrate_imu_camera --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml --cam /home/kint/work/kalibr_workspace/data/cam.yaml --imu /home/kint/work/kalibr_workspace/data/imu.yaml --bag /home/kint/work/vi_ws/cam_imu0.bag

importing libraries
the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'
Initializing IMUs:
	Number of messages: 65282
Initializing camera chain:
Extracting calibration target corners
  Extracted corners for 3777 images (of 10625 images)                              

Building the problem

Estimating time shift camera to imu:

Initializing a pose spline with 64959 knots (100.000000 knots per second over 649.593485 seconds)
  Time shift camera to imu (t_imu = t_cam + shift):
-0.049899223470539586

Estimating imu-camera rotation prior

Initializing a pose spline with 64959 knots (100.000000 knots per second over 649.593485 seconds)
Initializing a pose spline with 64971 knots (100.000000 knots per second over 649.713485 seconds)

Initializing the bias splines with 32486 knots
 
Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.5155776132784602, median 0.4129175412893566, std: 0.38771090059687746
 
Initializing
Optimization problem initialized with 129962 design variables and 473260 error terms
The Jacobian matrix is 1076724 x 584811
[0.0]: J: 2.92788e+07
[1]: J: 1.08954e+06, dJ: 2.81892e+07, deltaX: 1.27806, LM - lambda:10 mu:2
[2]: J: 172653, dJ: 916889, deltaX: 1.00136, LM - lambda:3.33333 mu:2

[24]: J: 62682.1, dJ: 0.0110847, deltaX: 0.000338729, LM - lambda:5.17786 mu:2
[25]: J: 62682.1, dJ: 0.00762514, deltaX: 0.000203091, LM - lambda:6.59487 mu:2

After Optimization (Results)
==================
Residuals
----------------------------
Results written to:
  Saving camera chain calibration to file: /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10--camchain-imucam.yaml
  Saving imu calibration to file: /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10--imu.yaml
  Detailed results written to file: /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10--results-imucam.txt

Generating result report... 

外参标定结果:rs_435_2024-01-05-09-59-10-camchain-imucam.yaml

cam0:
  T_cam_imu:
  - [0.9980660136864593, 0.061740755753177695, 0.007232662237817192, 0.03244989658293066]
  - [-0.06215949356871943, 0.9924592035136889, 0.10564528726002882, 0.035751985575793274]
  - [-0.0006555023266318506, -0.1058905493422342, 0.9943775751075137, -0.0025956152371000036]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.1275672152449369, -0.295951445225514, -0.0005669310686147839, -0.00039780673670447125]
  distortion_model: radtan
  intrinsics: [580.6826631313598, 582.9348842345427, 333.60826920015415, 244.81883705086946]
  resolution: [640, 480]
  rostopic: /camera/color/image_raw
  timeshift_cam_imu: 0.018309189059828063

rosrun  kalibr kalibr_calibrate_imu_camera --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml --cam /home/kint/work/kalibr_workspace/data/cam.yaml --imu /home/kint/work/vi_ws/src/calb_data/data/imu.yaml  --bag /home/kint/work/vi_ws/cam_imu_2024-01-04-15-39-36.bag

rosrun  kalibr kalibr_calibrate_imu_camera --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml --cam /home/kint/work/vi_ws/src/calb_data/data/rs_cam.yaml --imu /home/kint/work/vi_ws/src/calb_data/data/imu.yaml  --bag /home/kint/work/vi_ws/rs_d435_cam_imu_2024-01-04-17-23-03.bag

4. vins 实际运行

kailbr 离线相机imu外参标定 https://github.com/ethz-asl/kalibr/wiki/installation

标定板下载： https://github.com/ethz-asl/kalibr/wiki/downloads

vins-mono (noetic) ： https://github.com/kintzhao/VINS-Mono
主要是opencv 版本的差异调整

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

realsense d435 （30hz）+ 独立imu (100hz)

demo_rs.launch

<launch>
    <arg name="config_path" default = "$(find feature_tracker)/../config/demo/demo_rs_config.yaml" />
	  <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
    
    <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

    <node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
       <param name="config_file" type="string" value="$(arg config_path)" />
       <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

    <node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="visualization_shift_x" type="int" value="0" />
        <param name="visualization_shift_y" type="int" value="0" />
        <param name="skip_cnt" type="int" value="0" />
        <param name="skip_dis" type="double" value="0" />
    </node>

</launch>

demo_rs_config.yaml

%YAML:1.0

#common parameters
imu_topic: "/imu"
image_topic: "/camera/color/image_raw"
output_path: "/home/kint/work/vi_ws/out/"

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: -0.027643
   k2: 0.054881
   p1: -0.000528
   p2: -0.006785
projection_parameters:
   fx: 524.128809
   fy: 523.194792
   cx: 333.510587
   cy: 239.974191

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 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.9980660136864593, 0.061740755753177695, 0.007232662237817192,
          -0.06215949356871943, 0.9924592035136889, 0.10564528726002882, 
          -0.0006555023266318506, -0.1058905493422342, 0.9943775751075137]

#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.03244989658293066, 0.035751985575793274, -0.0025956152371000036]


#feature traker paprameters
max_cnt: 180 #150            # max feature number in feature tracking
min_dist: 20 #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: 1             # 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)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 3.4897398445637821e-02          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 1.0199941810169521e-03         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 8.1470209699926967e-04         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.4479924327109106e-06       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.8     # gravity magnitude

#loop closure parameters
loop_closure: 1                    # start loop closure
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0             # useful in real-time and large project
pose_graph_save_path: "/home/kint/work/vi_ws/out/pose_graph/" # save and load path

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

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

#visualization parameters
save_image: 1                   # 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

ref： https://www.cnblogs.com/lugendary/p/16717782.html

5. realsense

1. 库安装
sudo apt-get install libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libusb-1.0-0-dev pkg-config
sudo apt-get install libglfw3-dev
sudo apt-get install libssl-dev
 
 
2. librealsense 源码安装
下载librealsense源码： https://github.com/IntelRealSense/librealsense/releases
cd librealsense
sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && udevadm trigger 
mkdir build
cd build
cmake ../ -DBUILD_EXAMPLES=true
make
sudo make install

 
3. realsense ros编译
cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros.git
git clone https://github.com/pal-robotics/ddynamic_reconfigure.git
cd ~/catkin_ws && catkin_make
 

录包采集数据：

rosbag  record   /camera/color/camera_info /camera/color/image_raw    /imu  -o rs_435_cam_imu

问题：Spline Coefficient Buffer Exceeded. Set larger buffer margins!

Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 22710 design variables and 350146 error terms
The Jacobian matrix is 723008 x 102177
[0.0]: J: 1.40982e+06
Exception in thread block: [aslam::Exception] /home/kint/work/kalibr_workspace/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.70436e+09 <= 1.70436e+09 < 1.70436e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception]  

Exception in thread block: [aslam::Exception] /home/kint/work/kalibr_workspace/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.70436e+09 <= 1.70436e+09 < 1.70436e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1704363307.378352]: std::exception
[ERROR] [1704363307.380297]: Optimization failed!
Traceback (most recent call last):
 
RuntimeError: Optimization failed!

ref:
https://blog.csdn.net/qq_38337524/article/details/115589796
https://blog.csdn.net/qq_39779233/article/details/128704988
https://github.com/IntelRealSense/realsense-ros/issues/1665
解决方法：

  <arg name="initial_reset"             default="true"/>

修改代码/home/kint/work/vi_ws/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera

//LINE 199:
    iCal.buildProblem(splineOrder=6, 
                      poseKnotsPerSecond=100, 
                      biasKnotsPerSecond=50, 
                      doPoseMotionError=False,
                      doBiasMotionError=True,
                      blakeZisserCam=-1,
                      huberAccel=-1,
                      huberGyro=-1,
                      noTimeCalibration=parsed.no_time,
                      noChainExtrinsics=(not parsed.recompute_chain_extrinsics),
                      maxIterations=parsed.max_iter,
                      #timeOffsetPadding=parsed.timeoffset_padding,     #原来的代码
                      timeOffsetPadding=0.3,     #修改后
                      verbose = parsed.verbose) 

或者带参数运行

rosrun kalibr kalibr_calibrate_imu_camera --bag  calib_imu_cam.bag   --cam  camera.yaml     --imu imu.yaml --target  checkboard.yaml --timeoffset-padding 0.3

离线标定相关指令：

 rosrun kalibr kalibr_calibrate_cameras  --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml    --bag /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10.bag   --models pinhole-radtan  --topics /camera/color/image_raw  --show-extraction 

rosrun  kalibr kalibr_calibrate_imu_camera --target /home/kint/work/kalibr_workspace/data/april_6x6.yaml --cam /home/kint/work/vi_ws/src/calb_data/data/rs_cam.yaml --imu /home/kint/work/vi_ws/src/calb_data/data/imu.yaml  --bag    /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10.bag

!

roslaunch imu_node imu.launch 
roslaunch realsense2_camera d435.launch
roslaunch vins_estimator demo_rs.launch
roslaunch vins_estimator vins_rviz.launch