INDEMIND相机运行VINS-slam
所需资源：imsee-sdk & vnis-mono & camera_calibration & imu_utils
相机/imutopic：
基于indemind官网提供的sdk：https://imsee-sdk-docs.readthedocs.io/zh/latest/来创建基于ros的topic。
IMSEE-SDK
slam：
VINS-Mono
使用步骤：
- 下载安装imsee-sdk
git clone https://github.com/INDEMIND/IMSEE-SDK
cd <imsee_sdk>
make ros
- 下载安装VINS-mono
cd catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ../
catkin_make
- 安装camera_calibration
#安装camera_calibration，一般ros已经默认安装
sudo apt-get install ros-melodic-camera-calibration
- 下载安装imu_utils
参考：传感标定
标定步骤参考：用imu_utils标定IMU
mkdir -p imu_calib_ws/src
cd imu_calib_ws/src
git clone https://github.com/gaowenliang/code_utils
cd ..
catkin_make
cd imu_calib_ws/src
git clone https://github.com/gaowenliang/imu_utils
cd ..
catkin_make
报错：backward.hpp:No such file or directory
解决：code_utils/src/sumpixel_test.cpp文件中，修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译。
imu标定需将imu静止放置2个小时以上抓取bag。
rosbag play -r 200 imu.bag (这里要写你录制的包的路径)
cd imu_ws
source ./devel/setup.bash
roslaunch imu_utils mynt_imu.launch
INDEMIND imu 标定 launch文件参考：
>
>
>
>
>
>
>
>
>
- 相机启动
#启动相机及topic
cd <imsee-sdk>
sudo -s
source ros/devel/setup.sh
roslaunch imsee_ros_wrapper display.launch
#rviz add camera的topic，查看camera输出图像
#新开终端，查看topic
#相机：/imsee/image/left /imsee/image/right
#imu：/imsee/imu
rostopic list
- 相机标定
这里直接双目标定
rosrun camera_calibration cameracalibrator.py --size 9x7 --square 0.019 --no-service-check right:=/imsee/image/right left:=/imsee/image/left
标定的参数解释，及标定板见INDEMIND相机运行ORB2-slam。
标定完成会得到如下文件：其中left.yaml和right.yaml中就是两个相机的参数文件。
这里选取左相机来运行vnis-mono，左相机的标定结果：
image_width: 640 #图像宽
image_height: 400 #图像高
camera_name: narrow_stereo/left
camera_matrix:
rows: 3
cols: 3
data: [245.05414, 0. , 326.48479, #data[0,0]及data[1,1]分别对应焦距fx,fy;
7. , 245.45758, 219.4987 , #data[2,0]及data[2,1]分别对应图像中心cx,cy 0. , 0. , 1. ]
camera_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.162523, -0.138890, 0.001188, 0.003208, 0.000000] #前四个参数分别对应矫正参数:k1, k2, p1, p2
rectification_matrix:
rows: 3
cols: 3
data: [ 0.99916629, -0.00384649, -0.04064403,
0.00427531, 0.99993606, 0.01046904,
0.04060116, -0.01063408, 0.99911884]
projection_matrix:
rows: 3
cols: 4
data: [292.27304, 0. , 281.13719, 0. ,
8. , 292.27304, 186.9123 , 0. ,
9. 0. , 0. , 1. , 0. ]
以下内容来自在ROS中实现双目相机校正:
7. IMU标定
imu_utils/launch下创建imu.launch文件，内容如下：
>
>
>
>
>
>
>
>
>
#在相机启动的前提下，静置相机两小时，录制imu topic bag
rosbag recore -O imu.bag /imsee/imu
#静止两小时后，ctrl+c停止录制
cd imu_calib_ws
source devel/setup.sh
rosbag play -r 200 imu.bag (这里要写你录制的包的路径)
roslaunch imu_utils imu.launch
标定完成后，data下生成标定文件imutest_imu_param.yaml，内容如下：avg:gyr_n，gyr_w与avg:acc_n，acc_w是我们需要的标定结果值。
%YAML:1.0
---
type: IMU
name: imutest
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 4.3640683630372197e-03 #陀螺仪随机噪声
gyr_w: 9.7385146937453744e-05 #陀螺仪偏置
x-axis:
gyr_n: 7.4206276278530074e-03
gyr_w: 2.3161327092970066e-04
y-axis:
gyr_n: 3.2267487733831143e-03
gyr_w: 3.9038387654271017e-05
z-axis:
gyr_n: 2.4448286878755374e-03
gyr_w: 2.1503782228389590e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 3.5150854597494917e-02 #加速度计随机噪声
acc_w: 9.2026307997110271e-04 #加速度计偏置
x-axis:
acc_n: 3.7171726966421861e-02
acc_w: 9.9000255391234224e-04
y-axis:
acc_n: 4.2027165328840821e-02
acc_w: 1.4118229439303129e-03
z-axis:
acc_n: 2.6253671497222079e-02
acc_w: 3.5896374207065315e-04
- vins 配置，参考小觅摄像头 VINS-MONO。
cd <VINS-mono path>/config
mkdir inde
在inde下创建inde_config.yaml
内容参考如下：
%YAML:1.0
#common parameters
imu_topic: "/imsee/imu" #换成你的IMU的话题
image_topic: "/imsee/image/left" #换成你的相机的话题
output_path: "/home/xxx_ws/src/VINS-Mono/output" #换成你的路径
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 640 #换成你的相机参数（step2中获取的参数）
image_height: 400 #换成你的相机参数
distortion_parameters: #换成你的畸变参数
k1: 0.162523
k2: -0.138890
p1: 0.001188
p2: 0.003208
projection_parameters: #换成你的相机内参
fx: 245.054
fy: 245.458
cx: 326.485
cy: 219.499
#estimate_extrinsic 选2 则由vnis在线标定相机及imu外参。如果选0/1，则要使用lalibr等工具对相机及imu的外参进行标定，并对应更改Rotation和Translation。
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 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.00646620000000000, -0.99994994000000004, -0.00763565000000000, 0.99997908999999996, -0.00646566000000000, -0.00009558000000000, 0.00004620000000000, -0.00763611000000000, 0.99997084000000003]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [0.00533646000000000, -0.04302922000000000, 0.02303124000000000]
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
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: 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的内参标定结果更改以下对应位置。
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # 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/xxx_ws/src/VINS-Mono/output/" # #换成你的路径
#unsynchronization parameters
estimate_td: 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 # 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
在VINS-Mono/vins_estimator/launch下创建inde.launch文件，内容如下：
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
- 启动vnis
roslaunch vins_estimator inde.launch
roslaunch vins_estimator vins_rviz.launch
注意：
VINS-slam需要在相机运动中初始化，充分激活imu，否则会产生定位的漂移。
即在执行inde.launch时，要移动相机。
INDEMIND 运行VINS-mono参考：
INDEMIND模组运行VINS算法示例
INDEMIND
INDEMIND运行VINS-Fusion
- 启动imsee-sdk
roslaunch imsee_ros_wrapper start.launch
- 启动vins-fusion
roslaunch vins vins_rviz.launch
rosrun vins vins_node xxx/config/xxxstereo_imu_config.yaml
INDEMIND双目惯性模组运行VINS-Fusion