<commit_before>/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2019, 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 "maps-precomp.h"  // Precomp header

#include <mrpt/config/CConfigFile.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/os.h>
#include <mrpt/math/geometry.h>
#include <mrpt/serialization/CArchive.h>

#include <mrpt/maps/CPointsMap.h>
#include <mrpt/maps/CSimplePointsMap.h>

#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CPointCloudColoured.h>

// Observations:
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CObservationVelodyneScan.h>
#include <mrpt/obs/CObservationPointCloud.h>

#if MRPT_HAS_PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#   include <pcl/registration/icp.h>
#endif

#if MRPT_HAS_SSE2
#include <mrpt/core/SSE_types.h>
#include <mrpt/core/SSE_macros.h>
#endif

#if MRPT_HAS_MATLAB
#include <mexplus.h>
#endif

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

IMPLEMENTS_VIRTUAL_SERIALIZABLE(CPointsMap, CMetricMap, mrpt::maps)

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CPointsMap::CPointsMap()
	: insertionOptions(), likelihoodOptions(), m_x(), m_y(), m_z()

{
	mark_as_modified();
}

/*---------------------------------------------------------------
						Destructor
  ---------------------------------------------------------------*/
CPointsMap::~CPointsMap() = default;
/*---------------------------------------------------------------
					save2D_to_text_file
  Save to a text file. In each line there are a point coordinates.
	Returns false if any error occured, true elsewere.
  ---------------------------------------------------------------*/
bool CPointsMap::save2D_to_text_file(const string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (unsigned int i = 0; i < m_x.size(); i++)
		os::fprintf(f, "%f %f\n", m_x[i], m_y[i]);

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

/*---------------------------------------------------------------
					save3D_to_text_file
  Save to a text file. In each line there are a point coordinates.
	Returns false if any error occured, true elsewere.
  ---------------------------------------------------------------*/
bool CPointsMap::save3D_to_text_file(const string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (unsigned int i = 0; i < m_x.size(); i++)
		os::fprintf(f, "%f %f %f\n", m_x[i], m_y[i], m_z[i]);

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

/*---------------------------------------------------------------
					load2Dor3D_from_text_file
  Load from a text file. In each line there are a point coordinates.
	Returns false if any error occured, true elsewere.
  ---------------------------------------------------------------*/
bool CPointsMap::load2Dor3D_from_text_file(
	const std::string& file, const bool is_3D)
{
	MRPT_START

	mark_as_modified();

	FILE* f = os::fopen(file.c_str(), "rt");
	if (!f) return false;

	char str[1024];
	char *ptr, *ptr1, *ptr2, *ptr3;

	// Clear current map:
	this->clear();

	while (!feof(f))
	{
		// Read one string line:
		str[0] = 0;
		if (!fgets(str, sizeof(str), f)) break;

		// Find the first digit:
		ptr = str;
		while (ptr[0] && (ptr[0] == ' ' || ptr[0] == '\t' || ptr[0] == '\r' ||
						  ptr[0] == '\n'))
			ptr++;

		// And try to parse it:
		float xx = strtod(ptr, &ptr1);
		if (ptr1 != str)
		{
			float yy = strtod(ptr1, &ptr2);
			if (ptr2 != ptr1)
			{
				if (!is_3D)
				{
					this->insertPoint(xx, yy, 0);
				}
				else
				{
					float zz = strtod(ptr2, &ptr3);
					if (ptr3 != ptr2)
					{
						this->insertPoint(xx, yy, zz);
					}
				}
			}
		}
	}

	os::fclose(f);
	return true;

	MRPT_END
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::maps::CPointsMap)
#endif

mxArray* CPointsMap::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
	MRPT_TODO("Create 3xN array xyz of points coordinates")
	const char* fields[] = {"x", "y", "z"};
	mexplus::MxArray map_struct(
		mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));

	map_struct.set("x", m_x);
	map_struct.set("y", m_y);
	map_struct.set("z", m_z);
	return map_struct.release();
#else
	THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
#endif
}

/*---------------------------------------------------------------
					getPoint
			Access to stored points coordinates:
  ---------------------------------------------------------------*/
void CPointsMap::getPoint(size_t index, float& x, float& y) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
}
void CPointsMap::getPoint(size_t index, float& x, float& y, float& z) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
	z = m_z[index];
}
void CPointsMap::getPoint(size_t index, double& x, double& y) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
}
void CPointsMap::getPoint(size_t index, double& x, double& y, double& z) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
	z = m_z[index];
}

/*---------------------------------------------------------------
						getPointsBuffer
 ---------------------------------------------------------------*/
void CPointsMap::getPointsBuffer(
	size_t& outPointsCount, const float*& xs, const float*& ys,
	const float*& zs) const
{
	outPointsCount = size();

	if (outPointsCount > 0)
	{
		xs = &m_x[0];
		ys = &m_y[0];
		zs = &m_z[0];
	}
	else
	{
		xs = ys = zs = nullptr;
	}
}

/*---------------------------------------------------------------
						clipOutOfRangeInZ
 ---------------------------------------------------------------*/
void CPointsMap::clipOutOfRangeInZ(float zMin, float zMax)
{
	const size_t n = size();
	vector<bool> deletionMask(n);

	// Compute it:
	for (size_t i = 0; i < n; i++)
		deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);

	// Perform deletion:
	applyDeletionMask(deletionMask);

	mark_as_modified();
}

/*---------------------------------------------------------------
						clipOutOfRange
 ---------------------------------------------------------------*/
