/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "obs-precomp.h"  // Precompiled headers
//
#include <mrpt/3rdparty/do_opencv_includes.h>
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/core/bits_mem.h>  // vector_strong_clear
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/lazy_load_path.h>
#include <mrpt/math/CLevenbergMarquardt.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/ops_containers.h>  // norm(), etc.
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/string_utils.h>

#include <cstring>
#include <limits>
#include <mutex>
#include <unordered_map>

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:

// Index info: camera parameters + range_is_depth
struct LUT_info
{
  mrpt::img::TCamera calib;
  mrpt::poses::CPose3D sensorPose;
  bool range_is_depth;
};

inline bool operator==(const LUT_info& a, const LUT_info& o)
{
  return a.calib == o.calib && a.sensorPose == o.sensorPose && a.range_is_depth == o.range_is_depth;
}

namespace std
{
template <>
struct hash<LUT_info>
{
  size_t operator()(const LUT_info& k) const
  {
    size_t res = 17;
    res = res * 31 + hash<mrpt::img::TCamera>()(k.calib);
    res = res * 31 + hash<bool>()(k.range_is_depth);
    for (unsigned int i = 0; i < 6; i++) res = res * 31 + hash<double>()(k.sensorPose[i]);
    return res;
  }
};
}  // namespace std

namespace
{
struct LUT_Storage
{
  static LUT_Storage& Instance()
  {
    static LUT_Storage lut;
    return lut;
  }
  std::unordered_map<LUT_info, CObservation3DRangeScan::unproject_LUT_t> LUTs;
  std::mutex LUTs_mtx;
};

}  // namespace

const CObservation3DRangeScan::unproject_LUT_t& CObservation3DRangeScan::get_unproj_lut() const
{
#if MRPT_HAS_OPENCV

  LUT_Storage& ls = LUT_Storage::Instance();

  // Access to, or create upon first usage:
  LUT_info linfo;
  linfo.calib = this->cameraParams;
  linfo.sensorPose = this->sensorPose;
  linfo.range_is_depth = this->range_is_depth;

  // Important: keep lock until we return since this function modifies its
  // contents:
  auto lck = mrpt::lockHelper(ls.LUTs_mtx);

  // Protect against infinite memory growth: imagine sensorPose gets changed
  // every time for a sweeping sensor, etc.
  // Unlikely, but "just in case" (TM)
  if (ls.LUTs.size() > 100) ls.LUTs.clear();

  const unproject_LUT_t& ret = ls.LUTs[linfo];

  ASSERT_EQUAL_(rangeImage.cols(), static_cast<int>(cameraParams.ncols));
  ASSERT_EQUAL_(rangeImage.rows(), static_cast<int>(cameraParams.nrows));

  // already existed and was filled?
  unsigned int H = cameraParams.nrows, W = cameraParams.ncols;
  const size_t WH = W * H;
  if (ret.Kxs.size() == WH) return ret;

  // fill LUT upon first use:
  auto& lut = const_cast<unproject_LUT_t&>(ret);

  lut.Kxs.resize(WH);
  lut.Kys.resize(WH);
  lut.Kzs.resize(WH);
  lut.Kxs_rot.resize(WH);
  lut.Kys_rot.resize(WH);
  lut.Kzs_rot.resize(WH);

  // Undistort all points:
  cv::Mat pts(1, WH, CV_32FC2), undistort_pts(1, WH, CV_32FC2);

  const auto& intrMat = cameraParams.intrinsicParams;
  const auto& dist = cameraParams.dist;
  cv::Mat cv_distortion(1, dist.size(), CV_64F, const_cast<double*>(&dist[0]));
  cv::Mat cv_intrinsics(3, 3, CV_64F);
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++) cv_intrinsics.at<double>(i, j) = intrMat(i, j);

  for (unsigned int r = 0; r < H; r++)
    for (unsigned int c = 0; c < W; c++)
    {
      auto& p = pts.at<cv::Vec2f>(r * W + c);
      p[0] = c;
      p[1] = r;
    }

  switch (cameraParams.distortion)
  {
    case mrpt::img::DistortionModel::none:
      cv_distortion = cv::Mat::zeros(1, dist.size(), CV_64F);
      cv::undistortPoints(pts, undistort_pts, cv_intrinsics, cv_distortion);
      break;

    case mrpt::img::DistortionModel::plumb_bob:
      cv::undistortPoints(pts, undistort_pts, cv_intrinsics, cv_distortion);
      break;

    case mrpt::img::DistortionModel::kannala_brandt:
      cv::fisheye::undistortPoints(pts, undistort_pts, cv_intrinsics, cv_distortion);
      break;

    default:
      THROW_EXCEPTION_FMT(
          "Unknown cameraParams.distortion=%d", static_cast<int>(cameraParams.distortion));
  };

  // Note: undistort_pts now holds point coordinates with (-1,-1)=top left,
  // (1,1)=bottom-right

  ASSERT_EQUAL_(undistort_pts.size().area(), static_cast<int>(WH));
  undistort_pts.reshape(WH);

  float* kxs = &lut.Kxs[0];
  float* kys = &lut.Kys[0];
  float* kzs = &lut.Kzs[0];
  float* kxs_rot = &lut.Kxs_rot[0];
  float* kys_rot = &lut.Kys_rot[0];
  float* kzs_rot = &lut.Kzs_rot[0];

  for (size_t idx = 0; idx < WH; idx++)
  {
    const auto& p = undistort_pts.at<cv::Vec2f>(idx);
    const float c = p[0], r = p[1];

    // XYZ -> (-Y,-Z, X)
    auto v = mrpt::math::TPoint3Df(1.0f, -c, -r);

    // Range instead of depth? Use a unit vector:
    if (!this->range_is_depth) v *= 1.0f / v.norm();

    // compute also the rotated version:
    auto v_rot = sensorPose.rotateVector(v);

    *kxs++ = v.x;
    *kys++ = v.y;
    *kzs++ = v.z;

    *kxs_rot++ = v_rot.x;
    *kys_rot++ = v_rot.y;
    *kzs_rot++ = v_rot.z;
  }

  return ret;
