vins 实机测试
文章目录
- 1. imu标定
- 2. camera内参标定
- 3. imu-cam 外参标定
- 4. vins 实际运行
- 5. realsense
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-03gyr_w: 1.2080473993001349e-05x-axis:gyr_n: 7.8848718937629160e-04gyr_w: 1.1330695111173287e-05y-axis:gyr_n: 1.6161269692829495e-03gyr_w: 7.6665713225286887e-06z-axis:gyr_n: 1.0690903001851347e-03gyr_w: 1.7244155545302071e-05
Acc:unit: " m/s^2"avg-axis:acc_n: 2.9601230975888886e-02acc_w: 7.9342809029857330e-04x-axis:acc_n: 2.4602981097020603e-02acc_w: 7.1949651700994606e-04y-axis:acc_n: 2.6079587164852428e-02acc_w: 7.3517233974109439e-04z-axis:acc_n: 3.8121124665793614e-02acc_w: 9.2561541414467935e-04
2. camera内参标定
ref: https://wiki.ros.org/camera_calibration
sudo apt install ros-noetic-usb-cam ros-noetic-camera-calibrationusb_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.launchrosrun 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
640height
480[narrow_stereo]camera matrix
524.128809 0.000000 333.510587
0.000000 523.194792 239.974191
0.000000 0.000000 1.000000distortion
-0.027643 0.054881 -0.000528 -0.006785 0.000000rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000projection
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-extractionrosrun 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: pinholedistortion_coeffs: [0.1275672152449369, -0.295951445225514, -0.0005669310686147839, -0.00039780673670447125]distortion_model: radtanintrinsics: [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) | 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: pinholedistortion_coeffs: [0.1275672152449369, -0.295951445225514, -0.0005669310686147839, -0.00039780673670447125]distortion_model: radtanintrinsics: [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 walkrostopic: /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.bagimporting 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 cornersExtracted corners for 3777 images (of 10625 images) Building the problemEstimating 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.049899223470539586Estimating imu-camera rotation priorInitializing 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 knotsResiduals
----------------------------
Reprojection error (cam0) [px]: mean 0.5155776132784602, median 0.4129175412893566, std: 0.38771090059687746Initializing
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:2After 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.yamlSaving imu calibration to file: /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10--imu.yamlDetailed results written to file: /home/kint/work/vi_ws/rs_435_2024-01-05-09-59-10--results-imucam.txtGenerating 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: pinholedistortion_coeffs: [0.1275672152449369, -0.295951445225514, -0.0005669310686147839, -0.00039780673670447125]distortion_model: radtanintrinsics: [580.6826631313598, 582.9348842345427, 333.60826920015415, 244.81883705086946]resolution: [640, 480]rostopic: /camera/color/image_rawtimeshift_cam_imu: 0.018309189059828063rosrun 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.027643k2: 0.054881p1: -0.000528p2: -0.006785
projection_parameters:fx: 524.128809fy: 523.194792cx: 333.510587cy: 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-matrixrows: 3cols: 3dt: ddata: [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-matrixrows: 3cols: 1dt: ddata: [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-dev2. 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 install3. 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
