相机标定和ORBSLAM2/VINS测试

一、相机标定

1.1 标定目的

【WHY：为什么要进行相机标定？】

相机标定的目的是：建立相机成像几何模型并矫正透镜畸变。

建立相机成像几何模型：计算机视觉的首要任务就是要通过拍摄到的图像信息获取到物体在真实三维世界里相对应的信息，于是，建立物体从三维世界映射到相机成像平面这一过程中的几何模型就显得尤为重要，而这一过程最关键的部分就是要得到相机的内参和外参（后续文有具体解释）。

矫正透镜畸变：我们最开始接触到的成像方面的知识应该是有关小孔成像的，但是由于这种成像方式只有小孔部分能透过光线就会导致物体的成像亮度很低，于是聪明的人类发明了透镜。虽然亮度问题解决了，但是新的问题又来了：由于透镜的制造工艺，会使成像产生多种形式的畸变，于是为了去除畸变（使成像后的图像与真实世界的景象保持一致），人们计算并利用畸变系数来矫正这种像差。虽然理论上可以设计出不产生畸变的透镜，但其制造工艺相对于球面透镜会复杂很多，所以相对于复杂且高成本的制造工艺，人们更喜欢用数学来解决问题。

【摄像机标定分为两部分】

1.从世界坐标系转换到相机坐标系，由于这两个坐标系都是三维的，所以这一部分就是三维空间转到另外一个三维空间
2.从相机坐标系转换到图像坐标系，由于图像坐标系是二维的，所以这一部分就是三维空间转到另外一个二维空间

• MATLAB单目-双目标定
• 双目立体矫正基本知识
• 双目标定与矫正完整流程
• OPENCV棋盘格标定
• 相机标定理论、概念
• 张氏标定
• 最详细、最完整的相机标定讲解
• OpenCV相机标定全过程
• 双目标定基本知识

1.2 常见模型介绍

1.2.1 相机模型

常见相机模型

1.2.2 畸变模型

1.3 双目标定

1.3.1 常见标定工具及对应使用场景

MATLAB和OpenCV 标定方法对比

MATLAB和OpenCV使用基本相同的校准算法。但是，MATLAB使用Levenberg-Marquardt非线性最小二乘算法进行优化（参见文档），而OpenCV使用梯度下降。我猜这可以解释重投影错误的大部分差异。此外，MATLAB和OpenCV使用不同的算法进行棋盘检测。Additionally, MATLAB and OpenCV use different algorithms for checkerboard detection.

1.3.2 Kalibr标定

Reference

• wiki
• 标定方法

Kalibr supports the following projection models:

• pinhole camera model (pinhole)
  (intrinsics vector: [fu fv pu pv])
• omnidirectional camera model (omni)
  (intrinsics vector: [xi fu fv pu pv])
• double sphere camera model (ds)
  (intrinsics vector: [xi alpha fu fv pu pv])
• extended unified camera model (eucm)
  (intrinsics vector: [alpha beta fu fv pu pv])

The intrinsics vector contains all parameters for the model:

fu, fv: focal-length
pu, pv: principal point
xi: mirror parameter (only omni)
xi, alpha: double sphere model parameters (only ds)
alpha, beta: extended unified model parameters (only eucm)

1.3.3 opencv双目标定

先标定单目，获得两个单目的内参和畸变系数，带入到下面程序中。MATLAB单目标定工具箱TOOLBOX_calib

data文件夹(存放左右图像)和主程序文件stereo.cpp，CMakeLists.txt在同一目录下，左右图像各13张，Clion直接运行。
参考：

• OpenCV相机标定全过程

• 双目相机标定和orbslam2双目参数详解

• 视觉SLAM ORB-SLAM2 双目相机实时实验 双目相机矫正 配置文件

经实测，opencv sample里面自带的双目标定程序（stereo_clib.cpp），对平角相机效果较好，如果你是平角相机跳过本段，直接查看ORBSLAM2的双目配置参数，而stereo_clib.cpp对广角相机效果奇差。

先自行标定左右相机的单目畸变参数，填入下面代码中的初始化参数，楼主用的kalibr标定的单目，opencv也可以标定，这个自己解决。
说明一点，单目预标定没必要标的很精确，差不多就成，当然标的精确更好，畸变参数4,5个都可以，单目的所有预标定参数将在下面的代码中进行迭代优化。

修改你的棋盘信息 纵横角点数 还有每个格子的大小 单位mm。

1.3.4 basalt标定

Reference
wiki