#else
  THROW_EXCEPTION("This method requires MRPT built against OpenCV");
#endif
}

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{0};
  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{0}, W{0};
  inline bool isSuitable(const CObservation3DRangeScan_Ranges_MemPoolParams& req) const
  {
    return H == req.H && W == req.W;
  }
};
struct CObservation3DRangeScan_Ranges_MemPoolData
{
  mrpt::math::CMatrix_u16 rangeImage;
};
using TMyRangesMemPool = mrpt::system::CGenericMemoryPool<
    CObservation3DRangeScan_Ranges_MemPoolParams,
    CObservation3DRangeScan_Ranges_MemPoolData>;

namespace
{
void mempool_donate_xyz_buffers(CObservation3DRangeScan& obs)
{
  if (obs.points3D_x.empty()) return;
  // Before dying, donate my memory to the pool for the joy of future
  // class-brothers...
  TMyPointsMemPool* pool = TMyPointsMemPool::getInstance();
  if (!pool) return;

  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);

  auto* 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);
}
}  // namespace

void mempool_donate_range_matrix(CObservation3DRangeScan& obs)
{
  if (obs.rangeImage.cols() == 0 || obs.rangeImage.rows() == 0) return;

  // Before dying, donate my memory to the pool for the joy of future
  // class-brothers...
  TMyRangesMemPool* pool = TMyRangesMemPool::getInstance();
  if (!pool) return;

  CObservation3DRangeScan_Ranges_MemPoolParams mem_params;
  mem_params.H = obs.rangeImage.rows();
  mem_params.W = obs.rangeImage.cols();

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

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

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

uint8_t CObservation3DRangeScan::serializeGetVersion() const { return 10; }
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;
  out << rangeUnits;  // new in v9
  if (hasRangeImage)
  {
    out.WriteAs<uint32_t>(rangeImage.rows());
    out.WriteAs<uint32_t>(rangeImage.cols());
    if (rangeImage.size() != 0)
      out.WriteBufferFixEndianness<uint16_t>(rangeImage.data(), rangeImage.size());
  }
  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.WriteAs<int8_t>(intensityImageChannel);

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

  // v10:
  out.WriteAs<uint8_t>(rangeImageOtherLayers.size());
  for (const auto& kv : rangeImageOtherLayers)
  {
    out << kv.first;
    const auto& ri = kv.second;
    out.WriteAs<uint32_t>(ri.cols());
    out.WriteAs<uint32_t>(ri.rows());
    if (!ri.empty()) out.WriteBufferFixEndianness<uint16_t>(ri.data(), ri.size());
  }
}

