利用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[