/* +------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)            |
|                          https://www.mrpt.org/                         |
|                                                                        |
| Copyright (c) 2005-2022, 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 "maps-precomp.h"  // Precomp header
//
#include <mrpt/core/bits_mem.h>
#include <mrpt/img/color_maps.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/serialization/aligned_serialization.h>
#include <mrpt/system/os.h>

#include "CPointsMap_crtp_common.h"

using namespace std;
using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::img;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::config;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
	"mrpt::maps::CColouredPointsMap,colourPointsMap",
	mrpt::maps::CColouredPointsMap)

CColouredPointsMap::TMapDefinition::TMapDefinition() = default;
void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
	const CConfigFileBase& source, const std::string& sectionNamePrefix)
{
	insertionOpts.loadFromConfigFile(
		source, sectionNamePrefix + string("_insertOpts"));
	likelihoodOpts.loadFromConfigFile(
		source, sectionNamePrefix + string("_likelihoodOpts"));
	colourOpts.loadFromConfigFile(
		source, sectionNamePrefix + string("_colorOpts"));
}

void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(
	std::ostream& out) const
{
	this->insertionOpts.dumpToTextStream(out);
	this->likelihoodOpts.dumpToTextStream(out);
	this->colourOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap* CColouredPointsMap::internal_CreateFromMapDefinition(
	const mrpt::maps::TMetricMapInitializer& _def)
{
	const CColouredPointsMap::TMapDefinition& def =
		*dynamic_cast<const CColouredPointsMap::TMapDefinition*>(&_def);
	auto* obj = new CColouredPointsMap();
	obj->insertionOptions = def.insertionOpts;
	obj->likelihoodOptions = def.likelihoodOpts;
	obj->colorScheme = def.colourOpts;
	return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CColouredPointsMap, CPointsMap, mrpt::maps)

void CColouredPointsMap::reserve(size_t newLength)
{
	m_x.reserve(newLength);
	m_y.reserve(newLength);
	m_z.reserve(newLength);
	m_color_R.reserve(newLength);
	m_color_G.reserve(newLength);
	m_color_B.reserve(newLength);
}

// Resizes all point buffers so they can hold the given number of points: newly
// created points are set to default values,
//  and old contents are not changed.
void CColouredPointsMap::resize(size_t newLength)
{
	m_x.resize(newLength, 0);
	m_y.resize(newLength, 0);
	m_z.resize(newLength, 0);
	m_color_R.resize(newLength, 1);
	m_color_G.resize(newLength, 1);
	m_color_B.resize(newLength, 1);
	mark_as_modified();
}

// Resizes all point buffers so they can hold the given number of points,
// *erasing* all previous contents
//  and leaving all points to default values.
void CColouredPointsMap::setSize(size_t newLength)
{
	m_x.assign(newLength, 0);
	m_y.assign(newLength, 0);
	m_z.assign(newLength, 0);
	m_color_R.assign(newLength, 1);
	m_color_G.assign(newLength, 1);
	m_color_B.assign(newLength, 1);
	mark_as_modified();
}

void CColouredPointsMap::impl_copyFrom(const CPointsMap& obj)
{
	// This also does a ::resize(N) of all data fields.
	CPointsMap::base_copyFrom(obj);

	const auto* pCol = dynamic_cast<const CColouredPointsMap*>(&obj);
	if (pCol)
	{
		m_color_R = pCol->m_color_R;
		m_color_G = pCol->m_color_G;
		m_color_B = pCol->m_color_B;
	}
}

uint8_t CColouredPointsMap::serializeGetVersion() const { return 9; }
void CColouredPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
{
	uint32_t n = m_x.size();

	// First, write the number of points:
	out << n;

	if (n > 0)
	{
		out.WriteBufferFixEndianness(&m_x[0], n);
		out.WriteBufferFixEndianness(&m_y[0], n);
		out.WriteBufferFixEndianness(&m_z[0], n);
	}
	out << m_color_R << m_color_G << m_color_B;	 // added in v4

	out << genericMapParams;  // v9
	insertionOptions.writeToStream(
		out);  // version 9?: insert options are saved with its own method
	likelihoodOptions.writeToStream(out);  // Added in version 5
}

void CColouredPointsMap::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 8:
		case 9:
		{
			mark_as_modified();

			// Read the number of points:
			uint32_t n;
			in >> n;

			this->resize(n);

			if (n > 0)
			{
				in.ReadBufferFixEndianness(&m_x[0], n);
				in.ReadBufferFixEndianness(&m_y[0], n);
				in.ReadBufferFixEndianness(&m_z[0], n);
			}
			in >> m_color_R >> m_color_G >> m_color_B;

			if (version >= 9) in >> genericMapParams;
			else
			{
				bool disableSaveAs3DObject;
				in >> disableSaveAs3DObject;
				genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
			}
			insertionOptions.readFromStream(in);
			likelihoodOptions.readFromStream(in);
		}
		break;

		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
		case 5:
		case 6:
		case 7:
		{
			mark_as_modified();

			// Read the number of points:
			uint32_t n;
			in >> n;

			this->resize(n);

			if (n > 0)
			{
				in.ReadBufferFixEndianness(&m_x[0], n);
				in.ReadBufferFixEndianness(&m_y[0], n);
				in.ReadBufferFixEndianness(&m_z[0], n);

				// Version 1: weights are also stored:
				// Version 4: Type becomes long int -> uint32_t for
				// portability!!
				if (version >= 1)
				{
					if (version >= 4)
					{
						if (version >= 7)
						{
							// Weights were removed from this class in v7 (MRPT
							// 0.9.5),
							//  so nothing else to do.
						}
						else
						{
							// Go on with old serialization format, but discard
							// weights:
							std::vector<uint32_t> dummy_pointWeight(n);
							in.ReadBufferFixEndianness(
								&dummy_pointWeight[0], n);
						}
					}
					else
					{
						std::vector<uint32_t> dummy_pointWeight(n);
						in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
					}
				}
			}

			if (version >= 2)
			{
				// version 2: options saved too
				in >> insertionOptions.minDistBetweenLaserPoints >>
					insertionOptions.addToExistingPointsMap >>
					insertionOptions.also_interpolate >>
					insertionOptions.disableDeletion >>
					insertionOptions.fuseWithExisting >>
					insertionOptions.isPlanarMap;

				if (version < 6)
				{
					bool old_matchStaticPointsOnly;
					in >> old_matchStaticPointsOnly;
				}

				in >> insertionOptions.maxDistForInterpolatePoints;
				{
					bool disableSaveAs3DObject;
					in >> disableSaveAs3DObject;
					genericMapParams.enableSaveAs3DObject =
						!disableSaveAs3DObject;
				}
			}

			if (version >= 3) { in >> insertionOptions.horizontalTolerance; }

			if (version >= 4)  // Color data
			{
				in >> m_color_R >> m_color_G >> m_color_B;
				if (version >= 7)
				{
					// Removed: in >> m_min_dist;
				}
				else
				{
					std::vector<float> dummy_dist;
					in >> dummy_dist;
				}
			}
			else
			{
				m_color_R.assign(m_x.size(), 1.0f);
				m_color_G.assign(m_x.size(), 1.0f);
				m_color_B.assign(m_x.size(), 1.0f);
				// m_min_dist.assign(x.size(),2000.0f);
			}

			if (version >= 5)  // version 5: added likelihoodOptions
				likelihoodOptions.readFromStream(in);

			if (version >= 8)  // version 8: added insertInvalidPoints
				in >> insertionOptions.insertInvalidPoints;
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

/*---------------------------------------------------------------
Clear
---------------------------------------------------------------*/
void CColouredPointsMap::internal_clear()
{
	// This swap() thing is the only way to really deallocate the memory.
	vector_strong_clear(m_x);
	vector_strong_clear(m_y);
	vector_strong_clear(m_z);

	vector_strong_clear(m_color_R);
	vector_strong_clear(m_color_G);
	vector_strong_clear(m_color_B);

	mark_as_modified();
}

