/// Compute the robust resection of the 3D<->2D correspondences.
bool robustResection(
const std::pair<size_t,size_t> & imageSize,
const Mat & pt2D,
const Mat & pt3D,
std::vector<size_t> * pvec_inliers,
const Mat3 * K = NULL,
Mat34 * P = NULL,
double * maxError = NULL)
{
double dPrecision = std::numeric_limits<double>::infinity();
size_t MINIMUM_SAMPLES = 0;
// Classic resection
if (K == NULL)
{
typedef openMVG::resection::kernel::SixPointResectionSolver SolverType;
MINIMUM_SAMPLES = SolverType::MINIMUM_SAMPLES;
typedef ACKernelAdaptorResection<
SolverType, ResectionSquaredResidualError, UnnormalizerResection, Mat34>
KernelType;
KernelType kernel(pt2D, imageSize.first, imageSize.second, pt3D);
// Robustly estimation of the Projection matrix and it's precision
std::pair<double,double> ACRansacOut = ACRANSAC(kernel, *pvec_inliers,
ACRANSAC_ITER, P, dPrecision, true);
*maxError = ACRansacOut.first;
}
else
{
// If K is available use the Epnp solver
//typedef openMVG::euclidean_resection::kernel::EpnpSolver SolverType;
typedef openMVG::euclidean_resection::P3PSolver SolverType;
MINIMUM_SAMPLES = SolverType::MINIMUM_SAMPLES;
typedef ACKernelAdaptorResection_K<
SolverType, ResectionSquaredResidualError, UnnormalizerResection, Mat34> KernelType;
KernelType kernel(pt2D, pt3D, *K);
// Robustly estimation of the Projection matrix and it's precision
std::pair<double,double> ACRansacOut = ACRANSAC(kernel, *pvec_inliers,
ACRANSAC_ITER, P, dPrecision, true);
*maxError = ACRansacOut.first;
}
// Test if the found model is valid
if (pvec_inliers->size() > 2.5 * MINIMUM_SAMPLES)
{
// Re-estimate the model from the inlier data
// and/or LM to Refine f R|t
return true;
}
else{
P = NULL;
return false;
}
}
/// Estimate the essential matrix from point matches and K matrices.
bool robustEssential(
const Mat3 & K1, const Mat3 & K2,
const Mat & x1, const Mat & x2,
Mat3 * pE,
std::vector<size_t> * pvec_inliers,
const std::pair<size_t, size_t> & size_ima1,
const std::pair<size_t, size_t> & size_ima2,
double * errorMax,
double * NFA,
double precision = std::numeric_limits<double>::infinity())
{
assert(pvec_inliers != NULL);
assert(pE != NULL);
// Use the 5 point solver to estimate E
typedef openMVG::essential::kernel::FivePointKernel SolverType;
// Define the AContrario adaptor
typedef ACKernelAdaptorEssential<
SolverType,
openMVG::fundamental::kernel::EpipolarDistanceError,
UnnormalizerT,
Mat3>
KernelType;
KernelType kernel(x1, size_ima1.first, size_ima1.second,
x2, size_ima2.first, size_ima2.second, K1, K2);
// Robustly estimation of the Essential matrix and it's precision
std::pair<double,double> ACRansacOut = ACRANSAC(kernel, *pvec_inliers,
4096, pE, precision, true);
*errorMax = ACRansacOut.first;
*NFA = ACRansacOut.second;
return pvec_inliers->size() > 2.5 * SolverType::MINIMUM_SAMPLES;
}
