目前MOCO-8平台已经逐步稳定，在典型室内环境下已经具备可靠的行走能力，现在是是时候达成最初设计MOCO8替代室内SLAM小车的目标了，参考了很多网络教程了解到D435i在港科大的项目中是实现了很多基于视觉下的SLAM地图和避障，如Fastplanner等，下面给出了一个对D435i从标定到运行VINS相关代码的过程，期间参考了很多前辈的教程，总结如下的整个我走通流程，特此记录。

https://video.zhihu.com/video/1406398036217708544?player=%7B%22autoplay%22%3Afalse%2C%22shouldShowPageFullScreenButton%22%3Atrue%7D

首先确保在Ubuntu16.04以上的机子中已经安装好ROS，我采用的是VMware虚拟机进行测试，期间出现了很多别人教程没遇到的问题，这可能是虚拟机的问题，刚买来的435其相机内参是标定好的（据说其精度还可以），要实现视觉惯性融合则需要进一步标定，首先需要标定内部IMU参数，该部分又分为两个步骤（1）标定IMU尺度和零偏，这个采用类似无人机加速度计的6面校准方法；（2）构建内部传感器的噪声模型，这部分应该是用于后续视觉算法中滤波器部分参数的计算。最终，对相机和IMU的外参进行标定获取二者间的变换矩阵，本文仅对单目和IMU外参进行了标定尝试。

1 Realsense驱动与测试

首先，还是重新标定一下相机的内参，这里参考了教程：

realsense D435i 模块在Ubintu16.04虚拟机与Windows下的安装方法​blog.csdn.net/qq_43536313/article/details/109691518

首先在Windows环境下，测试设备是否可以正常工作，只需下载 Intel官方给出的应用程序 Intel.RealSense.Viewer.exe 即可，地址为 ：

https://www.intelrealsense.com/zh-hans/sdk-2/

设置虚拟机USB口为 3.0，如下图，打开虚拟机，插入模块，检查模块是否连接。

在虚拟机中安装realsense的相关驱动：

git clone https://github.com/IntelRealSense/librealsense
cd librealsense

安装依赖：

git clone https://github.com/IntelRealSense/librealsense
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

进入librealsense/build/examples/capture，输入 ./rs-capture 打开realsense-viewer工具查看效果，或者直接在控制台输入realsense-viewer。

安装realsense在ROS下的驱动包，将下列两个文件下载到catkin工作控制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

2 单目相机内参标定

在ROS驱动编译完成后，首先可以查看其出厂的内参，source工作空间后运行：

roslaunch realsense2_camera rs_camera.launch
rostopic echo /camera/color/camera_info

安装Kalbr库：

安装依赖:

sudo apt-get install python-setuptools
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen
sudo apt-get install ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

建立工作空间:

mkdir -p ~/kalibr_ws/src
cd ~/kalibr_ws/src
catkin_init_workspace
cd ..
catkin_make

其中会遇到如下几个问题：

（1）无法本地校验颁发者的权限，解决方法：

修改CMakeLists.txt来：

sudo nano ~/kalibr_workspace/src/kalibr/suitesparse/CMakeLists.txt

修改对应语句如下：

DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget --no-check-certificate http://faculty.cse.tamu.edu/davis/SuiteSparse/SuiteSparse-${VERSION}.tar.gz

（2）eigen错误，解决方法：

首先采用root控制台权限打开窗口：

sudo nautilus

修改/usr/local/include/eigen3/Eigen/src/Core/MatrixBase.h第466行的protected 为public!

安装好后要标定内参首先需要建立标定板，可以建立黑白棋盘格和Apriltag两个版本：

kalibr_create_target_pdf --type checkerboard --nx 6 --ny 7 --csx 0.030 --csy 0.030
kalibr_create_target_pdf --type 'apriltag' --nx 6--ny 7 --tsize 0.03 --tspace 0.2

之后要在在data文件下建立对应标定板的yaml配置文件：

Sudo nano checkerboard.yaml

棋盘格：

