視覺里程計- 位姿

 

 注意到位姿節點之間的變換並不是位姿，之前一直有誤解；

 

一般地；

路標節點：也就是觀測方程【數學形式下見】的觀測值，也就是特征點的像素坐標[u,v],或者該幀相機坐標系下的3d坐標[x,y,z];

 

位姿節點：也就是運動方程【數學形式下見】的輸出值。例如：上述x1、x2、x3、X4對應位姿為：Tcw1、Tcw2、Tcw3、Tcw4。這里的Tcw表示對應幀相機坐標系->世界坐標系的變換；比如：在x1處看到了路標點p1，在x2處也看到了路標點p1（當然是通過特征匹配才知道再次看到）考慮以下兩種情況：

 

1>  x1對應第一幀，x2對應第二幀

那么x1的相機坐標系即為：世界坐標系；那么第一幀位姿我們直接初始化為：

 

第二幀我們要：通過……solvePNPransac(points3d_1, points2d_2,R,t);來求解，points3d_1對應的是世界坐標系下的3D點（在這種情形，第一幀相機坐標系也就是世界坐標系，所以第一幀相機坐標系下的3D點）, points2d_2是第二幀像素點（設第二幀對應3D路標點（在第二幀相機坐標系下）為：points3d_2）。

在這里[左乘]，

                                                       Tcw2 * points3d_1 = points3d_2

     同時必須知道：（假設兩幀之間的變換 T12，也就是 位姿變換）

                                                                   T12 * Tcw1 = Tcw2

2> x2對應第2幀，x3對應第3幀

接着，我們再次想通過solvePNPransac(points3d_2_, points2d_3,R,t)來求解Tcw3，points3d_2_ （世界坐標系）肯定不是 points3d_2(相機坐標系)，所以在第一種情形你就應該提前將points3d_2 從它所在的相機坐標系映射到世界坐標系，故：

                                                                 points3d_2_ = Tcw2.inverse() * points3d_2

我們得出流程圖如下圖：

 

觀測模型：具體見下

運動模型：具體見下

兩個模型與G2O結合了解說明；

 

位姿：位置和姿態；

對於上述位姿圖，我們假設 x1 對應參考幀 ref_（同時假設是第一幀），x2對應 當前幀curr_（同時假設是第二幀）;我們通過......最后通過opencv中API solvePNPransac();求出的R|t 其實是Tcw；

Tcw指的是：當前幀與世界坐標系之間的變換；

Trw指的是：參考幀與世界坐標系之間的變換。那么待估計的兩幀之間的變換為：Tcr，構成左乘關系：

                                                                                       Tcr， s.t. Tcr * Trw = Tcw 

P_camera_curr 指的是：當前幀3D路標點 （相機坐標系）

