本文首先介紹如何使用OpenCV中的PnP求解3D-2D位姿變換，再介紹如何使用g2o對(duì)前面得出的結(jié)果進(jìn)行集束調(diào)整（Bundle Adjustment，BA）。

一、3D-2D相機(jī)位姿估計(jì)：PnP

PnP（Perspective-n-Point）描述了當(dāng)知道n個(gè)3D空間點(diǎn)及其投影位置時(shí)，如何估計(jì)相機(jī)的位姿。對(duì)應(yīng)到SLAM問(wèn)題上，在初始化完成后，前一幀圖像的特征點(diǎn)都已經(jīng)被三角化，即已經(jīng)知道了這些點(diǎn)的3D位置。那么新的幀到來(lái)后，通過(guò)圖像匹配就可以得到與那些3D點(diǎn)相對(duì)應(yīng)的2D點(diǎn)，再根據(jù)這些3D-2D的對(duì)應(yīng)關(guān)系，利用PnP算法解出當(dāng)前幀的相機(jī)位姿。

PnP問(wèn)題有多種求解方法，包括P3P、直接線性變換（DLT）、EPnP（Efficient PnP）、UPnP等等，而且它們?cè)贠penCV中都有提供。

下面我們就來(lái)看代碼吧。

二、PnP求解相機(jī)位姿代碼

這里直接使用深度圖中給出的像素深度來(lái)計(jì)算3D點(diǎn)坐標(biāo)，以簡(jiǎn)化代碼。調(diào)用cv::solvePnP即可求出旋轉(zhuǎn)向量和平移向量。

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 讀取圖像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"組匹配點(diǎn)"<<endl;

    // 建立3D點(diǎn)
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度圖為16位無(wú)符號(hào)數(shù)，單通道圖像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/1000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 調(diào)用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r為旋轉(zhuǎn)向量形式，用Rodrigues公式轉(zhuǎn)換為矩陣

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    //下一步使用集束調(diào)整優(yōu)化
    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

三、Bundle Adjustment

現(xiàn)在，終于要對(duì)前面得出的結(jié)果做優(yōu)化了。在SLAM中，優(yōu)化是系統(tǒng)穩(wěn)定運(yùn)行的核心，沒(méi)有優(yōu)化的結(jié)果誤差會(huì)越來(lái)越大，根本無(wú)法長(zhǎng)時(shí)間運(yùn)行。在本文集之前發(fā)表的文章中，曾使用Ceres和g2o對(duì)曲線擬合做非線性優(yōu)化，現(xiàn)在我們把同樣的方法用到3D-2D相機(jī)位姿估計(jì)上。這種優(yōu)化方法用在SLAM上時(shí)稱為集束調(diào)整（Bundle Adjustment，BA）?！凹{(diào)整”名稱的含義是說(shuō)，通過(guò)調(diào)整相機(jī)的姿態(tài)使3D路標(biāo)點(diǎn)發(fā)出的光線都能匯聚到相機(jī)的光心。

回顧g2o圖優(yōu)化庫(kù)的使用方法，將優(yōu)化變量作為頂點(diǎn)，誤差項(xiàng)作為邊，構(gòu)造一個(gè)圖。在本文的BA問(wèn)題中，頂點(diǎn)是3D點(diǎn)（認(rèn)為深度圖中的3D點(diǎn)有測(cè)量誤差）和相機(jī)位姿，誤差項(xiàng)是重投影誤差。幸運(yùn)的是，g2o特地為SLAM問(wèn)題提供了許多封裝好的頂點(diǎn)類和邊類。在本文的BA問(wèn)題中需要用到的3D點(diǎn)頂點(diǎn)類、相機(jī)位姿頂點(diǎn)類和重投影誤差類，我們都可以直接使用，而不需要像曲線擬合那樣自定義類以及重寫誤差計(jì)算方法了。

四、BA代碼

下面給出BA函數(shù)的代碼，它的流程就是初始化g2o求解器、定義頂點(diǎn)（包括位姿頂點(diǎn)和所有3D點(diǎn)頂點(diǎn)）、定義邊、開始優(yōu)化。

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 維度為 6, landmark 維度為 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 線性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩陣塊求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必須設(shè)置 marg 參見第十講內(nèi)容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

可以看到，我們直接使用了g2o提供的相機(jī)位姿頂點(diǎn)類VertexSE3Expmap、3D路標(biāo)點(diǎn)類VertexSBAPointXYZ和重投影誤差邊類EdgeProjectXYZ2UV，這大大降低了程序的代碼量。如果對(duì)這幾個(gè)類的具體實(shí)現(xiàn)感興趣，可以查看g2o源碼。這里稍微提一下，相機(jī)位姿頂點(diǎn)類VertexSE3Expmap使用了李代數(shù)表示相機(jī)位姿，而不是使用旋轉(zhuǎn)矩陣和平移矩陣。這是因?yàn)樾D(zhuǎn)矩陣是有約束的矩陣，它必須是正交矩陣且行列式為1。使用它作為優(yōu)化變量就會(huì)引入額外的約束條件，從而增大優(yōu)化的復(fù)雜度。而將旋轉(zhuǎn)矩陣通過(guò)李群-李代數(shù)之間的轉(zhuǎn)換關(guān)系轉(zhuǎn)換為李代數(shù)表示，就可以把位姿估計(jì)變成無(wú)約束的優(yōu)化問(wèn)題，求解難度降低。在重投影誤差邊類EdgeProjectXYZ2UV中，已經(jīng)為相機(jī)位姿和3D點(diǎn)坐標(biāo)推導(dǎo)了雅克比矩陣，以計(jì)算迭代的增量方向。關(guān)于這部分的理論知識(shí)，強(qiáng)烈建議參考高翔《視覺(jué)SLAM十四講》的第4講李群和李代數(shù)以及第7講視覺(jué)里程計(jì)1的第8節(jié)。

最后，完整代碼見GitHub：https://github.com/jingedawang/FeatureMethod

五、參考資料

《視覺(jué)SLAM十四講》第4講 李群與李代數(shù) 高翔
《視覺(jué)SLAM十四講》第7講 視覺(jué)里程計(jì)1 高翔
Bundle adjustment Wikipedia