CObservation3DRangeScan.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 "obs-precomp.h"  // Precompiled headers

#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/opengl/CPointCloud.h>

#include <mrpt/math/CMatrix.h>
#include <mrpt/math/CLevenbergMarquardt.h>
#include <mrpt/math/ops_containers.h>  // norm(), etc.
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/core/bits_mem.h>  // vector_strong_clear
#include <limits>

using namespace std;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::img;
using mrpt::config::CConfigFileMemory;

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

// Static LUT:
static CObservation3DRangeScan::TCached3DProjTables lut_3dproj;
CObservation3DRangeScan::TCached3DProjTables&
    CObservation3DRangeScan::get_3dproj_lut()
{
    return lut_3dproj;
}

static bool EXTERNALS_AS_TEXT_value = false;
void CObservation3DRangeScan::EXTERNALS_AS_TEXT(bool value)
{
    EXTERNALS_AS_TEXT_value = value;
}
bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
{
    return EXTERNALS_AS_TEXT_value;
}

// Whether to use a memory pool for 3D points:
#define COBS3DRANGE_USE_MEMPOOL

// Do performance time logging?
//#define  PROJ3D_PERFLOG

// Data types for memory pooling CObservation3DRangeScan:
#ifdef COBS3DRANGE_USE_MEMPOOL

#include <mrpt/system/CGenericMemoryPool.h>

// Memory pool for XYZ points ----------------
struct CObservation3DRangeScan_Points_MemPoolParams
{
    /** Width*Height, that is, the number of 3D points */
    size_t WH;
    inline bool isSuitable(
        const CObservation3DRangeScan_Points_MemPoolParams& req) const
    {
        return WH >= req.WH;
    }
};
struct CObservation3DRangeScan_Points_MemPoolData
{
    std::vector<float> pts_x, pts_y, pts_z;
    /** for each point, the corresponding (x,y) pixel coordinates */
    std::vector<uint16_t> idxs_x, idxs_y;
};
using TMyPointsMemPool = mrpt::system::CGenericMemoryPool<
    CObservation3DRangeScan_Points_MemPoolParams,
    CObservation3DRangeScan_Points_MemPoolData>;

// Memory pool for the rangeImage matrix ----------------
struct CObservation3DRangeScan_Ranges_MemPoolParams
{
    /** Size of matrix */
    int H, W;
    inline bool isSuitable(
        const CObservation3DRangeScan_Ranges_MemPoolParams& req) const
    {
        return H == req.H && W == req.W;
    }
};
struct CObservation3DRangeScan_Ranges_MemPoolData
{
    mrpt::math::CMatrix rangeImage;
};
using TMyRangesMemPool = mrpt::system::CGenericMemoryPool<
    CObservation3DRangeScan_Ranges_MemPoolParams,
    CObservation3DRangeScan_Ranges_MemPoolData>;

void mempool_donate_xyz_buffers(CObservation3DRangeScan& obs)
{
    if (!obs.points3D_x.empty())
    {
        // Before dying, donate my memory to the pool for the joy of future
        // class-brothers...
        TMyPointsMemPool* pool = TMyPointsMemPool::getInstance();
        if (pool)
        {
            CObservation3DRangeScan_Points_MemPoolParams mem_params;
            mem_params.WH = obs.points3D_x.capacity();
            if (obs.points3D_y.capacity() != mem_params.WH)
                obs.points3D_y.resize(mem_params.WH);
            if (obs.points3D_z.capacity() != mem_params.WH)
                obs.points3D_z.resize(mem_params.WH);
            if (obs.points3D_idxs_x.capacity() != mem_params.WH)
                obs.points3D_idxs_x.resize(mem_params.WH);
            if (obs.points3D_idxs_y.capacity() != mem_params.WH)
                obs.points3D_idxs_y.resize(mem_params.WH);

            CObservation3DRangeScan_Points_MemPoolData* mem_block =
                new CObservation3DRangeScan_Points_MemPoolData();
            obs.points3D_x.swap(mem_block->pts_x);
            obs.points3D_y.swap(mem_block->pts_y);
            obs.points3D_z.swap(mem_block->pts_z);
            obs.points3D_idxs_x.swap(mem_block->idxs_x);
            obs.points3D_idxs_y.swap(mem_block->idxs_y);

            pool->dump_to_pool(mem_params, mem_block);
        }
    }
}
void mempool_donate_range_matrix(CObservation3DRangeScan& obs)
{
    if (obs.rangeImage.cols() > 1 && obs.rangeImage.rows() > 1)
    {
        // Before dying, donate my memory to the pool for the joy of future
        // class-brothers...
        TMyRangesMemPool* pool = TMyRangesMemPool::getInstance();
        if (pool)
        {
            CObservation3DRangeScan_Ranges_MemPoolParams mem_params;
            mem_params.H = obs.rangeImage.rows();
            mem_params.W = obs.rangeImage.cols();

            CObservation3DRangeScan_Ranges_MemPoolData* mem_block =
                new CObservation3DRangeScan_Ranges_MemPoolData();
            obs.rangeImage.swap(mem_block->rangeImage);

            pool->dump_to_pool(mem_params, mem_block);
        }
    }
}

