2021-03-31

https://lsy.cowtransfer.com/s/e49d4abb51d944
您好，文件已下载并上传完毕，请用浏览器打开链接下载
ttps://github.com/opencv/opencv

//
// Created by caoqi on 2018/9/5.
//
#include <iostream>
#include "math/vector.h"
class Camera{

public:

    // constructor
    Camera(){

        // 采用归一化坐标，不考虑图像尺寸
        c_[0]=c_[1] = 0.0;
    }

    // 相机投影过程
    math::Vec2d projection(math::Vec3d const & p3d){

        math::Vec2d p;
        /** TODO HERE
         *
         */
        return p;

        /**  Reference
        // 世界坐标系到相机坐标系
        double xc = R_[0] * p3d[0] + R_[1] * p3d[1] + R_[2]* p3d[2] + t_[0];
        double yc = R_[3] * p3d[0] + R_[4] * p3d[1] + R_[5]* p3d[2] + t_[1];
        double zc = R_[6] * p3d[0] + R_[7] * p3d[1] + R_[8]* p3d[2] + t_[2];

        // 相机坐标系到像平面
        double x = xc/zc;
        double y = yc/zc;

        // 径向畸变过程
        double r2 = x*x + y*y;
        double distort_ratio = 1+ dist_[0]* r2+ dist_[1]*r2*r2;

        // 图像坐标系到屏幕坐标系
        math::Vec2d p;
        p[0] = f_* distort_ratio*x + c_[0];
        p[1] = f_* distort_ratio*y + c_[1];

        return p;

         **/

    }

    // 相机在世界坐标中的位置 -R^T*t
    math::Vec3d pos_in_world(){

        math::Vec3d pos;
        pos[0] = R_[0]* t_[0] + R_[3]* t_[1] + R_[6]* t_[2];
        pos[1] = R_[1]* t_[0] + R_[4]* t_[1] + R_[7]* t_[2];
        pos[2] = R_[2]* t_[0] + R_[5]* t_[1] + R_[8]* t_[2];
        return -pos;
    }

    // 相机在世界坐标中的方向
    math::Vec3d dir_in_world(){

        math::Vec3d  dir (R_[6], R_[7],R_[8]);
        return dir;
    }
public:

    // 焦距f
    double f_;

    // 径向畸变系数k1, k2
    double dist_[2];

    // 中心点坐标u0, v0
    double c_[2];

    // 旋转矩阵
    /*
     * [ R_[0], R_[1], R_[2] ]
     * [ R_[3], R_[4], R_[5] ]
     * [ R_[6], R_[7], R_[8] ]
     */
    double R_[9];

    // 平移向量
    double t_[3];
};

int main(int argc, char* argv[]){


    Camera cam;

    //焦距
    cam.f_ = 0.920227;

    // 径向畸变系数
    cam.dist_[0] = -0.106599; cam.dist_[1] = 0.104385;

    // 平移向量
    cam.t_[0] = 0.0814358; cam.t_[1] =  0.937498;   cam.t_[2] = -0.0887441;

    // 旋转矩阵
    cam.R_[0] = 0.999796 ; cam.R_[1] = -0.0127375;  cam.R_[2] =  0.0156807;
    cam.R_[3] = 0.0128557; cam.R_[4] =  0.999894 ;  cam.R_[5] = -0.0073718;
    cam.R_[6] = -0.0155846; cam.R_[7] = 0.00757181; cam.R_[8] = 0.999854;

    // 三维点坐标
    math::Vec3d p3d ={1.36939, -1.17123, 7.04869};

    /*计算相机的投影点*/
    math::Vec2d p2d = cam.projection(p3d);
    std::cout<<"projection coord:\n "<<p2d<<std::endl;
    std::cout<<"result should be:\n 0.208188 -0.035398\n\n";

    /*计算相机在世界坐标系中的位置*/
    math::Vec3d pos = cam.pos_in_world();
    std::cout<<"cam position in world is:\n "<< pos<<std::endl;
    std::cout<<"result should be: \n -0.0948544 -0.935689 0.0943652\n\n";

    /*计算相机在世界坐标系中的方向*/
    math::Vec3d dir = cam.dir_in_world();
    std::cout<<"cam direction in world is:\n "<<dir<<std::endl;
    std::cout<<"result should be: \n -0.0155846 0.00757181 0.999854\n";
}

