orb-slam3 用 D435i 在 kinetic 下的验证。

1. 安装 realsense

安装 realsense
sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

sudo add-apt-repository "deb  xenial main" -u

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils
sudo apt-get install librealsense2-dev
sudo apt install ros-kinetic-rgbd-launch
使用realsense-viewer命令查看是否有图像， 执行 modinfo uvcvideo | grep "version:" 能看到 realsense 字样

2. 安装相机驱动realsense-ros

安装相机驱动realsense-ros

sudo apt install ros-kinetic-realsense2-camera ros-kinetic-realsense2-description

3. 配置依赖库
点击(此处)折叠或打开

/work/ws/3th/
wget ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar xvf ceres-solver-1.14.0.tar.gz
cd ceres-solver-1.14.0
mkdir build
cd build
cmake ..
make -j4
make test
sudo make install

ROS的包

cd /work/ws/src/
git clone http://github.com/UZ-SLAMLab/ORB_SLAM3.git
git clone https://gitclone.com/github.com/gaowenliang/code_utils
#将/catkin_ws/src/code_utils/src/sumpixel_test.cpp中的 #include "backward.hpp"修改为#include"code_utils/backward.hpp"  先编译
git clone http://github.com/gaowenliang/imu_utils

roslaunch realsense2_camera demo_pointcloud.launch 测试一下， 能在rviz中看到rgb和depth的数据。基本环境OK了。

3. 标定摄像头。 
目前D435i的固件版本是 05.12.11.00（用realsense-viewer -> info 看到）。

Intel RealSense D4×× 系列相机从 Firmware version 5.12.02.100 开始加入了自标定功能，大大提高了相机标定的自动化程度
https://dev.intelrealsense.com/docs/intel-realsensetm-d400-series-calibration-tools-user-guide 或者 参考 http参s://blog.csdn.net/a2824256/article/details/106729403

校准参数
动态校准是为了优化外部参数， 针对与camera的主轴（左右轴）的外参（平移和旋转）。 （失真/视野/噪点等是固有参数， 不属于动态校准范围）

左camera作为坐标原点，进行校准。
    内部参数（通过 /camera/color/camera_info获得）包括。

