D435i运行VINS-mono以及Kalib标定

D435i运行VINS-mono以及Kalib标定

系统说明：Ubuntu 18.04
内核版本：5.4.0

1、运行VINS-mono

参考博客VINS（D435i）测试
问题：
**（1）**下载源码部分：
步骤a：

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

原作者推荐2.19原因解释在于SDK2.19中包含了步骤3中Ros Wrapper中的ddynamic_reconfigure

但是目前直接git 下来版本为V2.36,需要注意这个SDK版本对应的realsense版本是需要安装ddynamic_reconfigure的，否则编译时候会报错缺少这个东西。
若不对应，会出现https://blog.csdn.net/weixin_38504276/article/details/103010109点击进入中的错误
步骤c

echo 'deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main' | sudo tee /etc/apt/sources.list.d/realsense-public.list

这一步需要注意xenial其实是UBuntu16.04对应的，但是18.04也能用，更建议换成melodic，参考官网linux下安装部分。
***（2）***pip问题
执行igraph时候（按照官方的github），会遇到pip的问题，百度换源
（给pip换源）安装。
（3）安装Kalib的时候，会出现suitparse问题，修改Cmakelists.txt中相应行如下：

DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget -O SuiteSparse-${VERSION}.tar.gz "https://github.com/jluttine/suitesparse/archive/v${VERSION}.tar.gz"

其他部分依次执行。

2、标定

补充一个重要问题：相机模型要选择pinhole-radtan，外参标出来才对，对于D435i是这样，不能选pinhole-equi。
较全的参考博客推荐：博主古路标定
问题记录：
**（1）**IMU标定

rs_imu_calibration.launch

该代码直接把rs_camera.launch复制过来就行，前提是在运行VINS中已经修改过参数。
**（2）**运行IMU校准参数时候，复制过来就可以，修改时间参数就行，我在此处将data_save_path修改为自己的参数，但是读取不了。
**（3）**相机标定，需要下载april grid，此处任意放置一个目录就行，后期运行标定命令修改路径。
**（4）**关闭IR发射器：关闭IR，每次重新插拔相机后要再关一次
**（5）**摄像头标定，多角度多位置移动，：参考视频
此处数据集录制不好，在执行标定命令时，会出现以下错误和警告，这三个均为不影响执行的

[ WARN] [1596511223.581216]: BagImageDatasetReader: truncated 1485 / 2834 images.

(Aprilgrid: Tag detection:24719): GLib-GObject-CRITICAL **: 11:32:39.553: g_object_unref: assertion 'G_IS_OBJECT (object)' failed

[TargetViewTable]: Tried to add second view to a given cameraId & timestamp. Maybe try to reduce the approximate syncing tolerance.

我个人标定了四次，有一次数据集在上面第三条命令之后又出现了下面一条错误直接中断，下面这个错误没有花时间去解决，换其他数据集没报错，所以在录制数据集的时候可以多录制几个。其次时间可以限制短一些，本人处理器为i7 10代，时间选10-100处理时间较长，建议选择一半。
导致中止的错误：
[ERROR] [1596519258.700033]: initialization of focal length for cam with topic /infra_right failed
这个错误github上是有解决方案的。
**（6）**多相机标定命令：

kalibr_calibrate_cameras --target /home/april_6x6_80x80cm.yaml --bag /home/multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --show-extraction --approx-sync 0.04 --bag-from-to 30 50

其中target命令和bag后面的路径修改成自己的。--approx-sync 0.04开头提出的参考博客中，开始标定命令后面没有这块代码，其后就会出现原博客报错。
**（7）**多相机标定报告生成后，可以和开头博客对比下，有时候重投影误差太大可以重新标一次。
**（8）**联合标定
a.联合标定开始提到的文件是需要自己写的，路径自己定，后面在标定命令中把相应路径填进去就行。
b.该imu.yaml(原博客此处拼写错误）开头topic有误：

rostopic: /camera/imu
update_rate: 200.0 #Hz
 
accelerometer_noise_density: 2.3786845794688424e-02  #白噪声
accelerometer_random_walk: 5.9166889270489845e-04  #偏置
gyroscope_noise_density: 2.1732068912927271e-03 #continous
gyroscope_random_walk: 1.7797900203083191e-05

仅修改第一行为：

rostopic: /imu

因为在后面限制频率后，修改了话题
***c***报错缺少模块：

NO module named scipy.optimize

使用命令：

sudo apt-get install python-scipy

此处试过网上很多命令，都不是很好使。参考帖子：进入
***d***录制数据集
录制数据：建议录制color+imu。多相机加IMU的优化矩阵很大，本人电脑配置优化失败，如果有较好的台式机可以一试。因为最后也只用color的数据，所以没必要多目加IMU标定。
***e***优化报错
尽管录制单目，但是也有可能优化失败：如下

Exception in thread block: Exception in thread block: [aslam::Exception] /home/louis/catkin_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [15106.3 <= 15106.3 < 15106.4]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[aslam::Exception] /home/louis/catkin_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [15118.2 <= 15118.2 < 15118.2]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/louis/catkin_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [15094.8 <= 15094.8 < 15094.9]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1454555378.858709]: Optimization failed!

