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