//Created by sway on 2018/8/29.
   /* 测试8点法求取基础矩阵F
    *
    * [直接线性变换法]
    * 双目视觉中相机之间存在对极约束
    *
    *                       p2'Fp1=0,
    *
    * 其中p1, p2 为来自两个视角的匹配对的归一化坐标，并表示成齐次坐标形式，
    * 即p1=[x1, y1, z1]', p2=[x2, y2, z2],将p1, p2的表达形式带入到
    * 上式中，可以得到如下表达形式
    *
    *          [x2] [f11, f12, f13] [x1, y1, z1]
    *          [y2] [f21, f22, f23]                = 0
    *          [z2] [f31, f32, f33]
    *
    * 进一步可以得到
    * x1*x2*f11 + x2*y1*f12 + x2*f13 + x1*y2*f21 + y1*y2*f22 + y2*f23 + x1*f31 + y1*f32 + f33=0
    *
    * 写成向量形式
    *               [x1*x2, x2*y1,x2, x1*y2, y1*y2, y2, x1, y1, 1]*f = 0,
    * 其中f=[f11, f12, f13, f21, f22, f23, f31, f32, f33]'
    *
    * 由于F无法确定尺度(up to scale, 回想一下三维重建是无法确定场景真实尺度的)，因此F秩为8，
    * 这意味着至少需要8对匹配对才能求的f的解。当刚好有8对点时，称为8点法。当匹配对大于8时需要用最小二乘法进行求解
    *
    *   [x11*x12, x12*y11,x12, x11*y12, y11*y12, y12, x11, y11, 1]
    *   [x21*x22, x22*y21,x22, x21*y22, y21*y22, y22, x21, y21, 1]
    *   [x31*x32, x32*y31,x32, x31*y32, y31*y32, y32, x31, y31, 1]
    * A=[x41*x42, x42*y41,x42, x41*y42, y41*y42, y42, x41, y41, 1]
    *   [x51*x52, x52*y51,x52, x51*y52, y51*y52, y52, x51, y51, 1]
    *   [x61*x62, x62*y61,x62, x61*y62, y61*y62, y62, x61, y61, 1]
    *   [x71*x72, x72*y71,x72, x71*y72, y71*y72, y72, x71, y71, 1]
    *   [x81*x82, x82*y81,x82, x81*y22, y81*y82, y82, x81, y81, 1]
    *
    *现在任务变成了求解线性方程
    *               Af = 0
    *（该方程与min||Af||, subject to ||f||=1 等价)
    *通常的解法是对A进行SVD分解，取最小奇异值对应的奇异向量作为f分解
    *
    *本项目中对矩阵A的svd分解并获取其最小奇异值对应的奇异向量的代码为
    *   math::Matrix<double, 9, 9> V;
    *   math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &V);
    *   math::Vector<double, 9> f = V.col(8);
    *
    *
    *[奇异性约束]
    *  基础矩阵F的一个重要的性质是F是奇异的，秩为2，因此有一个奇异值为0。通过上述直接线性法求得
    *  矩阵不具有奇异性约束。常用的方法是将求得得矩阵投影到满足奇异约束得空间中。
    *  具体地，对F进行奇异值分解
    *               F = USV'
    *  其中S是对角矩阵，S=diag[sigma1, sigma2, sigma3]
    *  将sigma3设置为0，并重构F
    *                       [sigma1, 0,     ,0]
    *                 F = U [  0   , sigma2 ,0] V'
    *                       [  0   , 0      ,0]
    */

#include <math/matrix_svd.h>
#include "math/matrix.h"
#include "math/vector.h"

typedef math::Matrix<double, 3, 3>  FundamentalMatrix;

FundamentalMatrix fundamental_8_point (math::Matrix<double, 3, 8> const& points1
                         , math::Matrix<double, 3, 8> const& points2
                        ){

    FundamentalMatrix F;
    /** TODO HERE
     * Coding Here!!
     */
    return F;
#if 0
    /* direct linear transform */
    math::Matrix<double, 8, 9> A;
    for(int i=0; i<8; i++)
    {
        math::Vec3d p1  = points1.col(i);
        math::Vec3d p2 = points2.col(i);

        A(i, 0) = p1[0]*p2[0];
        A(i, 1) = p1[1]*p2[0];
        A(i, 2) = p2[0];
        A(i, 3) = p1[0]*p2[1];
        A(i, 4) = p1[1]*p2[1];
        A(i, 5) = p2[1];
        A(i, 6) = p1[0];
        A(i, 7) = p1[1];
        A(i, 8) = 1.0;
    }

    math::Matrix<double, 9, 9> vv;
    math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &vv);
    math::Vector<double, 9> f = vv.col(8);

    FundamentalMatrix F;
    F(0,0) = f[0]; F(0,1) = f[1]; F(0,2) = f[2];
    F(1,0) = f[3]; F(1,1) = f[4]; F(1,2) = f[5];
    F(2,0) = f[6]; F(2,1) = f[7]; F(2,2) = f[8];

    /* singularity constraint */
    math::Matrix<double, 3, 3> U, S, V;
    math::matrix_svd(F, &U, &S, &V);
    S(2,2)=0;
    F = U*S*V.transpose();

    return F;
#endif

}

int main(int argc, char*argv[])
{

    // 第一幅图像中的对应点
    math::Matrix<double, 3, 8> pset1;
    pset1(0, 0) = 0.180123 ; pset1(1, 0)= -0.156584; pset1(2, 0)=1.0;
    pset1(0, 1) = 0.291429 ; pset1(1, 1)= 0.137662 ; pset1(2, 1)=1.0;
    pset1(0, 2) = -0.170373; pset1(1, 2)= 0.0779329; pset1(2, 2)=1.0;
    pset1(0, 3) = 0.235952 ; pset1(1, 3)= -0.164956; pset1(2, 3)=1.0;
    pset1(0, 4) = 0.142122 ; pset1(1, 4)= -0.216048; pset1(2, 4)=1.0;
    pset1(0, 5) = -0.463158; pset1(1, 5)= -0.132632; pset1(2, 5)=1.0;
    pset1(0, 6) = 0.0801864; pset1(1, 6)= 0.0236417; pset1(2, 6)=1.0;
    pset1(0, 7) = -0.179068; pset1(1, 7)= 0.0837119; pset1(2, 7)=1.0;
    //第二幅图像中的对应
    math::Matrix<double, 3, 8> pset2;
    pset2(0, 0) = 0.208264 ; pset2(1, 0)= -0.035405 ; pset2(2, 0) = 1.0;
    pset2(0, 1) = 0.314848 ; pset2(1, 1)=  0.267849 ; pset2(2, 1) = 1.0;
    pset2(0, 2) = -0.144499; pset2(1, 2)= 0.190208  ; pset2(2, 2) = 1.0;
    pset2(0, 3) = 0.264461 ; pset2(1, 3)= -0.0404422; pset2(2, 3) = 1.0;
    pset2(0, 4) = 0.171033 ; pset2(1, 4)= -0.0961747; pset2(2, 4) = 1.0;
    pset2(0, 5) = -0.427861; pset2(1, 5)= 0.00896567; pset2(2, 5) = 1.0;
    pset2(0, 6) = 0.105406 ; pset2(1, 6)= 0.140966  ; pset2(2, 6) = 1.0;
    pset2(0, 7) =  -0.15257; pset2(1, 7)= 0.19645   ; pset2(2, 7) = 1.0;

    FundamentalMatrix F = fundamental_8_point(pset1, pset2);


    std::cout<<"Fundamental matrix after singularity constraint is:\n "<<F<<std::endl;

    std::cout<<"Result should be: \n"<<"-0.0315082 -0.63238 0.16121\n"
                                     <<"0.653176 -0.0405703 0.21148\n"
                                     <<"-0.248026 -0.194965 -0.0234573\n" <<std::endl;

    return 0;
}

