OpenCV實現SfM（三）：多目三維重建

注意：本文中的代碼必須使用OpenCV3.0或以上版本進行編譯，因為很多函數是3.0以後才加入的。

問題簡化

終於有時間來填坑了，這次一口氣將雙目重建擴展為多目重建吧。首先，為了簡化問題，我們要做一個重要假設：用於多目重建的圖像是有序的，即相鄰圖像的拍攝位置也是相鄰的。多目重建本身比較復雜，我會盡量說得清晰，如有表述不清的地方，還請見諒並歡迎提問。

求第三個相機的變換矩陣

由前面的文章我們知道，兩個相機之間的變換矩陣可以通過findEssentialMat以及recoverPose函數來實現，設第一個相機的坐標系為世界坐標系，現在加入第三幅圖像（相機），如何確定第三個相機（後面稱為相機三）到到世界坐標系的變換矩陣呢？

最簡單的想法，就是沿用雙目重建的方法，即在第三幅圖像和第一幅圖像之間提取特征點，然後調用findEssentialMat和recoverPose。那麼加入第四幅、第五幅，乃至更多呢？隨著圖像數量的增加，新加入的圖像與第一幅圖像的差異可能越來越大，特征點的提取變得異常困難，這時就不能再沿用雙目重建的方法了。

那麼能不能用新加入的圖像和相鄰圖像進行特征匹配呢？比如第三幅與第二幅匹配，第四幅與第三幅匹配，以此類推。當然可以，但是這時就不能繼續使用findEssentialMat和recoverPose來求取相機的變換矩陣了，因為這兩個函數求取的是相對變換，比如相機三到相機二的變換，而我們需要的是相機三到相機一的變換。有人說，既然知道相機二到相機一的變換，又知道相機到三到相機二的變換，不就能求出相機三到相機一的變換嗎？實際上，通過這種方式，你只能求出相機三到相機一的旋轉變換（旋轉矩陣R），而他們之間的位移向量T，是無法求出的。這是因為上面兩個函數求出的位移向量，都是單位向量，丟失了相機之間位移的比例關系。

說了這麼多，我們要怎麼解決這些問題？現在請出本文的主角——solvePnP和solvePnPRansac。根據opencv的官方解釋，該函數根據空間中的點與圖像中的點的對應關系，求解相機在空間中的位置。也就是說，我知道一些空間當中點的坐標，還知道這些點在圖像中的像素坐標，那麼solvePnP就可以告訴我相機在空間當中的坐標。solvePnP和solvePnPRansac所實現的功能相同，只不過後者使用了隨機一致性采樣，使其對噪聲更魯棒，本文使用後者。

好了，有這麼好的函數，怎麼用於我們的三維重建呢？首先，使用雙目重建的方法，對頭兩幅圖像進行重建，這樣就得到了一些空間中的點，加入第三幅圖像後，使其與第二幅圖像進行特征匹配，這些匹配點中，肯定有一部分也是圖像二與圖像一之間的匹配點，也就是說，這些匹配點中有一部分的空間坐標是已知的，同時又知道這些點在第三幅圖像中的像素坐標，嗯，solvePnP所需的信息都有了，自然第三個相機的空間位置就求出來了。由於空間點的坐標都是世界坐標系下的（即第一個相機的坐標系），所以由solvePnP求出的相機位置也是世界坐標系下的，即相機三到相機一的變換矩陣。

加入更多圖像

通過上面的方法得到相機三的變換矩陣後，就可以使用上一篇文章提到的triangulatePoints方法將圖像三和圖像二之間的匹配點三角化，得到其空間坐標。為了使之後的圖像仍能使用以上方法求解變換矩陣，我們還需要將新得到的空間點和之前的三維點雲融合。已經存在的空間點，就沒必要再添加了，只添加在圖像二和三之間匹配，但在圖像一和圖像三中沒有匹配的點。如此反復。

為了方便點雲的融合以及今後的擴展，我們需要存儲圖像中每個特征點在空間中的對應點。在代碼中我使用了一個二維列表，名字為correspond_struct_idx，correspond_struct_idx[i][j]代表第i幅圖像第j個特征點所對應的空間點在點雲中的索引，若索引小於零，說明該特征點在空間當中沒有對應點。通過此結構，由特征匹配中的queryIdx和trainIdx就可以查詢某個特征點在空間中的位置。<喎?/kf/ware/vc/" target="_blank" class="keylink">vcD4NCjxoMiBpZD0="代碼實現">代碼實現