#endif

/*---------------------------------------------------------------
                            Constructor
 ---------------------------------------------------------------*/
CObservation3DRangeScan::CObservation3DRangeScan()
    : m_points3D_external_stored(false),
      m_rangeImage_external_stored(false),
      hasPoints3D(false),
      hasRangeImage(false),
      range_is_depth(true),
      hasIntensityImage(false),
      intensityImageChannel(CH_VISIBLE),
      hasConfidenceImage(false),
      pixelLabels(),  // Start without label info
      cameraParams(),
      cameraParamsIntensity(),
      relativePoseIntensityWRTDepth(
          0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90)),
      maxRange(5.0f),
      sensorPose(),
      stdError(0.01f)
{
}

CObservation3DRangeScan::~CObservation3DRangeScan()
{
#ifdef COBS3DRANGE_USE_MEMPOOL
    mempool_donate_xyz_buffers(*this);
    mempool_donate_range_matrix(*this);
#endif
}

uint8_t CObservation3DRangeScan::serializeGetVersion() const { return 8; }
void CObservation3DRangeScan::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    // The data
    out << maxRange << sensorPose;
    out << hasPoints3D;
    if (hasPoints3D)
    {
        ASSERT_(
            points3D_x.size() == points3D_y.size() &&
            points3D_x.size() == points3D_z.size() &&
            points3D_idxs_x.size() == points3D_x.size() &&
            points3D_idxs_y.size() == points3D_x.size());
        uint32_t N = points3D_x.size();
        out << N;
        if (N)
        {
            out.WriteBufferFixEndianness(&points3D_x[0], N);
            out.WriteBufferFixEndianness(&points3D_y[0], N);
            out.WriteBufferFixEndianness(&points3D_z[0], N);
            out.WriteBufferFixEndianness(&points3D_idxs_x[0], N);  // New in v8
            out.WriteBufferFixEndianness(&points3D_idxs_y[0], N);  // New in v8
        }
    }

    out << hasRangeImage;
    if (hasRangeImage) out << rangeImage;
    out << hasIntensityImage;
    if (hasIntensityImage) out << intensityImage;
    out << hasConfidenceImage;
    if (hasConfidenceImage) out << confidenceImage;

    out << cameraParams;  // New in v2
    out << cameraParamsIntensity;  // New in v4
    out << relativePoseIntensityWRTDepth;  // New in v4

    out << stdError;
    out << timestamp;
    out << sensorLabel;

    // New in v3:
    out << m_points3D_external_stored << m_points3D_external_file;
    out << m_rangeImage_external_stored << m_rangeImage_external_file;

    // New in v5:
    out << range_is_depth;

    // New in v6:
    out << static_cast<int8_t>(intensityImageChannel);

    // New in v7:
    out << hasPixelLabels();
    if (hasPixelLabels())
    {
        pixelLabels->writeToStream(out);
    }
}

void CObservation3DRangeScan::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
        case 5:
        case 6:
        case 7:
        case 8:
        {
            uint32_t N;

            in >> maxRange >> sensorPose;

            if (version > 0)
                in >> hasPoints3D;
            else
                hasPoints3D = true;

            if (hasPoints3D)
            {
                in >> N;
                resizePoints3DVectors(N);

                if (N)
                {
                    in.ReadBufferFixEndianness(&points3D_x[0], N);
                    in.ReadBufferFixEndianness(&points3D_y[0], N);
                    in.ReadBufferFixEndianness(&points3D_z[0], N);

                    if (version == 0)
                    {
                        vector<char> validRange(N);  // for v0.
                        in.ReadBuffer(
                            &validRange[0], sizeof(validRange[0]) * N);
                    }
                    if (version >= 8)
                    {
                        in.ReadBufferFixEndianness(&points3D_idxs_x[0], N);
                        in.ReadBufferFixEndianness(&points3D_idxs_y[0], N);
                    }
                }
            }
            else
            {
                this->resizePoints3DVectors(0);
            }

            if (version >= 1)
            {
                in >> hasRangeImage;
                if (hasRangeImage)
                {
#ifdef COBS3DRANGE_USE_MEMPOOL
                    // We should call "rangeImage_setSize()" to exploit the
                    // mempool:
                    this->rangeImage_setSize(480, 640);
#endif
                    in >> rangeImage;
                }

                in >> hasIntensityImage;
                if (hasIntensityImage) in >> intensityImage;

                in >> hasConfidenceImage;
                if (hasConfidenceImage) in >> confidenceImage;

                if (version >= 2)
                {
                    in >> cameraParams;

                    if (version >= 4)
                    {
                        in >> cameraParamsIntensity >>
                            relativePoseIntensityWRTDepth;
                    }
                    else
                    {
                        cameraParamsIntensity = cameraParams;
                        relativePoseIntensityWRTDepth = CPose3D();
                    }
                }
            }

            in >> stdError;
            in >> timestamp;
            in >> sensorLabel;

            if (version >= 3)
            {
                // New in v3:
                in >> m_points3D_external_stored >> m_points3D_external_file;
                in >> m_rangeImage_external_stored >>
                    m_rangeImage_external_file;
            }
            else
            {
                m_points3D_external_stored = false;
                m_rangeImage_external_stored = false;
            }

            if (version >= 5)
            {
                in >> range_is_depth;
            }
            else
            {
                range_is_depth = true;
            }

            if (version >= 6)
            {
                int8_t i;
                in >> i;
                intensityImageChannel = static_cast<TIntensityChannelID>(i);
            }
            else
            {
                intensityImageChannel = CH_VISIBLE;
            }

            pixelLabels.reset();  // Remove existing data first (_unique() is to
            // leave alive any user copies of the shared
            // pointer).
            if (version >= 7)
            {
                bool do_have_labels;
                in >> do_have_labels;

                if (do_have_labels)
                    pixelLabels.reset(
                        TPixelLabelInfoBase::readAndBuildFromStream(in));
            }
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
}

