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

#include <mrpt/hwdrivers/CSwissRanger3DCamera.h>
#include <mrpt/gui.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/CTexturedPlane.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/system/CTicTac.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::maps;
using namespace mrpt::obs;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::img;
using namespace std;

// ------------------------------------------------------
//              Test_SwissRanger
// ------------------------------------------------------
void Test_SwissRanger()
{
    CSwissRanger3DCamera cam;

    // Set params:
    cam.setOpenFromUSB(true);

    cam.setSave3D(true);
    cam.setSaveRangeImage(true);
    cam.setSaveIntensityImage(true);
    cam.setSaveConfidenceImage(false);

    // cam.enablePreviewWindow(true);

    // Open:
    cam.initialize();

    if (cam.isOpen())
        cout << "[Test_SwissRanger] Camera open, serial #"
             << cam.getCameraSerialNumber() << " resolution: " << cam.cols()
             << "x" << cam.rows() << " max. range: " << cam.getMaxRange()
             << endl;

    const double aspect_ratio = cam.rows() / double(cam.cols());

    {
        std::string ver;
        cam.getMesaLibVersion(ver);
        cout << "[Test_SwissRanger] Version: " << ver << "\n";
    }

    CObservation3DRangeScan obs;
    bool there_is_obs = true, hard_error;

    mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600);

    win3D.setCameraAzimuthDeg(140);
    win3D.setCameraElevationDeg(20);
    win3D.setCameraZoom(6.0);
    win3D.setCameraPointingToPoint(2.5, 0, 0);

    // mrpt::gui::CDisplayWindow  win2D("2D range image",200,200);
    // mrpt::gui::CDisplayWindow  winInt("Intensity range image",200,200);
    //  win2D.setPos(10,10);
    //  winInt.setPos(350,10);
    //  win3D.setPos(10,290);
    //  win3D.resize(400,200);

    // mrpt::opengl::CPointCloud::Ptr gl_points =
    // mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
    mrpt::opengl::CPointCloudColoured::Ptr gl_points =
        mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
    gl_points->setPointSize(4.5);

    mrpt::opengl::CTexturedPlane::Ptr gl_img_range =
        mrpt::make_aligned_shared<mrpt::opengl::CTexturedPlane>(
            0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
    mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity =
        mrpt::make_aligned_shared<mrpt::opengl::CTexturedPlane>(
            0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
    mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity_rect =
        mrpt::make_aligned_shared<mrpt::opengl::CTexturedPlane>(
            0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);

    {
        mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();

        // Create the Opengl object for the point cloud:
        scene->insert(gl_points);
        scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
        scene->insert(mrpt::opengl::stock_objects::CornerXYZ());

        const int VW_WIDTH = 200;
        const int VW_HEIGHT = 150;
        const int VW_GAP = 10;

        // Create the Opengl objects for the planar images, as textured planes,
        // each in a separate viewport:
        win3D.addTextMessage(
            30, -10 - 1 * (VW_GAP + VW_HEIGHT), "Range data", TColorf(1, 1, 1),
            1, MRPT_GLUT_BITMAP_HELVETICA_12);
        opengl::COpenGLViewport::Ptr viewRange =
            scene->createViewport("view2d_range");
        scene->insert(gl_img_range, "view2d_range");
        viewRange->setViewportPosition(
            5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
        viewRange->setTransparent(true);
        viewRange->getCamera().setOrthogonal(true);
        viewRange->getCamera().setAzimuthDegrees(90);
        viewRange->getCamera().setElevationDegrees(90);
        viewRange->getCamera().setZoomDistance(1.0);

        win3D.addTextMessage(
            30, -10 - 2 * (VW_GAP + VW_HEIGHT), "Intensity data",
            TColorf(1, 1, 1), 2, MRPT_GLUT_BITMAP_HELVETICA_12);
        opengl::COpenGLViewport::Ptr viewInt =
            scene->createViewport("view2d_int");
        scene->insert(gl_img_intensity, "view2d_int");
        viewInt->setViewportPosition(
            5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
        viewInt->setTransparent(true);
        viewInt->getCamera().setOrthogonal(true);
        viewInt->getCamera().setAzimuthDegrees(90);
        viewInt->getCamera().setElevationDegrees(90);
        viewInt->getCamera().setZoomDistance(1.0);

        win3D.addTextMessage(
            30, -10 - 3 * (VW_GAP + VW_HEIGHT), "Intensity data (undistorted)",
            TColorf(1, 1, 1), 3, MRPT_GLUT_BITMAP_HELVETICA_12);
        opengl::COpenGLViewport::Ptr viewIntRect =
            scene->createViewport("view2d_intrect");
        scene->insert(gl_img_intensity_rect, "view2d_intrect");
        viewIntRect->setViewportPosition(
            5, -10 - 3 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
        viewIntRect->setTransparent(true);
        viewIntRect->getCamera().setOrthogonal(true);
        viewIntRect->getCamera().setAzimuthDegrees(90);
        viewIntRect->getCamera().setElevationDegrees(90);
        viewIntRect->getCamera().setZoomDistance(1.0);

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

    CTicTac tictac;
    size_t nImgs = 0;

    bool endLoop = false;

    while (there_is_obs && !endLoop && win3D.isOpen())
    {
        // Grab new observation from the camera:
        cam.getNextObservation(obs, there_is_obs, hard_error);

        // Show ranges as 2D:
        if (there_is_obs && obs.hasRangeImage)
        {
            mrpt::img::CImage img;
            // Normalize the image
            CMatrixFloat range2D = obs.rangeImage;
            range2D *= 1.0 / cam.getMaxRange();
            img.setFromMatrix(range2D);

            win3D.get3DSceneAndLock();
            gl_img_range->assignImage_fast(img);
            win3D.unlockAccess3DScene();
        }

        // Show intensity image:
        if (there_is_obs && obs.hasIntensityImage)
        {
            win3D.get3DSceneAndLock();
            gl_img_intensity->assignImage(obs.intensityImage);

            CImage undistortImg;
            obs.intensityImage.rectifyImage(undistortImg, obs.cameraParams);
            gl_img_intensity_rect->assignImage(undistortImg);
            win3D.unlockAccess3DScene();
        }

        // Show 3D points:
        if (there_is_obs && obs.hasPoints3D)
        {
            // mrpt::maps::CSimplePointsMap  pntsMap;
            CColouredPointsMap pntsMap;
            pntsMap.colorScheme.scheme =
                CColouredPointsMap::cmFromIntensityImage;
            pntsMap.loadFromRangeScan(obs);

            win3D.get3DSceneAndLock();
            gl_points->loadFromPointsMap(&pntsMap);
            win3D.unlockAccess3DScene();
            win3D.repaint();
        }

        nImgs++;
        if (nImgs > 10)
        {
            win3D.get3DSceneAndLock();
            win3D.addTextMessage(
                0.01, 0.01, format("%.02f Hz", nImgs / tictac.Tac()),
                TColorf(0, 1, 1), 100, MRPT_GLUT_BITMAP_HELVETICA_12);
            win3D.unlockAccess3DScene();
            nImgs = 0;
            tictac.Tic();
        }

        // Process possible keyboard commands:
        // --------------------------------------
        if (win3D.keyHit())
        {
            const int key = tolower(win3D.getPushedKey());
            // cout << "key: " << key << endl;

            switch (key)
            {
                case 'h':
                    cam.enableImageHistEqualization(
                        !cam.isEnabledImageHistEqualization());
                    break;
                case 'g':
                    cam.enableConvGray(!cam.isEnabledConvGray());
                    break;
                case 'd':
                    cam.enableDenoiseANF(!cam.isEnabledDenoiseANF());
                    break;
                case 'f':
                    cam.enableMedianFilter(!cam.isEnabledMedianFilter());
                    break;
                case 27:
                    endLoop = true;
                    break;
            }
        }

        win3D.get3DSceneAndLock();
        win3D.addTextMessage(
            0.08, 0.02,
            format(
                "Keyboard switches: H (hist.equal: %s) | G (convGray: %s) | D "
                "(denoise: %s) | F (medianFilter: %s)",
                cam.isEnabledImageHistEqualization() ? "ON" : "OFF",
                cam.isEnabledConvGray() ? "ON" : "OFF",
                cam.isEnabledDenoiseANF() ? "ON" : "OFF",
                cam.isEnabledMedianFilter() ? "ON" : "OFF"),
            TColorf(0, 0, 1), 110, MRPT_GLUT_BITMAP_HELVETICA_18);
        win3D.unlockAccess3DScene();

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

int main(int argc, char** argv)
{
    try
    {
        Test_SwissRanger();
        return 0;
    }
    catch (std::exception& e)
    {
        std::cout << "EXCEPCION: " << e.what() << std::endl;
        return -1;
    }
    catch (...)
    {
        printf("Another exception!!");
        return -1;
    }
}