Example: obs_motion_model_demo

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/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/Scene.h>
#include <mrpt/opengl/pose_pdfs.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/random.h>
#include <mrpt/system/filesystem.h>

#include <iostream>

// demo rawlog path:
#include <mrpt/examples_config.h>

constexpr size_t NUM_PARTICLES = 100;

void DemoMotionModel(int argc, const char** argv)
{
  using namespace std::string_literals;

  mrpt::random::getRandomGenerator().randomize();

  // Default dataset, or override with user cli one:
  std::string datasetFile =
      MRPT_EXAMPLES_BASE_DIRECTORY + "../share/mrpt/datasets/intel_2003_partial.rawlog.gz"s;
  if (argc > 1) datasetFile = argv[1];

  std::string legendText;
  if (argc > 2) legendText = argv[2];

  ASSERT_FILE_EXISTS_(datasetFile);

  mrpt::obs::CRawlog dataset;
  dataset.loadFromRawLogFile(datasetFile);

  std::cout << "Read dataset: " << dataset.size() << " entries.\n";

  size_t position = 0;

  mrpt::obs::CActionCollection::Ptr actions;
  mrpt::obs::CSensoryFrame::Ptr sf;

  mrpt::poses::CPose2D deadReckoning;
  mrpt::poses::CPosePDFParticles parts;

  parts.resetDeterministic({0, 0, 0}, NUM_PARTICLES);

  // gui:
  mrpt::gui::CDisplayWindow3D win("Motion model uncertainty demo", 500, 1000);

  auto glPartsGroup = mrpt::opengl::CSetOfObjects::Create();
  auto glLidar = mrpt::opengl::CPlanarLaserScan::Create();
  auto glOdoTrack = mrpt::opengl::CSetOfLines::Create();
  mrpt::opengl::Viewport::Ptr glBottomView;
  glOdoTrack->appendLine(0, 0, 0, 0, 0, 0);
  glOdoTrack->setColor_u8(0x00, 0x00, 0x00);
  {
    auto& scene = win.get3DSceneAndLock();
    scene->insert(mrpt::opengl::CGridPlaneXY::Create());
    scene->insert(glPartsGroup);
    scene->insert(glLidar);
    scene->insert(glOdoTrack);

    // Create the two views:
    auto glMainView = scene->getViewport();
    glBottomView = scene->createViewport("bottom");

    glMainView->setViewportPosition(0, 0.5, 1.0, 0.5);
    glBottomView->setViewportPosition(0.01, 0.01, 0.98, 0.48);
    glBottomView->setCloneView("main");

    auto& glCloseCam = glBottomView->getCamera();
    glCloseCam.setAzimuthDegrees(90.0f);
    glCloseCam.setElevationDegrees(90.0f);
    glCloseCam.setZoomDistance(1.5f);

    win.setCameraAzimuthDeg(90.0f);
    win.setCameraElevationDeg(90.0f);
    win.setCameraZoom(15.0f);
    win.setCameraPointingToPoint(1.0f, -2.0f, .0f);

    win.addTextMessage(5, 5, legendText);

    win.unlockAccess3DScene();
  }

  std::optional<double> lastObsTime;
  bool paused = false;

  // iterate over the dataset:
  while (position + 1 < dataset.size() && dataset.getActionObservationPair(actions, sf, position))
  {
    const auto actMotion = actions->getActionByClass<mrpt::obs::CActionRobotMovement2D>();
    ASSERT_(actMotion);

    const auto obsLidar = sf->getObservationByClass<mrpt::obs::CObservation2DRangeScan>();
    ASSERT_(obsLidar);

    const double thisObsTime = mrpt::Clock::toDouble(obsLidar->timestamp);
    const double obsPeriod = std::max(1e-3, lastObsTime ? (thisObsTime - *lastObsTime) : .01);
    lastObsTime = thisObsTime;

    if (!paused)
    {
      // Accumulate the mean pose increments, as a gross pose estimation
      // (no filtering, no localization, no SLAM, just dead reckoning):
      deadReckoning += actMotion->poseChange->getMeanVal();

      // Propagate random samples:
      actMotion->prepareFastDrawSingleSamples();

      for (auto& part : parts.m_particles)
      {
        mrpt::poses::CPose2D odoSample;
        actMotion->fastDrawSingleSample(odoSample);

        part.d = part.d + odoSample.asTPose();
      }
    }

    const auto glParticles = parts.getAs3DObject<mrpt::opengl::CSetOfObjects::Ptr>();

    {
      win.get3DSceneAndLock();

      glPartsGroup->clear();
      glPartsGroup->insert(glParticles);

      glLidar->setScan(*obsLidar);
      glLidar->setPose(deadReckoning);

      glOdoTrack->appendLineStrip(deadReckoning.x(), deadReckoning.y(), 0.01);

      auto& glCloseCam = glBottomView->getCamera();
      glCloseCam.setPointingAt(deadReckoning.x(), deadReckoning.y(), .0);

      win.unlockAccess3DScene();
      win.forceRepaint();

      std::this_thread::sleep_for(std::chrono::microseconds(static_cast<int>(1e6 * obsPeriod)));
    }

    std::cout << "timestep: " << position << " deadRecoking: " << deadReckoning << "\n";

    if (win.keyHit())
    {
      switch (win.getPushedKey())
      {
        case ' ':
          paused = !paused;
          break;
        case 'R':
        case 'r':
          win.grabImagesStart();
          break;
      }
    }

    if (paused) position -= 2;
  }
}

int main(int argc, const char** argv)
{
  try
  {
    DemoMotionModel(argc, argv);
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}