test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
/*
  Example  : kinect-to-2d-laser-demo
  Web page : http://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/hwdrivers/CKinect.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/opengl/CFrustum.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/img/TColor.h>
#include <iostream>
 
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() : quit(false), pushed_key(0), Hz(0) {}
        volatile bool quit;
        volatile int pushed_key;
        volatile double Hz;
 
        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:
                        CObservation3DRangeScan::Ptr obs = mrpt::make_aligned_shared<
                                CObservation3DRangeScan>();  // Smart pointers to observations
                        CObservationIMU::Ptr obs_imu =
                                mrpt::make_aligned_shared<CObservationIMU>();
 
                        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 (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::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
        gl_points->setPointSize(2.5);
 
        // The 2D "laser scan" OpenGL object:
        mrpt::opengl::CPlanarLaserScan::Ptr gl_2d_scan =
                mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
        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::make_aligned_shared<mrpt::opengl::CFrustum>(
                        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::COpenGLViewport::Ptr viewRange,
                viewInt;  // Extra viewports for the RGB & D images.
        {
                mrpt::opengl::COpenGLScene::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::make_aligned_shared<mrpt::opengl::CGridPlaneXY>();
                        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", TColorf(1, 1, 1),
                        1, MRPT_GLUT_BITMAP_HELVETICA_12);
                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",
                        TColorf(1, 1, 1), 2, MRPT_GLUT_BITMAP_HELVETICA_12);
                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 *
                                                  (1.0 / 5.0);  // kinect.getMaxRange());
 
                                img.setFromMatrix(range2D);
 
                                win3D.get3DSceneAndLock();
                                viewRange->setImageView_fast(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 =
                                        mrpt::make_aligned_shared<CObservation2DRangeScan>();
                                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->project3DPointsFromDepthImageInto(*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), TColorf(1, 1, 1),
                                "sans", 15, mrpt::opengl::FILL, 100);
 
                        win3D.addTextMessage(
                                10, 10,
                                "'o'/'i'-zoom out/in, '2'/'3'/'f':show/hide 2D/3D/frustum "
                                "data, mouse: orbit 3D, ESC: quit",
                                TColorf(1, 1, 1), "sans", 10, mrpt::opengl::FILL, 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()),
                                TColorf(1, 1, 1), "sans", 10, mrpt::opengl::FILL, 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]),
                                        TColorf(.7, .7, .7), "sans", 10, mrpt::opengl::FILL, 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 (std::exception& e)
        {
                std::cout << "EXCEPCION: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Another exception!!");
                return -1;
        }
}