設計目的
Our job is to construct a 6-DOF trajectory using the video stream coming from Stereo camera(s).
系統需求：
算法概要：
算法實作
标定：Undistortion, Rectification
[J,newOrigin] = undistortImage(I,cameraParams)
[J1,J2] = rectifyStereoImages(I1,I2,stereoParams)
Note : that you need the Computer Vision Toolbox, and MATLAB R2014a or newer for these functions.
視差圖
1） Block-Matching Algorithm
直接調用Matlab已經實作的函數：
disparityMap1 = disparity(I1_l,I1_r, ‘DistanceThreshold’, 5);
特征檢測
采用FAST特征提取算法，并采用劃分“bucketing”來均勻特征點分布。
function points = bucketFeatures(I, h, b, h_break, b_break, numCorners)
% input image I should be grayscale
y = floor(linspace(1, h - h/h_break, h_break));
x = floor(linspace(1, b - b/b_break, b_break));
final_points = [];
for i=1:length(y)
for j=1:length(x)
roi = [x(j),y(i),floor(b/b_break),floor(h/h_break)];
corners = detectFASTFeatures(I, ‘MinQuality’, 0.00, ‘MinContrast’, 0.1, ‘ROI’,roi );
corners = corners.selectStrongest(numCorners);
final_points = vertcat(final_points, corners.Location);
end
end
points = cornerPoints(final_points);
特征跟蹤
KLT tracker
tracker = vision.PointTracker(‘MaxBidirectionalError’, 1);
initialize(tracker, points1_l.Location, I1_l);
[points2_l, validity] = step(tracker, I2_l);
3D PointCloud
通過雙目視差圖可以得到相機P1和P2的投影矩陣。Q表示相機的内參和外參。我們可以通過上圖關系擷取雙目相機中3D坐标。将擷取的點雲圖用資料集｛Wt、Wt+1｝.
内點與外點
該算法隻有内點檢測，沒有外點剔除。通過計算三維點在連續兩幀的距離，判斷是否為連通圖。為了得到比對的最大集合，我們形成一緻性矩陣M：
Mi,j={1,0,if the distance between i and j points is same in both the point cloudsotherwise(1) (1) M i , j = { 1 , if the distance between i and j points is same in both the point clouds 0 , otherwise
從原始點雲，我們希望選擇最大子集，使得它們在該子集中的所有點彼此一緻（減少一緻性矩陣中的每個元素為1）。這個問題等價于最大團問題（圖論中一個經典的組合優化問題，也是一類NP完全問題，圖的子集），M作為鄰接矩陣，它隻包含互相連接配接的節點。一個簡單的可視化方法是把一個圖表看作一個社會網絡，然後試圖找出最大的一群互相認識的人。該問題可以用下圖表示：
這個問題就是NP完全問題，不能找到任何實際情況下的最優解。是以，我們采用貪婪啟發式，接近最佳的解決方案。
算法實作通過下面兩個函數：
function cl = updateClique(potentialNodes, clique, M)
maxNumMatches = ;
curr_max = ;
for i = :length(potentialNodes)
if(potentialNodes(i)==)
numMatches = ;
for j = :length(potentialNodes)
if (potentialNodes(j) & M(i,j))
numMatches = numMatches + ;
end
end
if (numMatches>=maxNumMatches)
curr_max = i;
maxNumMatches = numMatches;
end
end
end
if (maxNumMatches~=)
clique(length(clique)+) = curr_max;
end
cl = clique;
function newSet = findPotentialNodes(clique, M)
newSet = M(:,clique());
if (size(clique)>)
for i=:length(clique)
newSet = newSet & M(:,clique(i));
end
end
for i=:length(clique)
newSet(clique(i)) = ;
end
計算剛體運動
重投影誤差：
In order to determine the rotation matrix R and translation vector t, we use Levenberg-Marquardt non-linear least squares minimization to minimize the following sum:
ϵ=∑Ft,Ft+1(jt−PTwt+1)2+(jt+1−PT−1wt)2(2) (2) ϵ = ∑ F t , F t + 1 ( j t − P T w t + 1 ) 2 + ( j t + 1 − P T − 1 w t ) 2
實作如下：
function F = minimize(PAR, F1, F2, W1, W2, P1)
r = PAR(:);
t = PAR(:);
%F1, F2 -> d coordinates of features in I1_l, I2_l
%W1, W2 -> d coordinates of the features that have been triangulated
%P1, P2 -> Projection matrices for the two cameras
%r, t -> x1 vectors, need to be varied for the minimization
F = zeros(*size(F1,), );
reproj1 = zeros(size(F1,), );
reproj2 = zeros(size(F1,), );
dcm = angle2dcm( r(), r(), r(), 'ZXZ' );
tran = [ horzcat(dcm, t); [ ]];
for k = :size(F1,)
f1 = F1(k, :)';
f1(3) = 1;
w2 = W2(k, :)';
w2() = ;
f2 = F2(k, :)';
f2(3) = 1;
w1 = W1(k, :)';
w1() = ;
f1_repr = P1*(tran)*w2;
f1_repr = f1_repr/f1_repr();
f2_repr = P1*pinv(tran)*w1;
f2_repr = f2_repr/f2_repr();
reproj1(k, :) = (f1 - f1_repr);
reproj2(k, :) = (f2 - f2_repr);
end
F = [reproj1; reproj2];
結果驗證
如果一組特定的R和T滿足以下條件，才被稱為是有效的：
- 如果集團中的特征數量至少為8。
- 重投影誤差小于某一門檻值。
上述限制有助于處理噪聲資料。
後話
為了防止同一紋理占據攝像頭視圖，我們針對主方向向前采用旋轉平移向量。
硬體系統采用雙目系統附帶IMU或者GPS。RTK可能是GPS+IMU實作的。
系統架構設計：
架構圖中，有一部分是離線擷取的資料，就不需要占用運算量，有一部分适合并行加速，但是總體任務管理還需要CPU完成。
軟體代碼實作：
參考文獻：
Avi Singh’s blog : Monocular Visual Odometry using OpenCV
Real-Time Stereo Visual Odometry for Autonomous Ground Vehicles (Howard2008)
百度百科：最大團問題 、NP完全問題