se2_l2_ransac.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "tfest-precomp.h"  // Precompiled headers

#include <mrpt/math/distributions.h>
#include <mrpt/math/geometry.h>
#include <mrpt/poses/CPoint2DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/random.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/tfest/se2.h>
#include <iostream>

using namespace mrpt;
using namespace mrpt::tfest;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

//#define AVOID_MULTIPLE_CORRESPONDENCES

// mark this pair as "selected" so it won't be picked again:
void markAsPicked(
    const TMatchingPair& c, std::vector<bool>& alreadySelectedThis,
    std::vector<bool>& alreadySelectedOther
#ifdef AVOID_MULTIPLE_CORRESPONDENCES
    ,
    const std::vector<std::vector<int>>& listDuplicatedLandmarksThis
#endif
)
{
    ASSERTDEB_(c.this_idx < alreadySelectedThis.size());
    ASSERTDEB_(c.other_idx < alreadySelectedOther.size());

#ifndef AVOID_MULTIPLE_CORRESPONDENCES
    alreadySelectedThis[c.this_idx] = true;
    alreadySelectedOther[c.other_idx] = true;
#else
    for (std::vector<int>::iterator it1 =
             listDuplicatedLandmarksThis[c.this_idx].begin();
         it1 != listDuplicatedLandmarksThis[c.this_idx].end(); it1++)
        alreadySelectedThis[*it1] = true;
    for (std::vector<int>::iterator it2 =
             listDuplicatedLandmarksOther[c.other_idx].begin();
         it2 != listDuplicatedLandmarksOther[c.other_idx].end(); it2++)
        alreadySelectedOther[*it2] = true;
#endif
}

/*---------------------------------------------------------------

                    robustRigidTransformation

  The technique was described in the paper:
    J.L. Blanco, J. Gonzalez-Jimenez and J.A. Fernandez-Madrigal.
    "A robust, multi-hypothesis approach to matching occupancy grid maps".
    Robotica, available on CJO2013. doi:10.1017/S0263574712000732.
    http://journals.cambridge.org/action/displayAbstract?aid=8815308

 This works as follows:
    - Repeat "results.ransac_iters" times:
        - Randomly pick TWO correspondences from the set "in_correspondences".
        - Compute the associated rigid transformation.
        - For "ransac_maxSetSize" randomly selected correspondences, test for
 "consensus" with the current group:
            - If if is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow
 the "consensus set"
            - If not, do not add it.
  ---------------------------------------------------------------*/
