如何利用Kneip算法及基于RANSAC的Kneip算法求解P3P问题，获得相机姿态（三维重建task2-2）

如何利用Kneip算法及基于RANSAC的Kneip算法求解P3P问题（三维重建task2-2）

简答介绍一下P3P法：需要4对不共面的点 求出2D点在当前相机坐标系中的3D点，然后进行3D-3D的姿态求解。
直接线性变换法（已知三维点和对应二位点，求解相机内外参数，前一个博客有分析）
Kneip算法的介绍：
 P3p: 从3对3D-2D的对应点中确定相机的朝向和位置
 通常会产生4对解，需要用第4对匹配关系确定
 一般的求解思路是首先计算点在相机中的3D坐标，然后通过ICP的方式计算相机姿态
 Kneip是一种close-form的P3p求解方式，主要思想是引入相机和世界坐标系的中间坐标系
计算它们之间的相对姿态和位置来得到相机的姿态，Kneip的优势是求解稳定、速度快
Kneip算法的实现流程：
（1）创建新的相机坐标系
（2）创建新的世界坐标系
（3）相机中心在平面中的表达
（4）相机中心和姿态在新建世界坐标系中表达
（5）将第三个点从新建世界坐标系转换到新建相机坐标系中
（6）构建四次方程

 * POSIT (Pose from Orthography and Scaling with iterations), 比例正交
 * 投影迭代变换算法，适用条件是物体在Z轴方向的厚度远小于其在Z轴方向的平均深度。
 */
#include <complex>
#include <algorithm>
#include<set>
#include <iostream>

#include "math/matrix_tools.h"
#include "math/matrix.h"
#include "math/vector.h"
#include "sfm/correspondence.h"
#include "sfm/defines.h"
#include "util/system.h"

typedef math::Matrix<double, 3, 4> Pose;
typedef std::vector<Pose> PutativePoses;

/**
 *
 * @param factors
 * @param real_roots
 */
void solve_quartic_roots (math::Vec5d const& factors, math::Vec4d* real_roots)
{
   
    double const A = factors[0];
    double const B = factors[1];
    double const C = factors[2];
    double const D = factors[3];
    double const E = factors[4];

    double const A2 = A * A;
    double const B2 = B * B;
    double const A3 = A2 * A;
    double const B3 = B2 * B;
    double const A4 = A3 * A;
    double const B4 = B3 * B;

    double const alpha = -3.0 * B2 / (8.0 * A2) + C / A;
    double const beta = B3 / (8.0 * A3)- B * C / (2.0 * A2) + D / A;
    double const gamma = -3.0 * B4 / (256.0 * A4) + B2 * C / (16.0 * A3) - B * D / (4.0 * A2) + E / A;

    double const alpha2 = alpha * alpha;
    double const alpha3 = alpha2 * alpha;
    double const beta2 = beta * beta;

    std::complex<double> P(-alpha2 / 12.0 - gamma, 0.0);
    std::complex<double> Q(-alpha3 / 108.0 + alpha * gamma / 3.0 - beta2 / 8.0, 0.0);
    std::complex<double> R = -Q / 2.0 + std::sqrt(Q * Q / 4.0 + P * P * P / 27.0);

    std::complex<double> U = std::pow(R, 1.0 / 3.0);
    std::complex<double> y = (U.real() == 0.0)
                             ? -5.0 * alpha / 6.0 - std::pow(Q, 1.0 / 3.0)
                             : -5.0 * alpha / 6.0 - P / (3.0 * U) + U;

    std::complex<double> w = std::sqrt(alpha + 2.0 * y);
    std::complex<double> part1 = -B / (4.0 * A);
    std::complex<double> part2 = 3.0 * alpha + 2.0 * y;
    std::complex<double> part3 = 2.0 * beta / w;

    std::complex<double> complex_roots[4];
    complex_roots[0] = part1 + 0.5 * (w + std::sqrt(-(part2 + part3)));
    complex_roots[1] = part1 + 0.5 * (w - std::sqrt(-(part2 + part3)));
    complex_roots[2] = part1 + 0.5 * (-w + std::sqrt(-(part2 - part3)));
    complex_roots[3] = part1 + 0.5 * (-w - std::sqrt(-(part2 - part3)));

    for (int i = 0; i < 4; ++i)
        (*real_roots)[i