void CPointsMap::clipOutOfRange(const TPoint2D& p, float maxRange)
{
	size_t i, n = size();
	vector<bool> deletionMask;

	// The deletion mask:
	deletionMask.resize(n);

	const auto max_sq = maxRange * maxRange;

	// Compute it:
	for (i = 0; i < n; i++)
		deletionMask[i] = square(p.x - m_x[i]) + square(p.y - m_y[i]) > max_sq;

	// Perform deletion:
	applyDeletionMask(deletionMask);

	mark_as_modified();
}

void CPointsMap::determineMatching2D(
	const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
	TMatchingPairList& correspondences, const TMatchingParams& params,
	TMatchingExtraResults& extraResults) const
{
	MRPT_START

	extraResults = TMatchingExtraResults();  // Clear output

	ASSERT_ABOVE_(params.decimation_other_map_points, 0);
	ASSERT_BELOW_(
		params.offset_other_map_points, params.decimation_other_map_points);
	ASSERT_(IS_DERIVED(otherMap2, CPointsMap));
	const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);

	const TPose2D otherMapPose(
		otherMapPose_.x(), otherMapPose_.y(), otherMapPose_.phi());

	const size_t nLocalPoints = otherMap->size();
	const size_t nGlobalPoints = this->size();
	float _sumSqrDist = 0;
	size_t _sumSqrCount = 0;
	size_t nOtherMapPointsWithCorrespondence =
		0;  // Number of points with one corrs. at least

	float local_x_min = std::numeric_limits<float>::max(),
		  local_x_max = -std::numeric_limits<float>::max();
	float global_x_min = std::numeric_limits<float>::max(),
		  global_x_max = -std::numeric_limits<float>::max();
	float local_y_min = std::numeric_limits<float>::max(),
		  local_y_max = -std::numeric_limits<float>::max();
	float global_y_min = std::numeric_limits<float>::max(),
		  global_y_max = -std::numeric_limits<float>::max();

	double maxDistForCorrespondenceSquared;
	float x_local, y_local;
	unsigned int localIdx;

	const float *x_other_it, *y_other_it, *z_other_it;

	// Prepare output: no correspondences initially:
	correspondences.clear();
	correspondences.reserve(nLocalPoints);
	extraResults.correspondencesRatio = 0;

	TMatchingPairList _correspondences;
	_correspondences.reserve(nLocalPoints);

	// Nothing to do if we have an empty map!
	if (!nGlobalPoints || !nLocalPoints) return;

	const double sin_phi = sin(otherMapPose.phi);
	const double cos_phi = cos(otherMapPose.phi);

// Do matching only there is any chance of the two maps to overlap:
// -----------------------------------------------------------
// Translate and rotate all local points, while simultaneously
// estimating the bounding box:
#if MRPT_HAS_SSE2
	// Number of 4-floats:
	size_t nPackets = nLocalPoints / 4;

	Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);

	// load 4 copies of the same value
	const __m128 cos_4val = _mm_set1_ps(cos_phi);
	const __m128 sin_4val = _mm_set1_ps(sin_phi);
	const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
	const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);

	// For the bounding box:
	__m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
	__m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
	__m128 y_mins = x_mins;
	__m128 y_maxs = x_maxs;

	const float* ptr_in_x = &otherMap->m_x[0];
	const float* ptr_in_y = &otherMap->m_y[0];
	float* ptr_out_x = &x_locals[0];
	float* ptr_out_y = &y_locals[0];

	for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
					 ptr_out_y += 4)
	{
		const __m128 xs = _mm_loadu_ps(ptr_in_x);  // *Unaligned* load
		const __m128 ys = _mm_loadu_ps(ptr_in_y);

		const __m128 lxs = _mm_add_ps(
			x0_4val,
			_mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
		const __m128 lys = _mm_add_ps(
			y0_4val,
			_mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
		_mm_store_ps(ptr_out_x, lxs);
		_mm_store_ps(ptr_out_y, lys);

		x_mins = _mm_min_ps(x_mins, lxs);
		x_maxs = _mm_max_ps(x_maxs, lxs);
		y_mins = _mm_min_ps(y_mins, lys);
		y_maxs = _mm_max_ps(y_maxs, lys);
	}

	// Recover the min/max:
	alignas(MRPT_MAX_ALIGN_BYTES) float temp_nums[4];

	_mm_store_ps(temp_nums, x_mins);
	local_x_min =
		min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
	_mm_store_ps(temp_nums, y_mins);
	local_y_min =
		min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
	_mm_store_ps(temp_nums, x_maxs);
	local_x_max =
		max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
	_mm_store_ps(temp_nums, y_maxs);
	local_y_max =
		max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));

	// transform extra pts & update BBox
	for (size_t k = 0; k < nLocalPoints % 4; k++)
	{
		float x = ptr_in_x[k];
		float y = ptr_in_y[k];
		float out_x = otherMapPose.x + cos_phi * x - sin_phi * y;
		float out_y = otherMapPose.y + sin_phi * x + cos_phi * y;

		local_x_min = std::min(local_x_min, out_x);
		local_x_max = std::max(local_x_max, out_x);

		local_y_min = std::min(local_y_min, out_y);
		local_y_max = std::max(local_y_max, out_y);

		ptr_out_x[k] = out_x;
		ptr_out_y[k] = out_y;
	}

