Example: maps_ransac_data_association

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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 <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/geometry.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/tfest/se2.h>

#include <iostream>

// Method explained in paper:
//  J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal,
//   "A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps",
//    Robotica, 2013.
// http://dx.doi.org/10.1017/S0263574712000732

// ============= PARAMETERS ===================
const size_t NUM_OBSERVATIONS_TO_SIMUL = 10;
const size_t RANSAC_MINIMUM_INLIERS = 9;  // Min. # of inliers to accept

#define LOAD_MAP_FROM_FILE 0  // 1: load from "sMAP_FILE", 0: random map.
#define SHOW_POINT_LABELS  0

const float normalizationStd = 0.15f;  // 1 sigma noise (meters)
const float ransac_mahalanobisDistanceThreshold = 5.0f;
const size_t MINIMUM_RANSAC_ITERS = 100000;

#if !LOAD_MAP_FROM_FILE
const size_t NUM_MAP_FEATS = 100;
const double MAP_SIZE_X = 50;
const double MAP_SIZE_Y = 25;
#else
// Expected format of the 2D map is, for each line (one per landmark):
//  ID X Y
const std::string sMAP_FILE = string("./DLRMap.txt");
#endif
// ==============================================

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::maps;
using namespace mrpt::tfest;
using namespace mrpt::img;
using namespace std;

struct TObs
{
  size_t ID;  // Ground truth ID
  double x, y;
};

// ------------------------------------------------------
//              TestRANSAC
// ------------------------------------------------------
void TestRANSAC()
{
  mrpt::gui::CDisplayWindow3D win("MRPT example: ransac-data-association", 800, 600);

  mrpt::system::CTimeLogger timelog;  // For dumping stats at the end
  mrpt::system::CTicTac timer;

  getRandomGenerator().randomize();  // randomize with time

  // --------------------------------
  // Load feature map:
  // --------------------------------
  CSimplePointsMap the_map;
#if LOAD_MAP_FROM_FILE
  {
    CMatrixDouble M;
    M.loadFromTextFile(sMAP_FILE);  // Launch except. on error
    ASSERT_(M.cols() == 3 && M.rows() > 2)

    const size_t nPts = M.rows();
    the_map.resize(nPts);
    for (size_t i = 0; i < nPts; i++) the_map.setPoint(i, M(i, 1), M(i, 2));
  }
#else
  // Generate random MAP:
  the_map.resize(NUM_MAP_FEATS);
  for (size_t i = 0; i < NUM_MAP_FEATS; i++)
  {
    the_map.setPoint(
        i, getRandomGenerator().drawUniform(0, MAP_SIZE_X),
        getRandomGenerator().drawUniform(0, MAP_SIZE_Y));
  }
#endif

  const size_t nMapPts = the_map.size();
  cout << "Loaded/generated map with " << nMapPts << " landmarks.\n";

  const size_t nObs = NUM_OBSERVATIONS_TO_SIMUL;

  mrpt::opengl::CPointCloud::Ptr gl_obs_map = mrpt::opengl::CPointCloud::Create();
  mrpt::opengl::CPointCloud::Ptr gl_result = mrpt::opengl::CPointCloud::Create();
  mrpt::opengl::CSetOfObjects::Ptr gl_obs = mrpt::opengl::CSetOfObjects::Create();
  mrpt::opengl::CSetOfObjects::Ptr gl_obs_txts = mrpt::opengl::CSetOfObjects::Create();
  mrpt::opengl::CSetOfLines::Ptr gl_lines = mrpt::opengl::CSetOfLines::Create();
  {
    mrpt::opengl::Scene::Ptr& scene = win.get3DSceneAndLock();

    scene->getViewport("main")->setCustomBackgroundColor(TColorf(0.8f, 0.8f, 0.8f));
    win.setCameraPointingToPoint(MAP_SIZE_X * 0.5, MAP_SIZE_Y * 0.5, 0);
    win.setCameraZoom(2 * MAP_SIZE_X);

    //
    scene->insert(mrpt::opengl::stock_objects::CornerXYZ());

    //
    mrpt::opengl::CPointCloud::Ptr gl_map = mrpt::opengl::CPointCloud::Create();
    gl_map->loadFromPointsMap(&the_map);
    gl_map->setColor(0, 0, 1);
    gl_map->setPointSize(3);

    scene->insert(gl_map);

#if SHOW_POINT_LABELS
    for (size_t i = 0; i < the_map.size(); i++)
    {
      mrpt::opengl::CText::Ptr gl_txt =
          mrpt::opengl::CText::Create(mrpt::format("%u", static_cast<unsigned int>(i)));
      double x, y;
      the_map.getPoint(i, x, y);
      gl_txt->setLocation(x + 0.05, y + 0.05, 0.01);

      scene->insert(gl_txt);
    }
#endif

    //
    scene->insert(gl_lines);

    //
    gl_obs_map->setColor(1, 0, 0);
    gl_obs_map->setPointSize(5);

    gl_result->setColor(0, 1, 0);
    gl_result->setPointSize(4);

    //
    gl_obs->insert(mrpt::opengl::stock_objects::CornerXYZ(0.6f));
    gl_obs->insert(gl_obs_map);
    gl_obs->insert(gl_obs_txts);
    scene->insert(gl_obs);
    scene->insert(gl_result);

    win.unlockAccess3DScene();
    win.repaint();
  }

  // Repeat for each set of observations in the input file
  while (win.isOpen())
  {
    // Read the observations themselves:
    vector<TObs> observations;
    observations.resize(nObs);

    const mrpt::poses::CPose2D GT_pose(
        mrpt::random::getRandomGenerator().drawUniform(-10, 10 + MAP_SIZE_X),
        mrpt::random::getRandomGenerator().drawUniform(-10, 10 + MAP_SIZE_Y),
        mrpt::random::getRandomGenerator().drawUniform(-M_PI, M_PI));

    const mrpt::poses::CPose2D GT_pose_inv = -GT_pose;

    std::vector<nanoflann::ResultItem<size_t, float>> idxs;
    the_map.kdTreeRadiusSearch2D(GT_pose.x(), GT_pose.y(), 1000, idxs);
    ASSERT_(idxs.size() >= nObs);

    for (size_t i = 0; i < nObs; i++)
    {
      double gx, gy;
      the_map.getPoint(idxs[i].first, gx, gy);

      double lx, ly;
      GT_pose_inv.composePoint(gx, gy, lx, ly);

      observations[i].ID = idxs[i].first;
      observations[i].x =
          lx + mrpt::random::getRandomGenerator().drawGaussian1D(0, normalizationStd);
      observations[i].y =
          ly + mrpt::random::getRandomGenerator().drawGaussian1D(0, normalizationStd);
    }

    // ----------------------------------------------------
    // Generate list of individual-compatible pairings
    // ----------------------------------------------------
    TMatchingPairList all_correspondences;

    all_correspondences.reserve(nMapPts * nObs);

    // ALL possibilities:
    for (size_t j = 0; j < nObs; j++)
    {
      TMatchingPair match;

      match.localIdx = j;
      match.local.x = observations[j].x;
      match.local.y = observations[j].y;

      for (size_t i = 0; i < nMapPts; i++)
      {
        match.globalIdx = i;
        the_map.getPoint(i, match.global.x, match.global.y);

        all_correspondences.push_back(match);
      }
    }
    cout << "Generated " << all_correspondences.size() << " potential pairings.\n";

    // ----------------------------------------------------
    //  Run RANSAC-based D-A
    // ----------------------------------------------------
    timelog.enter("robustRigidTransformation");
    timer.Tic();

    mrpt::tfest::TSE2RobustParams params;
    mrpt::tfest::TSE2RobustResult results;

    params.ransac_minSetSize = RANSAC_MINIMUM_INLIERS;  // ransac_minSetSize (to add the solution
    // to the SOG)
    params.ransac_maxSetSize =
        all_correspondences.size();  // ransac_maxSetSize: Test with all data points
    params.ransac_mahalanobisDistanceThreshold = ransac_mahalanobisDistanceThreshold;
    params.ransac_nSimulations = 0;  // 0=auto
    params.ransac_fuseByCorrsMatch = true;
    params.ransac_fuseMaxDiffXY = 0.01f;
    params.ransac_fuseMaxDiffPhi = 0.1_deg;
    params.ransac_algorithmForLandmarks = true;
    params.probability_find_good_model = 0.999999;
    params.ransac_min_nSimulations =
        MINIMUM_RANSAC_ITERS;  // (a lower limit to the auto-detected value
    // of ransac_nSimulations)
    params.verbose = true;

    // Run ransac data-association:
    mrpt::tfest::se2_l2_robust(all_correspondences, normalizationStd, params, results);

    timelog.leave("robustRigidTransformation");

    mrpt::poses::CPosePDFSOG& best_poses = results.transformation;
    TMatchingPairList& out_best_pairings = results.largestSubSet;

    const double tim = timer.Tac();
    cout << "RANSAC time: " << mrpt::system::formatTimeInterval(tim) << endl;

    cout << "# of SOG modes: " << best_poses.size() << endl;
    cout << "Best match has " << out_best_pairings.size() << " features:\n";
    for (size_t i = 0; i < out_best_pairings.size(); i++)
      cout << out_best_pairings[i].globalIdx << " <-> " << out_best_pairings[i].localIdx << endl;
    cout << endl;

    // Generate "association vector":
    vector<int> obs2map_pairings(nObs, -1);
    for (size_t i = 0; i < out_best_pairings.size(); i++)
      obs2map_pairings[out_best_pairings[i].localIdx] =
          out_best_pairings[i].globalIdx == ((unsigned int)-1) ? -1
                                                               : out_best_pairings[i].globalIdx;

    cout << "1\n";
    for (size_t i = 0; i < nObs; i++) cout << obs2map_pairings[i] << " ";
    cout << endl;

    gl_result->clear();

    // Reconstruct the SE(2) transformation for these pairings:
    mrpt::poses::CPosePDFGaussian solution_pose;
    mrpt::tfest::se2_l2(out_best_pairings, solution_pose);

    // Normalized covariance: scale!
    solution_pose.cov *= square(normalizationStd);

    cout << "Solution pose: " << solution_pose.mean << endl;
    cout << "Ground truth pose: " << GT_pose << endl;

    {
      // mrpt::opengl::Scene::Ptr &scene =
      win.get3DSceneAndLock();

      win.addTextMessage(
          5, 5,
          "Blue: map landmarks | Red: Observations | White lines: Found "
          "correspondences",
          0);

      //
      gl_obs_map->clear();
      for (size_t k = 0; k < nObs; k++)
        gl_obs_map->insertPoint(observations[k].x, observations[k].y, 0.0);

      gl_obs->setPose(solution_pose.mean);

#if SHOW_POINT_LABELS
      gl_obs_txts->clear();
      for (size_t i = 0; i < nObs; i++)
      {
        mrpt::opengl::CText::Ptr gl_txt =
            mrpt::opengl::CText::Create(mrpt::format("%u", static_cast<unsigned int>(i)));
        const double x = observations[i].x;
        const double y = observations[i].y;
        gl_txt->setLocation(x + 0.05, y + 0.05, 0.01);
        gl_obs_txts->insert(gl_txt);
      }
#endif

      //
      gl_lines->clear();
      double sqerr = 0;
      size_t nPairs = 0;
      for (size_t k = 0; k < nObs; k++)
      {
        int map_idx = obs2map_pairings[k];
        if (map_idx < 0) continue;
        nPairs++;

        double map_x, map_y;
        the_map.getPoint(map_idx, map_x, map_y);

        double obs_x, obs_y;
        solution_pose.mean.composePoint(observations[k].x, observations[k].y, obs_x, obs_y);

        const double z = 0;

        gl_lines->appendLine(map_x, map_y, 0, obs_x, obs_y, z);

        sqerr += mrpt::math::distanceSqrBetweenPoints<double>(map_x, map_y, obs_x, obs_y);
      }

      win.addTextMessage(5, 20, "Ground truth pose    : " + GT_pose.asString(), 1);
      win.addTextMessage(
          5, 35,
          "RANSAC estimated pose: " + solution_pose.mean.asString() +
              mrpt::format(" | RMSE=%f", (nPairs ? sqerr / nPairs : 0.0)),
          2);

      win.unlockAccess3DScene();
      win.repaint();

      cout << "nPairings: " << nPairs << " RMSE = " << (nPairs ? sqerr / nPairs : 0.0) << endl;

      win.waitForKey();
    }

  }  // end of for each set of observations
}

// ------------------------------------------------------
//                      MAIN
// ------------------------------------------------------
int main()
{
  try
  {
    TestRANSAC();
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
    return -1;
  }
}