/*
 * Copyright (C) 2015, Simon Fuhrmann
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */
#include <cassert> //add


#include <iostream>
#include <fstream>
#include <sstream>
#include <set>
#include <util/system.h>
#include <sfm/ransac_fundamental.h>
#include "math/functions.h"
#include "sfm/fundamental.h"
#include "sfm/correspondence.h"
#include "math/matrix_svd.h"

typedef math::Matrix<double, 3, 3> FundamentalMatrix;


/**
 * \description 用于RANSAC采样成功所需要的采样次数
 * @param p -- 内点的概率
 * @param K --拟合模型需要的样本个数，对应基础矩阵num_samples=8
 * @param z  -- 预期的采样成功的概率
 *                          log(1-z)
 *       需要的采样次数 M = -----------
 *                          log(1-p^K)
 * Example: For p = 50%, z = 99%, n = 8: M = log(0.001) / log(0.99609) = 1176.
 * 需要采样1176次从而保证RANSAC的成功率不低于0.99.
 * @return
 */
int  calc_ransac_iterations (double p,
                           int K,
                           double z = 0.99){
    //add by myself
    double iteration_time=std::log(1.0-z)
            /std::log(1.0-std::pow(p,K));
    return int(iteration_time );

    /** TODO HERE
     * Coding here**/
    return 0;


    /** Reference
    double prob_all_good = math::fastpow(p, K);
    double num_iterations = std::log(1.0 - z)
                            / std::log(1.0 - prob_all_good);
    return static_cast<int>(math::round(num_iterations));
     */

}

/**
 * \description 给定基础矩阵和一对匹配点，计算匹配点的sampson 距离，用于判断匹配点是否是内点,
 * 计算公式如下：
 *              SD = (x'Fx)^2 / ( (Fx)_1^2 + (Fx)_2^2 + (x'F)_1^2 + (x'F)_2^2 )
 * @param F-- 基础矩阵
 * @param m-- 匹配对
 * @return
 */
double  calc_sampson_distance (FundamentalMatrix const& F, sfm::Correspondence2D2D const& m) {

    double p2_F_p1 = 0.0;
    p2_F_p1 += m.p2[0] * (m.p1[0] * F[0] + m.p1[1] * F[1] + F[2]);
    p2_F_p1 += m.p2[1] * (m.p1[0] * F[3] + m.p1[1] * F[4] + F[5]);
    p2_F_p1 +=     1.0 * (m.p1[0] * F[6] + m.p1[1] * F[7] + F[8]);
    p2_F_p1 *= p2_F_p1;

    double sum = 0.0;
    sum += math::fastpow(m.p1[0] * F[0] + m.p1[1] * F[1] + F[2], 2);
    sum += math::fastpow(m.p1[0] * F[3] + m.p1[1] * F[4] + F[5], 2);
    sum += math::fastpow(m.p2[0] * F[0] + m.p2[1] * F[3] + F[6], 2);
    sum += math::fastpow(m.p2[0] * F[1] + m.p2[1] * F[4] + F[7], 2);

    return p2_F_p1 / sum;
}
/**
 * \description 8点发估计相机基础矩阵
 * @param pset1 -- 第一个视角的特征点
 * @param pset2 -- 第二个视角的特征点
 * @return 估计的基础矩阵
 */
void calc_fundamental_8_point (math::Matrix<double, 3, 8> const& pset1
        , math::Matrix<double, 3, 8> const& pset2
        ,FundamentalMatrix &F
){
    /* direct linear transform */
    math::Matrix<double, 8, 9> A;
    for(int i=0; i<8; i++)
    {
        math::Vec3d p1  = pset1.col(i);
        math::Vec3d p2 = pset2.col(i);

        A(i, 0) = p1[0]*p2[0];
        A(i, 1) = p1[1]*p2[0];
        A(i, 2) = p2[0];
        A(i, 3) = p1[0]*p2[1];
        A(i, 4) = p1[1]*p2[1];
        A(i, 5) = p2[1];
        A(i, 6) = p1[0];
        A(i, 7) = p1[1];
        A(i, 8) = 1.0;
    }

    math::Matrix<double, 9, 9> vv;
    math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &vv);
    math::Vector<double, 9> f = vv.col(8);

    F(0,0) = f[0]; F(0,1) = f[1]; F(0,2) = f[2];
    F(1,0) = f[3]; F(1,1) = f[4]; F(1,2) = f[5];
    F(2,0) = f[6]; F(2,1) = f[7]; F(2,2) = f[8];

    /* singularity constraint */
    math::Matrix<double, 3, 3> U, S, V;
    math::matrix_svd(F, &U, &S, &V);
    S(2,2)=0;
    F = U*S*V.transpose();

}

/**
 * \description 利用最小二乘法计算基础矩阵
 * @param matches--输入的匹配对 大于8对
 * @param F --基础矩阵
 */