void CObservation3DRangeScan::swap(CObservation3DRangeScan& o)
{
    CObservation::swap(o);

    std::swap(hasPoints3D, o.hasPoints3D);
    points3D_x.swap(o.points3D_x);
    points3D_y.swap(o.points3D_y);
    points3D_z.swap(o.points3D_z);
    points3D_idxs_x.swap(o.points3D_idxs_x);
    points3D_idxs_y.swap(o.points3D_idxs_y);
    std::swap(m_points3D_external_stored, o.m_points3D_external_stored);
    std::swap(m_points3D_external_file, o.m_points3D_external_file);

    std::swap(hasRangeImage, o.hasRangeImage);
    rangeImage.swap(o.rangeImage);
    std::swap(m_rangeImage_external_stored, o.m_rangeImage_external_stored);
    std::swap(m_rangeImage_external_file, o.m_rangeImage_external_file);

    std::swap(hasIntensityImage, o.hasIntensityImage);
    std::swap(intensityImageChannel, o.intensityImageChannel);
    intensityImage.swap(o.intensityImage);

    std::swap(hasConfidenceImage, o.hasConfidenceImage);
    confidenceImage.swap(o.confidenceImage);

    std::swap(pixelLabels, o.pixelLabels);

    std::swap(relativePoseIntensityWRTDepth, o.relativePoseIntensityWRTDepth);

    std::swap(cameraParams, o.cameraParams);
    std::swap(cameraParamsIntensity, o.cameraParamsIntensity);

    std::swap(maxRange, o.maxRange);
    std::swap(sensorPose, o.sensorPose);
    std::swap(stdError, o.stdError);
}

void CObservation3DRangeScan::load() const
{
    if (hasPoints3D && m_points3D_external_stored)
    {
        const string fil = points3D_getExternalStorageFileAbsolutePath();
        if (mrpt::system::strCmpI(
                "txt", mrpt::system::extractFileExtension(fil, true)))
        {
            CMatrixFloat M;
            M.loadFromTextFile(fil);
            ASSERT_EQUAL_(M.rows(), 3);

            M.extractRow(0, const_cast<std::vector<float>&>(points3D_x));
            M.extractRow(1, const_cast<std::vector<float>&>(points3D_y));
            M.extractRow(2, const_cast<std::vector<float>&>(points3D_z));
        }
        else
        {
            mrpt::io::CFileGZInputStream fi(fil);
            auto f = mrpt::serialization::archiveFrom(fi);
            f >> const_cast<std::vector<float>&>(points3D_x) >>
                const_cast<std::vector<float>&>(points3D_y) >>
                const_cast<std::vector<float>&>(points3D_z);
        }
    }

    if (hasRangeImage && m_rangeImage_external_stored)
    {
        const string fil = rangeImage_getExternalStorageFileAbsolutePath();
        if (mrpt::system::strCmpI(
                "txt", mrpt::system::extractFileExtension(fil, true)))
        {
            const_cast<CMatrix&>(rangeImage).loadFromTextFile(fil);
        }
        else
        {
            mrpt::io::CFileGZInputStream fi(fil);
            auto f = mrpt::serialization::archiveFrom(fi);
            f >> const_cast<CMatrix&>(rangeImage);
        }
    }
}

void CObservation3DRangeScan::unload()
{
    if (hasPoints3D && m_points3D_external_stored)
    {
        mrpt::vector_strong_clear(points3D_x);
        mrpt::vector_strong_clear(points3D_y);
        mrpt::vector_strong_clear(points3D_z);
    }

    if (hasRangeImage && m_rangeImage_external_stored) rangeImage.setSize(0, 0);

    intensityImage.unload();
    confidenceImage.unload();
}

void CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath(
    std::string& out_path) const
{
    ASSERT_(m_rangeImage_external_file.size() > 2);
    if (m_rangeImage_external_file[0] == '/' ||
        (m_rangeImage_external_file[1] == ':' &&
         m_rangeImage_external_file[2] == '\\'))
    {
        out_path = m_rangeImage_external_file;
    }
    else
    {
        out_path = CImage::getImagesPathBase();
        size_t N = CImage::getImagesPathBase().size() - 1;
        if (CImage::getImagesPathBase()[N] != '/' &&
            CImage::getImagesPathBase()[N] != '\\')
            out_path += "/";
        out_path += m_rangeImage_external_file;
    }
}
void CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath(
    std::string& out_path) const
{
    ASSERT_(m_points3D_external_file.size() > 2);
    if (m_points3D_external_file[0] == '/' ||
        (m_points3D_external_file[1] == ':' &&
         m_points3D_external_file[2] == '\\'))
    {
        out_path = m_points3D_external_file;
    }
    else
    {
        out_path = CImage::getImagesPathBase();
        size_t N = CImage::getImagesPathBase().size() - 1;
        if (CImage::getImagesPathBase()[N] != '/' &&
            CImage::getImagesPathBase()[N] != '\\')
            out_path += "/";
        out_path += m_points3D_external_file;
    }
}

void CObservation3DRangeScan::points3D_convertToExternalStorage(
    const std::string& fileName_, const std::string& use_this_base_dir)
{
    ASSERT_(!points3D_isExternallyStored());
    ASSERT_(
        points3D_x.size() == points3D_y.size() &&
        points3D_x.size() == points3D_z.size());

    if (EXTERNALS_AS_TEXT_value)
        m_points3D_external_file =
            mrpt::system::fileNameChangeExtension(fileName_, "txt");
    else
        m_points3D_external_file =
            mrpt::system::fileNameChangeExtension(fileName_, "bin");

    // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
    // instead of CImage::getImagesPathBase()
    const string savedDir = CImage::getImagesPathBase();
    CImage::setImagesPathBase(use_this_base_dir);
    const string real_absolute_file_path =
        points3D_getExternalStorageFileAbsolutePath();
    CImage::setImagesPathBase(savedDir);

    if (EXTERNALS_AS_TEXT_value)
    {
        const size_t nPts = points3D_x.size();

        CMatrixFloat M(3, nPts);
        M.insertRow(0, points3D_x);
        M.insertRow(1, points3D_y);
        M.insertRow(2, points3D_z);

        M.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
    }
    else
    {
        mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
        auto f = mrpt::serialization::archiveFrom(fo);
        f << points3D_x << points3D_y << points3D_z;
    }

    m_points3D_external_stored = true;

    // Really dealloc memory, clear() is not enough:
    vector_strong_clear(points3D_x);
    vector_strong_clear(points3D_y);
    vector_strong_clear(points3D_z);
    vector_strong_clear(points3D_idxs_x);
    vector_strong_clear(points3D_idxs_y);
}
void CObservation3DRangeScan::rangeImage_convertToExternalStorage(
    const std::string& fileName_, const std::string& use_this_base_dir)
{
    ASSERT_(!rangeImage_isExternallyStored());
    if (EXTERNALS_AS_TEXT_value)
        m_rangeImage_external_file =
            mrpt::system::fileNameChangeExtension(fileName_, "txt");
    else
        m_rangeImage_external_file =
            mrpt::system::fileNameChangeExtension(fileName_, "bin");

    // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
    // instead of CImage::getImagesPathBase()
    const string savedDir = CImage::getImagesPathBase();
    CImage::setImagesPathBase(use_this_base_dir);
    const string real_absolute_file_path =
        rangeImage_getExternalStorageFileAbsolutePath();
    CImage::setImagesPathBase(savedDir);

    if (EXTERNALS_AS_TEXT_value)
    {
        rangeImage.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
    }
    else
    {
        mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
        auto f = mrpt::serialization::archiveFrom(fo);
        f << rangeImage;
    }

    m_rangeImage_external_stored = true;
    rangeImage.setSize(0, 0);
}

// ==============  Auxiliary function for "recoverCameraCalibrationParameters"
// =========================

#define CALIB_DECIMAT 15

namespace mrpt::obs::detail
{
struct TLevMarData
{
    const CObservation3DRangeScan& obs;
    const double z_offset;
    TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
        : obs(obs_), z_offset(z_offset_)
    {
    }
};

void cam2vec(const TCamera& camPar, CVectorDouble& x)
{
    if (x.size() < 4 + 4) x.resize(4 + 4);

    x[0] = camPar.fx();
    x[1] = camPar.fy();
    x[2] = camPar.cx();
    x[3] = camPar.cy();

    for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
}
void vec2cam(const CVectorDouble& x, TCamera& camPar)
{
    camPar.intrinsicParams(0, 0) = x[0];  // fx
    camPar.intrinsicParams(1, 1) = x[1];  // fy
    camPar.intrinsicParams(0, 2) = x[2];  // cx
    camPar.intrinsicParams(1, 2) = x[3];  // cy

    for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
}
void cost_func(
    const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
{
    const CObservation3DRangeScan& obs = d.obs;

    TCamera params;
    vec2cam(par, params);

    const size_t nC = obs.rangeImage.cols();
    const size_t nR = obs.rangeImage.rows();

    err = CVectorDouble();  // .resize( nC*nR/square(CALIB_DECIMAT) );

    for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
    {
        for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
        {
            const size_t idx = nC * r + c;

            TPoint3D p(
                obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
                obs.points3D_z[idx]);
            TPoint3D P(-p.y, -p.z, p.x);
            TPixelCoordf pixel;
            {  // mrpt-obs shouldn't depend on mrpt-vision just for this!
                // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);

                // Pinhole model:
                const double x = P.x / P.z;
                const double y = P.y / P.z;

                // Radial distortion:
                const double r2 = square(x) + square(y);
                const double r4 = square(r2);

                pixel.x =
                    params.cx() +
                    params.fx() *
                        (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
                              2 * params.dist[2] * x * y +
                              params.dist[3] * (r2 + 2 * square(x))));
                pixel.y =
                    params.cy() +
                    params.fy() *
                        (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
                              2 * params.dist[3] * x * y +
                              params.dist[2] * (r2 + 2 * square(y))));
            }

            // In theory, it should be (r,c):
            err.push_back(c - pixel.x);
            err.push_back(r - pixel.y);
        }
    }
}  // end error_func
}

