【INDEMIND相机运行SLAM】

白白白白白kkk

已于 2023-12-07 21:22:27 修改

阅读量813

点赞数 1

文章标签：数码相机

于 2023-05-12 16:16:19 首次发布

本文链接：https://blog.csdn.net/qq_45306739/article/details/130642482

版权

本文章存在问题，可以参考新文章：https://blog.csdn.net/qq_45306739/article/details/134689638

相机型号：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

catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

建立好工作空间以后，进入/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.launch
 
source ~/catkfu_ws/devel/setup.bash
rosrun vins vins_node ~/catkfu_ws/src/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.888386751048

Residuals
----------------------------
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.88455288795

Transformation (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.0235370574968

Transformation (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.0234795013072

Baselines:
----------
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: pinhole
  Focal length: [261.10119622448775, 260.81049274045773]
  Principal point: [326.7955105592058, 197.6543353472613]
  Distortion model: radtan
  Distortion coefficients: [-0.0033024889310378073, 0.002406824733780496, 0.0012531219690516474, -5.251619311839587e-05]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.035 [m]
    Spacing 0.0105 [m]

cam1
-----
  Camera model: pinhole
  Focal length: [261.2777029401538, 260.85463998331545]
  Principal point: [326.08881642565717, 194.04870992466988]
  Distortion model: radtan
  Distortion coefficients: [-0.002640544250133058, 0.0021287882287486376, -0.0010310444977408623, 0.0005413865401160599]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.035 [m]
    Spacing 0.0105 [m]

IMU configuration
=================

IMU0:
 ----------------------------
  Model: calibrated
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.15 
    Noise density (discrete): 2.12132034356 
    Random walk: 0.00055
  Gyroscope:
    Noise density: 0.012
    Noise density (discrete): 0.169705627485 
    Random walk: 0.00027
  T_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-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 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-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 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

白白白白白kkk

关注

1
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
【INDEMIND相机运行SLAM】

相机型号：INDEMIND 双目惯导相机模组。
复制链接

扫一扫

【INDEMIND相机运行SLAM】

本文章存在问题，可以参考新文章：https://blog.csdn.net/qq_45306739/article/details/134689638

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

1.安装OpenCV3.4.3

2.安装MNN：

3.运行demo

2.标定设备

1.安装Kalibr

2.安装imu_utils

3.双目相机标定

4.imu标定

5.stereo+imu联合标定

3.运行vins-fusion

1.配置vins

2.配置config.yaml

3.运行vins

“相关推荐”对你有帮助么？