test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

// -----------------------------------------------------------------------------------------------------------------------
// For this example, the rawlog file must contain both laser data and stereo
// images (only the left one will be considered)
// It may be used with single image observations -> just employ
// "CObservationImage::Ptr" instead of "CObservationStereoImages::Ptr"
// and access to the contained "image" instead of "imageLeft".
// -----------------------------------------------------------------------------------------------------------------------

#include <mrpt/gui/CDisplayWindow.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <iostream>

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::gui;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::img;
using namespace std;

#include <mrpt/examples_config.h>

// Default path, or user path passed thru command line:
std::string RAWLOG_FILE = MRPT_EXAMPLES_BASE_DIRECTORY
    "../share/mrpt/datasets/localization_demo.rawlog";

// ------------------------------------------------------
//                  TestGeometry3D
// ------------------------------------------------------
void TestLaser2Imgs()
{
    // Set your rawlog file name
    if (!mrpt::system::fileExists(RAWLOG_FILE))
        THROW_EXCEPTION_FMT(
            "Rawlog file does not exist: %s", RAWLOG_FILE.c_str());

    CActionCollection::Ptr action;
    CSensoryFrame::Ptr observations;
    size_t rawlogEntry = 0;
    // bool                 end             = false;
    CDisplayWindow wind;

    // Set relative path for externally-stored images in rawlogs:
    string rawlog_images_path = extractFileDirectory(RAWLOG_FILE);
    rawlog_images_path += "/Images";
    CImage::setImagesPathBase(rawlog_images_path);  // Set it.

    mrpt::io::CFileGZInputStream rawlogFile(RAWLOG_FILE);

    for (;;)
    {
        if (os::kbhit())
        {
            char c = os::getch();
            if (c == 27) break;
        }

        // Load action/observation pair from the rawlog:
        // --------------------------------------------------
        cout << "Reading act/oct pair from rawlog..." << endl;
        auto arch = mrpt::serialization::archiveFrom(rawlogFile);
        if (!CRawlog::readActionObservationPair(
                arch, action, observations, rawlogEntry))
            break;  // file EOF

        // CAMERA............
        // Get CObservationStereoImages
        CObservationStereoImages::Ptr sImgs;
        CObservationImage::Ptr Img;

        sImgs = observations->getObservationByClass<CObservationStereoImages>();
        if (!sImgs)
        {
            Img = observations->getObservationByClass<CObservationImage>();
            if (!Img) continue;
        }

        CPose3D cameraPose;  // Get Camera Pose (B) (CPose3D)
        CMatrixDouble33 K;  // Get Calibration matrix (K)

        sImgs ? sImgs->getSensorPose(cameraPose)
              : Img->getSensorPose(cameraPose);
        K = sImgs ? sImgs->leftCamera.intrinsicParams
                  : Img->cameraParams.intrinsicParams;

        // LASER.............
        // Get CObservationRange2D
        CObservation2DRangeScan::Ptr laserScan =
            observations->getObservationByClass<CObservation2DRangeScan>();
        if (!laserScan) continue;

        // Get Laser Pose (A) (CPose3D)
        CPose3D laserPose;
        laserScan->getSensorPose(laserPose);

        if (std::abs(laserPose.yaw()) > 90.0_deg)
            continue;  // Only front lasers

        // Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D)
        CPoint3D point;
        CSimplePointsMap mapa;
        mapa.insertionOptions.minDistBetweenLaserPoints = 0;
        observations->insertObservationsInto(
            &mapa);  // <- The map contains the pose of the points (P1)

        // Get the points into the map
        vector<float> X, Y, Z;
        vector<float>::iterator itX, itY, itZ;
        mapa.getAllPoints(X, Y, Z);

        unsigned int imgW =
            sImgs ? sImgs->imageLeft.getWidth() : Img->image.getWidth();
        unsigned int imgH =
            sImgs ? sImgs->imageLeft.getHeight() : Img->image.getHeight();

        // unsigned int         idx = 0;
        vector<float> imgX, imgY;
        vector<float>::iterator itImgX, itImgY;
        imgX.resize(X.size());
        imgY.resize(Y.size());

        CImage image;
        image = sImgs ? sImgs->imageLeft : Img->image;

        // Get pixels in the image:
        // Pimg = (kx,ky,k)^T = K(I|0)*P2
        // Main loop
        for (itX = X.begin(), itY = Y.begin(), itZ = Z.begin(),
            itImgX = imgX.begin(), itImgY = imgY.begin();
             itX != X.end(); itX++, itY++, itZ++, itImgX++, itImgY++)
        {
            // Coordinates Transformation
            CPoint3D pLaser(*itX, *itY, *itZ);
            CPoint3D pCamera(pLaser - cameraPose);

            if (pCamera.z() > 0)
            {
                *itImgX = -K(0, 0) * ((pCamera.x()) / (pCamera.z())) + K(0, 2);
                *itImgY = -K(1, 1) * ((pCamera.y()) / (pCamera.z())) + K(1, 2);

                if (*itImgX > 0 && *itImgX<imgW&& * itImgY> 0 && *itImgY < imgH)
                    image.filledRectangle(
                        *itImgX - 1, *itImgY - 1, *itImgX + 1, *itImgY + 1,
                        TColor(255, 0, 0));
            }  // end if
        }  // end for

        action.reset();
        observations.reset();

        wind.showImage(image);

        std::this_thread::sleep_for(50ms);
    };  // end for

    mrpt::system::pause();
}

// ------------------------------------------------------
//                        MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
    try
    {
        if (argc > 1)
        {
            RAWLOG_FILE = std::string(argv[1]);
        }

        TestLaser2Imgs();
        return 0;
    }
    catch (exception& e)
    {
        cerr << "EXCEPTION: " << e.what() << endl;
        return -1;
    }
    catch (...)
    {
        cerr << "Untyped exception!!";
        return -1;
    }
}