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/detectors.h>
#include <mrpt/hwdrivers/CCameraSensor.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/gui.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/math/ops_containers.h>
#include <iostream>
#include <mrpt/core/exceptions.h>
#include <mrpt/examples_config.h>
#include <mrpt/img/TColor.h>
 
using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::hwdrivers;
using namespace mrpt::detectors;
using namespace mrpt::config;
using namespace std;
using namespace mrpt::img;
using namespace mrpt::serialization;
 
string myDataDir(MRPT_EXAMPLES_BASE_DIRECTORY + string("detectors_face/"));
string myInitFile(
        MRPT_EXAMPLES_BASE_DIRECTORY +
        string("detectors_face/FACE_DETECTION_TEST.INI"));
 
CFaceDetection faceDetector;  // Face detector object
 
bool showEachDetectedFace;  // If using a 3D face detection (actually with
// swissrange) and we want stop every a face is
// detected for analize it.
bool batchMode;
vector<string> rawlogs;
vector<std::vector<uint32_t>> falsePositives;
vector<std::vector<uint32_t>> ignored;
string rawlogsDir;
 
#ifdef MRPT_OPENCV_SRC_DIR
static string OPENCV_SRC_DIR = MRPT_OPENCV_SRC_DIR;
#else
static string OPENCV_SRC_DIR = "./";
#endif
 