target_type: 'checkerboard' #gridtype
targetCols: 6 #number of internal chessboard corners
targetRows: 7 #number of internal chessboard corners
rowSpacingMeters: 0.029 #size of one chessboard square [m]
colSpacingMeters: 0.029 #size of one chessboard square [m]

二维码：

target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 7 #number of apriltags
tagSize: 0.025 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize

标定相机内参可以采用两种方式，一是直接使用ROS自带的在线标定工具这个只能使用棋盘格，另一个是使用kalibr的标定工具，对于前者直接输入：

roslaunch realsense2_camera rs_camera.launch
rosrun camera_calibration cameracalibrator.py --size 6x7 --square 0.029

标定结果当中，Kalibr输出的内参格式为：fx，fy，cx，cy，畸变参数为：k1，k2，p1，p2（径向畸变参数k1、k2，切向畸变参数p1，p2）而ROS标定工具直接输出了内参矩阵K=[fx, 0, cx; 0, fy, cy; 0, 0, 1]，畸变参数为[k1, k2, p1, p2, k3]

对于后者在虚拟机中运行有几个问题，首先需要录制bag，打开相机：

roslaunch realsense2_camera rs_camera.launch

降低图像帧率：

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color

查看图像，也可以用rviz：

rosrun image_view image_view image:=/color

在data目录下录制bag：

rosbag record -O camd435i /color

通过不同视角拍摄5分钟后关闭录制，运行：

kalibr_calibrate_cameras --target /home/pi/catkin_ws_kal/src/kalibr-master/data/checkerboard.yaml --bag /home/pi/catkin_ws_kal/src/kalibr-master/data/camd435i.bag --models pinhole-radtan --show-extractio

运行后会显示标定过程，但是不知道什么问题我显示报错：

(Checkerboard corners:6032): GLib-GObject-CRITICAL **: 00:51:55.015: g_object_unref: assertion 'G_IS_OBJECT (object)' failed
Attempt to unlock mutex that was not locked
Aborted (core dumped)

如果不想识别过程可以将--show-extractio去除或者首先更新cmake：

ubuntu16.04更新cmake从3.5.x到3.16.0版本_Boys_Wu的博客-CSDN博客​blog.csdn.net/Boys_Wu/article/details/103475969

安装python-igraph：

sudo apt-get install python-igraph

或者:

pip install python_igraph

标定后会自动生成对应的yaml，将对应参数复制建立相机内参camchain.yaml:

cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [0.117693, -0.187879, 0.003735,
0]
distortion_model: radtan
intrinsics: [601.850243, 604.158612, 324.226026,243.532988]
resolution: [640, 480]
rostopic: /color

3 IMU内参标定

对于IMU标定分为两个步骤，首先标定加速度偏差和零偏，这里采用了realsense官方的库，安装依赖：

sudo pip install pyrealsense2

或：

sudo pip3 install pyrealsense2

在realsense驱动目录下librealsense/tools/rs-imu-calibration运行：

python rs-imu-calibration.py

则按步骤完成6面放置，在每个面有三个正确标志，当都为true后会读秒保持一段时间之后完成标定，标定朝向为：

6个姿态都采集完之后，标定结束，这个时候会提示：

Would you like to save the raw data?

是否将原始的数据保存？输入yes

Would you like to write the results to the camera?

是否将标定的结果写入IMU？输入Y，表示将标定的结果是写入到IMU，同时控制台会打印相应标定结果并在文件目录中生成yaml或txt（如果不想一个一个复制，需要自己搜索下，但对于运行算法基本不需要，因为已经写入内部）：

[-0.0042522  -0.00240991  0.00085414]
[1000 1000 1000 1000 1000 1000]
using 6000 measurements.
[[ 1.01886308  0.02635904  0.00542127]
 [-0.00358797  1.0201172  -0.03025821]
 [-0.01366641 -0.00326073  1.01143415]
 [ 0.19705213  0.18373709  0.23903654]]
residuals: [   2.68789634  296.74033999  159.76424254]
rank: 4
singular: [ 439.88120077  433.03470676  421.5980737    77.29827496]
norm (raw data  ): 9.644980
norm (fixed data): 9.801982 A good calibration will be near 9.806650