#else
	// Non SSE2 version:
	const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> x_org(
		const_cast<float*>(&otherMap->m_x[0]), otherMap->m_x.size(), 1);
	const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> y_org(
		const_cast<float*>(&otherMap->m_y[0]), otherMap->m_y.size(), 1);

	Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
		otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
	Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
		otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();

	local_x_min = x_locals.minCoeff();
	local_y_min = y_locals.minCoeff();
	local_x_max = x_locals.maxCoeff();
	local_y_max = y_locals.maxCoeff();
#endif

	// Find the bounding box:
	float global_z_min, global_z_max;
	this->boundingBox(
		global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
		global_z_max);

	// Only try doing a matching if there exist any chance of both maps
	// touching/overlaping:
	if (local_x_min > global_x_max || local_x_max < global_x_min ||
		local_y_min > global_y_max || local_y_max < global_y_min)
		return;  // We know for sure there is no matching at all

	// Loop for each point in local map:
	// --------------------------------------------------
	for (localIdx = params.offset_other_map_points,
		x_other_it = &otherMap->m_x[params.offset_other_map_points],
		y_other_it = &otherMap->m_y[params.offset_other_map_points],
		z_other_it = &otherMap->m_z[params.offset_other_map_points];
		 localIdx < nLocalPoints;
		 x_other_it += params.decimation_other_map_points,
		y_other_it += params.decimation_other_map_points,
		z_other_it += params.decimation_other_map_points,
		localIdx += params.decimation_other_map_points)
	{
		// For speed-up:
		x_local = x_locals[localIdx];  // *x_locals_it;
		y_local = y_locals[localIdx];  // *y_locals_it;

		// Find all the matchings in the requested distance:

		// KD-TREE implementation =================================
		// Use a KD-tree to look for the nearnest neighbor of:
		//   (x_local, y_local, z_local)
		// In "this" (global/reference) points map.

		float tentativ_err_sq;
		unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
			x_local, y_local,  // Look closest to this guy
			tentativ_err_sq  // save here the min. distance squared
		);

		// Compute max. allowed distance:
		maxDistForCorrespondenceSquared = square(
			params.maxAngularDistForCorrespondence *
				std::sqrt(
					square(params.angularDistPivotPoint.x - x_local) +
					square(params.angularDistPivotPoint.y - y_local)) +
			params.maxDistForCorrespondence);

		// Distance below the threshold??
		if (tentativ_err_sq < maxDistForCorrespondenceSquared)
		{
			// Save all the correspondences:
			_correspondences.resize(_correspondences.size() + 1);

			TMatchingPair& p = _correspondences.back();

			p.this_idx = tentativ_this_idx;
			p.this_x = m_x[tentativ_this_idx];
			p.this_y = m_y[tentativ_this_idx];
			p.this_z = m_z[tentativ_this_idx];

			p.other_idx = localIdx;
			p.other_x = *x_other_it;
			p.other_y = *y_other_it;
			p.other_z = *z_other_it;

			p.errorSquareAfterTransformation = tentativ_err_sq;

			// At least one:
			nOtherMapPointsWithCorrespondence++;

			// Accumulate the MSE:
			_sumSqrDist += p.errorSquareAfterTransformation;
			_sumSqrCount++;
		}

	}  // For each local point

	// Additional consistency filter: "onlyKeepTheClosest" up to now
	//  led to just one correspondence for each "local map" point, but
	//  many of them may have as corresponding pair the same "global point"!!
	// -------------------------------------------------------------------------
	if (params.onlyUniqueRobust)
	{
		ASSERTMSG_(
			params.onlyKeepTheClosest,
			"ERROR: onlyKeepTheClosest must be also set to true when "
			"onlyUniqueRobust=true.");
		_correspondences.filterUniqueRobustPairs(
			nGlobalPoints, correspondences);
	}
	else
	{
		correspondences.swap(_correspondences);
	}

	// If requested, copy sum of squared distances to output pointer:
	// -------------------------------------------------------------------
	if (_sumSqrCount)
		extraResults.sumSqrDist =
			_sumSqrDist / static_cast<double>(_sumSqrCount);
	else
		extraResults.sumSqrDist = 0;

	// The ratio of points in the other map with corrs:
	extraResults.correspondencesRatio = params.decimation_other_map_points *
										nOtherMapPointsWithCorrespondence /
										static_cast<float>(nLocalPoints);

	MRPT_END
}

/*---------------------------------------------------------------
				changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointsMap::changeCoordinatesReference(const CPose2D& newBase)
{
	const size_t N = m_x.size();

	const CPose3D newBase3D(newBase);

	for (size_t i = 0; i < N; i++)
		newBase3D.composePoint(
			m_x[i], m_y[i], m_z[i],  // In
			m_x[i], m_y[i], m_z[i]  // Out
		);

	mark_as_modified();
}

/*---------------------------------------------------------------
				changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointsMap::changeCoordinatesReference(const CPose3D& newBase)
{
	const size_t N = m_x.size();

	for (size_t i = 0; i < N; i++)
		newBase.composePoint(
			m_x[i], m_y[i], m_z[i],  // In
			m_x[i], m_y[i], m_z[i]  // Out
		);

	mark_as_modified();
}

/*---------------------------------------------------------------
				changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointsMap::changeCoordinatesReference(
	const CPointsMap& other, const CPose3D& newBase)
{
	*this = other;
	changeCoordinatesReference(newBase);
}

/*---------------------------------------------------------------
				isEmpty
 ---------------------------------------------------------------*/
bool CPointsMap::isEmpty() const { return m_x.empty(); }
/*---------------------------------------------------------------
				TInsertionOptions
 ---------------------------------------------------------------*/
CPointsMap::TInsertionOptions::TInsertionOptions()
	: horizontalTolerance(DEG2RAD(0.05))

