289 lines
9.6 KiB
C++
289 lines
9.6 KiB
C++
//
|
|
// Created by root on 5/24/21.
|
|
//
|
|
/*
|
|
* homography.cpp
|
|
* Adaptation of PTAM-GPL HomographyInit class.
|
|
* https://github.com/Oxford-PTAM/PTAM-GPL
|
|
* Licence: GPLv3
|
|
* Copyright 2008 Isis Innovation Limited
|
|
*
|
|
* Created on: Sep 2, 2012
|
|
* by: cforster
|
|
*/
|
|
|
|
#include <vio/homography.h>
|
|
#include <opencv2/opencv.hpp>
|
|
#include <stdio.h>
|
|
|
|
namespace vk {
|
|
|
|
Homography::
|
|
Homography(const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
|
|
const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
|
|
const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts1,
|
|
const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts2,
|
|
double _error_multiplier2,
|
|
double _thresh_in_px) :
|
|
thresh(_thresh_in_px),
|
|
error_multiplier2(_error_multiplier2),
|
|
fts_c1(_fts1),
|
|
fts_c2(_fts2),
|
|
pfts_c1(_pfts1),
|
|
pfts_c2(_pfts2)
|
|
{
|
|
}
|
|
|
|
void Homography::
|
|
calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1)
|
|
{
|
|
double d = n_c1.dot(xyz_c1); // normal distance from plane to KF
|
|
H_c2_from_c1 = T_c2_from_c1.rotation_matrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d;
|
|
}
|
|
|
|
void Homography::
|
|
calcFromMatches()
|
|
{
|
|
vector<cv::Point2f> src_pts(fts_c1.size()), dst_pts(fts_c1.size());
|
|
for(size_t i=0; i<fts_c1.size(); ++i)
|
|
{
|
|
src_pts[i] = cv::Point2f(fts_c1[i][0], fts_c1[i][1]);//refrance
|
|
dst_pts[i] = cv::Point2f(fts_c2[i][0], fts_c2[i][1]);//curent
|
|
}
|
|
|
|
// TODO: replace this function to remove dependency from opencv!
|
|
// cv::Mat cvH = cv::findHomography(src_pts, dst_pts, CV_RANSAC, 2./error_multiplier2);
|
|
cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC,2./error_multiplier2 );//2./error_multiplier2
|
|
H_c2_from_c1(0,0) = cvH.at<double>(0,0);
|
|
H_c2_from_c1(0,1) = cvH.at<double>(0,1);
|
|
H_c2_from_c1(0,2) = cvH.at<double>(0,2);
|
|
H_c2_from_c1(1,0) = cvH.at<double>(1,0);
|
|
H_c2_from_c1(1,1) = cvH.at<double>(1,1);
|
|
H_c2_from_c1(1,2) = cvH.at<double>(1,2);
|
|
H_c2_from_c1(2,0) = cvH.at<double>(2,0);
|
|
H_c2_from_c1(2,1) = cvH.at<double>(2,1);
|
|
H_c2_from_c1(2,2) = cvH.at<double>(2,2);
|
|
}
|
|
|
|
size_t Homography::
|
|
computeMatchesInliers()
|
|
{
|
|
inliers.clear(); inliers.resize(fts_c1.size());
|
|
size_t n_inliers = 0;
|
|
for(size_t i=0; i<fts_c1.size(); i++)
|
|
{
|
|
Vector3d e = pfts_c2[i] - (H_c2_from_c1 * pfts_c1[i]);
|
|
inliers[i] = (e.norm() < 15.0);
|
|
n_inliers += inliers[i];
|
|
}
|
|
return n_inliers;
|
|
|
|
}
|
|
|
|
bool Homography::
|
|
computeSE3fromMatches()
|
|
{
|
|
calcFromMatches();
|
|
bool res = decompose();
|
|
if(!res)
|
|
return false;
|
|
//computeMatchesInliers();
|
|
findBestDecomposition();
|
|
Eigen::Quaternion<double> q(decompositions.front().T.rotation_matrix());
|
|
q.vec() *= 1/q.norm();
|
|
q.w() *= 1/q.norm();
|
|
q.x()=0.0;
|
|
q.z()=0.0;
|
|
T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(), decompositions.front().T.translation().y(), decompositions.front().T.translation().z()));
|
|
return true;
|
|
}
|
|
|
|
bool Homography::
|
|
decompose()
|
|
{
|
|
decompositions.clear();
|
|
JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);
|
|
|
|
Vector3d singular_values = svd.singularValues();
|
|
|
|
double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
|
|
double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
|
|
double d3 = fabs(singular_values[2]);
|
|
|
|
Matrix3d U = svd.matrixU();
|
|
Matrix3d V = svd.matrixV(); // VT^T
|
|
double s = U.determinant() * V.determinant();
|
|
|
|
if(!(d1 != d2 && d2 != d3))
|
|
{
|
|
printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
|
|
return false;
|
|
}
|
|
|
|
double x1_PM;
|
|
double x3_PM;
|
|
|
|
// All below deals with the case = 1 case.
|
|
// Case 1 implies (d1 != d3)
|
|
{ // Eq. 12
|
|
x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
|
|
x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
|
|
};
|
|
|
|
double e1[4] = {1.0,-1.0, 1.0,-1.0};
|
|
double e3[4] = {1.0, 1.0,-1.0,-1.0};
|
|
|
|
Vector3d np;
|
|
HomographyDecomposition decomp;
|
|
|
|
// Case 1, d' > 0:
|
|
decomp.d = s * d2;
|
|
for(size_t signs=0; signs<4; signs++)
|
|
{
|
|
// Eq 13
|
|
decomp.R = Matrix3d::Identity();
|
|
double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
|
|
double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
|
|
/*
|
|
* correct the rotation matrix by Majid
|
|
*/
|
|
decomp.R(0,0) = dCosTheta; //1
|
|
decomp.R(0,2) = -dSinTheta; //0
|
|
decomp.R(2,0) = dSinTheta;
|
|
decomp.R(2,2) = dCosTheta;
|
|
|
|
// Eq 14
|
|
decomp.t[0] = 0.0;//(d1 - d3) * x1_PM * e1[signs];
|
|
decomp.t[1] = 0.0;
|
|
decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
|
|
|
|
np[0] = 0.0;//x1_PM * e1[signs];
|
|
np[1] = 0.0;
|
|
np[2] = x3_PM * e3[signs];
|
|
decomp.n = V * np;
|
|
|
|
decompositions.push_back(decomp);
|
|
}
|
|
|
|
// Case 1, d' < 0:
|
|
decomp.d = s * -d2;
|
|
for(size_t signs=0; signs<4; signs++)
|
|
{
|
|
// Eq 15
|
|
decomp.R = -1 * Matrix3d::Identity();
|
|
double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
|
|
double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
|
|
|
|
// correct the rotation matrix by Majid
|
|
|
|
decomp.R(0,0) = dCosPhi;
|
|
decomp.R(0,2) = dSinPhi;
|
|
decomp.R(2,0) = dSinPhi;
|
|
decomp.R(2,2) = -dCosPhi;
|
|
|
|
// Eq 16
|
|
decomp.t[0] = 0.0;//(d1 + d3) * x1_PM * e1[signs];
|
|
decomp.t[1] = 0.0;
|
|
decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
|
|
|
|
np[0] = 0.0;//x1_PM * e1[signs];
|
|
np[1] = 0.0;
|
|
np[2] = x3_PM * e3[signs];
|
|
decomp.n = V * np;
|
|
|
|
decompositions.push_back(decomp);
|
|
}
|
|
|
|
// Save rotation and translation of the decomposition
|
|
for(unsigned int i=0; i<decompositions.size(); i++)
|
|
{
|
|
Matrix3d R = s * U * decompositions[i].R * V.transpose();
|
|
Vector3d t = U * decompositions[i].t;
|
|
decompositions[i].T = Sophus::SE3(R, t);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool operator<(const HomographyDecomposition lhs, const HomographyDecomposition rhs)
|
|
{
|
|
return lhs.score < rhs.score;
|
|
}
|
|
|
|
void Homography::
|
|
findBestDecomposition()
|
|
{
|
|
assert(decompositions.size() == 8);
|
|
for(size_t i=0; i<decompositions.size(); i++)
|
|
{
|
|
HomographyDecomposition &decom = decompositions[i];
|
|
size_t nPositive = 0;
|
|
for(size_t m=0; m<fts_c1.size(); m++)
|
|
{
|
|
/* if(!inliers[m])
|
|
continue;*/
|
|
const Vector2d& v2 = fts_c1[m];
|
|
double dVisibilityTest = (H_c2_from_c1(2,0) * v2[0] + H_c2_from_c1(2,1) * v2[1] + H_c2_from_c1(2,2)) / decom.d;
|
|
if(dVisibilityTest > 0.0)
|
|
nPositive++;
|
|
}
|
|
decom.score = -nPositive;
|
|
}
|
|
|
|
sort(decompositions.begin(), decompositions.end());
|
|
decompositions.resize(4);
|
|
|
|
for(size_t i=0; i<decompositions.size(); i++)
|
|
{
|
|
HomographyDecomposition &decom = decompositions[i];
|
|
int nPositive = 0;
|
|
for(size_t m=0; m<fts_c1.size(); m++)
|
|
{
|
|
/* if(!inliers[m])
|
|
continue;*/
|
|
Vector3d v3 = pfts_c1[m];
|
|
double dVisibilityTest = v3.dot(decom.n) / decom.d;
|
|
if(dVisibilityTest > 0.0)
|
|
nPositive++;
|
|
};
|
|
decom.score = -nPositive;
|
|
}
|
|
|
|
sort(decompositions.begin(), decompositions.end());
|
|
decompositions.resize(2);
|
|
|
|
// According to Faugeras and Lustman, ambiguity exists if the two scores are equal
|
|
// but in practive, better to look at the ratio!
|
|
double dRatio = (double) decompositions[1].score / (double) decompositions[0].score;
|
|
|
|
if(dRatio < 0.9) // no ambiguity!
|
|
decompositions.erase(decompositions.begin() + 1);
|
|
else // two-way ambiguity. Resolve by sampsonus score of all points.
|
|
{
|
|
double dErrorSquaredLimit = thresh * thresh * 4;
|
|
double adSampsonusScores[2];
|
|
for(size_t i=0; i<2; i++)
|
|
{
|
|
Sophus::SE3 T = decompositions[i].T;
|
|
Matrix3d Essential = T.rotation_matrix() * sqew(T.translation());
|
|
double dSumError = 0;
|
|
for(size_t m=0; m < fts_c1.size(); m++ )
|
|
{
|
|
double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]);
|
|
if(d > dErrorSquaredLimit)
|
|
d = dErrorSquaredLimit;
|
|
dSumError += d;
|
|
}
|
|
adSampsonusScores[i] = dSumError;
|
|
}
|
|
|
|
if(adSampsonusScores[0] <= adSampsonusScores[1])
|
|
decompositions.erase(decompositions.begin() + 1);
|
|
else
|
|
decompositions.erase(decompositions.begin());
|
|
}
|
|
}
|
|
|
|
|
|
} /* end namespace vk */
|
|
|