// ------------------------------------------------------
//                              TestCamera3DFaceDetection
// ------------------------------------------------------
void TestCamera3DFaceDetection(CCameraSensor::Ptr cam)
{
        CDisplayWindow win("Live video");
        CDisplayWindow win2("FaceDetected");
 
        cout << "Close the window to exit." << endl;
 
        mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600);
        mrpt::gui::CDisplayWindow3D win3D2;
 
        win3D.setCameraAzimuthDeg(140);
        win3D.setCameraElevationDeg(20);
        win3D.setCameraZoom(6.0);
        win3D.setCameraPointingToPoint(2.5, 0, 0);
 
        mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
        mrpt::opengl::COpenGLScene::Ptr scene2;
 
        mrpt::opengl::CPointCloudColoured::Ptr gl_points =
                mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
        gl_points->setPointSize(4.5);
 
        mrpt::opengl::CPointCloudColoured::Ptr gl_points2 =
                mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
        gl_points2->setPointSize(4.5);
 
        // 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());
 
        win3D.unlockAccess3DScene();
 
        if (showEachDetectedFace)
        {
                win3D2.setWindowTitle("3D Face detected");
                win3D2.resize(400, 300);
 
                win3D2.setCameraAzimuthDeg(140);
                win3D2.setCameraElevationDeg(20);
                win3D2.setCameraZoom(6.0);
                win3D2.setCameraPointingToPoint(2.5, 0, 0);
 
                scene2 = win3D2.get3DSceneAndLock();
 
                scene2->insert(gl_points2);
                scene2->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
 
                win3D2.unlockAccess3DScene();
        }
 
        double counter = 0;
        mrpt::system::CTicTac tictac;
 
        CVectorDouble fps;
 
        while (win.isOpen())
        {
                if (!counter) tictac.Tic();
 
                CObservation3DRangeScan::Ptr o;
 
                try
                {
                        o = std::dynamic_pointer_cast<CObservation3DRangeScan>(
                                cam->getNextFrame());
                }
                catch (CExceptionEOF&)
                {
                        break;
                }
                ASSERT_(o);
 
                vector_detectable_object detected;
 
                // CObservation3DRangeScan::Ptr o =
                // std::dynamic_pointer_cast<CObservation3DRangeScan>(obs);
 
                faceDetector.detectObjects(o, detected);
                // static int x = 0;
 
                if (detected.size() > 0)
                {
                        for (unsigned int i = 0; i < detected.size(); i++)
                        {
                                ASSERT_(IS_CLASS(detected[i], CDetectable3D));
                                CDetectable3D::Ptr obj =
                                        std::dynamic_pointer_cast<CDetectable3D>(detected[i]);
 
                                if (showEachDetectedFace)
                                {
                                        CObservation3DRangeScan face;
                                        o->getZoneAsObs(
                                                face, obj->m_y, obj->m_y + obj->m_height, obj->m_x,
                                                obj->m_x + obj->m_width);
                                        win2.showImage(face.intensityImage);
 
                                        if (o->hasPoints3D)
                                        {
                                                win3D2.get3DSceneAndLock();
 
                                                CColouredPointsMap pntsMap;
 
                                                if (!o->hasConfidenceImage)
                                                {
                                                        pntsMap.colorScheme.scheme =
                                                                CColouredPointsMap::cmFromIntensityImage;
                                                        pntsMap.loadFromRangeScan(face);
                                                }
                                                else
                                                {
                                                        vector<float> xs, ys, zs;
                                                        unsigned int i = 0;
                                                        for (unsigned int j = 0;
                                                                 j < face.confidenceImage.getHeight(); j++)
                                                                for (unsigned int k = 0;
                                                                         k < face.confidenceImage.getWidth();
                                                                         k++, i++)
                                                                {
                                                                        unsigned char c =
                                                                                *(face.confidenceImage.get_unsafe(
                                                                                        k, j, 0));
                                                                        if (c > faceDetector.m_options
                                                                                                .confidenceThreshold)
                                                                        {
                                                                                xs.push_back(face.points3D_x[i]);
                                                                                ys.push_back(face.points3D_y[i]);
                                                                                zs.push_back(face.points3D_z[i]);
                                                                        }
                                                                }
 
                                                        pntsMap.setAllPoints(xs, ys, zs);
                                                }
 
                                                gl_points2->loadFromPointsMap(&pntsMap);
 
                                                win3D2.unlockAccess3DScene();
                                                win3D2.repaint();
                                        }
                                }
 
                                o->intensityImage.rectangle(
                                        obj->m_x, obj->m_y, obj->m_x + obj->m_width,
                                        obj->m_y + obj->m_height, TColor(255, 0, 0));
 
                                // x++;
                                // if (( showEachDetectedFace ) && ( x > 430 ) )
                                // system::pause();
                        }
                }
 
                win.showImage(o->intensityImage);
 
                /*if (( showEachDetectedFace ) && ( detected.size() ))
                                system::pause();*/
 
                win3D.get3DSceneAndLock();
                CColouredPointsMap pntsMap;
                pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage;
                pntsMap.loadFromRangeScan(*(o.get()));
 
                gl_points->loadFromPointsMap(&pntsMap);
                win3D.unlockAccess3DScene();
                win3D.repaint();
 
                if (++counter == 10)
                {
                        double t = tictac.Tac();
                        cout << "Frame Rate: " << counter / t << " fps" << endl;
                        fps.push_back(counter / t);
                        counter = 0;
                }
                std::this_thread::sleep_for(2ms);
        }
 
        cout << "Fps mean: " << fps.sumAll() / fps.size() << endl;
 
        faceDetector.experimental_showMeasurements();
 
        cout << "Closing..." << endl;
}
 
