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
{
namespace obs
{
namespace 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
}  // namespace detail
}  // namespace obs
}  // namespace mrpt
 
/** 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)
{
}