void calc_fundamental_least_squares(sfm::Correspondences2D2D const & matches, FundamentalMatrix&F){

    if (matches.size() < 8)
        throw std::invalid_argument("At least 8 points required");
    /* Create Nx9 matrix A. Each correspondence creates on row in A. */
    std::vector<double> A(matches.size() * 9);
    for (std::size_t i = 0; i < matches.size(); ++i)
    {
        sfm::Correspondence2D2D const& p = matches[i];
        A[i * 9 + 0] = p.p2[0] * p.p1[0];
        A[i * 9 + 1] = p.p2[0] * p.p1[1];
        A[i * 9 + 2] = p.p2[0] * 1.0;
        A[i * 9 + 3] = p.p2[1] * p.p1[0];
        A[i * 9 + 4] = p.p2[1] * p.p1[1];
        A[i * 9 + 5] = p.p2[1] * 1.0;
        A[i * 9 + 6] = 1.0     * p.p1[0];
        A[i * 9 + 7] = 1.0     * p.p1[1];
        A[i * 9 + 8] = 1.0     * 1.0;
    }

    /* Compute fundamental matrix using SVD. */
    std::vector<double> vv(9 * 9);
    math::matrix_svd<double>(&A[0], matches.size(), 9, nullptr, nullptr, &vv[0]);

    /* Use last column of V as solution. */
    for (int i = 0; i < 9; ++i)
        F[i] = vv[i * 9 + 8];

    /* singularity constraint */
    math::Matrix<double, 3, 3> U, S, V;
    math::matrix_svd(F, &U, &S, &V);
    S(2,2)=0;
    F = U*S*V.transpose();
}
/**
 * \description 给定匹配对和基础矩阵，计算内点的个数
 * @param matches
 * @param F
 * @return
 */
std::vector<int> find_inliers(sfm::Correspondences2D2D const & matches
    ,FundamentalMatrix const & F, const double & thresh){
    const double squared_thresh = thresh* thresh;

    std::vector<int> inliers;
    for(int i=0; i< matches.size(); i++){
        if(calc_sampson_distance(F,matches[i])<squared_thresh){
            inliers.push_back(i);
        }
    }
    /**
     * TODO HERE
     *
     * Coding here **/

    /** Reference
    for(int i=0; i< matches.size(); i++){
        double error = calc_sampson_distance(F, matches[i]);
        if(error< squared_thresh){
            inliers.push_back(i);
        }
    }
     **/
    return inliers;
}



int main(int argc, char *argv[]){

    /** 加载归一化后的匹配对 */
    sfm::Correspondences2D2D corr_all;
    std::ifstream in("./examples/task1/correspondences.txt");
    assert(in.is_open());

    std::string line, word;
    int n_line = 0;
    while(getline(in, line)){

        std::stringstream stream(line);
        if(n_line==0){
            int n_corrs = 0;
            stream>> n_corrs;
            corr_all.resize(n_corrs);

            n_line ++;
            continue;
        }
        if(n_line>0){

            stream>>corr_all[n_line-1].p1[0]>>corr_all[n_line-1].p1[1]
                  >>corr_all[n_line-1].p2[0]>>corr_all[n_line-1].p2[1];
        }
        n_line++;
    }

    /* 计算采用次数 */
    const float inlier_ratio =0.5;
    const int n_samples=8;
    int n_iterations = calc_ransac_iterations(inlier_ratio, n_samples);

    // 用于判读匹配对是否为内点
    const double inlier_thresh = 0.0015;

    // ransac 最终估计的内点
    std::vector<int> best_inliers;

    std::cout << "RANSAC-F: Running for " << n_iterations
              << " iterations, threshold " << inlier_thresh
              << "..." << std::endl;
    for(int i=0; i<n_iterations; i++){

        /* 1.0 随机找到8对不重复的匹配点 */
        std::set<int> indices;
        while(indices.size()<8){
            indices.insert(util::system::rand_int() % corr_all.size());
        }

        math::Matrix<double, 3, 8> pset1, pset2;
        std::set<int>::const_iterator iter = indices.cbegin();
        for(int j=0; j<8; j++, iter++){
            sfm::Correspondence2D2D const & match = corr_all[*iter];

            pset1(0, j) = match.p1[0];
            pset1(1, j) = match.p1[1];
            pset1(2, j) = 1.0;

            pset2(0, j) = match.p2[0];
            pset2(1, j) = match.p2[1];
            pset2(2, j) = 1.0;
        }

        /*2.0 8点法估计相机基础矩阵*/
        FundamentalMatrix F;
        calc_fundamental_8_point(pset1, pset2,F);

        /*3.0 统计所有的内点个数*/
        std::vector<int> inlier_indices = find_inliers(corr_all, F, inlier_thresh);

        if(inlier_indices.size()> best_inliers.size()){

//            std::cout << "RANSAC-F: Iteration " << i
//                      << ", inliers " << inlier_indices.size() << " ("
//                      << (100.0 * inlier_indices.size() / corr_all.size())
//                      << "%)" << std::endl;
            best_inliers.swap(inlier_indices);
        }
    }

    sfm::Correspondences2D2D corr_f;
    for(int i=0; i< best_inliers.size(); i++){
        corr_f.push_back(corr_all[best_inliers[i]]);
    }

    /*利用所有的内点进行最小二乘估计*/
    FundamentalMatrix F;
    calc_fundamental_least_squares(corr_f, F);

    std::cout<<"inlier number: "<< best_inliers.size()<<std::endl;
    std::cout<<"F\n: "<< F<<std::endl;

    std::cout<<"result should be: \n"
             <<"inliner number: 272\n"
             <<"F: \n"
             <<"-0.00961384 -0.0309071 0.703297\n"
             <<"0.0448265 -0.00158655 -0.0555796\n"
             <<"-0.703477 0.0648517 -0.0117791\n";

    return 0;
}

//
// Created by caoqi on 2018/9/5.
//

#include <math/matrix.h>
#include <math/matrix_svd.h>

typedef math::Matrix<double, 3, 3> FundamentalMatrix;
typedef math::Matrix<double, 3, 3> EssentialMatrix;

//用于测试相机姿态的正确性
math::Vec2d p1={0.18012331426143646, -0.15658402442932129};
math::Vec2d p2={0.2082643061876297, -0.035404585301876068};
/*第一个相机的内外参数*/
double f1 = 0.972222208;
/*第二个相机的内外参数*/
double f2 = 0.972222208;