// ------------------------------------------------------
//                              TestCameraFaceDetection
// ------------------------------------------------------
void TestCameraFaceDetection()
{
        CCameraSensor::Ptr cam = prepareVideoSourceFromUserSelection();
 
        if (!cam)
        {
                cerr << "The user didn't pick any camera. Exiting." << endl;
                return;
        }
 
        mrpt::obs::CObservation::Ptr obs = cam->getNextFrame();
        ASSERT_(obs);
 
        if (IS_CLASS(obs, CObservation3DRangeScan))
        {
                TestCamera3DFaceDetection(cam);
                return;
        }
 
        CDisplayWindow win("Live video");
 
        cout << "Close the window to exit." << endl;
 
        double counter = 0;
        mrpt::system::CTicTac tictac;
 
        while (win.isOpen())
        {
                if (!counter) tictac.Tic();
 
                mrpt::obs::CObservation::Ptr obs;
                try
                {
                        obs = cam->getNextFrame();
                }
                catch (CExceptionEOF&)  // Check if eof, f.i. for RawLog files
                {
                        break;
                }
                ASSERT_(obs);
 
                if (IS_CLASS(obs, CObservationImage))
                {
                        vector_detectable_object detected;
                        faceDetector.detectObjects(obs, detected);
 
                        CObservationImage::Ptr o =
                                std::dynamic_pointer_cast<CObservationImage>(obs);
                        for (unsigned int i = 0; i < detected.size(); i++)
                        {
                                ASSERT_(IS_CLASS(detected[i], CDetectable2D));
                                CDetectable2D::Ptr obj =
                                        std::dynamic_pointer_cast<CDetectable2D>(detected[i]);
                                o->image.rectangle(
                                        obj->m_x, obj->m_y, obj->m_x + obj->m_width,
                                        obj->m_y + obj->m_height, TColor(255, 0, 0));
                        }
 
                        win.showImage(o->image);
                }
                else if (IS_CLASS(obs, CObservationStereoImages))
                {
                        vector_detectable_object detected;
                        faceDetector.detectObjects(obs, detected);
 
                        CObservationStereoImages::Ptr o =
                                std::dynamic_pointer_cast<CObservationStereoImages>(obs);
 
                        for (unsigned int i = 0; i < detected.size(); i++)
                        {
                                ASSERT_(IS_CLASS(detected[i], CDetectable2D));
                                CDetectable2D::Ptr obj =
                                        std::dynamic_pointer_cast<CDetectable2D>(detected[i]);
                                o->imageRight.rectangle(
                                        obj->m_x, obj->m_y, obj->m_x + obj->m_width,
                                        obj->m_y + obj->m_height, TColor(255, 0, 0));
                        }
 
                        win.showImage(o->imageRight);
                }
 
                if (++counter == 10)
                {
                        double t = tictac.Tac();
                        cout << "Frame Rate: " << counter / t << " fps" << endl;
                        counter = 0;
                }
                std::this_thread::sleep_for(2ms);
        }
 
        cout << "Closing..." << endl;
}
 
// ------------------------------------------------------
//                               TestImagesFaceDetection
// ------------------------------------------------------
void TestImagesFaceDetection(int argc, char* argv[])
{
        CImage img;
        CDisplayWindow win("Result");
        mrpt::system::CTicTac tictac;
 
        // For each aditional argument, tty to load an image and detect faces
        for (int i = 1; i < argc; i++)
        {
                string fileName(argv[i]);
 
                if (!img.loadFromFile(myDataDir + fileName))
                {
                        cerr << "Cannot load " << myDataDir + fileName << endl;
                        continue;
                }
 
                vector_detectable_object detected;
 
                tictac.Tic();
 
                faceDetector.detectObjects(&img, detected);
 
                cout << "Detection time: " << tictac.Tac() << " s" << endl;
 
                for (unsigned int i = 0; i < detected.size(); i++)
                {
                        ASSERT_(IS_CLASS(detected[i], CDetectable2D));
                        CDetectable2D::Ptr obj =
                                std::dynamic_pointer_cast<CDetectable2D>(detected[i]);
                        img.rectangle(
                                obj->m_x, obj->m_y, obj->m_x + obj->m_width,
                                obj->m_y + obj->m_height, TColor(255, 0, 0));
                }
 
                win.showImage(img);
 
                mrpt::system::pause();
        }
}
 