{
}

// Binary dump to/read from stream - for usage in derived classes' serialization
void CPointsMap::TInsertionOptions::writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const int8_t version = 0;
	out << version;

	out << minDistBetweenLaserPoints << addToExistingPointsMap
		<< also_interpolate << disableDeletion << fuseWithExisting
		<< isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
		<< insertInvalidPoints;  // v0
}

void CPointsMap::TInsertionOptions::readFromStream(
	mrpt::serialization::CArchive& in)
{
	int8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		{
			in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
				also_interpolate >> disableDeletion >> fuseWithExisting >>
				isPlanarMap >> horizontalTolerance >>
				maxDistForInterpolatePoints >> insertInvalidPoints;  // v0
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

CPointsMap::TLikelihoodOptions::TLikelihoodOptions()

	= default;

void CPointsMap::TLikelihoodOptions::writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const int8_t version = 0;
	out << version;
	out << sigma_dist << max_corr_distance << decimation;
}

void CPointsMap::TLikelihoodOptions::readFromStream(
	mrpt::serialization::CArchive& in)
{
	int8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		{
			in >> sigma_dist >> max_corr_distance >> decimation;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

void CPointsMap::TRenderOptions::writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const int8_t version = 0;
	out << version;
	out << point_size << color << int8_t(colormap);
}

void CPointsMap::TRenderOptions::readFromStream(
	mrpt::serialization::CArchive& in)
{
	int8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		{
			in >> point_size >> this->color;
			in.ReadAsAndCastTo<int8_t>(this->colormap);
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

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

	LOADABLEOPTS_DUMP_VAR(minDistBetweenLaserPoints, double);
	LOADABLEOPTS_DUMP_VAR(maxDistForInterpolatePoints, double);
	LOADABLEOPTS_DUMP_VAR_DEG(horizontalTolerance);

	LOADABLEOPTS_DUMP_VAR(addToExistingPointsMap, bool);
	LOADABLEOPTS_DUMP_VAR(also_interpolate, bool);
	LOADABLEOPTS_DUMP_VAR(disableDeletion, bool);
	LOADABLEOPTS_DUMP_VAR(fuseWithExisting, bool);
	LOADABLEOPTS_DUMP_VAR(isPlanarMap, bool);

	LOADABLEOPTS_DUMP_VAR(insertInvalidPoints, bool);

	out << endl;
}

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

	LOADABLEOPTS_DUMP_VAR(sigma_dist, double);
	LOADABLEOPTS_DUMP_VAR(max_corr_distance, double);
	LOADABLEOPTS_DUMP_VAR(decimation, int);
}

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

	LOADABLEOPTS_DUMP_VAR(point_size, float);
	LOADABLEOPTS_DUMP_VAR(color.R, float);
	LOADABLEOPTS_DUMP_VAR(color.G, float);
	LOADABLEOPTS_DUMP_VAR(color.B, float);
	// LOADABLEOPTS_DUMP_VAR(colormap, int);
}
/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CPointsMap::TInsertionOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(minDistBetweenLaserPoints, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(addToExistingPointsMap, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(also_interpolate, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(disableDeletion, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(fuseWithExisting, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(isPlanarMap, bool, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(maxDistForInterpolatePoints, float, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(insertInvalidPoints, bool, iniFile, section);
}

void CPointsMap::TLikelihoodOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(sigma_dist, double, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(max_corr_distance, double, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
}

void CPointsMap::TRenderOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(point_size, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(color.R, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(color.G, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(color.B, float, iniFile, section);
	colormap = iniFile.read_enum(section, "colormap", this->colormap);
}

/*---------------------------------------------------------------
						getAs3DObject
---------------------------------------------------------------*/
void CPointsMap::getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const
{
	if (!genericMapParams.enableSaveAs3DObject) return;

	if (renderOptions.colormap == mrpt::img::cmNONE)
	{
		// Single color:
		auto obj = opengl::CPointCloud::Create();
		obj->loadFromPointsMap(this);
		obj->setColor(renderOptions.color);
		obj->setPointSize(renderOptions.point_size);
		obj->enableColorFromZ(true);

		obj->setGradientColors(TColorf(0.0, 0, 0), renderOptions.color);
		outObj->insert(obj);
	}
	else
	{
		auto obj = opengl::CPointCloudColoured::Create();
		obj->loadFromPointsMap(this);
		obj->setPointSize(renderOptions.point_size);
		mrpt::math::TPoint3D pMin, pMax;
		this->boundingBox(pMin, pMax);
		obj->recolorizeByCoordinate(
			pMin.z, pMax.z, 2 /*z*/, renderOptions.colormap);
		outObj->insert(obj);
	}
}

float CPointsMap::compute3DMatchingRatio(
	const mrpt::maps::CMetricMap* otherMap2,
	const mrpt::poses::CPose3D& otherMapPose,
	const TMatchingRatioParams& mrp) const
{
	TMatchingPairList correspondences;
	TMatchingParams params;
	TMatchingExtraResults extraResults;

	params.maxDistForCorrespondence = mrp.maxDistForCorr;

	this->determineMatching3D(
		otherMap2->getAsSimplePointsMap(), otherMapPose, correspondences,<commit_msg>no colormap means no colormap<commit_after>/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2019, 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 "maps-precomp.h"  // Precomp header

#include <mrpt/config/CConfigFile.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/os.h>
#include <mrpt/math/geometry.h>
#include <mrpt/serialization/CArchive.h>

#include <mrpt/maps/CPointsMap.h>
#include <mrpt/maps/CSimplePointsMap.h>

#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CPointCloudColoured.h>

// Observations:
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CObservationVelodyneScan.h>
#include <mrpt/obs/CObservationPointCloud.h>

#if MRPT_HAS_PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#   include <pcl/registration/icp.h>
#endif

#if MRPT_HAS_SSE2
#include <mrpt/core/SSE_types.h>
#include <mrpt/core/SSE_macros.h>
#endif

#if MRPT_HAS_MATLAB
#include <mexplus.h>
#endif

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

IMPLEMENTS_VIRTUAL_SERIALIZABLE(CPointsMap, CMetricMap, mrpt::maps)

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CPointsMap::CPointsMap()
	: insertionOptions(), likelihoodOptions(), m_x(), m_y(), m_z()

{
	mark_as_modified();
}

/*---------------------------------------------------------------
						Destructor
  ---------------------------------------------------------------*/
CPointsMap::~CPointsMap() = default;
/*---------------------------------------------------------------
					save2D_to_text_file
  Save to a text file. In each line there are a point coordinates.
	Returns false if any error occured, true elsewere.
  ---------------------------------------------------------------*/
bool CPointsMap::save2D_to_text_file(const string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (unsigned int i = 0; i < m_x.size(); i++)
		os::fprintf(f, "%f %f\n", m_x[i], m_y[i]);

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

/*---------------------------------------------------------------
					save3D_to_text_file
  Save to a text file. In each line there are a point coordinates.
	Returns false if any error occured, true elsewere.
  ---------------------------------------------------------------*/
bool CPointsMap::save3D_to_text_file(const string& file) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	for (unsigned int i = 0; i < m_x.size(); i++)
		os::fprintf(f, "%f %f %f\n", m_x[i], m_y[i], m_z[i]);

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

/*---------------------------------------------------------------
					load2Dor3D_from_text_file
  Load from a text file. In each line there are a point coordinates.
	Returns false if any error occured, true elsewere.
  ---------------------------------------------------------------*/
bool CPointsMap::load2Dor3D_from_text_file(
	const std::string& file, const bool is_3D)
{
	MRPT_START

	mark_as_modified();

	FILE* f = os::fopen(file.c_str(), "rt");
	if (!f) return false;

	char str[1024];
	char *ptr, *ptr1, *ptr2, *ptr3;

	// Clear current map:
	this->clear();

	while (!feof(f))
	{
		// Read one string line:
		str[0] = 0;
		if (!fgets(str, sizeof(str), f)) break;

		// Find the first digit:
		ptr = str;
		while (ptr[0] && (ptr[0] == ' ' || ptr[0] == '\t' || ptr[0] == '\r' ||
						  ptr[0] == '\n'))
			ptr++;

		// And try to parse it:
		float xx = strtod(ptr, &ptr1);
		if (ptr1 != str)
		{
			float yy = strtod(ptr1, &ptr2);
			if (ptr2 != ptr1)
			{
				if (!is_3D)
				{
					this->insertPoint(xx, yy, 0);
				}
				else
				{
					float zz = strtod(ptr2, &ptr3);
					if (ptr3 != ptr2)
					{
						this->insertPoint(xx, yy, zz);
					}
				}
			}
		}
	}

	os::fclose(f);
	return true;

	MRPT_END
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::maps::CPointsMap)
#endif

mxArray* CPointsMap::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
	MRPT_TODO("Create 3xN array xyz of points coordinates")
	const char* fields[] = {"x", "y", "z"};
	mexplus::MxArray map_struct(
		mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));

	map_struct.set("x", m_x);
	map_struct.set("y", m_y);
	map_struct.set("z", m_z);
	return map_struct.release();
#else
	THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
#endif
}

/*---------------------------------------------------------------
					getPoint
			Access to stored points coordinates:
  ---------------------------------------------------------------*/
void CPointsMap::getPoint(size_t index, float& x, float& y) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
}
void CPointsMap::getPoint(size_t index, float& x, float& y, float& z) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
	z = m_z[index];
}
void CPointsMap::getPoint(size_t index, double& x, double& y) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
}
void CPointsMap::getPoint(size_t index, double& x, double& y, double& z) const
{
	ASSERT_BELOW_(index, m_x.size());
	x = m_x[index];
	y = m_y[index];
	z = m_z[index];
}

/*---------------------------------------------------------------
						getPointsBuffer
 ---------------------------------------------------------------*/
void CPointsMap::getPointsBuffer(
	size_t& outPointsCount, const float*& xs, const float*& ys,
	const float*& zs) const
{
	outPointsCount = size();

	if (outPointsCount > 0)
	{
		xs = &m_x[0];
		ys = &m_y[0];
		zs = &m_z[0];
	}
	else
	{
		xs = ys = zs = nullptr;
	}
}

/*---------------------------------------------------------------
						clipOutOfRangeInZ
 ---------------------------------------------------------------*/
void CPointsMap::clipOutOfRangeInZ(float zMin, float zMax)
{
	const size_t n = size();
	vector<bool> deletionMask(n);

	// Compute it:
	for (size_t i = 0; i < n; i++)
		deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);

	// Perform deletion:
	applyDeletionMask(deletionMask);

	mark_as_modified();
}

/*---------------------------------------------------------------
						clipOutOfRange
 ---------------------------------------------------------------*/
void CPointsMap::clipOutOfRange(const TPoint2D& p, float maxRange)
{
	size_t i, n = size();
	vector<bool> deletionMask;

	// The deletion mask:
	deletionMask.resize(n);

	const auto max_sq = maxRange * maxRange;

	// Compute it:
	for (i = 0; i < n; i++)
		deletionMask[i] = square(p.x - m_x[i]) + square(p.y - m_y[i]) > max_sq;

	// Perform deletion:
	applyDeletionMask(deletionMask);

	mark_as_modified();
}

void CPointsMap::determineMatching2D(
	const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
	TMatchingPairList& correspondences, const TMatchingParams& params,
	TMatchingExtraResults& extraResults) const
{
	MRPT_START

	extraResults = TMatchingExtraResults();  // Clear output

	ASSERT_ABOVE_(params.decimation_other_map_points, 0);
	ASSERT_BELOW_(
		params.offset_other_map_points, params.decimation_other_map_points);
	ASSERT_(IS_DERIVED(otherMap2, CPointsMap));
	const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);

	const TPose2D otherMapPose(
		otherMapPose_.x(), otherMapPose_.y(), otherMapPose_.phi());

	const size_t nLocalPoints = otherMap->size();
	const size_t nGlobalPoints = this->size();
	float _sumSqrDist = 0;
	size_t _sumSqrCount = 0;
	size_t nOtherMapPointsWithCorrespondence =
		0;  // Number of points with one corrs. at least

	float local_x_min = std::numeric_limits<float>::max(),
		  local_x_max = -std::numeric_limits<float>::max();
	float global_x_min = std::numeric_limits<float>::max(),
		  global_x_max = -std::numeric_limits<float>::max();
	float local_y_min = std::numeric_limits<float>::max(),
		  local_y_max = -std::numeric_limits<float>::max();
	float global_y_min = std::numeric_limits<float>::max(),
		  global_y_max = -std::numeric_limits<float>::max();

	double maxDistForCorrespondenceSquared;
	float x_local, y_local;
	unsigned int localIdx;

	const float *x_other_it, *y_other_it, *z_other_it;

	// Prepare output: no correspondences initially:
	correspondences.clear();
	correspondences.reserve(nLocalPoints);
	extraResults.correspondencesRatio = 0;

	TMatchingPairList _correspondences;
	_correspondences.reserve(nLocalPoints);

	// Nothing to do if we have an empty map!
	if (!nGlobalPoints || !nLocalPoints) return;

	const double sin_phi = sin(otherMapPose.phi);
	const double cos_phi = cos(otherMapPose.phi);

// Do matching only there is any chance of the two maps to overlap:
// -----------------------------------------------------------
// Translate and rotate all local points, while simultaneously
// estimating the bounding box:
#if MRPT_HAS_SSE2
	// Number of 4-floats:
	size_t nPackets = nLocalPoints / 4;

	Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);

	// load 4 copies of the same value
	const __m128 cos_4val = _mm_set1_ps(cos_phi);
	const __m128 sin_4val = _mm_set1_ps(sin_phi);
	const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
	const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);

	// For the bounding box:
	__m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
	__m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
	__m128 y_mins = x_mins;
	__m128 y_maxs = x_maxs;

	const float* ptr_in_x = &otherMap->m_x[0];
	const float* ptr_in_y = &otherMap->m_y[0];
	float* ptr_out_x = &x_locals[0];
	float* ptr_out_y = &y_locals[0];

	for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
					 ptr_out_y += 4)
	{
		const __m128 xs = _mm_loadu_ps(ptr_in_x);  // *Unaligned* load
		const __m128 ys = _mm_loadu_ps(ptr_in_y);

		const __m128 lxs = _mm_add_ps(
			x0_4val,
			_mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
		const __m128 lys = _mm_add_ps(
			y0_4val,
			_mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
		_mm_store_ps(ptr_out_x, lxs);
		_mm_store_ps(ptr_out_y, lys);

		x_mins = _mm_min_ps(x_mins, lxs);
		x_maxs = _mm_max_ps(x_maxs, lxs);
		y_mins = _mm_min_ps(y_mins, lys);
		y_maxs = _mm_max_ps(y_maxs, lys);
	}

	// Recover the min/max:
	alignas(MRPT_MAX_ALIGN_BYTES) float temp_nums[4];

	_mm_store_ps(temp_nums, x_mins);
	local_x_min =
		min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
	_mm_store_ps(temp_nums, y_mins);
	local_y_min =
		min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
	_mm_store_ps(temp_nums, x_maxs);
	local_x_max =
		max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
	_mm_store_ps(temp_nums, y_maxs);
	local_y_max =
		max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));

	// transform extra pts & update BBox
	for (size_t k = 0; k < nLocalPoints % 4; k++)
	{
		float x = ptr_in_x[k];
		float y = ptr_in_y[k];
		float out_x = otherMapPose.x + cos_phi * x - sin_phi * y;
		float out_y = otherMapPose.y + sin_phi * x + cos_phi * y;

		local_x_min = std::min(local_x_min, out_x);
		local_x_max = std::max(local_x_max, out_x);

		local_y_min = std::min(local_y_min, out_y);
		local_y_max = std::max(local_y_max, out_y);

		ptr_out_x[k] = out_x;
		ptr_out_y[k] = out_y;
	}

#else
	// Non SSE2 version:
	const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> x_org(
		const_cast<float*>(&otherMap->m_x[0]), otherMap->m_x.size(), 1);
	const Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1>> y_org(
		const_cast<float*>(&otherMap->m_y[0]), otherMap->m_y.size(), 1);

	Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
		otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
	Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
		otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();

	local_x_min = x_locals.minCoeff();
	local_y_min = y_locals.minCoeff();
	local_x_max = x_locals.maxCoeff();
	local_y_max = y_locals.maxCoeff();
#endif

	// Find the bounding box:
	float global_z_min, global_z_max;
	this->boundingBox(
		global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
		global_z_max);

	// Only try doing a matching if there exist any chance of both maps
	// touching/overlaping:
	if (local_x_min > global_x_max || local_x_max < global_x_min ||
		local_y_min > global_y_max || local_y_max < global_y_min)
		return;  // We know for sure there is no matching at all

	// Loop for each point in local map:
	// --------------------------------------------------
	for (localIdx = params.offset_other_map_points,
		x_other_it = &otherMap->m_x[params.offset_other_map_points],
		y_other_it = &otherMap->m_y[params.offset_other_map_points],
		z_other_it = &otherMap->m_z[params.offset_other_map_points];
		 localIdx < nLocalPoints;
		 x_other_it += params.decimation_other_map_points,
		y_other_it += params.decimation_other_map_points,
		z_other_it += params.decimation_other_map_points,
		localIdx += params.decimation_other_map_points)
	{
		// For speed-up:
		x_local = x_locals[localIdx];  // *x_locals_it;
		y_local = y_locals[localIdx];  // *y_locals_it;

		// Find all the matchings in the requested distance:

		// KD-TREE implementation =================================
		// Use a KD-tree to look for the nearnest neighbor of:
		//   (x_local, y_local, z_local)
		// In "this" (global/reference) points map.

		float tentativ_err_sq;
		unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
			x_local, y_local,  // Look closest to this guy
			tentativ_err_sq  // save here the min. distance squared
		);

		// Compute max. allowed distance:
		maxDistForCorrespondenceSquared = square(
			params.maxAngularDistForCorrespondence *
				std::sqrt(
					square(params.angularDistPivotPoint.x - x_local) +
					square(params.angularDistPivotPoint.y - y_local)) +
			params.maxDistForCorrespondence);

		// Distance below the threshold??
		if (tentativ_err_sq < maxDistForCorrespondenceSquared)
		{
			// Save all the correspondences:
			_correspondences.resize(_correspondences.size() + 1);

			TMatchingPair& p = _correspondences.back();

			p.this_idx = tentativ_this_idx;
			p.this_x = m_x[tentativ_this_idx];
			p.this_y = m_y[tentativ_this_idx];
			p.this_z = m_z[tentativ_this_idx];

			p.other_idx = localIdx;
			p.other_x = *x_other_it;
			p.other_y = *y_other_it;
			p.other_z = *z_other_it;

			p.errorSquareAfterTransformation = tentativ_err_sq;

			// At least one:
			nOtherMapPointsWithCorrespondence++;

			// Accumulate the MSE:
			_sumSqrDist += p.errorSquareAfterTransformation;
			_sumSqrCount++;
		}

	}  // For each local point

	// Additional consistency filter: "onlyKeepTheClosest" up to now
	//  led to just one correspondence for each "local map" point, but
	//  many of them may have as corresponding pair the same "global point"!!
	// -------------------------------------------------------------------------
	if (params.onlyUniqueRobust)
	{
		ASSERTMSG_(
			params.onlyKeepTheClosest,
			"ERROR: onlyKeepTheClosest must be also set to true when "
			"onlyUniqueRobust=true.");
		_correspondences.filterUniqueRobustPairs(
			nGlobalPoints, correspondences);
	}
	else
	{
		correspondences.swap(_correspondences);
	}

	// If requested, copy sum of squared distances to output pointer:
	// -------------------------------------------------------------------
	if (_sumSqrCount)
		extraResults.sumSqrDist =
			_sumSqrDist / static_cast<double>(_sumSqrCount);
	else
		extraResults.sumSqrDist = 0;

	// The ratio of points in the other map with corrs:
	extraResults.correspondencesRatio = params.decimation_other_map_points *
										nOtherMapPointsWithCorrespondence /
										static_cast<float>(nLocalPoints);

	MRPT_END
}

/*---------------------------------------------------------------
				changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointsMap::changeCoordinatesReference(const CPose2D& newBase)
{
	const size_t N = m_x.size();

	const CPose3D newBase3D(newBase);

	for (size_t i = 0; i < N; i++)
		newBase3D.composePoint(
			m_x[i], m_y[i], m_z[i],  // In
			m_x[i], m_y[i], m_z[i]  // Out
		);

	mark_as_modified();
}

/*---------------------------------------------------------------
				changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointsMap::changeCoordinatesReference(const CPose3D& newBase)
{
	const size_t N = m_x.size();

	for (size_t i = 0; i < N; i++)
		newBase.composePoint(
			m_x[i], m_y[i], m_z[i],  // In
			m_x[i], m_y[i], m_z[i]  // Out
		);

	mark_as_modified();
}

/*---------------------------------------------------------------
				changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointsMap::changeCoordinatesReference(
	const CPointsMap& other, const CPose3D& newBase)
{
	*this = other;
	changeCoordinatesReference(newBase);
}

/*---------------------------------------------------------------
				isEmpty
 ---------------------------------------------------------------*/
bool CPointsMap::isEmpty() const { return m_x.empty(); }
/*---------------------------------------------------------------
				TInsertionOptions
 ---------------------------------------------------------------*/
CPointsMap::TInsertionOptions::TInsertionOptions()
	: horizontalTolerance(DEG2RAD(0.05))

{
}

// Binary dump to/read from stream - for usage in derived classes' serialization
void CPointsMap::TInsertionOptions::writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const int8_t version = 0;
	out << version;

	out << minDistBetweenLaserPoints << addToExistingPointsMap
		<< also_interpolate << disableDeletion << fuseWithExisting
		<< isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
		<< insertInvalidPoints;  // v0
}

void CPointsMap::TInsertionOptions::readFromStream(
	mrpt::serialization::CArchive& in)
{
	int8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		{
			in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
				also_interpolate >> disableDeletion >> fuseWithExisting >>
				isPlanarMap >> horizontalTolerance >>
				maxDistForInterpolatePoints >> insertInvalidPoints;  // v0
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

CPointsMap::TLikelihoodOptions::TLikelihoodOptions()

	= default;

void CPointsMap::TLikelihoodOptions::writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const int8_t version = 0;
	out << version;
	out << sigma_dist << max_corr_distance << decimation;
}

void CPointsMap::TLikelihoodOptions::readFromStream(
	mrpt::serialization::CArchive& in)
{
	int8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		{
			in >> sigma_dist >> max_corr_distance >> decimation;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

void CPointsMap::TRenderOptions::writeToStream(
	mrpt::serialization::CArchive& out) const
{
	const int8_t version = 0;
	out << version;
	out << point_size << color << int8_t(colormap);
}

void CPointsMap::TRenderOptions::readFromStream(
	mrpt::serialization::CArchive& in)
{
	int8_t version;
	in >> version;
	switch (version)
	{
		case 0:
		{
			in >> point_size >> this->color;
			in.ReadAsAndCastTo<int8_t>(this->colormap);
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

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

	LOADABLEOPTS_DUMP_VAR(minDistBetweenLaserPoints, double);
	LOADABLEOPTS_DUMP_VAR(maxDistForInterpolatePoints, double);
	LOADABLEOPTS_DUMP_VAR_DEG(horizontalTolerance);

	LOADABLEOPTS_DUMP_VAR(addToExistingPointsMap, bool);
	LOADABLEOPTS_DUMP_VAR(also_interpolate, bool);
	LOADABLEOPTS_DUMP_VAR(disableDeletion, bool);
	LOADABLEOPTS_DUMP_VAR(fuseWithExisting, bool);
	LOADABLEOPTS_DUMP_VAR(isPlanarMap, bool);

	LOADABLEOPTS_DUMP_VAR(insertInvalidPoints, bool);

	out << endl;
}

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

	LOADABLEOPTS_DUMP_VAR(sigma_dist, double);
	LOADABLEOPTS_DUMP_VAR(max_corr_distance, double);
	LOADABLEOPTS_DUMP_VAR(decimation, int);
}

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

	LOADABLEOPTS_DUMP_VAR(point_size, float);
	LOADABLEOPTS_DUMP_VAR(color.R, float);
	LOADABLEOPTS_DUMP_VAR(color.G, float);
	LOADABLEOPTS_DUMP_VAR(color.B, float);
	// LOADABLEOPTS_DUMP_VAR(colormap, int);
}
/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CPointsMap::TInsertionOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(minDistBetweenLaserPoints, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(addToExistingPointsMap, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(also_interpolate, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(disableDeletion, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(fuseWithExisting, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(isPlanarMap, bool, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(maxDistForInterpolatePoints, float, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(insertInvalidPoints, bool, iniFile, section);
}

void CPointsMap::TLikelihoodOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(sigma_dist, double, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(max_corr_distance, double, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
}

void CPointsMap::TRenderOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(point_size, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(color.R, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(color.G, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(color.B, float, iniFile, section);
	colormap = iniFile.read_enum(section, "colormap", this->colormap);
}

/*---------------------------------------------------------------
						getAs3DObject
---------------------------------------------------------------*/
void CPointsMap::getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const
{
	if (!genericMapParams.enableSaveAs3DObject) return;

	if (renderOptions.colormap == mrpt::img::cmNONE)
	{
		// Single color:
		auto obj = opengl::CPointCloud::Create();
		obj->loadFromPointsMap(this);
		obj->setColor(renderOptions.color);
		obj->setPointSize(renderOptions.point_size);
		obj->enableColorFromZ(false);
		outObj->insert(obj);
	}
	else
	{
		auto obj = opengl::CPointCloudColoured::Create();
		obj->loadFromPointsMap(this);
		obj->setPointSize(renderOptions.point_size);
		mrpt::math::TPoint3D pMin, pMax;
		this->boundingBox(pMin, pMax);
		obj->recolorizeByCoordinate(
			pMin.z, pMax.z, 2 /*z*/, renderOptions.colormap);
		outObj->insert(obj);
	}
}

float CPointsMap::compute3DMatchingRatio(
	const mrpt::maps::CMetricMap* otherMap2,
	const mrpt::poses::CPose3D& otherMapPose,
	const TMatchingRatioParams& mrp) const
{
	TMatchingPairList correspondences;
	TMatchingParams params;
	TMatchingExtraResults extraResults;

	params.maxDistForCorrespondence = mrp.maxDistForCorr;

	this->determineMatching3D(
		otherMap2->getAsSimplePointsMap(), otherMapPose, correspondences,
{'fraction_non_alphanumeric': 0.22450017288018387, 'fraction_numerical': 0.008420281896393923, 'mean_word_length': 2.805278229239223, 'pattern_counts': {'":': 0, '<': 205, '<?xml version=': 0, '>': 232, 'https://': 0, 'lorem ipsum': 0, 'www.': 6, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3725504', 'n_tokens_mistral': 18381, 'n_tokens_neox': 17064, 'n_words': 3345}