相机型号：INDEMIND 双目惯导相机模组
官方SDK：https://imsee-sdk-docs.readthedocs.io/zh/latest/src/sdk/contents.html

1.先运行一下Demo，测试一下相机和环境

将源码放到主目录下，并初始化：

sudo apt-get install git
git clone https://github.com/indemind/IMSEE-SDK.git
cd IMSEE-SDK/
make init

我是在ubuntu18.04环境下测试的，提前安装好了ROS，自带OpenCV3.2.0

但是查看说明手册，他这里第一句话应该是说安装的ubuntu16.04的ROS，可以不需要安装OpenCV了，所以ubuntu18.04还是需要修改OpenCV的版本：

1.安装OpenCV3.4.3

不需要卸载原先ROS自带的3.2.0，我们可以安装在一个指定的目录中，ROS自带的OpenCV3.2.0将库文件安装在了usr/include下，我们可以跟自己安装库一样放在local下。
先从网上下载OpenCV3.4.3：https://opencv.org/releases/page/5/
保存在之前建立好的3rdparty文件夹下:

mkdir build && cd build
# cmake编译的时候加上参数，指定编译位置在/usr/local/opencv/opencv343
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local/opencv/opencv343 ..
make -j12
sudo make install  # 在这一步后，usr/local下出现了opencv

将OpenCV的库添加到路径，从而可以让系统找到

sudo gedit /etc/ld.so.conf.d/opencv.conf

里面是没有东西的，填入下面两句,保存后退出，这两步命令是连接到库文件的:

/usr/local/opencv/opencv343
/usr/local/opencv/opencv343/lib

再执行一下配置命令，一定要执行修改配置，否则你只是写进去了，系统还是不能检测到：

sudo ldconfig

配置bash，这一步其实就是版本号修改了，库文件在之前已经链接了

sudo gedit /etc/bash.bashrc

在文本最末尾添加：

PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/opencv/opencv343/lib/pkgconfig
export PKG_CONFIG_PATH

执行如下命令使得配置生效

source /etc/bash.bashrc
sudo updatedb

2.安装MNN：

安装MNN前需要protobuf的依赖，但是之前忘记哪一步我已经安装好了，刚好符合3.0以上的版本

没装过的可以参考他给的源码安装方式。
MNN就正常cmake编译安装即可，没啥问题。

3.运行demo

进入源码文件找到/demo，用cmake编译一下，生成了一个output文件，执行里面的程序：

找不到3.3的包，因为我们安装的是3.4.3，不是默认的OpenCV3.3.1，SDK里有说明，要替换一下就行了。找到lib下面的others，将lib/others/x64-opencv3.4.3/libindemind.so复制，粘贴到x86-64下（唉他是封装好的，所以不需要修改CMakeLists）。

再编译一下，通过了

最后运行可执行程序的时候，一定要加sudo，否则就是核心已转储…

2.标定设备

上一步运行可执行程序的时候，执行程序是可以直接得到内参的，但是用了一下感觉效果一般。

./get_device_info

而且查看imu话题的时候也发现加速度的值也不稳定，大约在9.6了，所以重新标定一下。下面的命令可以看到imu的运行情况，右手坐标系，三个方向的平移和旋转。

rostopic echo /imsee/imu

1.安装Kalibr

参考官网wiki：https://github.com/ethz-asl/kalibr/wiki/installation
官网还在更新，支持很多Ubuntu版本，我用的是Ubuntu18.04，之前也已经装好ros和其他的一些库，下面更新源以后添加一些依赖：

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 16.04
sudo apt-get install -y python2.7-dev python-pip python-scipy \python-matplotlib ipython python-wxgtk3.0 python-tk python-igraph python-pyx
# Ubuntu 18.04
sudo apt-get install -y python3-dev python-pip python-scipy \python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph python-pyx
# 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

跟vins一样，创建一个初始化工作空间，我的ros是melodic，注意ros的版本：

mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
# kinetic=16.04, melodic=18.04, noetic=20.04catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Releas

建立好工作空间以后，进入/src，不要用git去官网抓，要手动去Kalibr的github上下载zip：https://github.com/ethz-asl/kalibr
确保下载的是master主分支的源码
解压缩后，返回到上一级，进行编译，Kalibr文件比较大，可以多给线程进行工作，否则要好久好久，中间还可能有依赖的问题，否则很麻烦。

cd ~/kalibr_workspace/
catkin build -DCMAKE_BUILD_TYPE=Release -j16

编译完成后应该显示这个样子：

检查一下是否可以运行：

source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr + TAB键

如果source以后按tab键补全没有反应，可以返回再编译一次，可能漏掉了一些文件，可能是内存问题造成一些包没安装好，也是正常的。

2.安装imu_utils

imu内参标定
这篇文章解释的不错，imu_utils更新进行了优化，输出连续时间单位：
https://github.com/mintar/code_utils
https://github.com/mintar/imu_utils
安装顺序不要错，ceres库我用的还是1.14.0。

3.双目相机标定

这款相机比较拉，如果将相机的发布频率设置为4hz（官方推荐），经常会出现相机不同步的报错：

Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.

官方issue里有相关的解释，可以增加宽容度来解决，即在rosrun kalibr kalibr_calibrate_cameras 后面加：

--approx-sync 0.04

但是仍然不起作用，我怀疑是相机出厂调试没做到位，两个左右目相机时间戳没有严格同步，所以我提高了topic的发布频率，改为20hz，其实也有博客里面用20hz来标定相机的，但是过程一定要缓慢，否则出现画面不清晰得到的标定效果不佳。

# 进入管理员模式，启动相机，查看相机启动发布的话题
cd IMSEE-SDK/
sudo su
source ros/devel/setup.bash
roslaunch imsee_ros_wrapper display.launch
rostopic list# 可以通过echo来查看输出、频率
rostopic echo /imsee/..
rostopic hz /imsee/..
#或者用rqt来查看
rosrun rqt_topic rqt_topic

indemind双目模组原生频率：左右目50hz，imu1000hz。跟SDK中说的一样。

修改相机发布频率之前，这里面的话题还包括了一个修正后的左右目，接下来用这个rectified话题，畸变参数能小一些：

rosrun topic_tools throttle messages /imsee/image/rectified/left 20.0 /imsee/left_rec_20
rosrun topic_tools throttle messages /imsee/image/rectified/right 20.0 /imsee/right_rec_20

修改完成后，还是可以查看话题，发现多了两个我们修改以后的。找一个合适的位置打开终端，开始记录bag，之前我启动相机的时候已经打开了rviz的窗口，可以看到相机的实时画面，没有这个功能的打开rviz添加话题即可。

rosbag record -O stereo.bag /imsee/left_rec_20 /imsee/right_rec_20

我用的是Aprilgrid，直接用显示器做标定板了，调整一下亮度和对比度，因为是显示器的缘故，室内光线不要太亮，距离差不多半米，录制大概90s，上下左右以及斜着4个方向都尽可能估计到，动作可以大一点，但是速度一定要慢。
结束后Ctrl+C中断进程，发现本地文件里多了一个bag文件，可以rosbag info查看包里的内容和话题，发现话题数量差不多以后用Kalibr标定：

source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --bag stereocam.bag --target april.yaml --models pinhole-radtan pinhole-radtan --topics /imsee/left_rec_20 /imsee/right_rec_20

标定完成以后是这样的：

查看pdf中的标定效果：

这个效果还是可以的，可以继续进行标定了。

4.imu标定

这个设备我没调好，imu太难调（稍微大一点的晃动直接飘飞，有大佬可以教我一下嘛），无论是用之前的公制参数，还是用vins-mono配置文件下的参考，甚至是自己标定，启动后总会有漂移，这个问题以后我会看的。
静置了3h，还是根据之前先配置好launch文件里的标定时间，然后播包。

