CObservationRGBD360.h

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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/img/CImage.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/opengl/pointcloud_adapters.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CSerializable.h>

namespace mrpt
{
namespace obs
{
//  namespace detail {
//      // Implemented in CObservationRGBD360_project3D_impl.h
//      template <class POINTMAP>
//      void unprojectInto(CObservationRGBD360 &src_obs,
// POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const
// mrpt::poses::CPose3D * robotPoseInTheWorld, const bool PROJ3D_USE_LUT);
//  }

/** Declares a class derived from "CObservation" that
 *      encapsules an omnidirectional RGBD measurement from a set of RGBD
 *sensors.
 *  This kind of observations can carry one or more of these data fields:
 *    - 3D point cloud (as float's).
 *    - 2D range image (as a matrix): Each entry in the matrix
 *"rangeImage(ROW,COLUMN)" contains a distance or a depth (in meters), depending
 *on \a range_is_depth.
 *    - 2D intensity (grayscale or RGB) image (as a mrpt::img::CImage): For
 *SwissRanger cameras, a logarithmic A-law compression is used to convert the
 *original 16bit intensity to a more standard 8bit graylevel.
 *
 *  The coordinates of the 3D point cloud are in millimeters with respect to the
 *RGB camera origin of coordinates
 *
 *  The 2D images and matrices are stored as common images, with an up->down
 *rows order and left->right, as usual.
 *   Optionally, the intensity and confidence channels can be set to
 *delayed-load images for off-rawlog storage so it saves
 *   memory by having loaded in memory just the needed images. See the methods
 *load() and unload().
 *  Due to the intensive storage requirements of this kind of observations, this
 *observation is the only one in MRPT
 *   for which it's recommended to always call "load()" and "unload()" before
 *and after using the observation, *ONLY* when
 *   the observation was read from a rawlog dataset, in order to make sure that
 *all the externally stored data fields are
 *   loaded and ready in memory.
 *
 *  Classes that grab observations of this type are:
 *      - mrpt::hwdrivers::CSwissRanger3DCamera
 *      - mrpt::hwdrivers::CKinect
 *
 *
 *  3D point clouds can be generated at any moment after grabbing with
 *CObservationRGBD360::unprojectInto() and
 *CObservationRGBD360::unprojectInto(), provided the correct
 *   calibration parameters.
 *
 *  \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud
 *and the rangeImage can both be stored externally to save rawlog space.
 *  \note Starting at serialization version 5 (MRPT 0.9.5+), the new field \a
 *range_is_depth
 *  \note Starting at serialization version 6 (MRPT 0.9.5+), the new field \a
 *intensityImageChannel
 *
 * \sa mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::COpenNI2_RGBD360,
 *CObservation
 * \ingroup mrpt_obs_grp
 */
class CObservationRGBD360 : public CObservation
{
    DEFINE_SERIALIZABLE(CObservationRGBD360, mrpt::obs)

   protected:
    /** If set to true, m_points3D_external_file is valid. */
    bool m_points3D_external_stored{false};
    /** 3D points are in CImage::getImagesPathBase()+<this_file_name> */
    std::string m_points3D_external_file;

    /** If set to true, m_rangeImage_external_file is valid. */
    bool m_rangeImage_external_stored{false};
    /** rangeImage is in CImage::getImagesPathBase()+<this_file_name> */
    std::string m_rangeImage_external_file;

   public:
    /** Default constructor */
    CObservationRGBD360();
    /** Destructor */
    ~CObservationRGBD360() override;

    static const unsigned int NUM_SENSORS = 2;

    mrpt::system::TTimeStamp timestamps[NUM_SENSORS];

    /** true means the field rangeImage contains valid data */
    bool hasRangeImage{false};
    /** If hasRangeImage=true, a matrix of floats with the range data as
     * captured by the camera (in meters) \sa range_is_depth */
    mrpt::math::CMatrix_u16 rangeImages[NUM_SENSORS];

    /** Units for integer depth values in rangeImages */
    float rangeUnits = 0.001f;

    /** Similar to calling "rangeImage.setSize(H,W)" but this method provides
     * memory pooling to speed-up the memory allocation. */
    void rangeImage_setSize(
        const int HEIGHT, const int WIDTH, const unsigned sensor_id);

    /** true means the field intensityImage contains valid data */
    bool hasIntensityImage{false};
    /** If hasIntensityImage=true, a color or gray-level intensity image of the
     * same size than "rangeImage" */
    mrpt::img::CImage intensityImages[NUM_SENSORS];
    /** Projection parameters of the 8 RGBD sensor. */
    mrpt::img::TCamera sensorParamss[NUM_SENSORS];

    /** The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
     */
    float maxRange{10.0f};
    /** The 6D pose of the sensor on the robot. */
    mrpt::poses::CPose3D sensorPose;
    /** The "sigma" error of the device in meters, used while inserting the scan
     * in an occupancy grid. */
    float stdError{0.01f};

    // See base class docs
    void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
    {
        out_sensorPose = sensorPose;
    }
    void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
    {
        sensorPose = newSensorPose;
    }
    void getDescriptionAsText(std::ostream& o) const override;

};  // End of class def.

}  // namespace obs

}  // namespace mrpt