CObservationStereoImages.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          |
   +------------------------------------------------------------------------+ */

#include "obs-precomp.h"  // Precompiled headers

#include <mrpt/math/CMatrixF.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/serialization/CArchive.h>

#if MRPT_HAS_MATLAB
#include <mexplus/mxarray.h>
#endif
#include <algorithm>  // all_of

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

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

uint8_t CObservationStereoImages::serializeGetVersion() const { return 6; }
void CObservationStereoImages::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    // The data
    out << cameraPose << leftCamera << rightCamera << imageLeft;
    out << hasImageDisparity << hasImageRight;
    if (hasImageRight) out << imageRight;
    if (hasImageDisparity) out << imageDisparity;
    out << timestamp;
    out << rightCameraPose;
    out << sensorLabel;
}

void CObservationStereoImages::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 6:
        {
            in >> cameraPose >> leftCamera >> rightCamera >> imageLeft;
            in >> hasImageDisparity >> hasImageRight;
            if (hasImageRight) in >> imageRight;
            if (hasImageDisparity) in >> imageDisparity;
            in >> timestamp;
            in >> rightCameraPose;
            in >> sensorLabel;
        }
        break;

        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
        case 5:
        {
            // This, for backwards compatibility before version 6:
            hasImageRight = true;
            hasImageDisparity = false;

            if (version < 5)
            {
                CPose3D aux;
                in >> aux;
                cameraPose = CPose3DQuat(aux);
            }

            if (version >= 5)
            {
                in >> cameraPose >> leftCamera >> rightCamera;
            }
            else
            {
                CMatrixF intParams;
                // Get the intrinsic params
                in >> intParams;
                // Set them to both cameras
                leftCamera.intrinsicParams = intParams;
                // ... distortion parameters are set to zero
                rightCamera.intrinsicParams = intParams;
            }

            in >> imageLeft >> imageRight;  // For all the versions

            if (version >= 1)
                in >> timestamp;
            else
                timestamp = INVALID_TIMESTAMP;  // For version 1 to 5
            if (version >= 2)
            {
                if (version < 5)
                {
                    CPose3D aux;
                    in >> aux;
                    rightCameraPose = CPose3DQuat(aux);
                }
                else
                    in >> rightCameraPose;
            }
            else
                rightCameraPose = CPose3DQuat(
                    0.10f, 0, 0,
                    mrpt::math::CQuaternionDouble(
                        1, 0, 0, 0));  // For version 1 to 5

            if (version >= 3 && version < 5)  // For versions 3 & 4
            {
                double foc;
                in >> foc;  // Get the focal length in meters
                leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
                    foc;  // ... and set it to both cameras
            }
            else if (version < 3)
                leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
                    0.002;  // For version 0, 1 & 2 (from version 5, this
            // parameter is included in the TCamera objects)

            if (version >= 4)
                in >> sensorLabel;
            else
                sensorLabel = "";  // For version 1 to 5
        }
        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::CObservationStereoImages)
#endif

mxArray* CObservationStereoImages::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
    const char* fields[] = {"class",   "ts",     "sensorLabel", "imageL",
                            "imageR",  "poseL",  "poseLR",      "poseR",
                            "paramsL", "paramsR"};
    mexplus::MxArray obs_struct(
        mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));

    obs_struct.set("class", this->GetRuntimeClass()->className);
    obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
    obs_struct.set("sensorLabel", this->sensorLabel);
    obs_struct.set("imageL", this->imageLeft);
    obs_struct.set("imageR", this->imageRight);
    obs_struct.set("poseL", this->cameraPose);
    obs_struct.set("poseR", this->cameraPose + this->rightCameraPose);
    obs_struct.set("poseLR", this->rightCameraPose);
    obs_struct.set("paramsL", this->leftCamera);
    obs_struct.set("paramsR", this->rightCamera);
    return obs_struct.release();
#else
    THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
#endif
}

/** Populates a TStereoCamera structure with the parameters in \a leftCamera, \a
 * rightCamera and \a rightCameraPose */
void CObservationStereoImages::getStereoCameraParams(
    mrpt::img::TStereoCamera& out_params) const
{
    out_params.leftCamera = this->leftCamera;
    out_params.rightCamera = this->rightCamera;
    out_params.rightCameraPose = this->rightCameraPose.asTPose();
}

/** Sets \a leftCamera, \a rightCamera and \a rightCameraPose from a
 * TStereoCamera structure */
void CObservationStereoImages::setStereoCameraParams(
    const mrpt::img::TStereoCamera& in_params)
{
    this->leftCamera = in_params.leftCamera;
    this->rightCamera = in_params.rightCamera;
    this->rightCameraPose = in_params.rightCameraPose;
}

/** This method only checks whether ALL the distortion parameters in \a
 * leftCamera are set to zero, which is
 * the convention in MRPT to denote that this pair of stereo images has been
 * rectified.
 */
bool CObservationStereoImages::areImagesRectified() const
{
    return std::all_of(
        leftCamera.dist.begin(), leftCamera.dist.end(),
        [](auto v) { return v == 0; });
}

// Do an efficient swap of all data members of this object with "o".
void CObservationStereoImages::swap(CObservationStereoImages& o)
{
    CObservation::swap(o);

    imageLeft.swap(o.imageLeft);
    imageRight.swap(o.imageRight);
    imageDisparity.swap(o.imageDisparity);

    std::swap(hasImageDisparity, o.hasImageDisparity);
    std::swap(hasImageRight, o.hasImageRight);

    std::swap(leftCamera, o.leftCamera);
    std::swap(rightCamera, o.rightCamera);

    std::swap(cameraPose, o.cameraPose);
    std::swap(rightCameraPose, o.rightCameraPose);
}

void CObservationStereoImages::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<CMatrixDouble44>() << "\n"
      << "Camera pose: " << cameraPose << "\n"
      << "Camera pose (YPR): " << CPose3D(cameraPose) << "\n"
      << "\n";

    mrpt::img::TStereoCamera stParams;
    getStereoCameraParams(stParams);
    o << stParams.dumpAsText() << "\n";

    o << "Right camera pose wrt left camera (YPR):"
      << "\n"
      << CPose3D(stParams.rightCameraPose) << "\n";

    if (imageLeft.isExternallyStored())
        o << " Left image is stored externally in file: "
          << imageLeft.getExternalStorageFile() << "\n";

    o << " Right image";
    if (hasImageRight)
    {
        if (imageRight.isExternallyStored())
            o << " is stored externally in file: "
              << imageRight.getExternalStorageFile() << "\n";
    }
    else
        o << " : No.\n";

    o << " Disparity image";
    if (hasImageDisparity)
    {
        if (imageDisparity.isExternallyStored())
            o << " is stored externally in file: "
              << imageDisparity.getExternalStorageFile() << "\n";
    }
    else
        o << " : No.\n";

    if (!imageLeft.isEmpty())
    {
        o << format(
            " Image size: %ux%u pixels\n", (unsigned int)imageLeft.getWidth(),
            (unsigned int)imageLeft.getHeight());

        o << " Channels order: " << imageLeft.getChannelsOrder() << "\n";

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

void CObservationStereoImages::load() const
{
    imageLeft.forceLoad();
    imageRight.forceLoad();
    imageDisparity.forceLoad();
}