相機標定和ORB-SLAM2測試

一、相機標定

1.1 標定目的

【WHY：爲什麼要進行相機標定？】

相機標定的目的是：建立相機成像幾何模型並矯正透鏡畸變。

建立相機成像幾何模型：計算機視覺的首要任務就是要通過拍攝到的圖像信息獲取到物體在真實三維世界裏相對應的信息，於是，建立物體從三維世界映射到相機成像平面這一過程中的幾何模型就顯得尤爲重要，而這一過程最關鍵的部分就是要得到相機的內參和外參（後續文有具體解釋）。

矯正透鏡畸變：我們最開始接觸到的成像方面的知識應該是有關小孔成像的，但是由於這種成像方式只有小孔部分能透過光線就會導致物體的成像亮度很低，於是聰明的人類發明了透鏡。雖然亮度問題解決了，但是新的問題又來了：由於透鏡的製造工藝，會使成像產生多種形式的畸變，於是爲了去除畸變（使成像後的圖像與真實世界的景象保持一致），人們計算並利用畸變係數來矯正這種像差。雖然理論上可以設計出不產生畸變的透鏡，但其製造工藝相對於球面透鏡會複雜很多，所以相對於複雜且高成本的製造工藝，人們更喜歡用數學來解決問題。

【攝像機標定分爲兩部分】

1.從世界座標系轉換到相機座標系，由於這兩個座標系都是三維的，所以這一部分就是三維空間轉到另外一個三維空間
2.從相機座標系轉換到圖像座標系，由於圖像座標系是二維的，所以這一部分就是三維空間轉到另外一個二維空間

1.2 雙目標定

常見相機模型

針孔相機模型(pinhole camera model）
Unified Camera Model
Extended Unified Camera Model
Kannala-Brandt Camera Model(popuplar)
Field-of-View Camera Model
Double Sphere Camera Model (novel)

1.2.1 Kalibr標定

Reference

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.2.2 opencv雙目標定

先標定單目，獲得兩個單目的內參和畸變係數，帶入到下面程序中。MATLAB單目標定工具箱TOOLBOX_calib

data文件夾(存放左右圖像)和主程序文件stereo.cpp，CMakeLists.txt在同一目錄下，左右圖像各13張，Clion直接運行。
參考：

https://blog.csdn.net/czhzasui/article/details/83273380?depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-1&utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-1
雙目相機標定和orbslam2雙目參數詳解
視覺SLAM ORB-SLAM2 雙目相機實時實驗雙目相機矯正配置文件

經實測，opencv sample裏面自帶的雙目標定程序（stereo_clib.cpp），對平角相機效果較好，如果你是平角相機跳過本段，直接查看ORBSLAM2的雙目配置參數，而stereo_clib.cpp對廣角相機效果奇差。

先自行標定左右相機的單目畸變參數，填入下面代碼中的初始化參數，樓主用的kalibr標定的單目，opencv也可以標定，這個自己解決。
說明一點，單目預標定沒必要標的很精確，差不多就成，當然標的精確更好，畸變參數4,5個都可以，單目的所有預標定參數將在下面的代碼中進行迭代優化。

修改你的棋盤信息縱橫角點數還有每個格子的大小單位mm。

1.2.3 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

標定方法對比

MATLAB和OpenCV使用基本相同的校準算法。但是，MATLAB使用Levenberg-Marquardt非線性最小二乘算法進行優化（參見文檔），而OpenCV使用梯度下降。我猜這可以解釋重投影錯誤的大部分差異。此外，MATLAB和OpenCV使用不同的算法進行棋盤檢測。Additionally, MATLAB and OpenCV use different algorithms for checkerboard detection.

二、ORB-SLAM2測試

2.1 修改Euroc配置文件

// 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

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

三、注意事項

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…

相機標定和ORB-SLAM2測試

相機標定和ORB-SLAM2測試

一、相機標定

1.1 標定目的

1.2 雙目標定

1.2.1 Kalibr標定

1.2.2 opencv雙目標定

1.2.3 basalt標定

標定方法對比

二、ORB-SLAM2測試

2.1 修改Euroc配置文件

2.2 ROS下跑通bag數據集

2.2.1 創建ROS空間

2.2.2修改topic(雙目)

2.2.3 編譯

2.3.4 執行

三、注意事項

ORB-SLAM ROS下編譯出錯

教你用Perl實現Smgp協議

如何通過前端表格控件在10分鐘內完成一張分組報表？

win11關閉自動檢測病毒刪文件

千兆寬帶實際網速能到達多少？

C++與數據結構常見知識點（二）

2020華爲暑期實習筆試題

Cmake小結

面試題64：求1+2+…+n

相機標定和ORB-SLAM2測試

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結