利用深度图实现3D-2D的PNP相机位姿估计

需要安装opencv3并且编译了features模块

代码中的1.png

代码中的1_depth.png

代码中的2.png

代码中的2_depth.png

#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>
// 这里会在/usr/local/include/g2o/solvers/csparse/csparse_extension.h报错
// 修改文件中的  #include <cs.h> 为 #include<suitesparse/cs.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;

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);

// BA
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("1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("2.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 d1 = imread("1_depth.png", CV_LOAD_IMAGE_UNCHANGED);    // 16位无符号数，单通道
    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/5000.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);    // 调用opencv的pnp求解，可选择epnp,dls等方法
    Mat R;
    cv::Rodrigues(r, R);    // r为旋转向量形式，用Rodrigues公式转换为矩阵(罗德里格斯公式)

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

    cout<<"calling bundle adjustment"<<endl;
    // BA这块会报错，就没写出来了
    bundleAdjustment(pts_3d, pts_2d, K, R, t);

    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;
    Ptr<FeatureDetector>detector = ORB::create();
    Ptr<DescriptorExtractor>descriptor = ORB::create();
    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描述子进行匹配，使用汉明距离
    vector<DMatch> match;
    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;
        min_dist = min_dist<dist?min_dist:dist;
        max_dist = max_dist>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]);
        }
    }
}

Point2d pixel2cam(const Point2d& p, const Mat& K){
    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)
    );
}

// BA
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);
}