这一步的标定参数不要直接用于后续联合标定，建议按照10倍规律放缩修改一下。

5.stereo+imu联合标定

联合标定目的是求得相机的外参矩阵，这一步其实跟imu的噪声参数文件关系不大，重点还是在如何录制bag。
Kalibr标定搬运视频：bilibili
采集的时候做三轴的平移和旋转，最后再几次随机的运动，确保全方位激活imu。
我采用的频率：双目20hz，imu200hz。
还是先启动相机，同上，然后修改话题：

rosrun topic_tools throttle messages /imsee/imu 200.0 /imu
rosrun topic_tools throttle messages /imsee/image/rectified/left 20.0 /imsee/left_rec_20
rosrun topic_tools throttle messages /imsee/image/rectified/right 20.0 /imsee/right_rec_20
rosbag record -O stereo_imu.bag /imsee/left_rec_20 /imsee/right_rec_20 /imu

得到文件后，进行标定：

source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_camera --bag stereo_imu.bag --target april.yaml --imu imu.yaml --cam stereocam-camchain.yaml

查看pdf中标定情况：

3.运行vins-fusion

1.配置vins

跟配置vins-mono一样，创建工作空间，然后git、编译即可，依赖的库和头文件都是不变的。重新开一个空间：

mkdir -p ~/catkfu_ws/src
cd ~/catkfu_ws/src
catkin_init_workspace
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkfu_ws/devel/setup.bash

测试一下：

source ~/catkfu_ws/devel/setup.bash
roslaunch vins vins_rviz.launchsource ~/catkfu_ws/devel/setup.bash
rosrun vins vins_node ~/catkfu_ws/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml
#可选择运行，执行回环检测
(optional) rosrun loop_fusion loop_fusion_node ~/catkfu_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml source ~/catkfu_ws/devel/setup.bash
rosbag play MH_01_easy.bag

Green path is VIO odometry; red path is odometry under visual loop closure.
绿色轨迹是视觉惯性里程计，红色轨迹是回环检测轨迹。

2.配置config.yaml

前面的标定准备工作就是为了配置启动文件，观察一下yaml文件内容：
vins-fusion也用了因特尔realsense和小觅的相机进行了测试，而且都是测试的stereo+imu的场景，可以参考一下：

下面是我们配置自己的文件：
先写左右目各自的yaml，包含了相机模型（针孔相机），分辨率（640*400），畸变参数（k1,k2,p1,p2）和相机内参（fx,fy,cx,cy），这里的畸变参数也可以写0，因为我之前就是用的已经矫正的左右目来标定的，也可以发现其实已经比较接近0了：

一个总的config.yaml，对照的之前标定得到的stereo_imu-results-imucam.txt文件进行修改（和pdf同文件夹下的）。