void BatchMode()
{
        vector<double> frame_rates;
 
        for (size_t i = 0; i < rawlogs.size(); i++)
        {
                CRawlog rawlog;
 
                rawlog.loadFromRawLogFile(string(rawlogsDir + rawlogs[i] + ".rawlog"));
 
                cout << "Processing Rawlog [" << i + 1 << "/" << rawlogs.size()
                         << "]: " << rawlogs[i] << endl;
 
                const unsigned int size = rawlog.size();
 
                mrpt::system::CTicTac tictac;
                size_t counter = 0;
 
                // PROCESS RAWLOG
                for (unsigned int j = 1; j < size; j++)
                {
                        if (!counter) tictac.Tic();
 
                        CObservation3DRangeScan::Ptr o =
                                std::dynamic_pointer_cast<CObservation3DRangeScan>(
                                        rawlog.getAsObservation(j));
 
                        ASSERT_(o);
 
                        vector_detectable_object detected;
 
                        faceDetector.detectObjects(o, detected);
 
                        if (++counter == 10)
                        {
                                double t = tictac.Tac();
                                frame_rates.push_back(counter / t);
                                counter = 0;
                        }
 
                        std::this_thread::sleep_for(2ms);
                }
 
                unsigned int falsePositivesDeleted, realFacesDeleted;
                faceDetector.debug_returnResults(
                        falsePositives[i], ignored[i], falsePositivesDeleted,
                        realFacesDeleted);
                cout << "False positives deleted: " << falsePositivesDeleted << endl;
                cout << "Real faces delted: " << realFacesDeleted << endl << endl;
        }
 
        cout << "Mean frame rate: " << sum(frame_rates) / frame_rates.size();
 
        // faceDetector.experimental_showMeasurements();
 
        system::pause();
}
 
void mySplit(const string& str)
{
        char *cstr, *p;
 
        cstr = new char[str.size() + 1];
        strcpy(cstr, str.c_str());
 
        // cstr now contains a c-string copy of str
 
        p = strtok(cstr, " ");
        while (p != nullptr)
        {
                string part(p);
                rawlogs.push_back(part);
                p = strtok(nullptr, " ");
        }
 
        delete[] cstr;
}
 
// ------------------------------------------------------
//                                      TestPrepareDetector
// ------------------------------------------------------
void TestPrepareDetector()
{
        CConfigFile cfg(myInitFile);
 
        int classifierType = cfg.read_int("Example", "classifierType", 0);
 
        if (classifierType == 0)  // Haar
                cfg.write(
                        "CascadeClassifier", "cascadeFileName",
                        OPENCV_SRC_DIR +
                                "/data/haarcascades/haarcascade_frontalface_alt2.xml");
        else if (classifierType == 1)  // LBP
                cfg.write(
                        "CascadeClassifier", "cascadeFileName",
                        OPENCV_SRC_DIR + "/data/lbpcascades/lbpcascade_frontalface.xml");
        else
                throw std::runtime_error("Incorrect cascade classifier type.");
 
        showEachDetectedFace = cfg.read_bool("Example", "showEachDetectedFace", 0);
        batchMode = cfg.read_bool("Example", "batchMode", false);
 
        if (batchMode)
        {
                string str = cfg.read_string("Example", "rawlogs", "noRawlogs");
                mySplit(str);
 
                size_t numRawlogs = rawlogs.size();
                falsePositives.resize(numRawlogs);
                ignored.resize(numRawlogs);
 
                for (size_t i = 0; i < numRawlogs; i++)
                {
                        cfg.read_vector(
                                rawlogs[i], "falsePositives", std::vector<uint32_t>(),
                                falsePositives[i]);
                        cfg.read_vector(
                                rawlogs[i], "ignored", std::vector<uint32_t>(), ignored[i]);
                }
 
                rawlogsDir = cfg.read_string("Example", "rawlogsDir", "");
        }
 
        faceDetector.init(cfg);
}
 
// ------------------------------------------------------
//                                              MAIN
// ------------------------------------------------------
int main(int argc, char* argv[])
{
        try
        {
                registerClass(CLASS_ID(CDetectableObject));
                registerClass(CLASS_ID(CDetectable2D));
                registerClass(CLASS_ID(CDetectable3D));
 
                TestPrepareDetector();
 
                if (batchMode)
                        BatchMode();
                else
                {
                        if (argc > 1)
                                TestImagesFaceDetection(argc, argv);
                        else
                                TestCameraFaceDetection();
                }
 
                return 0;
        }
        catch (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}