利用RANSAC求解基础矩阵（三维重建task1-4）

利用RANSAC求解基础矩阵（三维重建task1-4）

代码呈上

#include <iostream>
#include <fstream>
#include <sstream>
#include<cassert>
#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){
   

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



}

/**
 * \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[