•         焦距： [fx; [fy]以像素为单位，用于左，右和RGB摄像机
•          失真： 布朗的失真模型[k1; k2; p1; p2; k3]，用于左，右和RGB相机

外部参数（需要借助工具进行校准， 例如此处为使用intel官方方式进行校准）包括

• RotationLeftRight-从右摄像机坐标系到左摄像机坐标系的旋转，指定为3x3旋转矩阵
• TranslationLeftRight-从右摄像机坐标系到左摄像机坐标系的转换，指定为以毫米为单位的3x1向量
• RotationLeftRGB-从RGB相机坐标系到左相机坐标系的旋转，指定为3x3旋转矩阵
• TranslationLeftRGB-从RGB相机坐标系到左相机坐标系的转换，指定为以毫米为单位的3x1向量

在 ubuntu 16.04上。

sudo apt-get install libusb-dev libusb-1.0-0-dev
sudo apt-get install libglfw3 libglfw3-dev
sudo apt-get install freeglut3 freeglut3-dev
sudo apt-get install libpng12-dev

echo 'deb  xenial main' | sudo tee /etc/apt/sources.list.d/realsense-public.list
sudo apt-key adv --keyserver keys.gnupg.net --recv-key 6F3EFCDE

sudo apt-get update
sudo apt-get install librscalibrationtool  英特尔?实感?动态校准器的软件包

可执行文件安装在/ usr / bin下，库文件安装在/ usr / lib下，其他文件包括示例代码在/ usr / share / doc下

cd /usr/share/doc/librscalibrationtool
/usr/bin/Intel.Realsense.DynamicCalibrator -v 查看版本
/usr/bin/Intel.Realsense.DynamicCalibrator -list 能看到摄像头设备
其实 realsense-viewer 将 “Stereo Module” 中的 “Emitter Enabled” 设置为 “Laser”；
在 “More” 下拉菜单中选择 “On-Chip Calibration”；选择 “Calibrate”,会弹出一个中间是蓝色长方形的标定界面

校准流程
执行 Intel.Realsense.DynamicCalibrator， 图形界面下，一般采用默认配置。
点击 《start calibration》(在校准期间，UI在右上角显示经过的时间，在左下角显示状态反馈，并在右下角显示帧数或目标距离：)

(Iphone)在app store里面搜寻下载’Dynamic Target Tool’,按照软件的引导将手机对准摄像头标定。
将摄像机对准校准目标，然后缓慢移动摄像机，将目标定位在蓝色方块上，直到所有目标均被清除
清除所有蓝色方块后，整流阶段就完成了。中间结果将应用于当前流

秤校准阶段将在此后立即开始。将相机对准目标并缓慢移动。在进行标尺校准时，将出现绿色的进度条。将捕获并分析目标图像，并在屏幕上突出显示接受的目标位置。总共将接受15张成功的目标图像。绿色条延伸至满后，刻度校准完成

完成后会自动写入外部参数。 

校准后， 使用viewer 

将相机对准平坦的表面，例如约1至2米（3至6英尺）的墙壁。避免黑色表面。

目视检查墙壁的深度图像显示。图像上有很多黑点或黑洞，表示相机未校准。

4. 安装 orb_slam3

从 readme.md中首先安装依赖关系。

ORB-SLAM3 发布于 2020.09.04
支持  monocular, stereo and RGB-D， 支持pin-hole和 fisheye 镜头。 可配置为 visual视觉， visual-inertial 视觉导航， multi-Map Slam.

建议使用i7, 保证更稳定、更准确的结果
依赖
1. pangolin 提供UI， 基于opengl的库。 https://github.com/stevenlovegrove/Pangolin

sudo apt-get install libglew-dev
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install libpython2.7-dev cmake

cd /work/3th
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake -DCPP11_NO_BOOST=1 ..
make -j 8; sudo make install


opencv : http://opencv.org 在 OpenCV 3.2.0**测试过， 我们也用这个版本。
wget -c https://github.com/opencv/opencv/archive/3.2.0.tar.gz
手动下载
https://raw.githubusercontent.com/opencv/opencv_3rdparty/ippicv/master_20151201/ippicv/ippicv_linux_20151201.tgz 到 /work/3th/opencv-3.2.0/3rdparty/ippicv/downloads/linux-808b791a6eac9ed78d32a7666804320e/下面 (Intel? Integrated Performance Primitives (Intel? IPP) Library for OpenCV)。
mkdir build; cd build; cmake ../; sudo make install

Eigen3; 使用 g2o, http://eigen.tuxfamily.org. **Required at least 3.1.0**
在gitlab上下载 当前为3.4.99 版本。

DBoW2 and g2o https://github.com/RainerKuemmerle/g2o/archive/20170730_git.tar.gz

python 需要 numpy支持
sudo apt install libpython2.7-dev

编译ORB_SLAM3

git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
编译库 - libORB_SLAM3.so 以及范例 Examples
cd ORB_SLAM3
chmod +x build.sh
./build.sh

编译ROS的接口和范例
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/work/ws/src/ORB_SLAM3/Examples/ROS
chmod +x build_ros.sh
./build_ros.sh

调用方式
点击(此处)折叠或打开

rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ##用 `/camera/image_raw`

rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION] ## 单目带惯性导航 /imu.

rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION ##双目 `/camera/left/image_raw` and `/camera/right/image_raw`

rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION] ##双目 + 惯性导航 /imu

rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ## RGB `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`

例如 rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true
rosbag play --pause V1_02_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu

OK, 准备用 d435i进行实测
1. 准备yaml。d435i.yaml 我们先准备用 rgb-d的测试。
  Examples/RGB-D/d435i.yaml （拷贝自TUM1.yaml）。 内容 

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

header:
  seq: 56
  stamp:
    secs: 1615951749
    nsecs: 285255194
  frame_id: "camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [913.7498779296875, 0.0, 643.4906005859375, 0.0, 911.7105712890625, 361.6775207519531, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [913.7498779296875, 0.0, 643.4906005859375, 0.0, 0.0, 911.7105712890625, 361.6775207519531, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---
K就是我们得到的内参矩阵写成了向量模式。
K = [fx 0 cx 0 fy cy 0 0 1 ]
baseline，要根据官方datasheet， D435i是 55mm。 bf的值为bf = baseline (in meters) * fx。
畸变参数已经标定过了， 所以此处全部为0即可。
通过 /usr/bin/Intel.Realsense.CustomRW -r 查看左右深度和rgb的外参， 其中 Distortion（失真）部分代表 布朗失真模型 [k1; k2; p1; p2; k3]的顺序
根据这些内容， 修改yaml的值。其他默认使用即可。
%YAML:1.0
Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 913.7498779296875
Camera.fy: 911.7105712890625
Camera.cx: 643.4906005859375
Camera.cy: 361.6775207519531

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0


Camera.width: 1280
Camera.height: 720

# Camera frames per second 
Camera.fps: 30.0
# IR projector baseline times fx (aprox.) 0.055 *  913.7498779296875 = 50.2562432856
Camera.bf: 50.2562432856


# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1


# Close/Far threshold. Baseline times.
ThDepth: 40.0


# Deptmap values factor 
DepthMapFactor: 5000.0 # 1.0 for ROS_bag


#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------


# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000