stereo_imu-results-imucam.txt如下：

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.0744784765817, median 0.0690505809014, std: 0.0405895163235
Reprojection error (cam1):     mean 0.0855176672694, median 0.0804464956505, std: 0.044618793169
Gyroscope error (imu0):        mean 0.103602722682, median 0.0856946737669, std: 0.0747746435721
Accelerometer error (imu0):    mean 0.750727590727, median 0.350036118126, std: 0.888386751048Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.0744784765817, median 0.0690505809014, std: 0.0405895163235
Reprojection error (cam1) [px]:     mean 0.0855176672694, median 0.0804464956505, std: 0.044618793169
Gyroscope error (imu0) [rad/s]:     mean 0.0175819650618, median 0.0145428683837, std: 0.0126896778073
Accelerometer error (imu0) [m/s^2]: mean 1.59253371068, median 0.742538738362, std: 1.88455288795Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0):
[[ 0.98832123  0.13519381  0.07031204  0.04295684][-0.13400527  0.99075004 -0.02137638 -0.04955579][-0.07255161  0.01170455  0.99729598 -0.00967837][ 0.          0.          0.          1.        ]]T_ic:  (cam0 to imu0):
[[ 0.98832123 -0.13400527 -0.07255161 -0.04979807][ 0.13519381  0.99075004  0.01170455  0.04340318][ 0.07031204 -0.02137638  0.99729598  0.00557249][ 0.          0.          0.          1.        ]]timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.0235370574968Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1):
[[ 0.98881123  0.13207947  0.0693351  -0.07454531][-0.13146306  0.99123051 -0.01339939 -0.04211118][-0.07049685  0.00413446  0.99750343 -0.00864215][ 0.          0.          0.          1.        ]]T_ic:  (cam1 to imu0):
[[ 0.98881123 -0.13146306 -0.07049685  0.06756593][ 0.13207947  0.99123051  0.00413446  0.05162353][ 0.0693351  -0.01339939  0.99750343  0.01322492][ 0.          0.          0.          1.        ]]timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
0.0234795013072Baselines:
----------
Baseline (cam0 to cam1):
[[ 0.99999455 -0.00313031 -0.0010463  -0.11766717][ 0.00313836  0.99996484  0.0077766   0.00738332][ 0.00102192 -0.00777985  0.99996921  0.00060649][ 0.          0.          0.          1.        ]]
baseline norm:  0.117900142896 [m]Gravity vector in target coords: [m/s^2]
[-0.93686021  9.74538383 -0.5641009 ]Calibration configuration
=========================cam0
-----Camera model: pinholeFocal length: [261.10119622448775, 260.81049274045773]Principal point: [326.7955105592058, 197.6543353472613]Distortion model: radtanDistortion coefficients: [-0.0033024889310378073, 0.002406824733780496, 0.0012531219690516474, -5.251619311839587e-05]Type: aprilgridTags: Rows: 6Cols: 6Size: 0.035 [m]Spacing 0.0105 [m]cam1
-----Camera model: pinholeFocal length: [261.2777029401538, 260.85463998331545]Principal point: [326.08881642565717, 194.04870992466988]Distortion model: radtanDistortion coefficients: [-0.002640544250133058, 0.0021287882287486376, -0.0010310444977408623, 0.0005413865401160599]Type: aprilgridTags: Rows: 6Cols: 6Size: 0.035 [m]Spacing 0.0105 [m]IMU configuration
=================IMU0:----------------------------Model: calibratedUpdate rate: 200.0Accelerometer:Noise density: 0.15 Noise density (discrete): 2.12132034356 Random walk: 0.00055Gyroscope:Noise density: 0.012Noise density (discrete): 0.169705627485 Random walk: 0.00027T_ib (imu0 to imu0)[[ 1.  0.  0.  0.][ 0.  1.  0.  0.][ 0.  0.  1.  0.][ 0.  0.  0.  1.]]time offset with respect to IMU0: 0.0 [s]

indemind.yaml文件如下：

body_T_cam0：外参矩阵，也就是imu-to-cam0的变换矩阵，应该是T_ci
body_T_cam1：外参矩阵，也就是imu-to-cam1的变换矩阵
kalibr中关于变换矩阵的解释

%YAML:1.0#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2  imu_topic: "/imu"
image0_topic: "/imsee/left_rec_20"
image1_topic: "/imsee/right_rec_20"
output_path: "/home/zh/output/"cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 400# 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.body_T_cam0: !!opencv-matrixrows: 4cols: 4dt: ddata: [ 0.98832123,  0.13519381,  0.07031204,  0.04295684,-0.13400527,  0.99075004, -0.02137638, -0.04955579,-0.07255161,  0.01170455,  0.99729598, -0.00967837,0.,          0.,          0.,          1.        ]body_T_cam1: !!opencv-matrixrows: 4cols: 4dt: ddata: [ 0.98881123,  0.13207947, 0.0693351,  -0.07454531,-0.13146306,  0.99123051, -0.01339939, -0.04211118,-0.07049685,  0.00413446,  0.99750343, -0.00864215,0.,          0.,         0.,          1.        ]#Multiple thread support
multiple_thread: 1#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
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy#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: 0.15           # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.012         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.00055         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 0.00027      # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.74538383         # gravity magnitude#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: 0.0235                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/zh/output/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0