/** Changes a given point from map. First index is 0.
 * \exception Throws std::exception on index out of bound.
 */
void CColouredPointsMap::setPointRGB(
	size_t index, float x, float y, float z, float R, float G, float B)
{
	if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
	m_x[index] = x;
	m_y[index] = y;
	m_z[index] = z;
	this->m_color_R[index] = R;
	this->m_color_G[index] = G;
	this->m_color_B[index] = B;
	mark_as_modified();
}

/** Changes just the color of a given point from the map. First index is 0.
 * \exception Throws std::exception on index out of bound.
 */
void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
{
	if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
	this->m_color_R[index] = R;
	this->m_color_G[index] = G;
	this->m_color_B[index] = B;
	// mark_as_modified();  // No need to rebuild KD-trees, etc...
}

void CColouredPointsMap::insertPointFast(float x, float y, float z)
{
	m_x.push_back(x);
	m_y.push_back(y);
	m_z.push_back(z);
	m_color_R.push_back(1);
	m_color_G.push_back(1);
	m_color_B.push_back(1);

	// mark_as_modified(); -> Fast
}

void CColouredPointsMap::insertPointRGB(
	float x, float y, float z, float R, float G, float B)
{
	m_x.push_back(x);
	m_y.push_back(y);
	m_z.push_back(z);
	m_color_R.push_back(R);
	m_color_G.push_back(G);
	m_color_B.push_back(B);

	mark_as_modified();
}