# ORB Extractor: Scale factor between levels in the scale pyramid  
ORBextractor.scaleFactor: 1.2


# ORB Extractor: Number of levels in the scale pyramid 
ORBextractor.nLevels: 8


# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast 
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7


#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

附件：https://blog.csdn.net/qq_42688495/article/details/107640826

IMU标定

在确认了 code_utils 和 imu_utils.
执行 roslaunch realsense2_camera rs_camera.launch unite_imu_method:="linear_interpolation"。 
执行launch。 

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/camera/imu"/> #imu topic的名字
        <param name="imu_name" type="string" value= "d435i"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/> #标定的时长
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

将RealsenseD435i静止摆放两个小时， 完成后就会生成d435i_imu_param.yaml文件

IMU与相机联合标定 

参考 https://blog.csdn.net/qq_42688495/article/details/107640826

roslaunch realsense2_camera rs_camera.launch unite_imu_method:="linear_interpolation"
将图像频率设置为20Hz，IMU频率设置为200Hz：
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
录制rosbag
rosbag record -O dynamic /color /imu

开始联合标定

新建一个camchain.yaml文件，内容如下，将intrinsics和distortion_coeffs的数值改成之前标定的结果
点击(此处)折叠或打开

cam0:
  camera_model: pinhole
  intrinsics: [637.1779587247433, 633.7804879350356, 317.29745787695197, 222.79306023564143]
  distortion_model: equidistant
  distortion_coeffs: [0.12699077558547853, -0.2663283480947607, -0.003966617209275872, 0.002919700067058629]
  T_cam_imu:
  - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
  - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
  - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.121e-05
  rostopic: /color
  resolution: [640, 480]

再新建一个imu.yaml文件，内容如下，将下面的四个参数的值设置为imu标定的结果。

rostopic: /imu
update_rate: 200.0 #Hz

accelerometer_noise_density: 0.030017 #continous
accelerometer_random_walk: 0.000876
gyroscope_noise_density: 0.00294 #continous
gyroscope_random_walk: 3.14268e-05

进行标定

kalibr_calibrate_imu_camera --target ~/bagfiles/target.yaml --cam ~/bagfiles/camchain.yaml --imu ~/bagfiles/imu.yaml --bag ~/bagfiles/dynamic.bag --show-extraction

标定结果如下：

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.645979524406, median 0.55076768161, std: 0.441172957301
Gyroscope error (imu0): mean 0.632165457077, median 0.51915755434, std: 0.418834270707
Accelerometer error (imu0): mean 0.315519030434, median 0.26144132228, std: 0.220137274831

Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.645979524406, median 0.55076768161, std: 0.441172957301
Gyroscope error (imu0) [rad/s]: mean 0.026284098714, median 0.0215854698381, std: 0.017414240517
Accelerometer error (imu0) [m/s^2]: mean 0.133939243527, median 0.110983013877, std: 0.0934492604853

Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99914747 -0.03946207 -0.01212755  0.04307462]
 [ 0.03965755  0.99907997  0.01632454 -0.01789784]
 [ 0.0114722 -0.01679157  0.99979319 -0.01402533]
 [ 0. 0. 0. 1. ]]

T_ic: (cam0 to imu0):
[[ 0.99914747  0.03965755  0.0114722 -0.04216722]
 [-0.03946207  0.99907997 -0.01679157  0.01934568]
 [-0.01212755  0.01632454  0.99979319  0.01483699]
 [ 0. 0. 0. 1. ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0302255306475


Gravity vector in target coords: [m/s^2]
[-0.48824837 -9.78365493 -0.45840216]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [637.1779587247433, 633.7804879350356]
  Principal point: [317.29745787695197, 222.79306023564143]
  Distortion model: equidistant
  Distortion coefficients: [0.12699077558547853, -0.2663283480947607, -0.003966617209275872, 0.002919700067058629]
  Type: aprilgrid
  Tags:
    Rows: 6
    Cols: 6
    Size: 0.02 [m]
    Spacing 0.006 [m]



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

IMU0:
----------------------------
  Model: calibrated
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.030017
    Noise density (discrete): 0.424504485018
    Random walk: 0.000876
  Gyroscope:
    Noise density: 0.00294
    Noise density (discrete): 0.0415778787338
    Random walk: 3.14268e-05
  T_i_b
    [[ 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]

参考
点击(此处)折叠或打开

https://blog.csdn.net/qq_33642342/article/details/108870290
https://blog.csdn.net/weixin_44350238/article/details/107641849
基础知识
http://www.360doc.com/content/20/0831/00/32196507_933171912.shtml
标定
https://dev.intelrealsense.com/docs/intel-realsensetm-d400-series-calibration-tools-user-guide
https://blog.csdn.net/a2824256/article/details/106729403