/**
 * \description 对匹配点进行三角化得到空间三维点
 * @param p1 -- 第一幅图像中的特征点
 * @param p2 -- 第二幅图像中的特征点
 * @param K1 -- 第一幅图像的内参数矩阵
 * @param R1 -- 第一幅图像的旋转矩阵
 * @param t1 -- 第一幅图像的平移向量
 * @param K2 -- 第二幅图像的内参数矩阵
 * @param R2 -- 第二幅图像的旋转矩阵
 * @param t2 -- 第二幅图像的平移向量
 * @return 三维点
 */
math::Vec3d triangulation(math::Vec2d const & p1
        , math::Vec2d const & p2
        , math::Matrix3d const &K1
        , math::Matrix3d const &R1
        , math::Vec3d const & t1
        , math::Matrix3d const &K2
        , math::Matrix3d const &R2
        , math::Vec3d const& t2){


    // 构造投影矩阵
    math::Matrix<double, 3, 4>P1, P2;

    math::Matrix<double, 3, 3> KR1 = K1 * R1;
    math::Matrix<double, 3, 1> Kt1(*(K1 * t1));
    P1 = KR1.hstack(Kt1);

    math::Matrix<double, 3, 3> KR2 = K2 * R2;
    math::Matrix<double, 3, 1> Kt2(*(K2 * t2));
    P2 = KR2.hstack(Kt2);

    std::cout<<"P1: "<<P1<<std::endl;
    std::cout<<"P1 for fist pose should be\n"
             <<"0.972222 0 0 0\n"
             <<"0 0.972222 0 0\n"
             <<"0 0 1 0\n";

    std::cout<<"P2: "<<P2<<std::endl;
    std::cout<<"P2 for fist pose should be\n"
             <<" -0.957966 0.165734 -0.00707496 0.0774496\n"
             <<"0.164089 0.952816 0.102143 0.967341\n"
             <<"0.0250416 0.102292 -0.994439 0.0605768\n";

    /* 构造A矩阵 */
    math::Matrix<double, 4, 4> A;
    // 对A的每一列分别进行赋值
    for(int i=0; i<4; i++){
        // 第1个点
        A(0, i) = p1[0]*P1(2, i) - P1(0, i);
        A(1, i) = p1[1]*P1(2, i) - P1(1, i);

        // 第2个点
        A(2, i) = p2[0]*P2(2, i) - P2(0, i);
        A(3, i) = p2[1]*P2(2, i) - P2(1, i);
    }

    std::cout<<"A: "<<std::endl;
    std::cout<<"A for first pose should be:\n"
             <<"-0.972222 0 0.180123 0\n"
             <<"-0 -0.972222 -0.156584 -0\n"
             <<"0.963181 -0.14443 -0.200031 -0.0648336\n"
               <<"-0.164975 -0.956437 -0.0669352 -0.969486\n";

    math::Matrix<double, 4, 4> V;
    math::matrix_svd<double, 4, 4> (A, nullptr, nullptr, &V);
    math::Vec3d X;
    X[0] = V(0, 3)/V(3, 3);
    X[1] = V(1, 3)/V(3, 3);
    X[2] = V(2, 3)/V(3, 3);

    std::cout<<"X for first pose should be:\n"
             <<"3.2043116948585566 -2.7710180887818652 17.195578538234088\n";
    return X;
}
/**
 * \description 判断相机姿态是否正确，方法是计算三维点在两个相机中的坐标，要求其z坐标大于0，即
 * 三维点同时位于两个相机的前方
 * @param match
 * @param pose1
 * @param pose2
 * @return
 */
bool  is_correct_pose (math::Matrix3d const &R1, math::Vec3d const & t1
                   ,math::Matrix3d const &R2, math::Vec3d const & t2) {

    /* 相机内参矩阵 */
    math::Matrix3d K1(0.0), K2(0.0);
    K1(0, 0) = K1(1, 1) = f1;
    K2(0, 0) = K2(1, 1) = f2;
    K1(2,2) = 1.0;
    K2(2,2) = 1.0;

    math::Vec3d p3d = triangulation(p1, p2, K1, R1, t1, K2, R2, t2);
    math::Vector<double, 3> x1 = R1 * p3d + t1;
    math::Vector<double, 3> x2 = R2 * p3d + t2;
    return x1[2] > 0.0f && x2[2] > 0.0f;
}