P_world_ref      指的是：參考幀3D路標點    (世界坐標系）

                                                                               P_camera_curr = Tcw*P_world_ref

這里 opencv中的 solvePNPransac 求出來的是 T = （R|t）；依據上述公式：所以你要將當前幀3D路標點轉化到世界坐標系，應該是左乘：Tcw.inverse() * P_camera_curr = P_world_ref;請看以下代碼【就是匹配，第一幀相機坐標系當作世界坐標系，第二幀就是相機坐標系，重點關注代碼 81行 - 124行】：

 

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

#include"sophus\se3.h"
#include"sophus\so3.h"

void find_feature_matches(
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches);

// 像素坐標轉相機歸一化坐標
Point2d pixel2cam(const Point2d& p, const Mat& K);

void bundleAdjustment(
const vector<Point3f> points_3d,
const vector<Point2f> points_2d,
const Mat& K,
Mat& R, Mat& t
);

int main()
{
    //-- 讀取圖像
    Mat img_1 = imread("color/b.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("color/c.png", 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() << "組匹配點" << endl;

    // 建立3D點
    Mat depth_1 = imread("depth/b.pgm", CV_LOAD_IMAGE_UNCHANGED);       // 深度圖為16位無符號數，單通道圖像
    Mat depth_2 = imread("depth/c.pgm", CV_LOAD_IMAGE_UNCHANGED);       // 深度圖為16位無符號數，單通道圖像
                                                                   //Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    Mat K = (Mat_<double>(3, 3) << 518.0, 0, 325.5, 0, 519.0, 253.5, 0, 0, 1);
    vector<Point3f> pts_3d1;//世界坐標點
    vector<Point3f> pts_3d2;
    vector<Point2f> pts_2d;//相機坐標系點（歸一化？？？）
                           //【注：】這里表示同一個3D點在兩幅圖中的圖二對應像素點，本來就是求世界坐標系下的r、t
    for (DMatch m : matches)
    {
        //取出深度值，此時彩圖和深度圖已經對齊
        ushort d1 = depth_1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        ushort d2 = depth_2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
        if ((d1 == 0) || (d2 == 0))   // bad depth
            continue;
        float dd1 = d1 / 1.0;
        float dd2 = d2 / 1.0;
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K); // 相機坐標系歸一化平面：p1的形式 [x/z,y/z,1]
        Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        pts_3d1.push_back(Point3f(p1.x*dd1, p1.y*dd1, dd1));//乘以Z就得到形式  [x,y,z]?????左邊相機為參考，把他當作世界坐標系,確實是這樣的
        pts_3d2.push_back(Point3f(p2.x*dd2, p2.y*dd2, dd2));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);//圖二中的2D像素坐標[u,v]
    }

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

    Mat r, t;
    // 調用OpenCV 的 PnP 求解r t，可選擇EPNP，DLS等方法
    //solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); 
    //solvePnP(pts_3d1, pts_2d, K, Mat(), r, t, false, SOLVEPNP_EPNP);

    cv::solvePnPRansac(pts_3d1, pts_2d, K, Mat(), r, t, false, 100, 1.0, 0.99);

    Mat R;
    cv::Rodrigues(r, R); // r為旋轉向量形式，用Rodrigues公式轉換為矩陣
                         //RR = -R.inv();
    cout << "R=" << endl << R << endl;
    cout << "t=" << endl << t << endl;
    //************************************************************************************************
    // 李群
    //************************************************************************************************
    Sophus::SE3 T_se3 = Sophus::SE3(
        Sophus::SO3(r.at<double>(0, 0), r.at<double>(1, 0), r.at<double>(2, 0)),
        Sophus::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
    );
    cout << "T_se3 = " << T_se3.matrix() << endl;
    //************************************************************************************************
    // 歐氏群
    //************************************************************************************************
    Eigen::Isometry3d T_Isometry3d = Eigen::Isometry3d::Identity();
    //Eigen::AngleAxisd rot_vec(r.at<double>(0, 0), r.at<double>(1, 0), r.at<double>(2, 0));
    Eigen::Vector3d trans(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0));
    Eigen::Matrix<double, 3, 3> R_;
    R_ << 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);
    T_Isometry3d.rotate(R_);
    T_Isometry3d.pretranslate(trans);
    cout << " T_Isometry3d = " << T_Isometry3d.matrix() << endl;

    cout << "calling bundle adjustment" << endl;
    bundleAdjustment(pts_3d1, pts_2d, K, R, t);
    //************************************************************************************************
    // 計算誤差：error = point3d_curr -( R * point3d_ref + t )
    //************************************************************************************************
    for (int i = 0; i < 20; i++)
    {
        cout << " pts_3d1 =  " << pts_3d1[i] << endl;
        cout << " pts_3d2 =  " << pts_3d2[i] << endl;
        cout << " error " <<
            (Mat_<double>(3, 1) << pts_3d2[i].x, pts_3d2[i].y, pts_3d2[i].z)
            - R*((Mat_<double>(3, 1) << pts_3d1[i].x, pts_3d1[i].y, pts_3d1[i].z)) - t
            << endl;
        cout << endl;
    }

    waitKey(0);
    return 0;
}

void find_feature_matches(const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches)
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create(1000);
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:檢測 Oriented FAST 角點位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根據角點位置計算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配點對篩選
    double min_dist = 10000, max_dist = 0;

    //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限.
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }



    Mat img_match;
    Mat img_goodmatch;
    drawMatches(img_1, keypoints_1, img_2, keypoints_2, match, img_match);
    drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_goodmatch);
    namedWindow("所有匹配點對", CV_WINDOW_NORMAL);
    imshow("所有匹配點對", img_match);
    namedWindow("優化后匹配點對", CV_WINDOW_NORMAL);
    imshow("優化后匹配點對", img_goodmatch);
}

Point2d pixel2cam(const Point2d& p, const Mat& K)// [u,v,1] - > [x/z, y/z, 1]
{
    return Point2d
    (
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}


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 中必須設置 marg 參見第十講內容
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;
}

 這里我們只關心運動，不關心結構。換句話說，只要通過特征點成功求出了運動，我們就不再需要這幀的特征點了。這種做法當然會有缺陷，但是忽略掉數量龐大的特征點可以節省許多的計算量。然后，在 t 到 t + 1 時刻，我們又以 t 時刻為參考幀，考慮 t 到 t + 1 間的運動關系。如此往復，就得到了一條運動軌跡。