bool SolveProjectionFromF(const cv::Mat &F, cv::Mat &P1, cv::Mat &P2) {
P1 = cv::Mat::eye(3, 4, CV_64F);
P2 = cv::Mat::zeros(3, 4, CV_64F);
cv::Mat e2 = cv::Mat::zeros(3, 1, CV_64F);
cv::SVD::solveZ(F.t(), e2);
// TODO: Verify e2 is valid.
cv::Mat P33 = P2(cv::Rect(0, 0, 3, 3));
P33 = SkewSymmetricMatrix(e2) * F;
e2.copyTo(P2(cv::Rect(3, 0, 1, 3)));
std::cout << "Compute P from F...\nP1:\n" << P1 << "\nP2:\n" << P2
<< std::endl;
return true;
}
cv::Mat LocalMapping::ComputeF12(kfptr &pKF1, kfptr &pKF2) {
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
cv::Mat R12 = R1w * R2w.t();
cv::Mat t12 = -R1w * R2w.t() * t2w + t1w;
cv::Mat t12x = SkewSymmetricMatrix(t12);
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;
return K1.t().inv() * t12x * R12 * K2.inv();
}
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
cv::Mat R12 = R1w*R2w.t();
cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;
cv::Mat t12x = SkewSymmetricMatrix(t12);
cv::Mat K1 = pKF1->GetCalibrationMatrix();
cv::Mat K2 = pKF2->GetCalibrationMatrix();
return K1.t().inv()*t12x*R12*K2.inv();
}