bool calc_cam_poses(FundamentalMatrix const &F
                   , const double f1, const double f2
                   , math::Matrix3d& R
                   , math::Vec3d & t)
{
    /* 相机内参矩阵 */
    math::Matrix3d K1(0.0), K2(0.0);
    K1(0, 0) = K1(1, 1) = f1; K1(2,2)=1.0;
    K2(0, 0) = K2(1, 1) = f2; K2(2,2) =1.0;

    /**  TODO BERE
     * 计算本质矩阵E*/
    EssentialMatrix E;

    /** Reference
     *  EssentialMatrix E = K2.transpose() * F * K1;
     */


    std::cout<<"EssentialMatrix result is "<<E<<std::endl;
    std::cout<<"EssentialMatrix should be: \n"
             <<"-0.00490744 -0.0146139 0.34281\n"
             <<"0.0212215 -0.000748851 -0.0271105\n"
             <<"-0.342111 0.0315182 -0.00552454\n";

    /* 本质矩阵求解的是相机之间的相对姿态，第一个相机姿态可以设置为[I|0], 第二个相机的姿态[R|t]
     * 可以通过对本质矩阵进行分解来求得, E=U*S*V',其中S是进行尺度归一化之后是diag(1,1,0)
     */

    math::Matrix<double, 3, 3> W(0.0);
    W(0, 1) = -1.0; W(1, 0) = 1.0; W(2, 2) = 1.0;
    math::Matrix<double, 3, 3> Wt(0.0);
    Wt(0, 1) = 1.0; Wt(1, 0) = -1.0; Wt(2, 2) = 1.0;

    math::Matrix<double, 3, 3> U, S, V;
    math::matrix_svd(E, &U, &S, &V);

    // 保证旋转矩阵 det(R) = 1 (instead of -1).
    if (math::matrix_determinant(U) < 0.0)
        for (int i = 0; i < 3; ++i)
            U(i,2) = -U(i,2);
    if (math::matrix_determinant(V) < 0.0)
        for (int i = 0; i < 3; ++i)
            V(i,2) = -V(i,2);


    /* 相机的姿态一共有4中情况*/
    V.transpose();
    std::vector<std::pair<math::Matrix3d, math::Vec3d> > poses(4);
    poses[0].first = U * W * V;
    poses[1].first = U * W * V;
    poses[2].first = U * Wt * V;
    poses[3].first = U * Wt * V;
    poses[0].second = U.col(2);
    poses[1].second = -U.col(2);
    poses[2].second = U.col(2);
    poses[3].second = -U.col(2);

    std::cout<<"Result of 4 candidate camera poses shoule be \n"
    <<"R0:\n"
      <<"-0.985336 0.170469 -0.0072771\n"
     <<"0.168777 0.980039 0.105061\n"
     <<"0.0250416 0.102292 -0.994439\n"
     <<"t0:\n"
     <<" 0.0796625 0.99498 0.0605768\n"
     <<"R1: \n"
     <<"-0.985336 0.170469 -0.0072771\n"
     <<"0.168777 0.980039 0.105061\n"
     <<"0.0250416 0.102292 -0.994439\n"
     <<"t1:\n"
     <<"-0.0796625 -0.99498 -0.0605768\n"
     <<"R2: \n"
     <<"0.999827 -0.0119578 0.0142419\n"
     <<"0.0122145 0.999762 -0.0180719\n"
     <<"-0.0140224 0.0182427 0.999735\n"
     <<"t2:\n"
     <<"0.0796625 0.99498 0.0605768\n"
     <<"R3: \n"
     <<"0.999827 -0.0119578 0.0142419\n"
     <<"0.0122145 0.999762 -0.0180719\n"
     <<"-0.0140224 0.0182427 0.999735\n"
     <<"t3: \n"
     <<"-0.0796625 -0.99498 -0.0605768\n";

    // 第一个相机的旋转矩阵R1设置为单位矩阵，平移向量t1设置为0
    math::Matrix3d R1;
    math::matrix_set_identity(&R1);
    math::Vec3d t1;
    t1.fill(0.0);

    // 判断姿态是否合理
    bool flags[4];
    for(int i=0; i<4; i++){
        flags[i] = is_correct_pose(R1, t1, poses[i].first, poses[i].second);
    }
    //找到正确的姿态
    if(flags[0]||flags[1]||flags[2]||flags[3]){
        for(int i=0; i<4; i++) {
            if(!flags[i])continue;
            R = poses[i].first;
            t =  poses[i].second;
        }
        return true;
    }
    return false;
}


int main(int argc, char* argv[]){

    FundamentalMatrix F;
    F[0] = -0.0051918668202215884;
    F[1] = -0.015460923969578466;
    F[2] = 0.35260470328319654;
    F[3] = 0.022451443619913483;
    F[4] = -0.00079225386526248181;
    F[5] = -0.027885130552744289;
    F[6] = -0.35188558059920161;
    F[7] = 0.032418724757766811;
    F[8] = -0.005524537443406155;


    math::Matrix3d R;
    math::Vec3d t;
    if(calc_cam_poses(F, f1,f2,R, t)){
        std::cout<<"Correct pose found!"<<std::endl;
        std::cout<<"R: "<<R<<std::endl;
        std::cout<<"t: "<<t<<std::endl;
    }

    std::cout<<"Result should be: \n";
    std::cout<<"R: \n"
             << "0.999827 -0.0119578 0.0142419\n"
             << "0.0122145 0.999762 -0.0180719\n"
             << "-0.0140224 0.0182427 0.999735\n";
    std::cout<<"t: \n"
             <<"0.0796625 0.99498 0.0605768\n";


    return 0;
}

/*
 * Copyright (C) 2015, Simon Fuhrmann
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */

#include <iostream>
#include <fstream>

#include "util/aligned_memory.h"
#include "util/timer.h"
#include "core/image.h"
#include "core/image_tools.h"
#include "core/image_io.h"

#include "features/surf.h"
#include "features/sift.h"
#include "features/matching.h"
#include "sfm/feature_set.h"
#include "visualizer.h"


core::ByteImage::Ptr
visualize_matching (features::Matching::Result const& matching,
    core::ByteImage::Ptr image1, core::ByteImage::Ptr image2,
    std::vector<math::Vec2f> const& pos1, std::vector<math::Vec2f> const& pos2)
{
    /* Visualize keypoints. */
    sfm::Correspondences2D2D vis_matches;
    for (std::size_t i = 0; i < matching.matches_1_2.size(); ++i)
    {
        if (matching.matches_1_2[i] < 0)
            continue;
        int const j = matching.matches_1_2[i];

        sfm::Correspondence2D2D match;
        std::copy(pos1[i].begin(), pos1[i].end(), match.p1);
        std::copy(pos2[j].begin(), pos2[j].end(), match.p2);
        vis_matches.push_back(match);
    }

    std::cout << "Drawing " << vis_matches.size() << " matches..." << std::endl;
    core::ByteImage::Ptr match_image = sfm::Visualizer::draw_matches
        (image1, image2, vis_matches);
    return match_image;
}

