利用深度图使用3d-3dICP估计位姿

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

代码中的1.png

代码中的1_depth.png

代码中的2.png

代码中的2_depth.png

// 来自高翔SLAM十四讲
#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<Eigen/SVD>

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

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    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 depth1 = imread("1_depth.png", CV_LOAD_IMAGE_COLOR);    // 16位无符号数
    Mat depth2 = imread("2_depth.png", CV_LOAD_IMAGE_COLOR);
    Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts1, pts2;

    for(DMatch m:matches){
        ushort d1 = depth1.ptr<unsigned short>(int ( keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int ( keypoints_2[m.trainIdx].pt.x)];
        if( d1 ==0 || d2 == 0) continue;    // bad depth
        Point2d p1 = pixel2cam( keypoints_1[m.queryIdx].pt, K);
        Point2d p2 = pixel2cam( keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1)/5000.0;
        float dd2 = float(d2)/5000.0;
        pts1.push_back(Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }

    cout<<"3d-3d pairs : "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout<<"ICP via SVD results : "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() * t <<endl;    // 这个是表示的什么

    // 下面是 bundle adjustment有问题就写了
    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);

    // 第三步：匹配描述子
    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);
    // 以两倍最小距离作为最小值
    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)
    );
}

// 位姿估计
void pose_estimation_3d3d(
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
){
    Point3f p1, p2; // 全部匹配的点的中间值
    int N = pts1.size();
    for(int i=0; i<N; i++){
        p1 += pts1[i];
        p2 += pts2[i];
    }
    // 相对于中间点的位置
    p1 = Point3f(Vec3f(p1) / N);
    p2 = Point3f(Vec3f(p2) / N);
    vector<Point3f> q1(N), q2(N);  
    for( int i=0; i<N; i++){
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // 计算 q1 * q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i=0; i<N; i++){
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W = \n"<<W <<endl;

    // 对W进行SVD分解
    Eigen::JacobiSVD<Eigen::Matrix3d>svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    if(U.determinant() * V.determinant() < 0){
        for(int x=0; x<3; x++){
            U(x, 2) *= -1;
        }
    }
    cout<<"U = \n"<<U<<endl;
    cout<<"V = \n"<<V<<endl;

    Eigen::Matrix3d R_ = U*(V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d( p2.x, p2.y, p2.z );

    // 转换为 cv::Mat
    R = (Mat_<double>(3,3) << 
            R_(0,0), R_(0,1), R_(0,2),
            R_(1,0), R_(1,1), R_(1,2),
             R_(2,0), R_(2,1), R_(2,2)
        );
    t = (Mat_<double>(3, 1) << t_(0,0), t_(0,1), t_(0,2));
}








// 3d到3d的估计是对空间点运动的估计，并没有出现相机模型
// 也就是说，仅考虑3D点之间的变换时，与相机并没有关系