/** 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;
}
Example #2
0
//---------------------------------------------------------------------------
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;
}