相机模型（三维重建-------3【任务4】代码）

代码

1（判断相机姿态是否正确，方法是计算三维点在两个相机中的坐标，要求其z坐标大于0）

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;
}

2.通过相机内参和基础矩阵F得到本质矩阵

 /* 相机内参矩阵 */
    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;

    /* 计算本质矩阵E*/
    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";

3.本质矩阵求解特征向量

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

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

5.利用上面的判断进行姿态的选择

    // 第一个相机的旋转矩阵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;
        }