void CObservation3DRangeScan::internal_setRangeImageFromMatrixF(const mrpt::math::CMatrixF& ri)
{
  const uint32_t rows = ri.rows(), cols = ri.cols();

  // Call "rangeImage_setSize()" to exploit the mempool:
  if (rows > 0 && cols > 0)
  {
    rangeImage_setSize(rows, cols);

    for (uint32_t r = 0; r < rows; r++)
      for (uint32_t c = 0; c < cols; c++)
        rangeImage(r, c) = static_cast<uint16_t>(mrpt::round(ri(r, c) / rangeUnits));
  }
}

void CObservation3DRangeScan::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  m_deserializedFromVersionOlder_v9 = false;

  switch (version)
  {
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    case 7:
    case 8:
    case 9:
    case 10:
    {
      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 (version >= 9)
          in >> rangeUnits;
        else
          rangeUnits = 1e-3f;  // default units

        if (hasRangeImage)
        {
          if (version < 9)
          {
            // Convert from old format:
            mrpt::math::CMatrixF ri;
            in >> ri;
            internal_setRangeImageFromMatrixF(ri);
            m_deserializedFromVersionOlder_v9 = true;
          }
          else
          {
            const uint32_t rows = in.ReadAs<uint32_t>();
            const uint32_t cols = in.ReadAs<uint32_t>();

            // Call "rangeImage_setSize()" to exploit the mempool:
            rangeImage_setSize(rows, cols);

            // new v9:
            if (rangeImage.size() != 0)
              in.ReadBufferFixEndianness<uint16_t>(rangeImage.data(), rangeImage.size());
          }
        }

        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));
      }

      rangeImageOtherLayers.clear();
      if (version >= 10)
      {
        const auto numLayers = in.ReadAs<uint8_t>();
        for (size_t i = 0; i < numLayers; i++)
        {
          std::string name;
          in >> name;
          auto& ri = rangeImageOtherLayers[name];

          const uint32_t rows = in.ReadAs<uint32_t>();
          const uint32_t cols = in.ReadAs<uint32_t>();
          ri.resize(rows, cols);
          if (ri.size() != 0) in.ReadBufferFixEndianness<uint16_t>(ri.data(), ri.size());
        }
      }

      // auto-fix wrong camera resolution in parameters:
      if (hasRangeImage && !rangeImage_isExternallyStored() &&
          (static_cast<int>(cameraParams.ncols) != rangeImage.cols() ||
           static_cast<int>(cameraParams.nrows) != rangeImage.rows()))
      {
        std::cerr << "[CObservation3DRangeScan] Warning: autofixing "
                     "incorrect camera resolution in TCamera:"
                  << cameraParams.ncols << "x" << cameraParams.nrows << " => " << rangeImage.cols()
                  << "x" << rangeImage.rows() << "\n";
        cameraParams.ncols = rangeImage.cols();
        cameraParams.nrows = rangeImage.rows();
      }
    }
    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);

  rangeImageOtherLayers.swap(o.rangeImageOtherLayers);
}