#define DISCRETIZE_DESCRIPTORS 0
template <typename T>
void
convert_sift_descriptors(features::Sift::Descriptors const& sift_descr,
    util::AlignedMemory<math::Vector<T, 128> >* aligned_descr)
{
    aligned_descr->resize(sift_descr.size());
    T* data_ptr = aligned_descr->data()->begin();
    for (std::size_t i = 0; i < sift_descr.size(); ++i, data_ptr += 128)
    {
        sfm::Sift::Descriptor const& d = sift_descr[i];
#if DISCRETIZE_DESCRIPTORS
        for (int j = 0; j < 128; ++j)
        {
            float value = d.data[j];
            value = math::clamp(value, 0.0f, 1.0f);
            value = math::round(value * 255.0f);
            data_ptr[j] = static_cast<unsigned char>(value);
        }
#else
        std::copy(d.data.begin(), d.data.end(), data_ptr);
#endif
    }
}

template <typename T>
void
convert_surf_descriptors(sfm::Surf::Descriptors const& surf_descr,
    util::AlignedMemory<math::Vector<T, 64> >* aligned_descr)
{
    aligned_descr->resize(surf_descr.size());
    T* data_ptr = aligned_descr->data()->begin();
    for (std::size_t i = 0; i < surf_descr.size(); ++i, data_ptr += 64)
    {
        sfm::Surf::Descriptor const& d = surf_descr[i];
#if DISCRETIZE_DESCRIPTORS
        for (int j = 0; j < 64; ++j)
        {
            float value = d.data[j];
            value = math::clamp(value, -1.0f, 1.0f);
            value = math::round(value * 127.0f);
            data_ptr[j] = static_cast<signed char>(value);
        }
#else
        std::copy(d.data.begin(), d.data.end(), data_ptr);
#endif
    }
}

void
feature_set_matching (core::ByteImage::Ptr image1, core::ByteImage::Ptr image2)
{
    /*FeatureSet 计算并存储一个视角的特征点，包含SIFT和SURF特征点 */
    sfm::FeatureSet::Options feature_set_opts;
    //feature_types设置为FEATURE_ALL表示检测SIFT和SURF两种特征点进行匹配
    feature_set_opts.feature_types = sfm::FeatureSet::FEATURE_ALL;
    feature_set_opts.sift_opts.verbose_output = true;
    //feature_set_opts.surf_opts.verbose_output = true;
    //feature_set_opts.surf_opts.contrast_threshold = 500.0f;

    // 计算第一幅图像的SIT和SURF特征点
    sfm::FeatureSet feat1(feature_set_opts);
    feat1.compute_features(image1);
    // 计算第二幅图像的SIFT和SURF特征点
    sfm::FeatureSet feat2(feature_set_opts);
    feat2.compute_features(image2);

    /* 对sift特征描述子进行匹配 */
    // sift 特征描述子匹配参数
    sfm::Matching::Options sift_matching_opts;
    sift_matching_opts.lowe_ratio_threshold = 0.8f;
    sift_matching_opts.descriptor_length = 128;
    sift_matching_opts.distance_threshold = std::numeric_limits<float>::max();

#if DISCRETIZE_DESCRIPTORS
    util::AlignedMemory<math::Vec128us, 16> sift_descr1, sift_descr2;
#else
    util::AlignedMemory<math::Vec128f, 16> sift_descr1, sift_descr2;
#endif
    // 将描述子转成特定的内存格式
    convert_sift_descriptors(feat1.sift_descriptors, &sift_descr1);
    convert_sift_descriptors(feat2.sift_descriptors, &sift_descr2);

    // 进行双向匹配
    sfm::Matching::Result sift_matching;
    sfm::Matching::twoway_match(sift_matching_opts,
        sift_descr1.data()->begin(), sift_descr1.size(),
        sift_descr2.data()->begin(), sift_descr2.size(),
        &sift_matching);

    // 去除不一致的匹配对，匹配对feature1和feature2是一致的需要满足，feature1的最近邻居
    // 是feature2，feature2的最近邻是feature1
    sfm::Matching::remove_inconsistent_matches(&sift_matching);
    std::cout << "Consistent Sift Matches: "
        << sfm::Matching::count_consistent_matches(sift_matching)
        << std::endl;


    /*  对surf特征描述子进行匹配  */
    // surf特征匹配参数
    sfm::Matching::Options surf_matching_opts;
    // 最近邻和次近邻的比
    surf_matching_opts.lowe_ratio_threshold = 0.7f;
    // 特征描述子的维度
    surf_matching_opts.descriptor_length = 64;
    surf_matching_opts.distance_threshold = std::numeric_limits<float>::max();

#if DISCRETIZE_DESCRIPTORS
    util::AlignedMemory<math::Vec64s, 16> surf_descr1, surf_descr2;
#else
    util::AlignedMemory<math::Vec64f, 16> surf_descr1, surf_descr2;
#endif
    // 将描述子转化成特殊的格式
    convert_surf_descriptors(feat1.surf_descriptors, &surf_descr1);
    convert_surf_descriptors(feat2.surf_descriptors, &surf_descr2);

    // 进行surf描述子的双向匹配
    sfm::Matching::Result surf_matching;
    sfm::Matching::twoway_match(surf_matching_opts,
        surf_descr1.data()->begin(), surf_descr1.size(),
        surf_descr2.data()->begin(), surf_descr2.size(),
        &surf_matching);
    // 去除不一致的匹配对，匹配对feature 1 和 feature 2 互为最近邻
    sfm::Matching::remove_inconsistent_matches(&surf_matching);
    std::cout << "Consistent Surf Matches: "
        << sfm::Matching::count_consistent_matches(surf_matching)
        << std::endl;

    // 对sift匹配的结果和surf匹配的结果进行融合
    sfm::Matching::Result matching;
    sfm::Matching::combine_results(sift_matching, surf_matching, &matching);

    std::cout << "Consistent Matches: "
        << sfm::Matching::count_consistent_matches(matching)
        << std::endl;

    /* 特征匹配可视化 */
    /* Draw features. */
    std::vector<sfm::Visualizer::Keypoint> features1;
    for (std::size_t i = 0; i < feat1.sift_descriptors.size(); ++i)
    {
        if (matching.matches_1_2[i] == -1)
            continue;

        sfm::Sift::Descriptor const& descr = feat1.sift_descriptors[i];
        sfm::Visualizer::Keypoint kp;
        kp.orientation = descr.orientation;
        kp.radius = descr.scale * 3.0f;
        kp.x = descr.x;
        kp.y = descr.y;
        features1.push_back(kp);
    }

    std::vector<sfm::Visualizer::Keypoint> features2;
    for (std::size_t i = 0; i < feat2.sift_descriptors.size(); ++i)
    {
        if (matching.matches_2_1[i] == -1)
            continue;

        sfm::Sift::Descriptor const& descr = feat2.sift_descriptors[i];
        sfm::Visualizer::Keypoint kp;
        kp.orientation = descr.orientation;
        kp.radius = descr.scale * 3.0f;
        kp.x = descr.x;
        kp.y = descr.y;
        features2.push_back(kp);
    }

    image1 = sfm::Visualizer::draw_keypoints(image1,
        features1, sfm::Visualizer::RADIUS_BOX_ORIENTATION);
    image2 = sfm::Visualizer::draw_keypoints(image2,
        features2, sfm::Visualizer::RADIUS_BOX_ORIENTATION);

    core::ByteImage::Ptr match_image = visualize_matching(
        matching, image1, image2, feat1.positions, feat2.positions);
    std::string output_filename = "./tmp/matching_featureset.png";
    std::cout << "Saving visualization to " << output_filename << std::endl;
    core::image::save_file(match_image, output_filename);
}

