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/gui.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/hwdrivers/CCameraSensor.h>
#include <mrpt/vision/CImagePyramid.h>
#include <mrpt/vision/types.h>
#include <mrpt/vision/CFeatureExtraction.h>
#include <mrpt/vision/TSimpleFeature.h>
#include <mrpt/img/TColor.h>
#include <iostream>
 
using namespace std;
using namespace mrpt;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::obs;
using namespace mrpt::vision;
using namespace mrpt::img;
 
// ------------------------------------------------------
//                              TestVideoBuildPyr
// ------------------------------------------------------
void TestVideoBuildPyr()
{
        size_t N_OCTAVES = 4;
        bool do_smooth = false;
        bool do_grayscale = false;
        bool do_features = false;
 
        // Ask for a different number of octaves:
        cout << "Number of octaves to use [4]: ";
        {
                std::string s;
                std::getline(cin, s);
                int i = atoi(s.c_str());
                if (i > 0) N_OCTAVES = i;
        }
 
        // Show to the user a list of possible camera drivers and creates and open
        // the selected camera.
        cout << "Please, select the input video file or camera...\n";
 
        mrpt::hwdrivers::CCameraSensor::Ptr cam =
                mrpt::hwdrivers::prepareVideoSourceFromUserSelection();
        if (!cam) return;
 
        cout << "Video stream open OK\n";
 
        // Create 3D window:
        CDisplayWindow3D win("Demo of pyramid building from live video", 800, 600);
 
        //  Get the smart pointer to the main viewport object in this window,
        //   and create other viewports for the smaller images:
        std::vector<COpenGLViewport::Ptr> gl_views(N_OCTAVES);
        {
                COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
                gl_views[0] = theScene->getViewport("main");
                ASSERT_(gl_views[0]);
 
                // Create the other viewports:
                for (size_t i = 1; i < N_OCTAVES; i++)
                        gl_views[i] = theScene->createViewport(format("view_%i", (int)i));
 
                // Assign sizes:
                //  It can be shown mathematically than if we want all viewports to be
                //  one next to each other
                //  horizontally so they fit to the viewport width (="1") and each is
                //  the half the previous one,
                //  the first one must have a width of 2^(n-1)/(2^n - 1)
                const double W0 =
                        (double(1 << (N_OCTAVES - 1))) / ((1 << N_OCTAVES) - 1);
 
                double X = 0;
                double W = W0;
                for (size_t i = 0; i < N_OCTAVES; i++)
                {
                        COpenGLViewport* vw = gl_views[i].get();
                        vw->setViewportPosition(X, .0, W, 1.);
                        // cout << "Created viewport " << i << " at X=" << X << " with
                        // Width=" << W << endl;
                        X += W;
                        W *= 0.5;
                }
 
                // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
                win.unlockAccess3DScene();
        }
 
        win.setPos(10, 10);
 
        win.addTextMessage(
                0.51, 5,  // X,Y<=1 means coordinates are factors over the entire
                // viewport area.
                "Keys: 's'=Smoothing, 'g': Grayscale 'f': Features", TColorf(.8, .8, 0),
                "sans", 10,  // font name & size
                mrpt::opengl::FILL,
                10  // An arbitrary ID to always overwrite the same, previous 2D text
                // message
                );
 
        // The image pyramid: Initially empty
        CImagePyramid imgpyr;
 
        cout << "Close the window to end.\n";
        while (win.isOpen())
        {
                win.addTextMessage(5, 5, format("%.02fFPS", win.getRenderingFPS()));
                std::this_thread::sleep_for(1ms);
 
                // Grab new video frame:
                CObservation::Ptr obs = cam->getNextFrame();
                if (obs)
                {
                        if (IS_CLASS(obs, CObservationImage))
                        {
                                // Get the observation object:
                                CObservationImage::Ptr o =
                                        std::dynamic_pointer_cast<CObservationImage>(obs);
 
                                // Update pyramid:
                                imgpyr.buildPyramidFast(
                                        o->image,  // This image is destroyed since we are calling
                                        // the *Fast() version
                                        N_OCTAVES, do_smooth, do_grayscale);
 
                                // Also detect features?
                                if (do_features)
                                {
                                        static const int threshold = 20;
 
                                        for (unsigned int level = 0; level < N_OCTAVES; ++level)
                                        {
                                                CImage gray_img(
                                                        imgpyr.images[level], FAST_REF_OR_CONVERT_TO_GRAY);
 
                                                TSimpleFeatureList feats;
                                                CFeatureExtraction::detectFeatures_SSE2_FASTER12(
                                                        gray_img, feats, threshold);
 
                                                imgpyr.images[level].drawFeaturesSimple(
                                                        feats, TColor::blue());
                                        }
                                }
 
                                win.get3DSceneAndLock();
 
                                for (size_t i = 0; i < N_OCTAVES; i++)
                                {
                                        COpenGLViewport* vw = gl_views[i].get();
                                        vw->setImageView(imgpyr.images[i]);
                                }
 
                                win.addTextMessage(
                                        0.51, 25,  // X,Y<=1 means coordinates are factors over the
                                        // entire viewport area.
                                        format(
                                                "Smooth=%i Grayscale=%i Features=%i",
                                                int(do_smooth ? 1 : 0), int(do_grayscale ? 1 : 0),
                                                int(do_features ? 1 : 0)),
                                        TColorf(.8, .8, 0), "sans", 10,  // font name & size
                                        mrpt::opengl::FILL,
                                        11  // An arbitrary ID to always overwrite the same,
                                        // previous 2D text message
                                        );
 
                                win.unlockAccess3DScene();
                                win.repaint();
                        }
 
                        if (win.keyHit())
                        {
                                mrptKeyModifier kmods;
                                int key = win.getPushedKey(&kmods);
 
                                if (key == MRPTK_ESCAPE) break;
 
                                if (key == 's' || key == 'S') do_smooth = !do_smooth;
                                if (key == 'g' || key == 'G') do_grayscale = !do_grayscale;
                                if (key == 'f' || key == 'F') do_features = !do_features;
                        }
                }
        }
}
 
// ------------------------------------------------------
//                                              MAIN
// ------------------------------------------------------
int main()
{
        try
        {
                TestVideoBuildPyr();
 
                return 0;
        }
        catch (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}