/** A Levenberg-Marquart-based optimizer to recover the calibration parameters
 * of a 3D camera given a range (depth) image and the corresponding 3D point
 * cloud.
 * \param camera_offset The offset (in meters) in the +X direction of the point
 * cloud. It's 1cm for SwissRanger SR4000.
 * \return The final average reprojection error per pixel (typ <0.05 px)
 */
double CObservation3DRangeScan::recoverCameraCalibrationParameters(
    const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
    const double camera_offset)
{
    MRPT_START

    ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
    ASSERT_(
        obs.points3D_x.size() == obs.points3D_y.size() &&
        obs.points3D_x.size() == obs.points3D_z.size());

    using TMyLevMar =
        CLevenbergMarquardtTempl<CVectorDouble, detail::TLevMarData>;
    TMyLevMar::TResultInfo info;

    const size_t nR = obs.rangeImage.rows();
    const size_t nC = obs.rangeImage.cols();

    TCamera camInit;
    camInit.ncols = nC;
    camInit.nrows = nR;
    camInit.intrinsicParams(0, 0) = 250;
    camInit.intrinsicParams(1, 1) = 250;
    camInit.intrinsicParams(0, 2) = nC >> 1;
    camInit.intrinsicParams(1, 2) = nR >> 1;

    CVectorDouble initial_x;
    detail::cam2vec(camInit, initial_x);

    initial_x.resize(8);
    CVectorDouble increments_x(initial_x.size());
    increments_x.assign(1e-4);

    CVectorDouble optimal_x;

    TMyLevMar lm;
    lm.execute(
        optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
        detail::TLevMarData(obs, camera_offset), info,
        mrpt::system::LVL_INFO, /* verbose */
        1000, /* max iter */
        1e-3, 1e-9, 1e-9, false);

    const double avr_px_err =
        sqrt(info.final_sqr_err / (nC * nR / square(CALIB_DECIMAT)));

    out_camParams.ncols = nC;
    out_camParams.nrows = nR;
    out_camParams.focalLengthMeters = camera_offset;
    detail::vec2cam(optimal_x, out_camParams);

    return avr_px_err;

    MRPT_END
}

