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