void CColouredPointsMap::getVisualizationInto(
	mrpt::opengl::CSetOfObjects& o) const
{
	if (!genericMapParams.enableSaveAs3DObject) return;

	opengl::CPointCloudColoured::Ptr obj =
		std::make_shared<opengl::CPointCloudColoured>();

	obj->loadFromPointsMap(this);
	obj->setColor(1, 1, 1, 1.0);

	obj->setPointSize(this->renderOptions.point_size);

	o.insert(obj);
}

/*---------------------------------------------------------------
TColourOptions
---------------------------------------------------------------*/
CColouredPointsMap::TColourOptions::TColourOptions()

	= default;

void CColouredPointsMap::TColourOptions::loadFromConfigFile(
	const CConfigFileBase& source, const std::string& section)
{
	scheme = source.read_enum(section, "scheme", scheme);
	MRPT_LOAD_CONFIG_VAR(z_min, float, source, section)
	MRPT_LOAD_CONFIG_VAR(z_max, float, source, section)
	MRPT_LOAD_CONFIG_VAR(d_max, float, source, section)
}

void CColouredPointsMap::TColourOptions::dumpToTextStream(
	std::ostream& out) const
{
	out << "\n----------- [CColouredPointsMap::TColourOptions] ------------ "
		   "\n\n";

	out << "scheme                                  = " << scheme << endl;
	out << "z_min                                   = " << z_min << endl;
	out << "z_max                                   = " << z_max << endl;
	out << "d_max                                   = " << d_max << endl;
}

void CColouredPointsMap::getPointRGB(
	size_t index, float& x, float& y, float& z, float& R, float& G,
	float& B) const
{
	if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");

	x = m_x[index];
	y = m_y[index];
	z = m_z[index];
	R = m_color_R[index];
	G = m_color_G[index];
	B = m_color_B[index];
}

/** Retrieves a point color (colors range is [0,1])
 */
void CColouredPointsMap::getPointColor(
	size_t index, float& R, float& G, float& B) const
{
	if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");

	R = m_color_R[index];
	G = m_color_G[index];
	B = m_color_B[index];
}

// This function is duplicated from
// "mrpt::vision::pinhole::projectPoint_with_distortion", to avoid
//  a dependency on mrpt-vision.
static void aux_projectPoint_with_distortion(
	const mrpt::math::TPoint3D& P, const TCamera& params, TPixelCoordf& pixel,
	[[maybe_unused]] bool accept_points_behind)
{
	// 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);
	const double r6 = r2 * r4;

	pixel.x = params.cx() +
		params.fx() *
			(x *
				 (1 + params.dist[0] * r2 + params.dist[1] * r4 +
				  params.dist[4] * r6) +
			 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 +
				  params.dist[4] * r6) +
			 2 * params.dist[3] * x * y +
			 params.dist[2] * (r2 + 2 * square(y)));
}