标定完加速度偏差后，需要标定其噪声模型，这里参考：

https://blog.csdn.net/qq_35616298/article/details/116190164

首先安装cere：

GitHub - ceres-solver/ceres-solver: A large scale non-linear optimization library​github.com/ceres-solver/ceres-solver

建立新的工作空间，下载code_utils：

git clone https://github.com/gaowenliang/code_utils.git

编译：

cd ~/imu_catkin_ws/
catkin_make

这里会遇到如下问题：

fatal error: backward.hpp: 没有那个文件或目录 #include “backward.hpp”

解决办法：修改～/imuCali_ws/src/code_utils/src/sumpixel_test.hpp文件中的头文件为：

#include "code_utils/backward.hpp"

编译完成后才能继续下载imu标定工具：

git clone https://github.com/gaowenliang/imu_utils.git
cd ~/imu_catkin_ws/src/
catkin_make

建立采集launch（其中30对应标定数据时间，单位为min）：

<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/camera/imu"/>
<param name="imu_name" type="string" value= "d435i_imu_calibration"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "30"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>

修改 rs_camera.launch：

<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="enable_sync" default="false"/>
<arg name="unite_imu_method" default="linear_interpolation"/>

重新运行后查看是不是imu话题中同时包括加速度计和陀螺仪数据：

rostopic echo /camera/imu

之后运行采集launch并录制bag：

rosbag record -O imu_calibration /camera/imu

录制完成后，播放数据开始标定：

rosbag play -r 200 imu_calibration.bag
source ~/imu_catkin_ws/devel/setup.sh
roslaunch imu_utils d435i_imu_calibration.launch

另外也可以直接运行不录制bag，然后直接运行标定采集的launch！

标定完成后会生成对应的d435i_imu_calibration_imu_param.yam：

type: IMU
name: d435i_imu_calibration
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.5826034824378552e-03
      gyr_w: 1.9061577574077550e-05
   x-axis:
      gyr_n: 1.3723950098817163e-03
      gyr_w: 1.6550312538870268e-05
   y-axis:
      gyr_n: 2.1898446843020341e-03
      gyr_w: 2.2163547291741309e-05
   z-axis:
      gyr_n: 1.1855707531298150e-03
      gyr_w: 1.8470872891621073e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.0106184816966095e-02
      acc_w: 4.6727808301496226e-04
   x-axis:
      acc_n: 1.9900521623631238e-02
      acc_w: 4.1213011628077919e-04
   y-axis:
      acc_n: 1.9586136818377457e-02
      acc_w: 7.1843749032937860e-04
   z-axis:
      acc_n: 2.0831896008889593e-02
      acc_w: 2.7126664243472899e-04

其中就avg部分有用，在之前的data文件夹中建立imu.yaml：

#Accelerometers
accelerometer_noise_density: 2.0106184816966095e-02 #Noise density (continuous-time)
accelerometer_random_walk: 4.6727808301496226e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 1.5826034824378552e-03 #Noise density (continuous-time)
gyroscope_random_walk: 1.9061577574077550e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)

4 联合标定

最终对IMU和相机进行联合标定，这里还是要使用之前的棋盘格，运行realsense后映射话题：

rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

录制bag:

rosbag record -O dynamic /color /imu

运行标定：

kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camd435i.yaml --imu imu.yaml --bag dynamic.bag

同样--show-extraction(会报错)

最终确认重投影误差，我的误差为0.56个像素我看网上其他人能到0.15以下已经是很好的了。

5 运行VINS-Mono

下载相关代码修改realsense包里的realsense_color_config.yaml文件，将之前的内外参数写入对应部分，之后运行：

roslaunch realsense2_camera rs_camera_vins.launch
roslaunch vins_estimator realsense_color.launch
roslaunch vins_estimator vins_rviz.launch

在运动初始化后VINS就可以正常定位，但是我目前遇到的问题是会莫名其妙的飞，这个网上好多人也遇到过，目前不清楚是标定还是分辨率用640太小的问题，也希望有经验的人帮解答下！