bool tfest::se2_l2_robust(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    const double normalizationStd, const TSE2RobustParams& params,
    TSE2RobustResult& results)
{
    //#define DO_PROFILING

#ifdef DO_PROFILING
    CTimeLogger timlog;
#endif

    const size_t nCorrs = in_correspondences.size();

    // Default: 2 * normalizationStd ("noise level")
    const double MAX_RMSE_TO_END =
        (params.max_rmse_to_end <= 0 ? 2 * normalizationStd
                                     : params.max_rmse_to_end);

    MRPT_START

    // Asserts:
    if (nCorrs < params.ransac_minSetSize)
    {
        // Nothing to do!
        results.transformation.clear();
        results.largestSubSet = TMatchingPairList();
        return false;
    }

#ifdef DO_PROFILING
    timlog.enter("ransac.find_max*");
#endif
    // Find the max. index of "this" and "other:
    unsigned int maxThis = 0, maxOther = 0;
    for (const auto& in_correspondence : in_correspondences)
    {
        maxThis = max(maxThis, in_correspondence.this_idx);
        maxOther = max(maxOther, in_correspondence.other_idx);
    }
#ifdef DO_PROFILING
    timlog.leave("ransac.find_max*");
#endif

#ifdef DO_PROFILING
    timlog.enter("ransac.count_unique_corrs");
#endif

    // Fill out 2 arrays indicating whether each element has a correspondence:
    std::vector<bool> hasCorrThis(maxThis + 1, false);
    std::vector<bool> hasCorrOther(maxOther + 1, false);
    unsigned int howManyDifCorrs = 0;
    for (const auto& in_correspondence : in_correspondences)
    {
        if (!hasCorrThis[in_correspondence.this_idx] &&
            !hasCorrOther[in_correspondence.other_idx])
        {
            hasCorrThis[in_correspondence.this_idx] = true;
            hasCorrOther[in_correspondence.other_idx] = true;
            howManyDifCorrs++;
        }
    }
#ifdef DO_PROFILING
    timlog.leave("ransac.count_unique_corrs");
#endif

    // Clear the set of output particles:
    results.transformation.clear();

    // If there are less different correspondences than the minimum required,
    // quit:
    if (howManyDifCorrs < params.ransac_minSetSize)
    {
        // Nothing we can do here!!! :~$
        results.transformation.clear();
        results.largestSubSet = TMatchingPairList();
        return false;
    }

#ifdef AVOID_MULTIPLE_CORRESPONDENCES
    unsigned k;
    // Find duplicated landmarks (from SIFT features with different
    // descriptors,etc...)
    //   this is to avoid establishing multiple correspondences for the same
    //   physical point!
    // ------------------------------------------------------------------------------------------------
    std::vector<std::vector<int>> listDuplicatedLandmarksThis(maxThis + 1);
    ASSERT_(nCorrs >= 1);
    for (k = 0; k < nCorrs - 1; k++)
    {
        std::vector<int> duplis;
        for (unsigned j = k; j < nCorrs - 1; j++)
        {
            if (in_correspondences[k].this_x == in_correspondences[j].this_x &&
                in_correspondences[k].this_y == in_correspondences[j].this_y &&
                in_correspondences[k].this_z == in_correspondences[j].this_z)
                duplis.push_back(in_correspondences[j].this_idx);
        }
        listDuplicatedLandmarksThis[in_correspondences[k].this_idx] = duplis;
    }

    std::vector<std::vector<int>> listDuplicatedLandmarksOther(maxOther + 1);
    for (k = 0; k < nCorrs - 1; k++)
    {
        std::vector<int> duplis;
        for (unsigned j = k; j < nCorrs - 1; j++)
        {
            if (in_correspondences[k].other_x ==
                    in_correspondences[j].other_x &&
                in_correspondences[k].other_y ==
                    in_correspondences[j].other_y &&
                in_correspondences[k].other_z == in_correspondences[j].other_z)
                duplis.push_back(in_correspondences[j].other_idx);
        }
        listDuplicatedLandmarksOther[in_correspondences[k].other_idx] = duplis;
    }
#endif

    std::deque<TMatchingPairList> alreadyAddedSubSets;

    CPosePDFGaussian referenceEstimation;
    CPoint2DPDFGaussian pt_this;

    const double ransac_consistency_test_chi2_quantile = 0.99;
    const double chi2_thres_dim1 =
        mrpt::math::chi2inv(ransac_consistency_test_chi2_quantile, 1);

    // -------------------------
    //      The RANSAC loop
    // -------------------------
    size_t largest_consensus_yet = 0;  // Used for dynamic # of steps
    double largestSubSet_RMSE = std::numeric_limits<double>::max();

    results.ransac_iters = params.ransac_nSimulations;
    const bool use_dynamic_iter_number = results.ransac_iters == 0;
    if (use_dynamic_iter_number)
    {
        ASSERT_(
            params.probability_find_good_model > 0 &&
            params.probability_find_good_model < 1);
        // Set an initial # of iterations:
        results.ransac_iters = 10;  // It doesn't matter actually, since will be
        // changed in the first loop
    }

    std::vector<bool> alreadySelectedThis, alreadySelectedOther;

    if (!params.ransac_algorithmForLandmarks)
    {
        alreadySelectedThis.assign(maxThis + 1, false);
        alreadySelectedOther.assign(maxOther + 1, false);
    }
    // else -> It will be done anyway inside the for() below

    // First: Build a permutation of the correspondences to pick from it
    // sequentially:
    std::vector<size_t> corrsIdxs(nCorrs), corrsIdxsPermutation;
    for (size_t i = 0; i < nCorrs; i++) corrsIdxs[i] = i;

    size_t iter_idx;
    for (iter_idx = 0; iter_idx < results.ransac_iters;
         iter_idx++)  // results.ransac_iters can be dynamic
    {
#ifdef DO_PROFILING
        CTimeLoggerEntry tle(timlog, "ransac.iter");
#endif

#ifdef DO_PROFILING
        timlog.enter("ransac.permute");
#endif
        getRandomGenerator().permuteVector(corrsIdxs, corrsIdxsPermutation);

#ifdef DO_PROFILING
        timlog.leave("ransac.permute");
#endif

        TMatchingPairList subSet;

        // Select a subset of correspondences at random:
        if (params.ransac_algorithmForLandmarks)
        {
#ifdef DO_PROFILING
            timlog.enter("ransac.reset_selection_marks");
#endif
            alreadySelectedThis.assign(maxThis + 1, false);
            alreadySelectedOther.assign(maxOther + 1, false);
#ifdef DO_PROFILING
            timlog.leave("ransac.reset_selection_marks");
#endif
        }
        else
        {
            // For points: Do not repeat the corrs, and take the number of corrs
            // as weights
        }

// Try to build a subsetof "ransac_maxSetSize" (maximum) elements that achieve
// consensus:
// ------------------------------------------------------------------------------------------
#ifdef DO_PROFILING
        timlog.enter("ransac.inner_loops");
#endif
        for (unsigned int j = 0;
             j < nCorrs && subSet.size() < params.ransac_maxSetSize; j++)
        {
            const size_t idx = corrsIdxsPermutation[j];

            const TMatchingPair& corr_j = in_correspondences[idx];

            // Don't pick the same features twice!
            if (alreadySelectedThis[corr_j.this_idx] ||
                alreadySelectedOther[corr_j.other_idx])
                continue;

            // Additional user-provided filter:
            if (params.user_individual_compat_callback)
            {
                mrpt::tfest::TPotentialMatch pm;
                pm.idx_this = corr_j.this_idx;
                pm.idx_other = corr_j.other_idx;
                if (!params.user_individual_compat_callback(pm))
                    continue;  // Skip this one!
            }

            if (subSet.size() < 2)
            {
                // ------------------------------------------------------------------------------------------------------
                // If we are within the first two correspondences, just add them
                // to the subset:
                // ------------------------------------------------------------------------------------------------------
                subSet.push_back(corr_j);
                markAsPicked(corr_j, alreadySelectedThis, alreadySelectedOther);

                if (subSet.size() == 2)
                {
                    // Consistency Test: From

                    // Check the feasibility of this pair "idx1"-"idx2":
                    //  The distance between the pair of points in MAP1 must be
                    //  very close
                    //   to that of their correspondences in MAP2:
                    const double corrs_dist1 =
                        mrpt::math::distanceBetweenPoints(
                            subSet[0].this_x, subSet[0].this_y,
                            subSet[1].this_x, subSet[1].this_y);

                    const double corrs_dist2 =
                        mrpt::math::distanceBetweenPoints(
                            subSet[0].other_x, subSet[0].other_y,
                            subSet[1].other_x, subSet[1].other_y);

                    // Is is a consistent possibility?
                    //  We use a chi2 test (see paper for the derivation)
                    const double corrs_dist_chi2 =
                        square(square(corrs_dist1) - square(corrs_dist2)) /
                        (8.0 * square(normalizationStd) *
                         (square(corrs_dist1) + square(corrs_dist2)));

                    bool is_acceptable = (corrs_dist_chi2 < chi2_thres_dim1);

                    if (is_acceptable)
                    {
                        // Perform estimation:
                        tfest::se2_l2(subSet, referenceEstimation);
                        // Normalized covariance: scale!
                        referenceEstimation.cov *= square(normalizationStd);

                        // Additional filter:
                        //  If the correspondences as such the transformation
                        //  has a high ambiguity, we discard it!
                        is_acceptable =
                            (referenceEstimation.cov(2, 2) <
                             square(DEG2RAD(5.0f)));
                    }

                    if (!is_acceptable)
                    {
                        // Remove this correspondence & try again with a
                        // different pair:
                        subSet.erase(subSet.begin() + (subSet.size() - 1));
                    }
                    else
                    {
                        // Only mark as picked if we're really keeping it:
                        markAsPicked(
                            corr_j, alreadySelectedThis, alreadySelectedOther);
                    }
                }
            }
            else
            {
#ifdef DO_PROFILING
                timlog.enter("ransac.test_consistency");
#endif

                // ------------------------------------------------------------------------------------------------------
                // The normal case:
                //  - test for "consensus" with the current group:
                //      - If it is compatible (ransac_maxErrorXY,
                // ransac_maxErrorPHI), grow the "consensus set"
                //      - If not, do not add it.
                // ------------------------------------------------------------------------------------------------------

                // Test for the mahalanobis distance between:
                //  "referenceEstimation (+) point_other" AND "point_this"
                referenceEstimation.composePoint(
                    mrpt::math::TPoint2D(corr_j.other_x, corr_j.other_y),
                    pt_this);

                const double maha_dist = pt_this.mahalanobisDistanceToPoint(
                    corr_j.this_x, corr_j.this_y);

                const bool passTest =
                    maha_dist < params.ransac_mahalanobisDistanceThreshold;

                if (passTest)
                {
                    // OK, consensus passed:
                    subSet.push_back(corr_j);
                    markAsPicked(
                        corr_j, alreadySelectedThis, alreadySelectedOther);
                }
                // else -> Test failed

#ifdef DO_PROFILING
                timlog.leave("ransac.test_consistency");
#endif
            }  // end else "normal case"

        }  // end for j
#ifdef DO_PROFILING
        timlog.leave("ransac.inner_loops");
#endif

        const bool has_to_eval_RMSE =
            (subSet.size() >= params.ransac_minSetSize);

        // Compute the RMSE of this matching and the corresponding
        // transformation (only if we'll use this value below)
        double this_subset_RMSE = 0;
        if (has_to_eval_RMSE)
        {
#ifdef DO_PROFILING
            CTimeLoggerEntry tle(timlog, "ransac.comp_rmse");
#endif

            // Recompute referenceEstimation from all the corrs:
            tfest::se2_l2(subSet, referenceEstimation);
            // Normalized covariance: scale!
            referenceEstimation.cov *= square(normalizationStd);

            for (size_t k = 0; k < subSet.size(); k++)
            {
                double gx, gy;
                referenceEstimation.mean.composePoint(
                    subSet[k].other_x, subSet[k].other_y, gx, gy);

                this_subset_RMSE +=
                    mrpt::math::distanceSqrBetweenPoints<double>(
                        subSet[k].this_x, subSet[k].this_y, gx, gy);
            }
            this_subset_RMSE /= std::max(static_cast<size_t>(1), subSet.size());
        }
        else
        {
            this_subset_RMSE = std::numeric_limits<double>::max();
        }

        // Save the estimation result as a "particle", only if the subSet
        // contains
        //  "ransac_minSetSize" elements at least:
        if (subSet.size() >= params.ransac_minSetSize)
        {
            // If this subset was previously added to the SOG, just increment
            // its weight
            //  and do not add a new mode:
            int indexFound = -1;

            // JLBC Added DEC-2007: An alternative (optional) method to fuse
            // Gaussian modes:
            if (!params.ransac_fuseByCorrsMatch)
            {
                // Find matching by approximate match in the X,Y,PHI means
                // -------------------------------------------------------------------
                for (size_t i = 0; i < results.transformation.size(); i++)
                {
                    double diffXY =
                        results.transformation.get(i).mean.distanceTo(
                            referenceEstimation.mean);
                    double diffPhi = fabs(math::wrapToPi(
                        results.transformation.get(i).mean.phi() -
                        referenceEstimation.mean.phi()));
                    if (diffXY < params.ransac_fuseMaxDiffXY &&
                        diffPhi < params.ransac_fuseMaxDiffPhi)
                    {
                        // printf("Match by distance found: distXY:%f distPhi=%f
                        // deg\n",diffXY,RAD2DEG(diffPhi));
                        indexFound = i;
                        break;
                    }
                }
            }
            else
            {
                // Find matching mode by exact match in the list of
                // correspondences:
                // -------------------------------------------------------------------
                // Sort "subSet" in order to compare them easily!
                // std::sort( subSet.begin(), subSet.end() );

                // Try to find matching corrs:
                for (size_t i = 0; i < alreadyAddedSubSets.size(); i++)
                {
                    if (subSet == alreadyAddedSubSets[i])
                    {
                        indexFound = i;
                        break;
                    }
                }
            }

            if (indexFound != -1)
            {
                // This is an already added mode:
                if (params.ransac_algorithmForLandmarks)
                    results.transformation.get(indexFound).log_w = log(
                        1 + exp(results.transformation.get(indexFound).log_w));
                else
                    results.transformation.get(indexFound).log_w =
                        log(subSet.size() +
                            exp(results.transformation.get(indexFound).log_w));
            }
            else
            {
                // Add a new mode to the SOG:
                alreadyAddedSubSets.push_back(subSet);

                CPosePDFSOG::TGaussianMode newSOGMode;
                if (params.ransac_algorithmForLandmarks)
                    newSOGMode.log_w = 0;  // log(1);
                else
                    newSOGMode.log_w = log(static_cast<double>(subSet.size()));

                newSOGMode.mean = referenceEstimation.mean;
                newSOGMode.cov = referenceEstimation.cov;

                // Add a new mode to the SOG!
                results.transformation.push_back(newSOGMode);
            }
        }  // end if subSet.size()>=ransac_minSetSize

        const size_t ninliers = subSet.size();
        if (largest_consensus_yet < ninliers)
        {
            largest_consensus_yet = ninliers;

            // Dynamic # of steps:
            if (use_dynamic_iter_number)
            {
                // Update estimate of nCorrs, the number of trials to ensure we
                // pick,
                // with probability p, a data set with no outliers.
                const double fracinliers =
                    ninliers /
                    static_cast<double>(howManyDifCorrs);  // corrsIdxs.size());
                double pNoOutliers =
                    1 - pow(fracinliers, static_cast<double>(
                                             2.0 /*minimumSizeSamplesToFit*/));

                pNoOutliers = std::max(
                    std::numeric_limits<double>::epsilon(),
                    pNoOutliers);  // Avoid division by -Inf
                pNoOutliers = std::min(
                    1.0 - std::numeric_limits<double>::epsilon(),
                    pNoOutliers);  // Avoid division by 0.
                // Number of
                results.ransac_iters =
                    log(1 - params.probability_find_good_model) /
                    log(pNoOutliers);

                results.ransac_iters = std::max(
                    results.ransac_iters, params.ransac_min_nSimulations);

                if (params.verbose)
                    cout << "[tfest::RANSAC] Iter #" << iter_idx
                         << ":est. # iters=" << results.ransac_iters
                         << " pNoOutliers=" << pNoOutliers
                         << " #inliers: " << ninliers << endl;
            }
        }

        // Save the largest subset:
        if (subSet.size() >= params.ransac_minSetSize &&
            this_subset_RMSE < largestSubSet_RMSE)
        {
            if (params.verbose)
                cout << "[tfest::RANSAC] Iter #" << iter_idx
                     << " Better subset: " << subSet.size()
                     << " inliers, RMSE=" << this_subset_RMSE << endl;

            results.largestSubSet = subSet;
            largestSubSet_RMSE = this_subset_RMSE;
        }

        // Is the found subset good enough?
        if (subSet.size() >= params.ransac_minSetSize &&
            this_subset_RMSE < MAX_RMSE_TO_END)
        {
            break;  // end RANSAC iterations.
        }

#ifdef DO_PROFILING
        timlog.leave("ransac.iter");
#endif
    }  // end for each iteration

    if (params.verbose)
        cout << "[tfest::RANSAC] Finished after " << iter_idx
             << " iterations.\n";

    // Set the weights of the particles to sum the unity:
    results.transformation.normalizeWeights();

    // Done!

    MRPT_END_WITH_CLEAN_UP(
        printf("nCorrs=%u\n", static_cast<unsigned int>(nCorrs));
        printf("Saving '_debug_in_correspondences.txt'...");
        in_correspondences.dumpToFile("_debug_in_correspondences.txt");
        printf("Ok\n"); printf("Saving '_debug_results.transformation.txt'...");
        results.transformation.saveToTextFile(
            "_debug_results.transformation.txt");
        printf("Ok\n"););

    return true;
}