/*---------------------------------------------------------------
getPoint
---------------------------------------------------------------*/
bool CColouredPointsMap::colourFromObservation(
	const CObservationImage& obs, const CPose3D& robotPose)
{
	// Check if image is not grayscale
	ASSERT_(obs.image.isColor());

	CPose3D cameraPoseR;  // Get Camera Pose on the Robot(B) (CPose3D)
	CPose3D cameraPoseW;  // World Camera Pose

	obs.getSensorPose(cameraPoseR);
	cameraPoseW = robotPose + cameraPoseR;	// Camera Global Coordinates

	// Image Information
	unsigned int imgW = obs.image.getWidth();
	unsigned int imgH = obs.image.getHeight();

	// Projection related variables
	std::vector<TPixelCoordf>
		projectedPoints;  // The set of projected points in the image
	std::vector<TPixelCoordf>::iterator
		itProPoints;  // Iterator for projectedPoints
	std::vector<size_t> p_idx;
	std::vector<float> p_dist;
	std::vector<unsigned int> p_proj;

	// Get the N closest points
	kdTreeNClosestPoint2DIdx(
		cameraPoseW.x(), cameraPoseW.y(),  // query point
		200000,	 // number of points to search
		p_idx, p_dist);	 // indexes and distances of the returned points

	// Fill p3D vector
	for (size_t k = 0; k < p_idx.size(); k++)
	{
		float d = sqrt(p_dist[k]);
		size_t idx = p_idx[k];
		if (d < colorScheme.d_max)	//  && d < m_min_dist[idx] )
		{
			TPixelCoordf px;
			aux_projectPoint_with_distortion(
				TPoint3D(m_x[idx], m_y[idx], m_z[idx]), obs.cameraParams, px,
				true);
			projectedPoints.push_back(px);
			p_proj.push_back(k);
		}  // end if
	}  // end for

	// Get channel order
	unsigned int chR, chG, chB;
	if (obs.image.getChannelsOrder()[0] == 'B')
	{
		chR = 2;
		chG = 1;
		chB = 0;
	}
	else
	{
		chR = 0;
		chG = 1;
		chB = 2;
	}

	unsigned int n_proj = 0;
	const float factor = 1.0f / 255;  // Normalize pixels:

	// Get the colour of the projected points
	size_t k;
	for (itProPoints = projectedPoints.begin(), k = 0;
		 itProPoints != projectedPoints.end(); ++itProPoints, ++k)
	{
		// Only get the points projected inside the image
		if (itProPoints->x >= 0 && itProPoints->x < imgW &&
			itProPoints->y > 0 && itProPoints->y < imgH)
		{
			unsigned int ii = p_idx[p_proj[k]];
			uint8_t* p = obs.image(
				(unsigned int)itProPoints->x, (unsigned int)itProPoints->y);

			m_color_R[ii] = p[chR] * factor;  // R
			m_color_G[ii] = p[chG] * factor;  // G
			m_color_B[ii] = p[chB] * factor;  // B
			// m_min_dist[ii]	= p_dist[p_proj[k]];

			n_proj++;
		}
	}  // end for

	return true;
}  // end colourFromObservation

void CColouredPointsMap::resetPointsMinDist([[maybe_unused]] float defValue)
{
	// m_min_dist.assign(x.size(),defValue);
}