• 标定相机

 basalt_calibrate --dataset-path /media/gavyn/Gavyn/zhihui/5-9/calibr/0513_calib/1/cal_111-fix.bag  --dataset-type bag --aprilgrid aprilgrid_6x6.json --result-path basalt_calib_result/ --cam-types kb4 kb4

• Cam-IMU联合标定

basalt_calibrate_imu --dataset-path /media/gavyn/Gavyn/zhihui/5-9/calibr/0513_calib/1/cal_111-fix.bag --dataset-type bag --aprilgrid aprilgrid_6x6.json --result-path  basalt_calib_result/ --gyro-noise-std 0.005 --accel-noise-std 0.01 --gyro-bias-std 4.0e-06 --accel-bias-std 0.0002

二、ORB-SLAM2测试

2.1 修改Euroc配置文件

• 详细案例
  由于在 由于在 ROS下运行 下运行 .bag文件，双目配置以 文件，ORB-SLAM2中Euroc.yaml参数均可由OpenCV标定结果给出，参考例程：　https://github.com/zwl2017/stereo_calibrate
  

// Euroc.yaml.cpp
//Created by gavyn on 20-4-28.

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV)
/*
这个是 双目相机的参数不是单个的做相机的相机中心跟焦距。
其对应：extrinsics.yml中的 Pr：
例如我的是
Pr: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [ 2.8559499458758660e+02, 0., 2.7029193305969238e+02,
-3.9636548646706200e+04, 0., 2.8559499458758660e+02,
2.8112063348293304e+02, 0., 0., 0., 1., 0. ]
对应的修改焦距和相机中心如下：
Camera.fx: 2.8559499458758660e+02
Camera.fy: 2.8559499458758660e+02
Camera.cx: 2.7029193305969238e+02
Camera.cy: 2.8112063348293304e+02
*/
Camera.fx: 435.2046959714599
Camera.fy: 435.2046959714599
Camera.cx: 367.4517211914062
Camera.cy: 252.2008514404297

//默认不改，因代码中已做畸变纠正。故均为0.
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 1280
Camera.height: 640

# Camera frames per second
Camera.fps: 20.0

# stereo baseline times fx
/*
这个参数是个大坑，其为相机的基线×相机的焦距。orbslam的参数文件中单位是m,而opencv标定文件中的单位是mm
其数值同样可以在Pr中找出，定位在下面矩阵中的-3.9636548646706200e+04 这个数
Pr: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [ 2.8559499458758660e+02, 0., 2.7029193305969238e+02,
-3.9636548646706200e+04, 0., 2.8559499458758660e+02,
2.8112063348293304e+02, 0., 0., 0., 1., 0. ]

-3.9636548646706200e+04 就是要填入上面的参数，毫米转为米，求绝对值，填入Camera.bf:  3.9636548646706200e+01
*/
Camera.bf: 47.90639384423901

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

# Close/Far threshold. Baseline times.
/*
深度阈值，不是一个精确的数值，大概预估的，可以不改动，要改的话参考下述公式
        自己粗略估计一个相机可以良好显示的最大距离值为s = 10  如果fx = 100 Camera.bf = 20
那么 ThDepth = s*fx/Camera.bf = 10 *100 /20 = 50
将你自己的参数带入上述公式 可以得到大概的阈值。
*/
ThDepth: 35

#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 640
LEFT.width: 1280

/*
 * 左图像畸变参数，位于intrinsics.yml中的
cameraDistcoeffL: !!opencv-matrix
rows: 5
cols: 1
dt: d
data: [ -2.8632659642339481e-01, 6.6994801733091039e-02,
-5.4763802000265397e-04, -1.4767993829858197e-03,
-6.1039950504068767e-03 ]
填入下面的 LEFT.D: 即可 
*/
# 左图像畸变参数
LEFT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]

/*
左图像相机内参，可在intrinsics.yml 的cameraMatrixL:找到：
cameraMatrixL: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 2.8424872262658977e+02, 0., 3.3099977082276723e+02, 0.,
2.8535010886794362e+02, 2.5230877864759117e+02, 0., 0., 1. ]
填入LEFT.K:
*/
# 左图像内参
LEFT.K: !!opencv-matrix
        rows: 3
cols: 3
dt: d
data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]


/*
左相机校准变换矩阵：extrinsics.yml 中的 Rl:
Rl: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 9.9750705548699170e-01, 3.5207065558213610e-02,
6.1156657760632900e-02, -3.5691910468923047e-02,
9.9933934145707581e-01, 6.8533308118298173e-03,
-6.0874968425042433e-02, -9.0190437917577089e-03,
9.9810465136093429e-01 ]
填入下面的LEFT.R:
*/
# 左相机校准变换矩阵
LEFT.R:  !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]


