/** Get the inliers among all matches that comply with a given fundamental matrix.
* @param std::vector<Match> vector with feature matches
* @param Eigen::Matrix3f fundamental matrix
* @return std::vector<int> vector with indices of the inliers */
std::vector<int> MonoOdometer5::getInliers(std::vector<Match> matches, Eigen::Matrix3f F)
{
std::vector<int> inlierIndices;
for(int i=0; i<matches.size(); i++)
{
cv::Point2f pPrev = matches[i].pPrev_;
cv::Point2f pCurr = matches[i].pCurr_;
Eigen::Vector3f pPrevHomog, pCurrHomog;
pPrevHomog << pPrev.x, pPrev.y, 1.0;
pCurrHomog << pCurr.x, pCurr.y, 1.0;
// xCurr^T * F * xPrev
double x2tFx1 = pCurrHomog.transpose() * F * pPrevHomog;
// F * xPrev
Eigen::Vector3f Fx1 = F * pPrevHomog;
// F^T * xCurr
Eigen::Vector3f Ftx2 = F.transpose() * pCurrHomog;
// compute Sampson distance (distance to epipolar line)
double dSampson = (x2tFx1 * x2tFx1) / ((Fx1(0)*Fx1(0)) + (Fx1(1)*Fx1(1)) + (Ftx2(0)*Ftx2(0)) + (Ftx2(1)*Ftx2(1)));
if(dSampson < param_odometerInlierThreshold_)
{
inlierIndices.push_back(i);
}
}
return inlierIndices;
}
//---------------------------------------------------------------------------
double TfrmCalcParameter::bisection(double a, double b,double Wmax,double te, double t0)
{
double x = b;
double d = (a + b)/2.0;
while (fabs(x-d)/fabs(x) > 0.000000000001)
{
double valor1=Fx1(x,Wmax,te,t0);
double valor2=Fx1(a,Wmax,te,t0);
if (valor1 == 0)
break;
if (valor1 * valor2 < 0)
b = x;
else {
a = x;
}
d = x;
x = (a + b)/2.0;
}
return x;
}