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/system/CTimeLogger.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/system/filesystem.h>  // for ASSERT_FILE_EXISTS_
#include <mrpt/hwdrivers/CCameraSensor.h>
#include <mrpt/vision/CStereoRectifyMap.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <iostream>
 
using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::vision;
using namespace mrpt::system;
using namespace mrpt::img;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace mrpt::config;
using namespace mrpt::img;
using namespace std;
 
// ------------------------------------------------------
//                              TestStereoRectify
// ------------------------------------------------------
void TestStereoRectify(int argc, char** argv)
{
        CTimeLogger timlog;
        mrpt::vision::CStereoRectifyMap rectifyMap;
 
        // Parse optional arguments:
        if (argc != 1 && argc != 2)
        {
                cout
                        << "Usage:\n"
                        << argv[0]
                        << " ==> Run with default camera parameters (from rawlog file)\n"
                        << argv[0]
                        << "[params.cfg] ==> Load stereo camera parameters from cfg file\n";
        }
        if (argc == 2)
        {
                const string sCfgFile = argv[1];
                ASSERT_FILE_EXISTS_(sCfgFile);
 
                // Load params from file:
                mrpt::img::TStereoCamera params;
                params.loadFromConfigFile(
                        "CAMERA_PARAMS", mrpt::config::CConfigFile(sCfgFile));
 
                // Prepare rectify map:
                timlog.enter("rectifyMap.setFromCamParams");
                rectifyMap.setFromCamParams(params);
                timlog.leave("rectifyMap.setFromCamParams");
        }
 
        // Show to the user a list of possible camera drivers and creates and open
        // the selected camera.
        cout << "Please, select the input stereo camera or rawlog file (with "
                        "stereo images)...\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 stereo rectification", 1280, 600);
 
        // Create 2 viewports, one for each image:
        std::vector<COpenGLViewport::Ptr> gl_views(2);
        {
                COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();
                gl_views[0] = theScene->getViewport("main");
                ASSERT_(gl_views[0]);
                gl_views[1] = theScene->createViewport("right_image");
 
                // Assign sizes:
                gl_views[0]->setViewportPosition(0, 0, .5, 1.);
                gl_views[1]->setViewportPosition(.5, 0, .5, 1.);
 
                // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
                win.unlockAccess3DScene();
        }
 
        win.setPos(10, 10);
        //      win.addTextMessage(...
 
        bool enable_rectify = true;
        bool enable_draw_epipolar_lines = true;
        CImage img_left_rectified,
                img_right_rectified;  // Declared here to serve as a memory buffer
        // (avoid deallocating/allocating)
 
        cout << "Close the window to end.\n";
        while (win.isOpen())
        {
                win.addTextMessage(
                        5, 5, format("%.02fFPS", win.getRenderingFPS()), TColorf(1, 1, 1),
                        "sans", 15, mrpt::opengl::FILL, 0);
                win.addTextMessage(
                        5, 25, format(
                                           "'r': Switch rectify (Now is: %s) | '+'/'-': Modify "
                                           "alpha (Now is: %.02f)",
                                           enable_rectify ? "ON" : "OFF", rectifyMap.getAlpha()),
                        TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 10);
                win.addTextMessage(
                        5, 50,
                        format(
                                "'s': Switch resize output to 320x240 (Now is: %s) | 'c': "
                                "Switch no-disparity (Now is: %s) | 'e': Switch epipolar lines",
                                rectifyMap.isEnabledResizeOutput() ? "ON" : "OFF",
                                rectifyMap.isEnabledBothCentersCoincide() ? "ON" : "OFF"),
                        TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 11);
 
                std::this_thread::sleep_for(1ms);
 
                // Grab new video frame:
                CObservation::Ptr obs = cam->getNextFrame();
                if (obs)
                {
                        if (IS_CLASS(obs, CObservationStereoImages))
                        {
                                // Get the observation object:
                                CObservationStereoImages::Ptr o =
                                        std::dynamic_pointer_cast<CObservationStereoImages>(obs);
 
                                // If the rectification maps are still not ready, prepare them
                                // now:
                                if (!rectifyMap.isSet())
                                {
                                        timlog.enter("rectifyMap.setFromCamParams");
                                        rectifyMap.setFromCamParams(*o);
                                        timlog.leave("rectifyMap.setFromCamParams");
 
                                        /*mrpt::img::TStereoCamera params;
                                        o->getStereoCameraParams(params);
                                        cout << params.dumpAsText() << endl;*/
                                }
 
                                win.get3DSceneAndLock();
 
                                if (enable_rectify)
                                {
                                        // Rectify:
                                        timlog.enter("rectifyMap.rectify()");
 
                                        rectifyMap.rectify(
                                                o->imageLeft, o->imageRight, img_left_rectified,
                                                img_right_rectified);
 
                                        timlog.leave("rectifyMap.rectify()");
                                }
                                else
                                {
                                        // Don't rectify:
                                        img_left_rectified = o->imageLeft;
                                        img_right_rectified = o->imageRight;
                                }
 
                                // Draw lines:
                                if (enable_draw_epipolar_lines)
                                {
                                        const unsigned int LINES_SEP = 40;
                                        const unsigned int w = img_left_rectified.getWidth();
                                        const unsigned int h = img_left_rectified.getHeight();
                                        for (unsigned int y = 0; y < h; y += LINES_SEP)
                                        {
                                                img_left_rectified.line(
                                                        0, y, w - 1, y, mrpt::img::TColor::red(), 2);
                                                img_right_rectified.line(
                                                        0, y, w - 1, y, mrpt::img::TColor::red(), 2);
                                        }
                                }
 
                                gl_views[0]->setImageView(img_left_rectified);
                                gl_views[1]->setImageView(img_right_rectified);
 
                                win.addTextMessage(
                                        150, 5, mrpt::system::timeToString(o->timestamp),
                                        TColorf(1, 1, 1), "sans", 15, mrpt::opengl::FILL, 2);
 
                                win.unlockAccess3DScene();
                                win.repaint();
                        }
 
                        if (win.keyHit())
                        {
                                mrptKeyModifier kmods;
                                int key = win.getPushedKey(&kmods);
 
                                if (key == MRPTK_ESCAPE) break;
                                if (key == 'r' || key == 'R') enable_rectify = !enable_rectify;
                                if (key == 'e' || key == 'E')
                                        enable_draw_epipolar_lines = !enable_draw_epipolar_lines;
                                if (key == '+' || key == '-')
                                {
                                        double alpha =
                                                rectifyMap.getAlpha() + (key == '-' ? -0.1 : 0.1);
                                        alpha = std::min(1., std::max(0., alpha));
                                        rectifyMap.setAlpha(alpha);
                                }
                                if (key == 's' || key == 'S')
                                {
                                        rectifyMap.enableResizeOutput(
                                                !rectifyMap.isEnabledResizeOutput(), 320, 240);
                                }
                                if (key == 'c' || key == 'C')
                                {
                                        rectifyMap.enableBothCentersCoincide(
                                                !rectifyMap.isEnabledBothCentersCoincide());
                                }
                        }
                }
        }
}
 
// ------------------------------------------------------
//                                              MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
        try
        {
                TestStereoRectify(argc, argv);
                return 0;
        }
        catch (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}