/*
投影矩阵：
extrinsics.yml 中的 Pl:
Pl: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [ 2.8559499458758660e+02, 0., 2.7029193305969238e+02, 0., 0.,
2.8559499458758660e+02, 2.8112063348293304e+02, 0., 0., 0., 1.,
0. ]
填入下面的  LEFT.P:
 */
# 左相机在校准后坐标系的投影矩阵
LEFT.P:  !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [435.2046959714599, 0, 367.4517211914062, 0,  0, 435.2046959714599, 252.2008514404297, 0,  0, 0, 1, 0]

/*
RIGHT相机的设置与LEFT一致，唯一不同的就是RIGHT.P: 参数，
extrinsics.yml 中的 Pr:如下：
Pr: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [ 2.8559499458758660e+02, 0., 2.7029193305969238e+02,
-3.9636548646706200e+04, 0., 2.8559499458758660e+02,
2.8112063348293304e+02, 0., 0., 0., 1., 0. ]
对其进行修改，也就是data中的第4个值，需要转化单位从mm转为m。
所以应该填入RIGHT.P: 的数值为：
data: [ 2.8559499458758660e+02, 0., 2.7029193305969238e+02,
-3.9636548646706200e+01, 0., 2.8559499458758660e+02,
2.8112063348293304e+02, 0., 0., 0., 1., 0. ]
*/
RIGHT.height: 640
RIGHT.width: 1280
RIGHT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
RIGHT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
RIGHT.R:  !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P:  !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]



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

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

# 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

Camera.bf中的b指基线baseline（单位：米），
f是焦距fx（x轴和y轴差距不大），bf=b*f，
bf和ThDepth一起决定了深度点的范围（近双目特征点的定义是：匹配的深度值小于40倍双目或者RGB-D的基线，否则的话，是远特征点。）：
bf * ThDepth / fx
即大致为 b * ThDepth。

• 如ORB-SLAM中的双目示例中的EuRoC.yaml中的bf为47.9，ThDepth为35，fx为435.2，
  则有效深度为bf * ThDepth / fx = 47.9*35/435.3=3.85米；
• KITTI.yaml中的bf为387.57，ThDepth为40，fx为721.54，则有效深度为387.57*40/721.54=21.5米。
  这里的xtion的IR基线（其实也可以不这么叫）bf为40，
  ThDepth为50，fx为558.34，则有效深度为3.58米（官方为3.5米）。

2.2 ROS下跑通bag数据集

Reference

• ORB-SLAM2官方教程
• ROS下运行ORBSLAM2
• ORBSLAM2在ROS下运行
• 基于ubuntu16.04的ORB_SLAM2包的安装及配置

2.2.1 创建ROS空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
source devel/setup.bash
echo "source devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
//索性在catkin文件夹下面ctrl+h，找到隐藏文件夹.bashrc。在.bashrc 最下面添加，
source /opt/ros/indigo/setup.bash
source ~/catkin_ws/devel/setup.bas

2.2.2修改topic(双目)

首先在orb-slam2下找到ros_stereo.cc(如下图)文件然后对其中的rostopic节点进行修改 。先找到自己对应的节点在自己录制的.bag 数据集下打开终端执行 rosbag info xxx.bag在里面的topics中可以找到自己对应的节点。
ros_stereo.cc位置
节点信息

ros_stereo.cc中：

message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "camera/right/image_raw", 1);

修改为：

message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera_0/image", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera_1/image", 1);

2.2.3 编译

cd catkin_ws
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh
chmod +x build_ros.sh
./build_ros.sh

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/gavyn/catkin_ws/src/ORB_SLAM2/Examples/ROS

2.3.4 执行

roscore
rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/zhihui_2020507.yaml false
rosbag play /home/gavyn/calibr_camera-imu/calib_data/20200509/5-8/2020-05-07-18-29-52.bag

三、VINS测试

四、注意事项

ORB-SLAM ROS下编译出错

解决办法：在gedit ~/.bashrc时 需要注意环境变量设置顺序：

source /home/gavyn/catkin_ws/devel/setup.bash
source /opt/ros/kinetic/setup.bash
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/gavyn/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2

工作空间catkin_ws应该放在最前面（否则找不到包），**其次是ros，最后是Package（**否则无法编译）
保存退出更新一下source ~/.bashrc
再查看是否正确部署好环境：echo $ROS_PACKAGE_PATH
应该返回如下结果：/opt/ros/kinetic/share:/home/xxx/ORB_SLAM2/Examples/ROS/ORB_SLAM2
之后才能正确执行脚本编译./build_ros.sh
roscore
rosrun package…