bool CColouredPointsMap::save3D_and_colour_to_text_file(
	const std::string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (size_t i = 0; i < m_x.size(); i++)
		os::fprintf(
			f, "%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
			(uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
			(uint8_t)(255 * m_color_B[i]));

	os::fclose(f);
	return true;
}

// ================================ PLY files import & export virtual methods
// ================================

/** In a base class, reserve memory to prepare subsequent calls to
 * PLY_import_set_vertex */
void CColouredPointsMap::PLY_import_set_vertex_count(const size_t N)
{
	this->setSize(N);
}

/** In a base class, will be called after PLY_import_set_vertex_count() once for
 * each loaded point.
 *  \param pt_color Will be nullptr if the loaded file does not provide color
 * info.
 */
void CColouredPointsMap::PLY_import_set_vertex(
	const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
{
	if (pt_color)
		this->setPointRGB(
			idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
	else
		this->setPoint(idx, pt.x, pt.y, pt.z);
}

/** In a base class, will be called after PLY_export_get_vertex_count() once for
 * each exported point.
 *  \param pt_color Will be nullptr if the loaded file does not provide color
 * info.
 */
void CColouredPointsMap::PLY_export_get_vertex(
	const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
	TColorf& pt_color) const
{
	pt_has_color = true;

	pt.x = m_x[idx];
	pt.y = m_y[idx];
	pt.z = m_z[idx];

	pt_color.R = m_color_R[idx];
	pt_color.G = m_color_G[idx];
	pt_color.B = m_color_B[idx];
}

/*---------------------------------------------------------------
addFrom_classSpecific
---------------------------------------------------------------*/
void CColouredPointsMap::addFrom_classSpecific(
	const CPointsMap& anotherMap, const size_t nPreviousPoints)
{
	const size_t nOther = anotherMap.size();

	// Specific data for this class:
	const auto* anotheMap_col =
		dynamic_cast<const CColouredPointsMap*>(&anotherMap);

	if (anotheMap_col)
	{
		for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
		{
			m_color_R[j] = anotheMap_col->m_color_R[i];
			m_color_G[j] = anotheMap_col->m_color_G[i];
			m_color_B[j] = anotheMap_col->m_color_B[i];
		}
	}
}

namespace mrpt::maps::detail
{
using mrpt::maps::CColouredPointsMap;

template <>
struct pointmap_traits<CColouredPointsMap>
{
	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called only once before inserting
	 * points - this is the place to reserve memory in lric for extra working
	 * variables. */
	inline static void internal_loadFromRangeScan2D_init(
		CColouredPointsMap& me,
		mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
	{
		// Vars:
		//  [0] -> pR
		//  [1] -> pG
		//  [2] -> pB
		//  [3] -> Az_1_color
		lric.fVars.resize(4);

		ASSERT_NOT_EQUAL_(me.colorScheme.z_max, me.colorScheme.z_min);
		lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
	}

	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called once per range data */
	inline static void internal_loadFromRangeScan2D_prepareOneRange(
		CColouredPointsMap& me, [[maybe_unused]] const float gx,
		[[maybe_unused]] const float gy, const float gz,
		mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
	{
		// Relative height of the point wrt the sensor:
		const float rel_z = gz - lric.HM(2, 3);	 // m23;

		// Variable renaming:
		float& pR = lric.fVars[0];
		float& pG = lric.fVars[1];
		float& pB = lric.fVars[2];
		float Az_1_color = lric.fVars[3];

		// Compute color:
		switch (me.colorScheme.scheme)
		{
			// case cmFromHeightRelativeToSensor:
			case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
			case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
			{
				float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
				if (q < 0) q = 0;
				else if (q > 1)
					q = 1;

				if (me.colorScheme.scheme ==
					CColouredPointsMap::cmFromHeightRelativeToSensorGray)
				{ pR = pG = pB = q; }
				else
				{
					jet2rgb(q, pR, pG, pB);
				}
			}
			break;
			case CColouredPointsMap::cmFromIntensityImage:
			{
				// In 2D scans we don't have this channel.
				pR = pG = pB = 1.0;
			}
			break;
			default: THROW_EXCEPTION("Unknown color scheme");
		}
	}
	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called after each
	 * "{x,y,z}.push_back(...);" */
	inline static void internal_loadFromRangeScan2D_postPushBack(
		CColouredPointsMap& me,
		mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
	{
		float& pR = lric.fVars[0];
		float& pG = lric.fVars[1];
		float& pB = lric.fVars[2];
		me.m_color_R.push_back(pR);
		me.m_color_G.push_back(pG);
		me.m_color_B.push_back(pB);
		// m_min_dist.push_back(1e4);
	}

	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called only once before inserting
	 * points - this is the place to reserve memory in lric for extra working
	 * variables. */
	inline static void internal_loadFromRangeScan3D_init(
		CColouredPointsMap& me,
		mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
	{
		// Vars:
		//  [0] -> pR
		//  [1] -> pG
		//  [2] -> pB
		//  [3] -> Az_1_color
		//  [4] -> K_8u
		//  [5] -> cx
		//  [6] -> cy
		//  [7] -> fx
		//  [8] -> fy
		lric.fVars.resize(9);
		float& cx = lric.fVars[5];
		float& cy = lric.fVars[6];
		float& fx = lric.fVars[7];
		float& fy = lric.fVars[8];

		// unsigned int vars:
		//  [0] -> imgW
		//  [1] -> imgH
		//  [2] -> img_idx_x
		//  [3] -> img_idx_y
		lric.uVars.resize(4);
		unsigned int& imgW = lric.uVars[0];
		unsigned int& imgH = lric.uVars[1];
		unsigned int& img_idx_x = lric.uVars[2];
		unsigned int& img_idx_y = lric.uVars[3];

		// bool vars:
		//  [0] -> hasValidIntensityImage
		//  [1] -> hasColorIntensityImg
		//  [2] -> simple_3d_to_color_relation
		lric.bVars.resize(3);
		uint8_t& hasValidIntensityImage = lric.bVars[0];
		uint8_t& hasColorIntensityImg = lric.bVars[1];
		uint8_t& simple_3d_to_color_relation = lric.bVars[2];

		ASSERT_NOT_EQUAL_(me.colorScheme.z_max, me.colorScheme.z_min);
		lric.fVars[3] = 1.0 /
			(me.colorScheme.z_max - me.colorScheme.z_min);	// Az_1_color = ...
		lric.fVars[4] = 1.0f / 255;	 // K_8u

		hasValidIntensityImage = false;
		imgW = 0;
		imgH = 0;

		if (lric.rangeScan.hasIntensityImage)
		{
			// assure the size matches:
			if (lric.rangeScan.points3D_x.size() ==
				lric.rangeScan.intensityImage.getWidth() *
					lric.rangeScan.intensityImage.getHeight())
			{
				hasValidIntensityImage = true;
				imgW = lric.rangeScan.intensityImage.getWidth();
				imgH = lric.rangeScan.intensityImage.getHeight();
			}
		}

		hasColorIntensityImg =
			hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();

		// running indices for the image pixels for the gray levels:
		// If both range & intensity images coincide (e.g. SwissRanger), then we
		// can just
		// assign 3D points to image pixels one-to-one, but that's not the case
		// if
		//
		simple_3d_to_color_relation =
			(std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) <
			 1e-5);
		img_idx_x = 0;
		img_idx_y = 0;

		// Will be used just if simple_3d_to_color_relation=false
		cx = lric.rangeScan.cameraParamsIntensity.cx();
		cy = lric.rangeScan.cameraParamsIntensity.cy();
		fx = lric.rangeScan.cameraParamsIntensity.fx();
		fy = lric.rangeScan.cameraParamsIntensity.fy();
	}

	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called once per range data */
	inline static void internal_loadFromRangeScan3D_prepareOneRange(
		CColouredPointsMap& me, [[maybe_unused]] const float gx,
		[[maybe_unused]] const float gy, const float gz,
		mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
	{
		// Rename variables:
		float& pR = lric.fVars[0];
		float& pG = lric.fVars[1];
		float& pB = lric.fVars[2];
		float& Az_1_color = lric.fVars[3];
		float& K_8u = lric.fVars[4];
		float& cx = lric.fVars[5];
		float& cy = lric.fVars[6];
		float& fx = lric.fVars[7];
		float& fy = lric.fVars[8];

		unsigned int& imgW = lric.uVars[0];
		unsigned int& imgH = lric.uVars[1];
		unsigned int& img_idx_x = lric.uVars[2];
		unsigned int& img_idx_y = lric.uVars[3];

		const uint8_t& hasValidIntensityImage = lric.bVars[0];
		const uint8_t& hasColorIntensityImg = lric.bVars[1];
		const uint8_t& simple_3d_to_color_relation = lric.bVars[2];

		// Relative height of the point wrt the sensor:
		const float rel_z = gz - lric.HM(2, 3);	 // m23;

		// Compute color:
		switch (me.colorScheme.scheme)
		{
			// case cmFromHeightRelativeToSensor:
			case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
			case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
			{
				float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
				if (q < 0) q = 0;
				else if (q > 1)
					q = 1;

				if (me.colorScheme.scheme ==
					CColouredPointsMap::cmFromHeightRelativeToSensorGray)
				{ pR = pG = pB = q; }
				else
				{
					jet2rgb(q, pR, pG, pB);
				}
			}
			break;
			case CColouredPointsMap::cmFromIntensityImage:
			{
				// Do we have to project the 3D point into the image plane??
				bool hasValidColor = false;
				if (simple_3d_to_color_relation) { hasValidColor = true; }
				else
				{
					// Do projection:
					TPoint3D pt;  // pt_wrt_colorcam;
					lric.rangeScan.relativePoseIntensityWRTDepth
						.inverseComposePoint(
							lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y,
							pt.z);

					// Project to image plane:
					if (pt.z)
					{
						img_idx_x = cx + fx * pt.x / pt.z;
						img_idx_y = cy + fy * pt.y / pt.z;

						hasValidColor = img_idx_x < imgW &&	 // img_idx_x>=0
															 // isn't needed for
															 // unsigned.
							img_idx_y < imgH;
					}
				}

				const auto& ii = lric.rangeScan.intensityImage;
				if (hasValidColor && hasColorIntensityImg)
				{
					const auto* c = ii.ptr<uint8_t>(img_idx_x, img_idx_y);
					pR = c[2] * K_8u;
					pG = c[1] * K_8u;
					pB = c[0] * K_8u;
				}
				else if (hasValidColor && hasValidIntensityImage)
				{
					const auto c = ii.at<uint8_t>(img_idx_x, img_idx_y);
					pR = pG = pB = c * K_8u;
				}
				else
				{
					pR = pG = pB = 1.0;
				}
			}
			break;
			default: THROW_EXCEPTION("Unknown color scheme");
		}
	}

	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called after each
	 * "{x,y,z}.push_back(...);" */
	inline static void internal_loadFromRangeScan3D_postPushBack(
		CColouredPointsMap& me,
		mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
	{
		float& pR = lric.fVars[0];
		float& pG = lric.fVars[1];
		float& pB = lric.fVars[2];

		me.m_color_R.push_back(pR);
		me.m_color_G.push_back(pG);
		me.m_color_B.push_back(pB);

		// m_min_dist.push_back(1e4);
	}

	/** Helper method fot the generic implementation of
	 * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
	 * end */
	inline static void internal_loadFromRangeScan3D_postOneRange(
		[[maybe_unused]] CColouredPointsMap& me,
		mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
	{
		unsigned int& imgW = lric.uVars[0];
		unsigned int& img_idx_x = lric.uVars[2];
		unsigned int& img_idx_y = lric.uVars[3];

		const uint8_t& hasValidIntensityImage = lric.bVars[0];
		const uint8_t& simple_3d_to_color_relation = lric.bVars[2];

		// Advance the color pointer (just for simple cases, e.g. SwissRanger,
		// not for Kinect)
		if (simple_3d_to_color_relation && hasValidIntensityImage)
		{
			if (++img_idx_x >= imgW)
			{
				img_idx_y++;
				img_idx_x = 0;
			}
		}
	}
};
}  // namespace mrpt::maps::detail
/** See CPointsMap::loadFromRangeScan() */
void CColouredPointsMap::loadFromRangeScan(
	const CObservation2DRangeScan& rangeScan,
	const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
	mrpt::maps::detail::loadFromRangeImpl<CColouredPointsMap>::
		templ_loadFromRangeScan(*this, rangeScan, robotPose);
}

/** See CPointsMap::loadFromRangeScan() */
void CColouredPointsMap::loadFromRangeScan(
	const CObservation3DRangeScan& rangeScan,
	const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
	mrpt::maps::detail::loadFromRangeImpl<CColouredPointsMap>::
		templ_loadFromRangeScan(*this, rangeScan, robotPose);
}
