CObservationImage.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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 "obs-precomp.h"   // Precompiled headers

#include <mrpt/obs/CObservationImage.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/math/ops_vectors.h>  // << of std::vector()
#include <iostream>
#if MRPT_HAS_MATLAB
#       include <mexplus/mxarray.h>
#endif

using namespace mrpt::obs;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace mrpt::poses;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservationImage, CObservation,mrpt::obs)

/** Constructor
 */
CObservationImage::CObservationImage( void *iplImage  ) :
        cameraPose(),
        image( iplImage )
{
}

/*---------------------------------------------------------------
  Implements the writing to a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void  CObservationImage::writeToStream(mrpt::utils::CStream &out, int *version) const
{
        if (version)
                *version = 4;
        else
        {
                // The data
                out << cameraPose << cameraParams << image
                    << timestamp
                    << sensorLabel;
        }
}

/*---------------------------------------------------------------
  Implements the reading from a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void  CObservationImage::readFromStream(mrpt::utils::CStream &in, int version)
{
        switch(version)
        {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
                {
                        in >> cameraPose;

                        if (version>=4)
                        {
                                in >> cameraParams;
                        }
                        else
                        {
                                CMatrix  intrinsicParams, distortionParams;
                                in >> distortionParams >> intrinsicParams;

                                if (size(distortionParams,1)==1 && size(distortionParams,2)==5)
                                {
                                        const CMatrixDouble15 p = distortionParams.cast<double>();
                                        cameraParams.setDistortionParamsVector(p);
                                }
                                else    cameraParams.dist.assign(0);

                                cameraParams.intrinsicParams = intrinsicParams.block(0,0,3,3).cast<double>();
                        }

                        in >> image;

                        if (version>=1)
                                in >> timestamp;

                        if (version>=2)
                        {
                                if (version<4)
                                        in >> cameraParams.focalLengthMeters ;
                        }
                        else
                                cameraParams.focalLengthMeters = 0.002;

                        if (version>=3)
                                        in >> sensorLabel;
                        else    sensorLabel = "";

                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };

}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM( mrpt::obs::CObservationImage )

mxArray* CObservationImage::writeToMatlab() const
{
        const char* fields[] = {"class","ts","sensorLabel","image","pose","params"};
        mexplus::MxArray obs_struct( mexplus::MxArray::Struct(sizeof(fields)/sizeof(fields[0]),fields) );

        obs_struct.set("class", this->GetRuntimeClass()->className);
        obs_struct.set("ts", this->timestamp);
        obs_struct.set("sensorLabel", this->sensorLabel);
        obs_struct.set("image", this->image);
        obs_struct.set("pose", this->cameraPose);
        obs_struct.set("params", this->cameraParams);
        return obs_struct.release();
}
#endif

/*---------------------------------------------------------------
                                                getRectifiedImage
 ---------------------------------------------------------------*/
void  CObservationImage::getRectifiedImage( CImage &out_img ) const
{
        image.rectifyImage(out_img, cameraParams );
}

void CObservationImage::getDescriptionAsText(std::ostream &o) const
{
        using namespace std;
        CObservation::getDescriptionAsText(o);

        o << "Homogeneous matrix for the sensor's 3D pose, relative to robot base:\n";
        o << cameraPose.getHomogeneousMatrixVal()
        << cameraPose << endl;

        o << format("Focal length: %.03f mm\n",cameraParams.focalLengthMeters*1000);

        o << "Intrinsic parameters matrix for the camera:"<< endl
        << cameraParams.intrinsicParams.inMatlabFormat() << endl << cameraParams.intrinsicParams << endl;

        o << "Distorsion parameters for the camera: " << cameraParams.getDistortionParamsAsVector() << endl;

        if (image.isExternallyStored())
                o << " Image is stored externally in file: " << image.getExternalStorageFile() << endl;

        o << format(" Image size: %ux%u pixels\n", (unsigned int)image.getWidth(), (unsigned int)image.getHeight() );

        o << " Channels order: " << image.getChannelsOrder() << endl;

        o << format(" Rows are stored in top-bottom order: %s\n",
                                        image.isOriginTopLeft() ? "YES" : "NO");

}