前一篇文章的很多代碼不用修改，還可以繼續使用，但是程序的流程有了較大變化。首先是初始化點雲，也就是通過雙目重建方法對圖像序列的頭兩幅圖像進行重建，並初始化correspond_struct_idx。

void init_structure(
    Mat K,
    vector>& key_points_for_all, 
    vector>& colors_for_all,
    vector>& matches_for_all,
    vector& structure,
    vector>& correspond_struct_idx,
    vector& colors,
    vector& rotations,
    vector& motions
    )
{
    //計算頭兩幅圖像之間的變換矩陣
    vector p1, p2;
    vector c2;
    Mat R, T;   //旋轉矩陣和平移向量
    Mat mask;   //mask中大於零的點代表匹配點，等於零代表失配點
    get_matched_points(key_points_for_all[0], key_points_for_all[1], matches_for_all[0], p1, p2);
    get_matched_colors(colors_for_all[0], colors_for_all[1], matches_for_all[0], colors, c2);
    find_transform(K, p1, p2, R, T, mask);

    //對頭兩幅圖像進行三維重建
    maskout_points(p1, mask);
    maskout_points(p2, mask);
    maskout_colors(colors, mask);

    Mat R0 = Mat::eye(3, 3, CV_64FC1);
    Mat T0 = Mat::zeros(3, 1, CV_64FC1);
    reconstruct(K, R0, T0, R, T, p1, p2, structure);
    //保存變換矩陣
    rotations = { R0, R };
    motions = { T0, T };

    //將correspond_struct_idx的大小初始化為與key_points_for_all完全一致
    correspond_struct_idx.clear();
    correspond_struct_idx.resize(key_points_for_all.size());
    for (int i = 0; i < key_points_for_all.size(); ++i)
    {
        correspond_struct_idx[i].resize(key_points_for_all[i].size(), -1);
    }

    //填寫頭兩幅圖像的結構索引
    int idx = 0;
    vector& matches = matches_for_all[0];
    for (int i = 0; i < matches.size(); ++i)
    {
        if (mask.at(i) == 0)
            continue;

        correspond_struct_idx[0][matches[i].queryIdx] = idx;
        correspond_struct_idx[1][matches[i].trainIdx] = idx;
        ++idx;
    }
}

初始點雲得到後，就可以使用增量方式重建剩余圖像，注意，在代碼中為了方便實現，所有圖像之間的特征匹配已經事先完成了，並保存在matches_for_all這個列表中。增量重建的關鍵是調用solvePnPRansac，而這個函數需要空間點坐標和對應的像素坐標作為參數，有了correspond_struct_idx，實現這個對應關系的查找還是很方便的，如下。

void get_objpoints_and_imgpoints(
    vector& matches,
    vector& struct_indices, 
    vector& structure, 
    vector& key_points,
    vector& object_points,
    vector& image_points)
{
    object_points.clear();
    image_points.clear();

    for (int i = 0; i < matches.size(); ++i)
    {
        int query_idx = matches[i].queryIdx;
        int train_idx = matches[i].trainIdx;

        int struct_idx = struct_indices[query_idx];
        if (struct_idx < 0) continue;

        object_points.push_back(structure[struct_idx]);
        image_points.push_back(key_points[train_idx].pt);
    }
}

之後調用solvePnPRansac得到相機的旋轉向量和位移，由於我們使用的都是旋轉矩陣，所以這裡要調用opencv的Rodrigues函數將旋轉向量變換為旋轉矩陣。之後，使用上一篇文章中用到的reconstruct函數對匹配點進行重建（三角化），不過為了適用於多目重建，做了一些簡單修改。

void reconstruct(Mat& K, Mat& R1, Mat& T1, Mat& R2, Mat& T2, vector& p1, vector& p2, vector& structure)
{
    //兩個相機的投影矩陣[R T]，triangulatePoints只支持float型
    Mat proj1(3, 4, CV_32FC1);
    Mat proj2(3, 4, CV_32FC1);

    R1.convertTo(proj1(Range(0, 3), Range(0, 3)), CV_32FC1);
    T1.convertTo(proj1.col(3), CV_32FC1);

    R2.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
    T2.convertTo(proj2.col(3), CV_32FC1);

    Mat fK;
    K.convertTo(fK, CV_32FC1);
    proj1 = fK*proj1;
    proj2 = fK*proj2;

    //三角重建
    Mat s;
    triangulatePoints(proj1, proj2, p1, p2, s);

    structure.clear();
    structure.reserve(s.cols);
    for (int i = 0; i < s.cols; ++i)
    {
        Mat_ col = s.col(i);
        col /= col(3);  //齊次坐標，需要除以最後一個元素才是真正的坐標值
        structure.push_back(Point3f(col(0), col(1), col(2)));
    }
}

