vins 实机测试 rs_d435 + imu
发布时间：2024年01月05日
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) | 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
文章来源:https://blog.csdn.net/zyh821351004/article/details/135407568
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！
最新文章
- Python教程
- 深入理解 MySQL 中的 HAVING 关键字和聚合函数
- Qt之QChar编码（1）
- MyBatis入门基础篇
- 用Python脚本实现FFmpeg批量转换
- 【Java 进阶篇】Jedis 操作 Set 与 SortedSet 详解
- 医院患者满意度抽样方法
- 【linux】基本命令详解
- 9.电子教务管理系统
- 静态代码扫描
- 【超图】SuperMap iClient3D for WebGL/WebGPU —— 数据集合并缓存如何控制对象样式
- 2024年阿里云优惠券领取及使用教程
- 一文让你了解UI自动化测试（超详细总结）
- ubuntu 开机自报IP地址（用于无屏幕小车-远程连接）
- 结构体—内存对齐