设计目的

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｝.

内点与外点

通过图优化，形成联通网络。

function cl = updateClique(potentialNodes, clique, M)

maxNumMatches = 0;
curr_max = 0;
for i = 1:length(potentialNodes)
    if(potentialNodes(i)==1)
        numMatches = 0;
        for j = 1:length(potentialNodes)
            if (potentialNodes(j) & M(i,j))
                numMatches = numMatches + 1;
            end
        end
        if (numMatches>=maxNumMatches)
            curr_max = i;
            maxNumMatches = numMatches;
        end
    end
end

if (maxNumMatches~=0)
    clique(length(clique)+1) = curr_max;
end

cl = clique;


function newSet = findPotentialNodes(clique, M)

newSet = M(:,clique(1));
if (size(clique)>1)  
    for i=2:length(clique)
        newSet = newSet & M(:,clique(i));
    end
end

for i=1:length(clique)
    newSet(clique(i)) = 0;
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:

$$\begin{equation} \epsilon = \sum_{\mathcal{F}^{t}, \mathcal{F}^{t+1}} (\mathbf{j_{t}} - \mathbf{P}\mathbf{T}\mathbf{w_{t+1}})^{2} + (\mathbf{j_{t+1}} - \mathbf{P}\mathbf{T^{-1}}\mathbf{w_{t}})^{2} \end{equation}$$

实现如下：

function F = minimize(PAR, F1, F2, W1, W2, P1)
r = PAR(1:3);
t = PAR(4:6);
%F1, F2 -> 2d coordinates of features in I1_l, I2_l
%W1, W2 -> 3d coordinates of the features that have been triangulated
%P1, P2 -> Projection matrices for the two cameras
%r, t -> 3x1 vectors, need to be varied for the minimization
F = zeros(2*size(F1,1), 3);
reproj1 = zeros(size(F1,1), 3);
reproj2 = zeros(size(F1,1), 3);

dcm = angle2dcm( r(1), r(2), r(3), 'ZXZ' );
tran = [ horzcat(dcm, t); [0 0 0 1]];

for k = 1:size(F1,1)
    f1 = F1(k, :)';
    f1(3) = 1;
    w2 = W2(k, :)';
    w2(4) = 1;

    f2 = F2(k, :)';
    f2(3) = 1;
    w1 = W1(k, :)';
    w1(4) = 1;

    f1_repr = P1*(tran)*w2;
    f1_repr = f1_repr/f1_repr(3);
    f2_repr = P1*pinv(tran)*w1;
    f2_repr = f2_repr/f2_repr(3);

    reproj1(k, :) = (f1 - f1_repr);
    reproj2(k, :) = (f2 - f2_repr);    
end

F = [reproj1; reproj2];

结果验证

如果一组特定的R和T满足以下条件，才被称为是有效的：
- 如果集团中的特征数量至少为8。
- 重投影误差小于某一阈值。
上述约束有助于处理噪声数据。

后话

为了防止同一纹理占据摄像头视图，我们针对主方向向前采用旋转平移向量。

硬件系统采用双目系统附带IMU或者GPS。RTK可能是GPS+IMU实现的。

系统架构设计：

软件代码实现：

参考文献：

Avi Singh’s blog : Monocular Visual Odometry using OpenCV

Real-Time Stereo Visual Odometry for Autonomous Ground Vehicles (Howard2008)