最後，將重建結構與之前的點雲進行融合。

void fusion_structure(
    vector& matches, 
    vector& struct_indices, 
    vector& next_struct_indices,
    vector& structure, 
    vector& next_structure,
    vector& colors,
    vector& next_colors
    )
{
    for (int i = 0; i < matches.size(); ++i)
    {
        int query_idx = matches[i].queryIdx;
        int train_idx = matches[i].trainIdx;

        int struct_idx = struct_indices[query_idx];
        if (struct_idx >= 0) //若該點在空間中已經存在，則這對匹配點對應的空間點應該是同一個，索引要相同
        {
            next_struct_indices[train_idx] = struct_idx;
            continue;
        }

        //若該點在空間中已經存在，將該點加入到結構中，且這對匹配點的空間點索引都為新加入的點的索引
        structure.push_back(next_structure[i]);
        colors.push_back(next_colors[i]);
        struct_indices[query_idx] = next_struct_indices[train_idx] = structure.size() - 1;
    }
}

整個增量方式重建圖像的代碼大致如下。

//初始化結構（三維點雲）
init_structure(
    K,
    key_points_for_all,
    colors_for_all,
    matches_for_all,
    structure,
    correspond_struct_idx,
    colors,
    rotations,
    motions
    );

//增量方式重建剩余的圖像
for (int i = 1; i < matches_for_all.size(); ++i)
{
    vector object_points;
    vector image_points;
    Mat r, R, T;
    //Mat mask;

    //獲取第i幅圖像中匹配點對應的三維點，以及在第i+1幅圖像中對應的像素點
    get_objpoints_and_imgpoints(
        matches_for_all[i], 
        correspond_struct_idx[i], 
        structure,
        key_points_for_all[i+1], 
        object_points,
        image_points
        );

    //求解變換矩陣
    solvePnPRansac(object_points, image_points, K, noArray(), r, T);
    //將旋轉向量轉換為旋轉矩陣
    Rodrigues(r, R);
    //保存變換矩陣
    rotations.push_back(R);
    motions.push_back(T);

    vector p1, p2;
    vector c1, c2;
    get_matched_points(key_points_for_all[i], key_points_for_all[i + 1], matches_for_all[i], p1, p2);
    get_matched_colors(colors_for_all[i], colors_for_all[i + 1], matches_for_all[i], c1, c2);

    //根據之前求得的R，T進行三維重建
    vector next_structure;
    reconstruct(K, rotations[i], motions[i], R, T, p1, p2, next_structure);

    //將新的重建結果與之前的融合
    fusion_structure(
        matches_for_all[i], 
        correspond_struct_idx[i], 
        correspond_struct_idx[i + 1],
        structure, 
        next_structure,
        colors,
        c1
        );
}

測試

我用了八幅圖像進行測試，正如問題簡化中所要求的那樣，圖像是有序的。

程序的大部分時間花在特征提取和匹配上，真正的重建過程耗時很少。最終結果如下。

圖中每個彩色坐標系都代表一個相機。

思考

這個多目三維重建程序，要求圖像必須是有序的，如果圖像無序，比如只是對某個目標在不同角度的隨意拍攝，程序應該如何修改？ 增量式三維重建方法，有一個很大的缺點——隨著圖像的不斷增加，誤差會不斷累積，最後誤差過大以至於完全偏離重建的目標，怎麼解決？

有興趣的讀者可以思考一下上面兩個問題，第二個問題比較難，我會在下一篇文章中詳細介紹。

下載

程序使用VS2015開發，OpenCV版本為3.1且包含擴展部分，如果不使用SIFT特征，可以修改源代碼，然後使用官方未包含擴展部分的庫。軟件運行後會將三維重建的結果寫入Viewer目錄下的structure.yml文件中，在Viewer目錄下有一個SfMViewer程序，直接運行即可讀取yml文件並顯示三維結構。