void CObservation3DRangeScan::load_impl() 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);
      const auto N = M.cols();

      auto& xs = const_cast<std::vector<float>&>(points3D_x);
      auto& ys = const_cast<std::vector<float>&>(points3D_y);
      auto& zs = const_cast<std::vector<float>&>(points3D_z);
      xs.resize(N);
      ys.resize(N);
      zs.resize(N);
      std::memcpy(&xs[0], &M(0, 0), sizeof(float) * N);
      std::memcpy(&ys[0], &M(1, 0), sizeof(float) * N);
      std::memcpy(&zs[0], &M(2, 0), sizeof(float) * N);
    }
    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)
  {
    for (size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
    {
      std::string layerName;
      mrpt::math::CMatrix_u16* ri = nullptr;
      if (idx == 0)
        ri = const_cast<mrpt::math::CMatrix_u16*>(&rangeImage);
      else
      {
        auto it = rangeImageOtherLayers.begin();
        std::advance(it, idx - 1);
        layerName = it->first;
        ri = const_cast<mrpt::math::CMatrix_u16*>(&it->second);
      }

      // Already loaded? skip:
      if (ri->rows() != 0) continue;

      const string fil = rangeImage_getExternalStorageFileAbsolutePath(layerName);
      if (mrpt::system::strCmpI("txt", mrpt::system::extractFileExtension(fil, true)))
      {
        ri->loadFromTextFile(fil);
      }
      else
      {
        auto& me = const_cast<CObservation3DRangeScan&>(*this);

        mrpt::io::CFileGZInputStream fi(fil);
        auto f = mrpt::serialization::archiveFrom(fi);

        if (!m_deserializedFromVersionOlder_v9)
        {
          // Normal case: deserialize matrix:
          const uint32_t rows = f.ReadAs<uint32_t>();
          const uint32_t cols = f.ReadAs<uint32_t>();
          me.rangeImage_setSize(rows, cols);
          if (ri->size() != 0) f.ReadBufferFixEndianness<uint16_t>(ri->data(), ri->size());
        }
        else
        {
          // Special case: Convert from old format (< v9):
          mrpt::math::CMatrixF r;
          f >> r;
          const_cast<CObservation3DRangeScan*>(this)->internal_setRangeImageFromMatrixF(r);

          // Bug in old version of TUM RGBD rawlog datasets:
          if (ri->cols() == 640 && ri->rows() == 480 &&
              static_cast<int>(cameraParams.ncols) == ri->rows() &&
              static_cast<int>(cameraParams.nrows) == ri->cols())
          {
            std::swap(
                const_cast<uint32_t&>(cameraParams.ncols),
                const_cast<uint32_t&>(cameraParams.nrows));
          }
        }
      }
    }  // end for each layer
  }

  // CImage: no need to force load, they are lazy-loaded automatically upon
  // access.
  // intensityImage.forceLoad();
  // confidenceImage.forceLoad();
}