3.运行vins

启动相机，查看话题topic
打开三个中端来修改发布的话题：

rosrun topic_tools throttle messages /imsee/imu 200.0 /imu
rosrun topic_tools throttle messages /imsee/image/rectified/left 20.0 /imsee/left_rec_20
rosrun topic_tools throttle messages /imsee/image/rectified/right 20.0 /imsee/right_rec_20

打开新终端来运行rviz：

source ~/catkfu_ws/devel/setup.bash
roslaunch vins vins_rviz.launch

打开新终端运行launch文件：

source ~/catkfu_ws/devel/setup.bash
rosrun vins vins_node /home/zh/catkfu_ws/src/VINS-Fusion/config/indemind/indemind.yaml

【INDEMIND相机运行SLAM】相关推荐

使用自己的INDEMIND相机来运行ORBSLAM2单目,双目和深度模式(小觅相机和realsense通用)
流程一览配置ROS ROS环境准备(以16.04 ROS Kinetic为例) 创建自己的工作空间: 配置ORBSLAM 编译ORBSLAM2 ROS 常见错误及解决运行build-ros.sh时 ...
RGB-D相机视觉SLAM
RGB-D相机视觉SLAM Dense Visual SLAM for RGB-D Cameras 开源代码地址: vision.in.tum.de/data/software/dvo 摘要本文提出 ...
小觅相机运行VINS-Fusion（一）
写在前面的话: 1.本文基于自己的另一篇博文win10+ubuntu16.04+ROS Kinetic 2.参考[4]是一篇非常好的帖子,但由于小觅托管在github的代码在不断更新,故需在某些步骤及 ...
ROS入门：ROS下使用电脑相机运行ORB_Slam2
介绍: 最近在学习slam,想将其应用在ros平台上,故跑了orb-slam2的代码.这里粗略总结一下"ROS下使用电脑相机运行ORB_Slam2"的过程.本人菜鸟一枚,如有问题欢 ...
zed相机拆机_使用TX2+ZED相机运行ORBSLAM2和LearnVIORB
本周任务 1.完成TX2刷机 2.使用ZED运行ORBSLAM2和LearnVIORB 任务一: TX2的刷机部分完成,刷机较为顺利,遇到的问题较少. error: Finished Flashing ...
使用自制相机运行 VINS-Mono
使用自制相机运行VINS-Mono 1 相机与IMU标定 2 自制相机测试 3 运行效果参考资料 1 相机与IMU标定 VINSmono的安装这里就省略了,可以参考作者的github网页[2] 我所 ...
Gazebo-Ros搭建小车和场景并运行slam算法进行建图0--整体描述和资源
Gazebo-Ros搭建小车和场景并运行slam算法进行建图0–整体描述和资源 **背景:**需要在具体场景下运行slam算法,由于网上资源都是比较复杂的综合型机器人,作为新手不会删减改动,便想从头开 ...
基于鱼眼相机的SLAM方法介绍
本文是一篇关于鱼眼相机的SLAM的介绍以及开源demo体验的介绍,希望有兴趣的小伙伴能够自行体验,并积极分享相关内容.欢迎交流和讨论,联系邮箱:dianyunpcl@163.com {MultiCol ...
使用D435i深度相机运行ORB-SLAM3
下载安装链接下载ORB-SLAM3地址: git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git eigen3多版本安装:https://blog ...

【INDEMIND相机运行SLAM】