int
main (int argc, char** argv)
{
    if (argc < 3)
    {
        std::cerr << "Syntax: " << argv[0] << " image1 image2" << std::endl;
        return 1;
    }

 // 用于加速
#ifdef __SSE2__
    std::cout << "SSE2 is enabled!" << std::endl;
#endif
#ifdef __SSE3__
    std::cout << "SSE3 is enabled!" << std::endl;
#endif

    /* Regular two-view matching. */
    core::ByteImage::Ptr image1, image2;
    try
    {
        std::cout << "Loading " << argv[1] << "..." << std::endl;
        image1 = core::image::load_file(std::string(argv[1]));
        // 图像尺寸减半
        image1 = core::image::rescale_half_size<uint8_t>(image1);
        //image1 = core::image::rescale_half_size<uint8_t>(image1);
        //image1 = core::image::rotate<uint8_t>(image1, core::image::ROTATE_CCW);

        std::cout << "Loading " << argv[2] << "..." << std::endl;
        image2 = core::image::load_file(argv[2]);
        // 图像尺寸减半
        image2 = core::image::rescale_half_size<uint8_t>(image2);
        //image2 = core::image::rescale_half_size<uint8_t>(image2);
        //image2 = core::image::rotate<uint8_t>(image2, core::image::ROTATE_CCW);
    }
    catch (std::exception& e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }

    // 进行特征提取和特征匹配
    feature_set_matching(image1, image2);

    return 0;
}

/*
 * Copyright (C) 2015, Simon Fuhrmann
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */

#include <iostream>

#include "util/file_system.h"
#include "util/timer.h"
#include "core/image.h"
#include "core/image_tools.h"
#include "core/image_io.h"

#include "features/surf.h"
#include "features/sift.h"
#include "visualizer.h"

bool
sift_compare (features::Sift::Descriptor const& d1, features::Sift::Descriptor const& d2)
{
    return d1.scale > d2.scale;
}

int
main (int argc, char** argv)
{
    if (argc < 2)
    {
        std::cerr << "Syntax: " << argv[0] << " <image>" << std::endl;
        return 1;
    }

    /* 加载图像*/
    core::ByteImage::Ptr image;
    std::string image_filename = argv[1];
    try
    {
        std::cout << "Loading " << image_filename << "..." << std::endl;
        image = core::image::load_file(image_filename);
        //image = core::image::rescale_half_size<uint8_t>(image);
        //image = core::image::rescale_half_size<uint8_t>(image);
    }
    catch (std::exception& e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }


    /* SIFT 特征检测. */
    features::Sift::Descriptors sift_descr;
    features::Sift::Keypoints sift_keypoints;
    {
        features::Sift::Options sift_options;
        sift_options.verbose_output = true;
        sift_options.debug_output = true;
        features::Sift sift(sift_options);
        sift.set_image(image);

        util::WallTimer timer;
        sift.process(); // 主程序
        std::cout << "Computed SIFT features in "
                  << timer.get_elapsed() << "ms." << std::endl;

        sift_descr = sift.get_descriptors();
        sift_keypoints = sift.get_keypoints();
    }

    // 对特征点按照尺度进行排序
    std::sort(sift_descr.begin(), sift_descr.end(), sift_compare);

    std::vector<features::Visualizer::Keypoint> sift_drawing;
    for (std::size_t i = 0; i < sift_descr.size(); ++i)
    {
        features::Visualizer::Keypoint kp;
        kp.orientation = sift_descr[i].orientation;
        kp.radius = sift_descr[i].scale;
        kp.x = sift_descr[i].x;
        kp.y = sift_descr[i].y;
        sift_drawing.push_back(kp);
    }

    core::ByteImage::Ptr sift_image = features::Visualizer::draw_keypoints(image,
        sift_drawing, features::Visualizer::RADIUS_BOX_ORIENTATION);

    /* 保存图像文件名 */
    std::string sift_out_fname = "./tmp/" + util::fs::replace_extension
        (util::fs::basename(image_filename), "sift.png");
    std::cout << "保存图像: " << sift_out_fname << std::endl;
    core::image::save_file(sift_image, sift_out_fname);

    return 0;
}