/** Compute fundamental matrix out of eight feature correspondences between the previous and current frame.
* @param std::vector<Match> vector with feature matches
* @param std::vector<int> vector with eight indices of the vector with matches
* @return Eigen::Matrix3f fundamental matrix */
Eigen::Matrix3f MonoOdometer5::getF(std::vector<Match> matches, std::vector<int> indices)
{
int N = indices.size();
// see Trucco & Verri, Introductory Techniques for 3D Computer Vision, chapter 6
Eigen::Matrix<float, Eigen::Dynamic, 9> A;
A.resize(N, 9);
for(int i=0; i<N; i++)
{
cv::Point2f pPrev = matches[indices[i]].pPrev_;
cv::Point2f pCurr = matches[indices[i]].pCurr_;
A(i, 0) = pCurr.x * pPrev.x;
A(i, 1) = pCurr.x * pPrev.y;
A(i, 2) = pCurr.x;
A(i, 3) = pCurr.y * pPrev.x;
A(i, 4) = pCurr.y * pPrev.y;
A(i, 5) = pCurr.y;
A(i, 6) = pPrev.x;
A(i, 7) = pPrev.y;
A(i, 8) = 1;
}
Eigen::JacobiSVD<Eigen::Matrix<float, Eigen::Dynamic, 9>> svdA(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f F;
F << svdA.matrixV()(0, 8), svdA.matrixV()(1, 8), svdA.matrixV()(2, 8), svdA.matrixV()(3, 8), svdA.matrixV()(4, 8), svdA.matrixV()(5, 8), svdA.matrixV()(6, 8), svdA.matrixV()(7, 8), svdA.matrixV()(8, 8);
// re-enforce rank 2 constraint on fundamental matrix
Eigen::JacobiSVD<Eigen::Matrix3f> svdF(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f D(Eigen::Matrix3f::Zero());
D(0, 0) = svdF.singularValues()(0);
D(1, 1) = svdF.singularValues()(1);
F = (svdF.matrixU()) * D * (svdF.matrixV()).transpose();
return F;
}
matf GetFundamentalMat8Points(const vector<TrackedPoint> & points,
const vector<size_t> & sample) {
size_t n = sample.size();
matf A(max(n, (size_t)9), 9);
for (size_t i = 0; i < n; ++i) {
const TrackedPoint & p = points[sample[i]];
A(i, 0) = p.x1 * p.x2;
A(i, 1) = p.y1 * p.x2;
A(i, 2) = p.x2;
A(i, 3) = p.x1 * p.y2;
A(i, 4) = p.y1 * p.y2;
A(i, 5) = p.y2;
A(i, 6) = p.x1;
A(i, 7) = p.y1;
A(i, 8) = 1.0f;
}
for (size_t i = n; i < 9; ++i)
for (size_t j = 0; j < 9; ++j)
A(i, j) = 0.0f;
SVD svd(A);
matf F(3, 3);
F(0,0) = svd.vt.at<float>(8, 0);
F(0,1) = svd.vt.at<float>(8, 1);
F(0,2) = svd.vt.at<float>(8, 2);
F(1,0) = svd.vt.at<float>(8, 3);
F(1,1) = svd.vt.at<float>(8, 4);
F(1,2) = svd.vt.at<float>(8, 5);
F(2,0) = svd.vt.at<float>(8, 6);
F(2,1) = svd.vt.at<float>(8, 7);
F(2,2) = svd.vt.at<float>(8, 8);
// make F singular
SVD svdF(F);
svdF.w.at<float>(2, 0) = 0.0f;
return svdF.u * Mat::diag(svdF.w) * svdF.vt;
}