该错误解决方法见github：
github
与此相关的还有问题55，可在上面github的问题中搜索。
修改的文件

/kalib_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py

与问题171中提到的不太一样，但是timeOffsetPadding的确在此处修改，将值改为0.1（原来为0.02）。
然鹅，再一次优化失败，我又修改了数据集的时间长度，将其修改为后20s，才优化成功。

此处记录标定成功中遇到的问题，但是跑VINS效果不好，后续继续。
3、后续结果
经过一段时间标定，外参其实标定出来到一个大致的范围后，这个时候初始化就和一些因素有关。根据自己经验，首先初始化不能大幅度晃动，其次保证足够多的特征点（这一部分验证是通过上面标定的参考博客中有一条可以生成标定版pdf文件的命令，我直接生成150个特征点的，初始化对着它，立马进入后端），所以说环境有点影响。其次在我做完上述调教后，基本可以稳定运行，但是偶尔还是不太稳定，后来怀疑是室内环境问题（所处室内环境为一个玻璃围起来的会议室，白天没有太阳光照进来，都是灯光），因为后面拿去室外，效果立马就好了。而且仔细参考沈老师课题组的环境，可以发现不论室内还是室外，都有相对比较多的特征点，当然以上都是个人经验，仅供参考。下面为效果图：下面放一下配置文件外参部分，以及imu部分：

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0.115442075186306
   k2: -0.0715322860987043
   p1: 0.000362396472449387
   p2: 0.0105544503137209
projection_parameters:
   fx: 612.938848329915
   fy: 611.536386289859
   cx: 345.892041589929
   cy: 234.910747444319

# 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.99998638, -0.00511292,-0.00105044,
           0.00512328, 0.99993577, 0.01010986,
           0.00099868, -0.01011511,0.99994834 ]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [-0.03648963, -0.01213669, 0.02434708]
#   data: [0.00030446,-0.00008146, 0.00051927]

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 25            # 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: 0             # 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: 0.0223798934954       # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.00244184681729       # gyroscope measurement noise standard deviation.     #0.05
acc_w: 5.7300219137827041e-04        # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.77375580866e-05    # gyroscope bias random work noise standard deviation.     #4.0e-5
#acc_n: 0.0450928056481          # accelerometer measurement noise standard deviation. #0.2
#gyr_n: 0.00270640454684         # gyroscope measurement noise standard deviation.     #0.05
#acc_w: 0.00127994636405         # accelerometer bias random work noise standard deviation.  #0.02
#gyr_w: 3.7195573812e-05       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805       # gravity magnitude

根据理论判断，以及参考博主古路的结果，外参平移参数的y这一项应该可以再小一个量级，但是我们用的打印的在A4纸上的标定板，前后标定很多次，效果没有更好。有条件建议买一块标定板。

ps：最经又跑了一下FUSION，所有都用的原始参数，有一段时间相机启动会丢IMU，所以轨迹直接飘，此处参考高飞教授github提到的，多插拔几次（记得关结构光），就有效果了，而且感觉FUSION相对稳定，没有1初始化就会好很多，但是硬件链接感觉不稳，不知道相机的问题还是电脑的问题（当然确保了是usb3.0)，有看到的大佬可以给点意见。上述效果是未标定跑出来的结果，除过上面提到的硬件问题，也没做标定。准备标定下试试，有一部分原因在于多目联合IMU标定，太吃资源，之前做过一次，优化失败。目前也怀疑和数据集的录制有关系，后面标定下效果好再更新。
运行VINS-FUSION命令
在librealsense下面运行

roslaunch realsense2_camera rs_camera.launch

在编译过的VINS-FUSION下面运行，上一条命令就是启动相机，下面三条是运行FUSION相关。

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml