Example: hwdrivers_kinect_to_2d_scan_example

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          |
   +------------------------------------------------------------------------+ */

/*
  Example  : kinect-to-2d-laser-demo
  Web page : https://www.mrpt.org/Example_Kinect_To_2D_laser_scan (includes
  video demo)

  Purpose  : Demonstrate grabbing from CKinect, multi-threading
       and converting the 3D range data into an equivalent
       2D planar scan.
*/

#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CKinect.h>
#include <mrpt/img/TColor.h>
#include <mrpt/opengl/CFrustum.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>

#include <chrono>
#include <iostream>
#include <thread>

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::img;
using namespace std;

// Thread for grabbing: Do this is another thread so we divide rendering and
// grabbing
//   and exploit multicore CPUs.
struct TThreadParam
{
  TThreadParam() {}
  volatile bool quit{false};
  volatile int pushed_key{0};
  volatile double Hz{0};

  CObservation3DRangeScan::Ptr new_obs;  // RGB+D (+3D points)
  CObservationIMU::Ptr new_obs_imu;      // Accelerometers
};

void thread_grabbing(TThreadParam& p)
{
  try
  {
    CKinect kinect;

    // Set params:
    // kinect.enableGrab3DPoints(true);
    // kinect.enablePreviewRGB(true);
    //...
    const std::string cfgFile = "kinect_calib.cfg";
    if (mrpt::system::fileExists(cfgFile))
    {
      cout << "Loading calibration from: " << cfgFile << endl;
      kinect.loadConfig(mrpt::config::CConfigFile(cfgFile), "KINECT");
    }
    else
      cerr << "Warning: Calibration file [" << cfgFile << "] not found -> Using default params.\n";

    // Open:
    cout << "Calling CKinect::initialize()...";
    kinect.initialize();
    cout << "OK\n";

    CTicTac tictac;
    int nImgs = 0;
    bool there_is_obs = true, hard_error = false;

    while (!hard_error && !p.quit)
    {
      // Grab new observation from the camera:
      auto obs = CObservation3DRangeScan::Create();  // Smart pointers to
                                                     // observations
      CObservationIMU::Ptr obs_imu = CObservationIMU::Create();

      kinect.getNextObservation(*obs, *obs_imu, there_is_obs, hard_error);

      if (!hard_error && there_is_obs)
      {
        std::atomic_store(&p.new_obs, obs);
        std::atomic_store(&p.new_obs_imu, obs_imu);
      }

      if (p.pushed_key != 0)
      {
        switch (p.pushed_key)
        {
          case 27:
            p.quit = true;
            break;
        }

        // Clear pushed key flag:
        p.pushed_key = 0;
      }

      nImgs++;
      if (nImgs > 10)
      {
        p.Hz = nImgs / tictac.Tac();
        nImgs = 0;
        tictac.Tic();
      }
    }
  }
  catch (const std::exception& e)
  {
    cout << "Exception in Kinect thread: " << e.what() << endl;
    p.quit = true;
  }
}

// ------------------------------------------------------
//              Test_Kinect
// ------------------------------------------------------
void Test_Kinect()
{
  // Launch grabbing thread:
  // --------------------------------------------------------
  TThreadParam thrPar;
  std::thread thHandle(thread_grabbing, std::ref(thrPar));

  // Wait until data stream starts so we can say for sure the sensor has been
  // initialized OK:
  cout << "Waiting for sensor initialization...\n";
  do
  {
    CObservation3DRangeScan::Ptr possiblyNewObs = std::atomic_load(&thrPar.new_obs);
    if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP)
      break;
    else
      std::this_thread::sleep_for(10ms);
  } while (!thrPar.quit);

  // Check error condition:
  if (thrPar.quit) return;

  // Create window and prepare OpenGL object in the scene:
  // --------------------------------------------------------
  mrpt::gui::CDisplayWindow3D win3D("Kinect 3D -> 2D laser scan", 800, 600);

  win3D.setCameraAzimuthDeg(140);
  win3D.setCameraElevationDeg(30);
  win3D.setCameraZoom(10.0);
  win3D.setFOV(90);
  win3D.setCameraPointingToPoint(2.5, 0, 0);

  // The 3D point cloud OpenGL object:
  mrpt::opengl::CPointCloudColoured::Ptr gl_points = mrpt::opengl::CPointCloudColoured::Create();
  gl_points->setPointSize(2.5);

  // The 2D "laser scan" OpenGL object:
  mrpt::opengl::CPlanarLaserScan::Ptr gl_2d_scan = mrpt::opengl::CPlanarLaserScan::Create();
  gl_2d_scan->enablePoints(true);
  gl_2d_scan->enableLine(true);
  gl_2d_scan->enableSurface(true);
  gl_2d_scan->setSurfaceColor(0, 0, 1, 0.3);  // RGBA

  mrpt::opengl::CFrustum::Ptr gl_frustum =
      mrpt::opengl::CFrustum::Create(0.2f, 5.0f, 90.0f, 5.0f, 2.0f, true, true);

  const double aspect_ratio = 480.0 / 640.0;  // kinect.rows() / double( kinect.cols() );

  opengl::Viewport::Ptr viewRange,
      viewInt;  // Extra viewports for the RGB & D images.
  {
    mrpt::opengl::Scene::Ptr& scene = win3D.get3DSceneAndLock();

    // Create the Opengl object for the point cloud:
    scene->insert(gl_points);
    scene->insert(gl_2d_scan);
    scene->insert(gl_frustum);

    {
      mrpt::opengl::CGridPlaneXY::Ptr gl_grid = mrpt::opengl::CGridPlaneXY::Create();
      gl_grid->setColor(0.6, 0.6, 0.6);
      scene->insert(gl_grid);
    }
    {
      mrpt::opengl::CSetOfObjects::Ptr gl_corner = mrpt::opengl::stock_objects::CornerXYZ();
      gl_corner->setScale(0.2);
      scene->insert(gl_corner);
    }

    const int VW_WIDTH = 250;  // Size of the viewport into the window, in pixel units.
    const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
    const int VW_GAP = 30;

    // Create the Opengl objects for the planar images, as textured planes,
    // each in a separate viewport:
    win3D.addTextMessage(30, -25 - 1 * (VW_GAP + VW_HEIGHT), "Range data", 1);
    viewRange = scene->createViewport("view2d_range");
    viewRange->setViewportPosition(5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);

    win3D.addTextMessage(30, -25 - 2 * (VW_GAP + VW_HEIGHT), "Intensity data", 2);
    viewInt = scene->createViewport("view2d_int");
    viewInt->setViewportPosition(5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);

    win3D.unlockAccess3DScene();
    win3D.repaint();
  }

  CObservation3DRangeScan::Ptr last_obs;
  CObservationIMU::Ptr last_obs_imu;

  while (win3D.isOpen() && !thrPar.quit)
  {
    CObservation3DRangeScan::Ptr possiblyNewObs = std::atomic_load(&thrPar.new_obs);
    if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
        (!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
    {
      // It IS a new observation:
      last_obs = possiblyNewObs;
      last_obs_imu = std::atomic_load(&thrPar.new_obs_imu);

      // Update visualization ---------------------------------------
      bool do_refresh = false;

      // Show 2D ranges as a grayscale image:
      if (last_obs->hasRangeImage)
      {
        mrpt::img::CImage img;

        // Normalize the image
        static CMatrixFloat range2D;  // Static to save time allocating
        // the matrix in every iteration
        range2D = last_obs->rangeImage;
        range2D *= (1.0 / last_obs->maxRange);

        img.setFromMatrix(range2D);

        win3D.get3DSceneAndLock();
        viewRange->setImageView(std::move(img));
        win3D.unlockAccess3DScene();
        do_refresh = true;
      }

      // Convert ranges to an equivalent 2D "fake laser" scan:
      if (last_obs->hasRangeImage)
      {
        // Convert to scan:
        CObservation2DRangeScan::Ptr obs_2d = CObservation2DRangeScan::Create();
        const float vert_FOV = DEG2RAD(gl_frustum->getVertFOV());

        mrpt::obs::T3DPointsTo2DScanParams sp;
        sp.angle_inf = .5f * vert_FOV;
        sp.angle_sup = .5f * vert_FOV;
        sp.sensorLabel = "KINECT_2D_SCAN";

        last_obs->convertTo2DScan(*obs_2d, sp);

        // And load scan in the OpenGL object:
        gl_2d_scan->setScan(*obs_2d);
      }

      // Show intensity image:
      if (last_obs->hasIntensityImage)
      {
        win3D.get3DSceneAndLock();
        viewInt->setImageView(last_obs->intensityImage);  // This is not "_fast" since the
        // intensity image is used below
        // in the coloured point cloud.
        win3D.unlockAccess3DScene();
        do_refresh = true;
      }

      // Show 3D points:
      if (last_obs->hasPoints3D)
      {
        win3D.get3DSceneAndLock();
        mrpt::obs::T3DPointsProjectionParams pp;
        pp.takeIntoAccountSensorPoseOnRobot = false;

        last_obs->unprojectInto(*gl_points, pp);
        win3D.unlockAccess3DScene();
        do_refresh = true;
      }

      // Some text messages:
      win3D.get3DSceneAndLock();
      // Estimated grabbing rate:
      win3D.addTextMessage(-100, -20, format("%.02f Hz", thrPar.Hz), 100);

      win3D.addTextMessage(
          10, 10,
          "'o'/'i'-zoom out/in, '2'/'3'/'f':show/hide 2D/3D/frustum "
          "data, mouse: orbit 3D, ESC: quit",
          110);

      win3D.addTextMessage(
          10, 25,
          format(
              "Show: 3D=%s 2D=%s Frustum=%s (vert. FOV=%.1fdeg)",
              gl_points->isVisible() ? "YES" : "NO", gl_2d_scan->isVisible() ? "YES" : "NO",
              gl_frustum->isVisible() ? "YES" : "NO", gl_frustum->getVertFOV()),
          111);
      win3D.unlockAccess3DScene();

      // Do we have accelerometer data?
      if (last_obs_imu && last_obs_imu->dataIsPresent[IMU_X_ACC])
      {
        win3D.get3DSceneAndLock();
        win3D.addTextMessage(
            10, 65,
            format(
                "Acc: x=%.02f y=%.02f z=%.02f", last_obs_imu->rawMeasurements[IMU_X_ACC],
                last_obs_imu->rawMeasurements[IMU_Y_ACC], last_obs_imu->rawMeasurements[IMU_Z_ACC]),
            102);
        win3D.unlockAccess3DScene();
        do_refresh = true;
      }

      // Force opengl repaint:
      if (do_refresh) win3D.repaint();

    }  // end update visualization:

    // Process possible keyboard commands:
    // --------------------------------------
    if (win3D.keyHit() && thrPar.pushed_key == 0)
    {
      const int key = tolower(win3D.getPushedKey());

      switch (key)
      {
        // Some of the keys are processed in this thread:
        case 'o':
          win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
          win3D.repaint();
          break;
        case 'i':
          win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
          win3D.repaint();
          break;
        case '2':
          gl_2d_scan->setVisibility(!gl_2d_scan->isVisible());
          break;
        case '3':
          gl_points->setVisibility(!gl_points->isVisible());
          break;
        case 'F':
        case 'f':
          gl_frustum->setVisibility(!gl_frustum->isVisible());
          break;
        case '+':
          gl_frustum->setVertFOV(gl_frustum->getVertFOV() + 1);
          break;
        case '-':
          gl_frustum->setVertFOV(gl_frustum->getVertFOV() - 1);
          break;
        // ...and the rest in the kinect thread:
        default:
          thrPar.pushed_key = key;
          break;
      };
    }

    std::this_thread::sleep_for(1ms);
  }

  cout << "Waiting for grabbing thread to exit...\n";
  thrPar.quit = true;
  thHandle.join();
  cout << "Bye!\n";
}

int main(int argc, char** argv)
{
  try
  {
    Test_Kinect();

    std::this_thread::sleep_for(50ms);
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
    return -1;
  }
  catch (...)
  {
    printf("Another exception!!");
    return -1;
  }
}