void CObservation3DRangeScan::getZoneAsObs(
    CObservation3DRangeScan& obs, const unsigned int& r1,
    const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
{
    unsigned int cols = cameraParams.ncols;
    unsigned int rows = cameraParams.nrows;

    ASSERT_((r1 < r2) && (c1 < c2));
    ASSERT_((r2 < rows) && (c2 < cols));
    // Maybe we needed to copy more base obs atributes

    // Copy zone of range image
    obs.hasRangeImage = hasRangeImage;
    if (hasRangeImage)
        rangeImage.extractSubmatrix(r1, r2, c1, c2, obs.rangeImage);

    // Copy zone of intensity image
    obs.hasIntensityImage = hasIntensityImage;
    obs.intensityImageChannel = intensityImageChannel;
    if (hasIntensityImage)
        intensityImage.extract_patch(
            obs.intensityImage, c1, r1, c2 - c1, r2 - r1);

    // Copy zone of confidence image
    obs.hasConfidenceImage = hasConfidenceImage;
    if (hasConfidenceImage)
        confidenceImage.extract_patch(
            obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);

    // Zone labels: It's too complex, just document that pixel labels are NOT
    // extracted.

    // Copy zone of scanned points
    obs.hasPoints3D = hasPoints3D;
    if (hasPoints3D)
    {
        // Erase a possible previous content
        if (obs.points3D_x.size() > 0)
        {
            obs.points3D_x.clear();
            obs.points3D_y.clear();
            obs.points3D_z.clear();
        }

        for (unsigned int i = r1; i < r2; i++)
            for (unsigned int j = c1; j < c2; j++)
            {
                obs.points3D_x.push_back(points3D_x.at(cols * i + j));
                obs.points3D_y.push_back(points3D_y.at(cols * i + j));
                obs.points3D_z.push_back(points3D_z.at(cols * i + j));
            }
    }

    obs.maxRange = maxRange;
    obs.sensorPose = sensorPose;
    obs.stdError = stdError;

    obs.cameraParams = cameraParams;
}

/** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
 * \a points3D_z to allow the usage of the internal memory pool. */
void CObservation3DRangeScan::resizePoints3DVectors(const size_t WH)
{
#ifdef COBS3DRANGE_USE_MEMPOOL
    // If WH=0 this is a clear:
    if (!WH)
    {
        vector_strong_clear(points3D_x);
        vector_strong_clear(points3D_y);
        vector_strong_clear(points3D_z);
        vector_strong_clear(points3D_idxs_x);
        vector_strong_clear(points3D_idxs_y);
        return;
    }

    if (WH <= points3D_x.size())  // reduce size, don't realloc
    {
        points3D_x.resize(WH);
        points3D_y.resize(WH);
        points3D_z.resize(WH);
        points3D_idxs_x.resize(WH);
        points3D_idxs_y.resize(WH);
        return;
    }

    // Request memory for the X,Y,Z buffers from the memory pool:
    TMyPointsMemPool* pool = TMyPointsMemPool::getInstance();
    if (pool)
    {
        CObservation3DRangeScan_Points_MemPoolParams mem_params;
        mem_params.WH = WH;

        CObservation3DRangeScan_Points_MemPoolData* mem_block =
            pool->request_memory(mem_params);

        if (mem_block)
        {  // Take the memory via swaps:
            points3D_x.swap(mem_block->pts_x);
            points3D_y.swap(mem_block->pts_y);
            points3D_z.swap(mem_block->pts_z);
            points3D_idxs_x.swap(mem_block->idxs_x);
            points3D_idxs_y.swap(mem_block->idxs_y);
            delete mem_block;
        }
    }
#endif

    // Either if there was no pool memory or we got it, make sure the size of
    // vectors is OK:
    points3D_x.resize(WH);
    points3D_y.resize(WH);
    points3D_z.resize(WH);
    points3D_idxs_x.resize(WH);
    points3D_idxs_y.resize(WH);
}

size_t CObservation3DRangeScan::getScanSize() const
{
    // x,y,z vectors have the same size.
    return points3D_x.size();
}

// Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
// pooling to speed-up the memory allocation.
void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
{
#ifdef COBS3DRANGE_USE_MEMPOOL
    // Request memory from the memory pool:
    TMyRangesMemPool* pool = TMyRangesMemPool::getInstance();
    if (pool)
    {
        CObservation3DRangeScan_Ranges_MemPoolParams mem_params;
        mem_params.H = H;
        mem_params.W = W;

        CObservation3DRangeScan_Ranges_MemPoolData* mem_block =
            pool->request_memory(mem_params);

        if (mem_block)
        {  // Take the memory via swaps:
            rangeImage.swap(mem_block->rangeImage);
            delete mem_block;
            return;
        }
    }
// otherwise, continue with the normal method:
#endif
    // Fall-back to normal method:
    rangeImage.setSize(H, W);
}

// Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
// (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
bool CObservation3DRangeScan::doDepthAndIntensityCamerasCoincide() const
{
    static const double EPSILON = 1e-7;
    static mrpt::poses::CPose3D ref_pose(
        0, 0, 0, DEG2RAD(-90), 0, DEG2RAD(-90));

    return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
               .all() &&
           ((ref_pose.getRotationMatrix() -
             relativePoseIntensityWRTDepth.getRotationMatrix())
                .array()
                .abs() < EPSILON)
               .all();
}

void CObservation3DRangeScan::convertTo2DScan(
    mrpt::obs::CObservation2DRangeScan& out_scan2d,
    const T3DPointsTo2DScanParams& sp, const TRangeImageFilterParams& fp)
{
    out_scan2d.sensorLabel = sensorLabel;
    out_scan2d.timestamp = this->timestamp;

    if (!this->hasRangeImage)
    {  // Nothing to do!
        out_scan2d.resizeScan(0);
        return;
    }

    const size_t nCols = this->rangeImage.cols();
    const size_t nRows = this->rangeImage.rows();
    if (fp.rangeMask_min)
    {  // sanity check:
        ASSERT_EQUAL_(fp.rangeMask_min->cols(), rangeImage.cols());
        ASSERT_EQUAL_(fp.rangeMask_min->rows(), rangeImage.rows());
    }
    if (fp.rangeMask_max)
    {  // sanity check:
        ASSERT_EQUAL_(fp.rangeMask_max->cols(), rangeImage.cols());
        ASSERT_EQUAL_(fp.rangeMask_max->rows(), rangeImage.rows());
    }

    // Compute the real horizontal FOV from the range camera intrinsic calib
    // data:
    // Note: this assumes the range image has been "undistorted", which is true
    // for data
    //        from OpenNI, and will be in the future for libfreenect in MRPT,
    //        but it's
    //        not implemented yet (as of Mar 2012), so this is an approximation
    //        in that case.
    const double cx = this->cameraParams.cx();
    const double cy = this->cameraParams.cy();
    const double fx = this->cameraParams.fx();
    const double fy = this->cameraParams.fy();

    // (Imagine the camera seen from above to understand this geometry)
    const double real_FOV_left = atan2(cx, fx);
    const double real_FOV_right = atan2(nCols - 1 - cx, fx);

    // FOV of the equivalent "fake" "laser scanner":
    const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);

    // Now, we should create more "fake laser" points than columns in the image,
    //  since laser scans are assumed to sample space at evenly-spaced angles,
    //  while in images it is like ~tan(angle).
    ASSERT_ABOVE_(
        sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
    const size_t nLaserRays =
        static_cast<size_t>(nCols * sp.oversampling_ratio);

    // Prepare 2D scan data fields:
    out_scan2d.aperture = FOV_equiv;
    out_scan2d.maxRange = this->maxRange;
    out_scan2d.resizeScan(nLaserRays);

    out_scan2d.resizeScanAndAssign(
        nLaserRays, 2.0 * this->maxRange,
        false);  // default: all ranges=invalid
    if (sp.use_origin_sensor_pose)
        out_scan2d.sensorPose = mrpt::poses::CPose3D();
    else
        out_scan2d.sensorPose = this->sensorPose;

    // The vertical FOVs given by the user can be translated into limits of the
    // tangents (tan>0 means above, i.e. z>0):
    const float tan_min = -tan(std::abs(sp.angle_inf));
    const float tan_max = tan(std::abs(sp.angle_sup));

    // Precompute the tangents of the vertical angles of each "ray"
    // for every row in the range image:
    std::vector<float> vert_ang_tan(nRows);
    for (size_t r = 0; r < nRows; r++)
        vert_ang_tan[r] = static_cast<float>((cy - r) / fy);

    if (!sp.use_origin_sensor_pose)
    {
        // Algorithm 1: each column in the range image corresponds to a known
        // orientation in the 2D scan:
        // -------------------
        out_scan2d.rightToLeft = false;

        // Angle "counter" for the fake laser scan direction, and the increment:
        double ang = -FOV_equiv * 0.5;
        const double A_ang = FOV_equiv / (nLaserRays - 1);

        TRangeImageFilter rif(fp);

        // Go thru columns, and keep the minimum distance (along the +X axis,
        // not 3D distance!)
        // for each direction (i.e. for each column) which also lies within the
        // vertical FOV passed
        // by the user.
        for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
        {
            // Equivalent column in the range image for the "i'th" ray:
            const double tan_ang = tan(ang);
            // make sure we don't go out of range (just in case):
            const size_t c = std::min(
                static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
                nCols - 1);

            bool any_valid = false;
            float closest_range = out_scan2d.maxRange;

            for (size_t r = 0; r < nRows; r++)
            {
                const float D = this->rangeImage.coeff(r, c);
                if (!rif.do_range_filter(r, c, D)) continue;

                // All filters passed:
                const float this_point_tan = vert_ang_tan[r] * D;
                if (this_point_tan > tan_min && this_point_tan < tan_max)
                {
                    any_valid = true;
                    mrpt::keep_min(closest_range, D);
                }
            }

            if (any_valid)
            {
                out_scan2d.setScanRangeValidity(i, true);
                // Compute the distance in 2D from the "depth" in closest_range:
                out_scan2d.setScanRange(
                    i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
            }
        }  // end for columns
    }
    else
    {
        // Algorithm 2: project to 3D and reproject (for a different sensorPose
        // at the origin)
        // ------------------------------------------------------------------------
        out_scan2d.rightToLeft = true;

        T3DPointsProjectionParams projParams;
        projParams.takeIntoAccountSensorPoseOnRobot = true;

        mrpt::opengl::CPointCloud::Ptr pc =
            mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
        this->project3DPointsFromDepthImageInto(*pc, projParams, fp);

        const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
              &zs = pc->getArrayZ();
        const size_t N = xs.size();

        const double A_ang = FOV_equiv / (nLaserRays - 1);
        const double ang0 = -FOV_equiv * 0.5;

        for (size_t i = 0; i < N; i++)
        {
            if (zs[i] < sp.z_min || zs[i] > sp.z_max) continue;

            const double phi_wrt_origin = atan2(ys[i], xs[i]);

            int i_range = (phi_wrt_origin - ang0) / A_ang;
            if (i_range < 0 || i_range >= int(nLaserRays)) continue;

            const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
            if (out_scan2d.scan[i_range] > r_wrt_origin)
                out_scan2d.setScanRange(i_range, r_wrt_origin);
            out_scan2d.setScanRangeValidity(i_range, true);
        }
    }
}

void CObservation3DRangeScan::getDescriptionAsText(std::ostream& o) const
{
    CObservation::getDescriptionAsText(o);

    this->load();  // Make sure the 3D point cloud, etc... are all loaded in
    // memory.

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

    o << "maxRange = " << maxRange << " m" << endl;

    o << "Has 3D point cloud? ";
    if (hasPoints3D)
    {
        o << "YES: " << points3D_x.size() << " points";
        if (points3D_isExternallyStored())
            o << ". External file: " << points3D_getExternalStorageFile()
              << endl;
        else
            o << " (embedded)." << endl;
    }
    else
        o << "NO" << endl;

    o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
    if (hasRangeImage)
    {
        if (rangeImage_isExternallyStored())
            o << ". External file: " << rangeImage_getExternalStorageFile()
              << endl;
        else
            o << " (embedded)." << endl;
    }

    o << endl << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
    if (hasIntensityImage)
    {
        if (intensityImage.isExternallyStored())
            o << ". External file: " << intensityImage.getExternalStorageFile()
              << endl;
        else
            o << " (embedded).\n";
        // Channel?
        o << "Source channel: "
          << mrpt::typemeta::TEnumType<
                 CObservation3DRangeScan::TIntensityChannelID>::
                 value2name(intensityImageChannel)
          << endl;
    }

    o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
    if (hasConfidenceImage)
    {
        if (confidenceImage.isExternallyStored())
            o << ". External file: " << confidenceImage.getExternalStorageFile()
              << endl;
        else
            o << " (embedded)." << endl;
    }

    o << endl << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
    if (hasPixelLabels())
    {
        o << " Human readable labels:" << endl;
        for (TPixelLabelInfoBase::TMapLabelID2Name::const_iterator it =
                 pixelLabels->pixelLabelNames.begin();
             it != pixelLabels->pixelLabelNames.end(); ++it)
            o << " label[" << it->first << "]: '" << it->second << "'" << endl;
    }

    o << endl << endl;
    o << "Depth camera calibration parameters:" << endl;
    {
        CConfigFileMemory cfg;
        cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
        o << cfg.getContent() << endl;
    }
    o << endl << "Intensity camera calibration parameters:" << endl;
    {
        CConfigFileMemory cfg;
        cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
        o << cfg.getContent() << endl;
    }
    o << endl
      << endl
      << "Pose of the intensity cam. wrt the depth cam:\n"
      << relativePoseIntensityWRTDepth << endl
      << relativePoseIntensityWRTDepth
             .getHomogeneousMatrixVal<CMatrixDouble44>()
      << endl;
}

using plib = CObservation3DRangeScan::TPixelLabelInfoBase;
void plib::writeToStream(mrpt::serialization::CArchive& out) const
{
    const uint8_t version = 1;  // for possible future changes.
    out << version;
    // 1st: Save number MAX_NUM_DIFFERENT_LABELS so we can reconstruct the
    // object in the class factory later on.
    out << BITFIELD_BYTES;
    // 2nd: data-specific serialization:
    this->internal_writeToStream(out);
}

template <unsigned int BYTES_REQUIRED_>
void CObservation3DRangeScan::TPixelLabelInfo<
    BYTES_REQUIRED_>::internal_readFromStream(mrpt::serialization::CArchive& in)
{
    {
        uint32_t nR, nC;
        in >> nR >> nC;
        pixelLabels.resize(nR, nC);
        for (uint32_t c = 0; c < nC; c++)
            for (uint32_t r = 0; r < nR; r++) in >> pixelLabels.coeffRef(r, c);
    }
    in >> pixelLabelNames;
}
template <unsigned int BYTES_REQUIRED_>
void CObservation3DRangeScan::TPixelLabelInfo<BYTES_REQUIRED_>::
	internal_writeToStream(mrpt::serialization::CArchive& out) const
{
    {
        const uint32_t nR = static_cast<uint32_t>(pixelLabels.rows());
        const uint32_t nC = static_cast<uint32_t>(pixelLabels.cols());
        out << nR << nC;
        for (uint32_t c = 0; c < nC; c++)
            for (uint32_t r = 0; r < nR; r++) out << pixelLabels.coeff(r, c);
    }
    out << pixelLabelNames;
}

// Deserialization and class factory. All in one, ladies and gentlemen
CObservation3DRangeScan::TPixelLabelInfoBase* plib::readAndBuildFromStream(
    mrpt::serialization::CArchive& in)
{
    uint8_t version;
    in >> version;

    switch (version)
    {
        case 1:
        {
            // 1st: Read NUM BYTES
            uint8_t bitfield_bytes;
            in >> bitfield_bytes;

            // Hand-made class factory. May be a good solution if there will be
            // not too many different classes:
            CObservation3DRangeScan::TPixelLabelInfoBase* new_obj = nullptr;
            switch (bitfield_bytes)
            {
                case 1:
                    new_obj = new CObservation3DRangeScan::TPixelLabelInfo<1>();
                    break;
                case 2:
                    new_obj = new CObservation3DRangeScan::TPixelLabelInfo<2>();
                    break;
                case 3:
                case 4:
                    new_obj = new CObservation3DRangeScan::TPixelLabelInfo<4>();
                    break;
                case 5:
                case 6:
                case 7:
                case 8:
                    new_obj = new CObservation3DRangeScan::TPixelLabelInfo<8>();
                    break;
                default:
                    throw std::runtime_error(
                        "Unknown type of pixelLabel inner class while "
                        "deserializing!");
            };
            // 2nd: data-specific serialization:
            new_obj->internal_readFromStream(in);

            return new_obj;
        }
        break;

        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
            break;
    };
}

T3DPointsTo2DScanParams::T3DPointsTo2DScanParams()
    : angle_sup(mrpt::DEG2RAD(5)),
      angle_inf(mrpt::DEG2RAD(5)),
      z_min(-std::numeric_limits<double>::max()),
      z_max(std::numeric_limits<double>::max()),
      oversampling_ratio(1.2),
      use_origin_sensor_pose(false)
{
}