void CObservation3DRangeScan::unload() const
{
  auto& me = const_cast<CObservation3DRangeScan&>(*this);

  if (hasPoints3D && m_points3D_external_stored)
  {
    mrpt::vector_strong_clear(me.points3D_x);
    mrpt::vector_strong_clear(me.points3D_y);
    mrpt::vector_strong_clear(me.points3D_z);
  }

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

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

std::string CObservation3DRangeScan::rangeImage_getExternalStorageFile(
    const std::string& rangeImageLayer) const
{
  std::string filName = m_rangeImage_external_file;
  if (!rangeImageLayer.empty())
  {
    const auto curExt = mrpt::system::extractFileExtension(filName);
    mrpt::system::fileNameChangeExtension(
        filName, std::string("layer_") + rangeImageLayer + curExt);
  }
  return filName;
}

void CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath(
    std::string& out_path, const std::string& rangeImageLayer) const
{
  std::string filName = rangeImage_getExternalStorageFile(rangeImageLayer);

  out_path = mrpt::io::lazy_load_absolute_path(filName);
}
void CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath(
    std::string& out_path) const
{
  out_path = mrpt::io::lazy_load_absolute_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_path = points3D_getExternalStorageFileAbsolutePath();
  CImage::setImagesPathBase(savedDir);

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

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

    M.saveToTextFile(real_absolute_path, MATRIX_FORMAT_FIXED);
  }
  else
  {
    mrpt::io::CFileGZOutputStream fo(real_absolute_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);

  for (size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
  {
    std::string layerName;
    mrpt::math::CMatrix_u16* ri = nullptr;
    if (idx == 0)
      ri = &rangeImage;
    else
    {
      auto it = rangeImageOtherLayers.begin();
      std::advance(it, idx - 1);
      layerName = it->first;
      ri = &it->second;
    }
    const string real_absolute_path = rangeImage_getExternalStorageFileAbsolutePath(layerName);

    if (EXTERNALS_AS_TEXT_value)
    {
      ri->saveToTextFile(real_absolute_path, MATRIX_FORMAT_FIXED);
    }
    else
    {
      mrpt::io::CFileGZOutputStream fo(real_absolute_path);
      auto f = mrpt::serialization::archiveFrom(fo);

      f.WriteAs<uint32_t>(ri->rows());
      f.WriteAs<uint32_t>(ri->cols());
      if (ri->size() != 0) f.WriteBufferFixEndianness<uint16_t>(ri->data(), ri->size());
    }
  }

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

  CImage::setImagesPathBase(savedDir);
}

// 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_)
  {
  }
};

namespace
{
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 = mrpt::d2f(
            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 = mrpt::d2f(
            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
}  // namespace mrpt::obs::detail

/** 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.fill(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 / double(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) obs.rangeImage = rangeImage.asEigen().block(r2 - r1, c2 - c1, r1, c1);

  // 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)
{
  bool ri_done = rangeImage.cols() == W && rangeImage.rows() == H;

#ifdef COBS3DRANGE_USE_MEMPOOL
  // Request memory from the memory pool:
  TMyRangesMemPool* pool = TMyRangesMemPool::getInstance();
  if (pool && !ri_done)
  {
    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;
      ri_done = true;
    }
  }
// otherwise, continue with the normal method:
#endif
  // Fall-back to normal method:
  if (!ri_done) rangeImage.setSize(H, W);

  // and in all cases, do the resize for the other layers:
  for (auto& layer : rangeImageOtherLayers) layer.second.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, -90.0_deg, 0, -90.0_deg);

  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 = mrpt::d2f(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_GT_(sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
  const auto 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);

  // default: all ranges=invalid
  out_scan2d.resizeScanAndAssign(nLaserRays, 2.0f * this->maxRange, false);
  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 = mrpt::d2f(-tan(std::abs(sp.angle_inf)));
  const float tan_max = mrpt::d2f(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] = d2f((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 = rangeImage.coeff(r, c) * rangeUnits;
        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, mrpt::d2f(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::opengl::CPointCloud::Create();
    this->unprojectInto(*pc, projParams, fp);

    const std::vector<mrpt::math::TPoint3Df>& pts = pc->getArrayPoints();
    const size_t N = pts.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 (pts[i].z < sp.z_min || pts[i].z > sp.z_max) continue;

      const double phi_wrt_origin = atan2(pts[i].y, pts[i].x);

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

      const float r_wrt_origin = ::hypotf(pts[i].x, pts[i].y);
      if (out_scan2d.getScanRange(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>() << "\n" << sensorPose << "\n";

  o << "maxRange = " << maxRange << " [meters]"
    << "\n";
  o << "rangeUnits = " << rangeUnits << " [meters]"
    << "\n";

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

  o << "Range is depth: " << (range_is_depth ? "YES" : "NO") << "\n";
  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
  if (hasRangeImage)
  {
    if (rangeImage_isExternallyStored())
      o << ". External file: " << rangeImage_getExternalStorageFile("");
    else
      o << " (embedded).";
  }
  o << "\n";

  for (auto& layer : rangeImageOtherLayers)
  {
    o << "Additional rangeImage layer: '" << layer.first << "'";
    if (rangeImage_isExternallyStored())
      o << ". External file: " << rangeImage_getExternalStorageFile("");
    else
      o << " (embedded).";
    o << "\n";
  }

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

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

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

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

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())
{
}

void CObservation3DRangeScan::undistort()
{
#if MRPT_HAS_OPENCV

  // DEPTH image:
  {
    // OpenCV wrapper (copy-less) for rangeImage:

    const cv::Mat distortion(1, cameraParams.dist.size(), CV_64F, &cameraParams.dist[0]);
    const cv::Mat intrinsics(3, 3, CV_64F, &cameraParams.intrinsicParams(0, 0));

    const auto imgSize = cv::Size(rangeImage.rows(), rangeImage.cols());

    double alpha = 0;  // all depth pixels are visible in the output
    const cv::Mat newIntrinsics =
        cv::getOptimalNewCameraMatrix(intrinsics, distortion, imgSize, alpha);

    cv::Mat outRangeImg(rangeImage.rows(), rangeImage.cols(), CV_16UC1);

    // Undistort:
    const cv::Mat R_eye = cv::Mat::eye(3, 3, CV_32FC1);

    cv::Mat m1, m2;

    cv::initUndistortRectifyMap(
        intrinsics, distortion, R_eye, newIntrinsics, imgSize, CV_32FC1, m1, m2);

    for (size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
    {
      mrpt::math::CMatrix_u16* ri = nullptr;
      if (idx == 0)
        ri = &rangeImage;
      else
      {
        auto it = rangeImageOtherLayers.begin();
        std::advance(it, idx - 1);
        ri = &it->second;
      }
      cv::Mat rangeImg(ri->rows(), ri->cols(), CV_16UC1, ri->data());

      // Remap:
      cv::remap(rangeImg, outRangeImg, m1, m2, cv::INTER_NEAREST);
      // Overwrite:
      outRangeImg.copyTo(rangeImg);
    }

    cameraParams.dist.fill(0);
    for (int r = 0; r < 3; r++)
      for (int c = 0; c < 3; c++)
        cameraParams.intrinsicParams(r, c) = newIntrinsics.at<double>(r, c);
  }

  // RGB image:
  if (hasIntensityImage)
  {
    mrpt::img::CImage newIntImg;
    intensityImage.undistort(newIntImg, cameraParamsIntensity);

    intensityImage = std::move(newIntImg);
    cameraParamsIntensity.dist.fill(0);
  }

#else
  THROW_EXCEPTION("This method requires OpenCV");
#endif
}

mrpt::img::CImage CObservation3DRangeScan::rangeImageAsImage(
    const mrpt::math::CMatrix_u16& ri,
    float val_min,
    float val_max,
    float rangeUnits,
    const std::optional<mrpt::img::TColormap> color)
{
#if MRPT_HAS_OPENCV
  if (val_max < 1e-4f) val_max = ri.maxCoeff() * rangeUnits;

  ASSERT_GT_(val_max, val_min);

  const float range_inv = rangeUnits / (val_max - val_min);

  ASSERT_GT_(ri.cols(), 0);
  ASSERT_GT_(ri.rows(), 0);

  mrpt::img::CImage img;
  const int cols = ri.cols(), rows = ri.rows();

  const auto col = color.value_or(mrpt::img::TColormap::cmGRAYSCALE);

  const bool is_gray = (col == mrpt::img::TColormap::cmGRAYSCALE);

  img.resize(cols, rows, is_gray ? mrpt::img::CH_GRAY : mrpt::img::CH_RGB);

  for (int r = 0; r < rows; r++)
  {
    for (int c = 0; c < cols; c++)
    {
      // Normalized value in the range [0,1]:
      const float val_01 = (ri.coeff(r, c) - val_min) * range_inv;
      if (is_gray)
      {
        img.setPixel(c, r, static_cast<uint8_t>(val_01 * 255));
      }
      else
      {
        float R, G, B;
        mrpt::img::colormap(col, val_01, R, G, B);

        img.setPixel(
            c, r,
            mrpt::img::TColor(
                static_cast<uint8_t>(R * 255), static_cast<uint8_t>(G * 255),
                static_cast<uint8_t>(B * 255)));
      }
    }
  }

  return img;
#else
  THROW_EXCEPTION("This method requires OpenCV");
#endif
}

mrpt::img::CImage CObservation3DRangeScan::rangeImage_getAsImage(
    const std::optional<mrpt::img::TColormap> color,
    const std::optional<float> normMinRange,
    const std::optional<float> normMaxRange,
    const std::optional<std::string> additionalLayerName) const
{
  ASSERT_(this->hasRangeImage);
  const mrpt::math::CMatrix_u16* ri = (!additionalLayerName || additionalLayerName->empty())
                                          ? &rangeImage
                                          : &rangeImageOtherLayers.at(*additionalLayerName);

  const float val_min = normMinRange.value_or(.0f);
  const float val_max = normMaxRange.value_or(this->maxRange);
  ASSERT_GT_(val_max, val_min);

  return rangeImageAsImage(*ri, val_min, val_max, rangeUnits, color);
}
