/* +------------------------------------------------------------------------+
   |                     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 "vision-precomp.h"	 // Precompiled headers
//
#include <mrpt/3rdparty/do_opencv_includes.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/obs/CObservationVisualLandmarks.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/vision/CFeature.h>
#include <mrpt/vision/CFeatureExtraction.h>
#include <mrpt/vision/pinhole.h>
#include <mrpt/vision/utils.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::config;
using namespace mrpt::maps;
using namespace mrpt::tfest;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::poses;
using namespace mrpt::obs;
using namespace std;
const int FEAT_FREE = -1;
// const int NOT_ASIG = 0;
// const int ASG_FEAT = 1;
// const int AMB_FEAT = 2;

/*-------------------------------------------------------------
					openCV_cross_correlation
-------------------------------------------------------------*/
void vision::openCV_cross_correlation(
	const CImage& img, const CImage& patch_img, size_t& x_max, size_t& y_max,
	double& max_val, int x_search_ini, int y_search_ini, int x_search_size,
	int y_search_size)
{
	MRPT_START

#if MRPT_HAS_OPENCV
	bool entireImg =
		(x_search_ini < 0 || y_search_ini < 0 || x_search_size < 0 ||
		 y_search_size < 0);

	CImage im, patch_im;

	if (img.isColor() && patch_img.isColor())
	{
		img.grayscale(im);
		patch_img.grayscale(patch_im);
	}
	else
	{
		ASSERT_(!img.isColor() && !patch_img.isColor());
		im = img.makeShallowCopy();
		patch_im = patch_img.makeShallowCopy();
	}

	const int im_w = im.getWidth();
	const int im_h = im.getHeight();
	const int patch_w = patch_im.getWidth();
	const int patch_h = patch_im.getHeight();

	if (entireImg)
	{
		x_search_size = im_w - patch_w;
		y_search_size = im_h - patch_h;
	}

	// JLBC: Perhaps is better to raise the exception always??
	if ((x_search_ini + x_search_size + patch_w) > im_w)
		x_search_size -= (x_search_ini + x_search_size + patch_w) - im_w;

	if ((y_search_ini + y_search_size + patch_h) > im_h)
		y_search_size -= (y_search_ini + y_search_size + patch_h) - im_h;

	ASSERT_((x_search_ini + x_search_size + patch_w) <= im_w);
	ASSERT_((y_search_ini + y_search_size + patch_h) <= im_h);
	CImage img_region_to_search;

	if (entireImg) { img_region_to_search = im.makeShallowCopy(); }
	else
	{
		im.extract_patch(
			img_region_to_search,
			x_search_ini,  // start corner
			y_search_ini,
			patch_w + x_search_size,  // sub-image size
			patch_h + y_search_size);
	}

	cv::Mat result(cvSize(x_search_size + 1, y_search_size + 1), CV_32FC1);

	// Compute cross correlation:
	cv::matchTemplate(
		img_region_to_search.asCvMat<cv::Mat>(SHALLOW_COPY),
		patch_im.asCvMat<cv::Mat>(SHALLOW_COPY), result, CV_TM_CCORR_NORMED);

	// Find the max point:
	double mini;
	cv::Point min_point, max_point;

	cv::minMaxLoc(
		result, &mini, &max_val, &min_point, &max_point, cv::noArray());
	x_max = max_point.x + x_search_ini + (mrpt::round(patch_w - 1) >> 1);
	y_max = max_point.y + y_search_ini + (mrpt::round(patch_h - 1) >> 1);

#else
	THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif

	MRPT_END
}

/*-------------------------------------------------------------
					pixelTo3D
-------------------------------------------------------------*/
TPoint3D vision::pixelTo3D(const TPixelCoordf& xy, const CMatrixDouble33& A)
{
	TPoint3D res;

	// Build the vector:
	res.x = xy.x - A(0, 2);
	res.y = xy.y - A(1, 2);
	res.z = A(0, 0);

	// Normalize:
	const double u = res.norm();
	ASSERT_(u != 0);
	res *= 1.0 / u;

	return res;
}

/*-------------------------------------------------------------
					buildIntrinsicParamsMatrix
-------------------------------------------------------------*/
CMatrixDouble33 vision::buildIntrinsicParamsMatrix(
	const double focalLengthX, const double focalLengthY, const double centerX,
	const double centerY)
{
	CMatrixDouble33 A;

	A(0, 0) = focalLengthX;
	A(1, 1) = focalLengthY;
	A(2, 2) = 1;

	A(0, 2) = centerX;
	A(1, 2) = centerY;

	return A;
}

/*-------------------------------------------------------------
					defaultIntrinsicParamsMatrix
-------------------------------------------------------------*/
CMatrixDouble33 vision::defaultIntrinsicParamsMatrix(
	unsigned int camIndex, unsigned int resX, unsigned int resY)
{
	float fx, fy, cx, cy;

	switch (camIndex)
	{
		case 0:
			// Bumblebee:
			fx = 0.79345f;
			fy = 1.05793f;
			cx = 0.55662f;
			cy = 0.52692f;
			break;

		case 1:
			// Sony:
			fx = 0.95666094f;
			fy = 1.3983423f;
			cx = 0.54626328f;
			cy = 0.4939191f;
			break;

		default:
		{
			THROW_EXCEPTION_FMT(
				"Unknown camera index!! for 'camIndex'=%u", camIndex);
		}
	}

	return buildIntrinsicParamsMatrix(
		resX * fx, resY * fy, resX * cx, resY * cy);
}

/*-------------------------------------------------------------
						computeMsd
-------------------------------------------------------------*/
double vision::computeMsd(
	const TMatchingPairList& feat_list, const poses::CPose3D& Rt)
{
	CMatrixDouble44 mat;
	Rt.getHomogeneousMatrix(mat);
	double acum = 0.0;

	TMatchingPairList::const_iterator it;
	TPoint3D err;
	for (it = feat_list.begin(); it != feat_list.end(); it++)
	{
		err.x = it->local.x -
			(it->global.x * mat(0, 0) + it->global.y * mat(0, 1) +
			 it->global.z * mat(0, 2) + Rt.x());
		err.y = it->local.y -
			(it->global.x * mat(1, 0) + it->global.y * mat(1, 1) +
			 it->global.z * mat(1, 2) + Rt.y());
		err.z = it->local.z -
			(it->global.x * mat(2, 0) + it->global.y * mat(2, 1) +
			 it->global.z * mat(2, 2) + Rt.z());

		acum += err.norm();

	}  // end for
	return (acum / feat_list.size());
}  // end msd

/*-------------------------------------------------------------
					cloudsToMatchedList
-------------------------------------------------------------*/
void vision::cloudsToMatchedList(
	const CObservationVisualLandmarks& cloud1,
	const CObservationVisualLandmarks& cloud2, TMatchingPairList& outList)
{
	CLandmarksMap::TCustomSequenceLandmarks::const_iterator itLand1, itLand2;
	TMatchingPair pair;

	for (itLand1 = cloud1.landmarks.landmarks.begin();
		 itLand1 != cloud1.landmarks.landmarks.end(); itLand1++)
		for (itLand2 = cloud2.landmarks.landmarks.begin();
			 itLand2 != cloud2.landmarks.landmarks.end(); itLand2++)
			if (itLand1->ID == itLand2->ID)
			{
				// Match found!
				pair.globalIdx = pair.localIdx = (unsigned int)itLand1->ID;

				pair.global.x = itLand1->pose_mean.x;
				pair.global.y = itLand1->pose_mean.y;
				pair.global.z = itLand1->pose_mean.z;

				pair.local.x = itLand2->pose_mean.x;
				pair.local.y = itLand2->pose_mean.y;
				pair.local.z = itLand2->pose_mean.z;

				outList.push_back(pair);
			}  // end if
}  // end-cloudsToMatchedList

/*-------------------------------------------------------------
					computeMainOrientation
-------------------------------------------------------------*/
float vision::computeMainOrientation(
	const CImage& image, unsigned int x, unsigned int y)
{
	MRPT_START
	float orientation = 0;
	if ((int(x) - 1 >= 0) && (int(y) - 1 >= 0) && (x + 1 < image.getWidth()) &&
		(y + 1 < image.getHeight()))
		orientation = (float)atan2(
			(double)*image(x, y + 1) - (double)*image(x, y - 1),
			(double)*image(x + 1, y) - (double)*image(x - 1, y));

	// Convert from [-pi,pi] to [0,2pi]

	return orientation;

	MRPT_END
}  // end vision::computeMainOrientation

/*-------------------------------------------------------------
					normalizeImage
-------------------------------------------------------------*/
void vision::normalizeImage(const CImage& image, CImage& nimage)
{
	ASSERT_(image.getChannelCount() == 1);
	nimage.resize(image.getWidth(), image.getHeight(), image.getChannelCount());

	CMatrixFloat im, nim;
	nim.resize(image.getHeight(), image.getWidth());

	image.getAsMatrix(im);

	double m, s;
	mrpt::math::meanAndStd(im, m, s);

	for (int k1 = 0; k1 < (int)nim.cols(); ++k1)
		for (int k2 = 0; k2 < (int)nim.rows(); ++k2)
			nim(k2, k1) = (im(k2, k1) - m) / s;

	nimage.setFromMatrix(nim);
}

/*-------------------------------------------------------------
						matchFeatures
-------------------------------------------------------------*/
size_t vision::matchFeatures(
	const CFeatureList& list1, const CFeatureList& list2,
	CMatchedFeatureList& matches, const TMatchingOptions& options,
	const TStereoSystemParams& params)
{
	// Clear the output structure
	MRPT_START
	// matches.clear();

	// Preliminary comprobations
	size_t sz1 = list1.size(), sz2 = list2.size();

	ASSERT_((sz1 > 0) && (sz2 > 0));  // Both lists have features within it
	ASSERT_(
		list1.get_type() ==
		list2.get_type());	// Both lists must be of the same type

	CFeatureList::const_iterator itList1, itList2;	// Iterators for the lists

	// For SIFT & SURF
	float distDesc;	 // EDD or EDSD
	float minDist1;	 // Minimum EDD or EDSD
	float minDist2;	 // Second minimum EDD or EDSD

	// For Harris
	double maxCC1;	// Maximum CC
	double maxCC2;	// Second maximum CC

	// For SAD
	double minSAD1, minSAD2;

	vector<int> idxLeftList, idxRightList;
	idxLeftList.resize(sz1, FEAT_FREE);
	idxRightList.resize(sz2, FEAT_FREE);
	vector<double> distCorrs(sz1);
	int lFeat, rFeat;
	int minLeftIdx = 0, minRightIdx;
	int nMatches = 0;

	// For each feature in list1 ...
	for (lFeat = 0, itList1 = list1.begin(); itList1 != list1.end();
		 ++itList1, ++lFeat)
	{
		// For SIFT & SURF
		minDist1 = 1e5;
		minDist2 = 1e5;

		// For Harris
		maxCC1 = 0;
		maxCC2 = 0;

		// For SAD
		minSAD1 = 1e5;
		minSAD2 = 1e5;

		// For all the cases
		minRightIdx = 0;

		for (rFeat = 0, itList2 = list2.begin(); itList2 != list2.end();
			 ++itList2, ++rFeat)  // ... compare with all the features in list2.
		{
			// Filter out by epipolar constraint
			double d = 0.0;	 // Distance to the epipolar line
			if (options.useEpipolarRestriction)
			{
				if (options.parallelOpticalAxis)
					d = itList1->keypoint.pt.y - itList2->keypoint.pt.y;
				else
				{
					ASSERT_(options.hasFundamentalMatrix);

					// Compute epipolar line Ax + By + C = 0
					TLine2D epiLine;
					TPoint2D oPoint(
						itList2->keypoint.pt.x, itList2->keypoint.pt.y);

					CMatrixDouble31 l, p;
					p(0, 0) = itList1->keypoint.pt.x;
					p(1, 0) = itList1->keypoint.pt.y;
					p(2, 0) = 1;

					l = params.F * p;

					epiLine.coefs[0] = l(0, 0);
					epiLine.coefs[1] = l(1, 0);
					epiLine.coefs[2] = l(2, 0);

					d = epiLine.distance(oPoint);
				}  // end else
			}  // end if

			// Use epipolar restriction
			bool c1 = options.useEpipolarRestriction
				? fabs(d) < options.epipolar_TH
				: true;
			// Use x-coord restriction
			bool c2 = options.useXRestriction
				? itList1->keypoint.pt.x - itList2->keypoint.pt.x > 0
				: true;

			if (c1 && c2)
			{
				switch (options.matching_method)
				{
					case TMatchingOptions::mmDescriptorSIFT:
					{
						// Ensure that both features have SIFT descriptors
						ASSERT_(
							itList1->descriptors.hasDescriptorSIFT() &&
							itList2->descriptors.hasDescriptorSIFT());

						// Compute the Euclidean distance between descriptors
						distDesc = itList1->descriptorSIFTDistanceTo(*itList2);

						// Search for the two minimum values
						if (distDesc < minDist1)
						{
							minDist2 = minDist1;
							minDist1 = distDesc;
							minLeftIdx = lFeat;
							minRightIdx = rFeat;
						}
						else if (distDesc < minDist2)
							minDist2 = distDesc;

						break;
					}  // end mmDescriptorSIFT

					case TMatchingOptions::mmCorrelation:
					{
						size_t u, v;  // Coordinates of the peak
						double res;	 // Value of the peak

						// Ensure that both features have patches
						ASSERT_(
							itList1->patchSize > 0 && itList2->patchSize > 0);
						vision::openCV_cross_correlation(
							*itList1->patch, *itList2->patch, u, v, res);

						// Search for the two maximum values
						if (res > maxCC1)
						{
							maxCC2 = maxCC1;
							maxCC1 = res;
							minLeftIdx = lFeat;
							minRightIdx = rFeat;
						}
						else if (res > maxCC2)
							maxCC2 = res;

						break;
					}  // end mmCorrelation

					case TMatchingOptions::mmDescriptorSURF:
					{
						// Ensure that both features have SURF descriptors
						ASSERT_(
							itList1->descriptors.hasDescriptorSURF() &&
							itList2->descriptors.hasDescriptorSURF());

						// Compute the Euclidean distance between descriptors
						distDesc = itList1->descriptorSURFDistanceTo(*itList2);

						// Search for the two minimum values
						if (distDesc < minDist1)
						{
							minDist2 = minDist1;
							minDist1 = distDesc;
							minLeftIdx = lFeat;
							minRightIdx = rFeat;
						}
						else if (distDesc < minDist2)
							minDist2 = distDesc;

						break;	// end case featSURF
					}  // end mmDescriptorSURF

					case TMatchingOptions::mmDescriptorORB:
					{
						// Ensure that both features have SURF descriptors
						ASSERT_(
							itList1->descriptors.hasDescriptorORB() &&
							itList2->descriptors.hasDescriptorORB());
						distDesc = itList1->descriptorORBDistanceTo(*itList2);

						// Search for the two minimum values
						if (distDesc < minDist1)
						{
							minDist2 = minDist1;
							minDist1 = distDesc;
							minLeftIdx = lFeat;
							minRightIdx = rFeat;
						}
						else if (distDesc < minDist2)
							minDist2 = distDesc;

						break;
					}  // end mmDescriptorORB

					case TMatchingOptions::mmSAD:
					{
						// Ensure that both features have patches
						ASSERT_(
							itList1->patchSize > 0 &&
							itList2->patchSize == itList1->patchSize);
#if !MRPT_HAS_OPENCV
						THROW_EXCEPTION(
							"MRPT has been compiled without OpenCV");
#else
						const CImage aux1(
							*itList1->patch, FAST_REF_OR_CONVERT_TO_GRAY);
						const CImage aux2(
							*itList2->patch, FAST_REF_OR_CONVERT_TO_GRAY);
						const auto h = aux1.getHeight(), w = aux1.getWidth();

						double res = 0;
						for (unsigned int ii = 0; ii < h; ++ii)
							for (unsigned int jj = 0; jj < w; ++jj)
								res += std::abs(
									static_cast<double>(
										aux1.at<uint8_t>(jj, ii)) -
									static_cast<double>(
										aux2.at<uint8_t>(jj, ii)));
						res = res / (255.0 * w * h);

						if (res < minSAD1)
						{
							minSAD2 = minSAD1;
							minSAD1 = res;
							minLeftIdx = lFeat;
							minRightIdx = rFeat;
						}
						else if (res < minSAD2)
							minSAD2 = res;
#endif
						break;
					}  // end mmSAD
				}  // end switch
			}  // end if
		}  // end for 'list2' (right features)

		bool cond1 = false, cond2 = false;
		double minVal = 1.0;
		switch (options.matching_method)
		{
			case TMatchingOptions::mmDescriptorSIFT:
				cond1 = minDist1 < options.maxEDD_TH;  // Maximum Euclidean
				// Distance between SIFT
				// descriptors (EDD)
				cond2 = (minDist1 / minDist2) <
					options.EDD_RATIO;	// Ratio between the two lowest EDSD
				minVal = minDist1;
				break;
			case TMatchingOptions::mmCorrelation:
				cond1 = maxCC1 >
					options.minCC_TH;  // Minimum cross correlation value
				cond2 = (maxCC2 / maxCC1) <
					options.rCC_TH;	 // Ratio between the two highest cross
				// correlation values
				minVal = 1 - maxCC1;
				break;
			case TMatchingOptions::mmDescriptorSURF:
				cond1 = minDist1 < options.maxEDSD_TH;	// Maximum Euclidean
				// Distance between SURF
				// descriptors (EDSD)
				cond2 = (minDist1 / minDist2) <
					options.EDSD_RATIO;	 // Ratio between the two lowest EDSD
				minVal = minDist1;
				break;
			case TMatchingOptions::mmSAD:
				cond1 = minSAD1 < options.maxSAD_TH;
				cond2 = (minSAD1 / minSAD2) < options.SAD_RATIO;
				minVal = minSAD1;
				break;
			case TMatchingOptions::mmDescriptorORB:
				cond1 = minDist1 < options.maxORB_dist;
				cond2 = true;
				minVal = minDist1;
				break;
			default: THROW_EXCEPTION("Invalid value of 'matching_method'");
		}

		// PROCESS THE RESULTS
		if (cond1 && cond2)	 // The minimum distance must be below a threshold
		{
			int auxIdx = idxRightList[minRightIdx];
			if (auxIdx != FEAT_FREE)
			{
				if (distCorrs[auxIdx] > minVal)
				{
					// We've found a better match
					distCorrs[minLeftIdx] = minVal;
					idxLeftList[minLeftIdx] = minRightIdx;
					idxRightList[minRightIdx] = minLeftIdx;

					distCorrs[auxIdx] = 1.0;
					idxLeftList[auxIdx] = FEAT_FREE;
				}  // end-if
			}  // end-if
			else
			{
				idxRightList[minRightIdx] = minLeftIdx;
				idxLeftList[minLeftIdx] = minRightIdx;
				distCorrs[minLeftIdx] = minVal;
				nMatches++;
			}
		}  // end if
	}  // end for 'list1' (left features)

	if (!options.addMatches) matches.clear();

	TFeatureID idLeft = 0, idRight = 0;
	if (!matches.empty()) matches.getMaxID(bothLists, idLeft, idRight);

	for (int vCnt = 0; vCnt < (int)idxLeftList.size(); ++vCnt)
	{
		if (idxLeftList[vCnt] != FEAT_FREE)
		{
			std::pair<CFeature, CFeature> thisMatch;

			bool isGood = true;
			double dp1 = -1.0, dp2 = -1.0;
			TPoint3D p3D = TPoint3D();
			if (options.estimateDepth && options.parallelOpticalAxis)
			{
				projectMatchedFeature(
					list1[vCnt], list2[idxLeftList[vCnt]], p3D, params);
				dp1 = sqrt(p3D.x * p3D.x + p3D.y * p3D.y + p3D.z * p3D.z);
				dp2 = sqrt(
					(p3D.x - params.baseline) * (p3D.x - params.baseline) +
					p3D.y * p3D.y + p3D.z * p3D.z);

				if (dp1 > options.maxDepthThreshold ||
					dp2 > options.maxDepthThreshold)
					isGood = false;
			}  // end-if

			if (isGood)
			{
				// Set the features
				thisMatch.first = list1[vCnt];
				thisMatch.second = list2[idxLeftList[vCnt]];

				// Update the max ID value
				if (matches.empty())
				{
					idLeft = thisMatch.first.keypoint.ID;
					idRight = thisMatch.second.keypoint.ID;
				}
				else
				{
					keep_max(idLeft, thisMatch.first.keypoint.ID);
					matches.setLeftMaxID(idLeft);

					keep_max(idRight, thisMatch.second.keypoint.ID);
					matches.setRightMaxID(idRight);
				}

				// Set the depth and the 3D position of the feature
				if (options.estimateDepth && options.parallelOpticalAxis)
				{
					thisMatch.first.initialDepth = dp1;
					thisMatch.first.p3D = p3D;

					thisMatch.second.initialDepth = dp2;
					thisMatch.second.p3D =
						TPoint3D(p3D.x - params.baseline, p3D.y, p3D.z);
				}  // end-if

				// Insert the match into the matched list
				matches.push_back(thisMatch);
			}  // end-if-isGood
		}  // end-if
	}  // end-for-matches
	return matches.size();

	MRPT_END
}

/*-------------------------------------------------------------
			generateMask
-------------------------------------------------------------*/
// Insert zeros around the points in mList according to wSize
void vision::generateMask(
	const CMatchedFeatureList& mList, CMatrixBool& mask1, CMatrixBool& mask2,
	int wSize)
{
	ASSERT_(mList.size() > 0);

	//    cv::Mat *mask1 = static_cast<cv::Mat*>(_mask1);
	//    cv::Mat *mask2 = static_cast<cv::Mat*>(_mask2);

	int hwsize = (int)(0.5 * wSize);
	int mx = mask1.cols(), my = mask1.rows();

	int idx, idy;
	CMatchedFeatureList::const_iterator it;
	for (it = mList.begin(); it != mList.end(); ++it)
	{
		for (int ii = -hwsize; ii < hwsize; ++ii)
			for (int jj = -hwsize; jj < hwsize; ++jj)
			{
				idx = (int)(it->first.keypoint.pt.x) + ii;
				idy = (int)(it->first.keypoint.pt.y) + jj;
				if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
					mask1(idy, idx) = false;
			}

		for (int ii = -hwsize; ii < hwsize; ++ii)
			for (int jj = -hwsize; jj < hwsize; ++jj)
			{
				idx = (int)(it->second.keypoint.pt.x) + ii;
				idy = (int)(it->second.keypoint.pt.y) + jj;
				if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
					mask2(idy, idx) = false;
			}
	}  // end-for
}  // end generateMask

double vision::computeSAD(const CImage& p1, const CImage& p2)
{
	MRPT_START
#if MRPT_HAS_OPENCV
	ASSERT_(p1.getSize() == p2.getSize());
	const auto w = p1.getWidth(), h = p1.getHeight();
	double res = 0.0;
	for (unsigned int ii = 0; ii < h; ++ii)
		for (unsigned int jj = 0; jj < w; ++jj)
			res += std::abs(
				static_cast<double>(p1.at<uint8_t>(jj, ii)) -
				static_cast<double>(p2.at<uint8_t>(jj, ii)));

	return res / (255.0 * w * h);
#else
	THROW_EXCEPTION(
		"MRPT compiled without OpenCV, can't compute SAD of images!");
#endif
	MRPT_END
}

void vision::addFeaturesToImage(
	const CImage& inImg, const CFeatureList& theList, CImage& outImg)
{
	outImg = inImg;	 // Create a copy of the input image
	for (const auto& it : theList)
		outImg.rectangle(
			it.keypoint.pt.x - 5, it.keypoint.pt.y - 5, it.keypoint.pt.x + 5,
			it.keypoint.pt.y + 5, TColor(255, 0, 0));
}

/*-------------------------------------------------------------
					projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
	const CMatchedFeatureList& matches,
	const mrpt::img::TStereoCamera& stereo_camera, vector<TPoint3D>& out_points)
{
	out_points.clear();
	out_points.reserve(matches.size());
	for (const auto& match : matches)
	{
		const auto& pt1 = match.first.keypoint.pt;
		const auto& pt2 = match.second.keypoint.pt;

		const double disp = pt1.x - pt2.x;
		if (disp < 1) continue;

		const double b_d = stereo_camera.rightCameraPose.x / disp;
		out_points.emplace_back(
			(pt1.x - stereo_camera.leftCamera.cx()) * b_d,
			(pt1.y - stereo_camera.leftCamera.cy()) * b_d,
			stereo_camera.leftCamera.fx() * b_d);
	}
}
/*-------------------------------------------------------------
					projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
	const CFeatureList& leftList, const CFeatureList& rightList,
	vector<TPoint3D>& vP3D, const TStereoSystemParams& params)
{
	vP3D.reserve(leftList.size());
	CFeatureList::const_iterator it1, it2;
	for (it1 = leftList.begin(), it2 = rightList.begin(); it1 != leftList.end();
		 ++it1, ++it2)
	{
		TPoint3D p3D;
		projectMatchedFeature(*it1, *it2, p3D, params);
		if (p3D.z < params.maxZ && p3D.z > params.minZ && p3D.y < params.maxY)
			vP3D.push_back(p3D);
	}
}  // end projectMatchedFeatures

/*-------------------------------------------------------------
					projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeature(
	const CFeature& leftFeat, const CFeature& rightFeat, TPoint3D& p3D,
	const TStereoSystemParams& params)
{
	const double f0 = 600;
	double nfx1 = leftFeat.keypoint.pt.x, nfy1 = leftFeat.keypoint.pt.y,
		   nfx2 = rightFeat.keypoint.pt.x, nfy2 = rightFeat.keypoint.pt.y;

	const double x = nfx1 * f0;	 // x  = (x  / f0) * f0   x  = x
	const double y = nfy1 * f0;	 // y  = (y  / f0) * f0   y  = y
	const double xd = nfx2 * f0;  // x' = (x' / f0) * f0   x' = x'
	const double yd = nfy2 * f0;  // y' = (y' / f0) * f0   y' = y'

	const double f2 = f0 * f0;
	const double p9 = f2 * params.F(2, 2);
	const double Q00 = f2 *
		(params.F(0, 2) * params.F(0, 2) + params.F(1, 2) * params.F(1, 2) +
		 params.F(2, 0) * params.F(2, 0) + params.F(2, 1) * params.F(2, 1));

	double Jh = (std::numeric_limits<double>::max)();  // J hat = 
	double xh = x;	// x hat = x
	double yh = y;	// y hat = y
	double xhd = xd;  // x hat dash = x'
	double yhd = yd;  // y hat dash = y'
	double
		xt = 0,
		yt = 0, xtd = 0,
		ytd =
			0;	// x tilde = 0, y tilde = 0, x tilde dash = 0, y tilde dash = 0
	for (;;)
	{
		const double p1 = (xh * xhd + xhd * xt + xh * xtd) * params.F(0, 0);
		const double p2 = (xh * yhd + yhd * xt + xh * ytd) * params.F(0, 1);
		const double p3 = (f0 * (xh + xt)) * params.F(0, 2);
		const double p4 = (yh * xhd + xhd * yt + yh * xtd) * params.F(1, 0);
		const double p5 = (yh * yhd + yhd * yt + yh * ytd) * params.F(1, 1);
		const double p6 = (f0 * (yh + yt)) * params.F(1, 2);
		const double p7 = (f0 * (xhd + xtd)) * params.F(2, 0);
		const double p8 = (f0 * (yhd + ytd)) * params.F(2, 1);

		const double udotxi = p1 + p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;

		const double Q11 =
			(xh * xh + xhd * xhd) * params.F(0, 0) * params.F(0, 0);
		const double Q22 =
			(xh * xh + yhd * yhd) * params.F(0, 1) * params.F(0, 1);
		const double Q44 =
			(yh * yh + xhd * xhd) * params.F(1, 0) * params.F(1, 0);
		const double Q55 =
			(yh * yh + yhd * yhd) * params.F(1, 1) * params.F(1, 1);
		const double Q12 = xhd * yhd * params.F(0, 0) * params.F(0, 1);
		const double Q13 = f0 * xhd * params.F(0, 0) * params.F(0, 2);
		const double Q14 = xh * yh * params.F(0, 0) * params.F(1, 0);
		const double Q17 = f0 * xh * params.F(0, 0) * params.F(2, 0);
		const double Q23 = f0 * yhd * params.F(0, 1) * params.F(0, 2);
		const double Q25 = xh * yh * params.F(0, 1) * params.F(1, 1);
		const double Q28 = f0 * xh * params.F(0, 1) * params.F(2, 1);
		const double Q45 = xhd * yhd * params.F(1, 0) * params.F(1, 1);
		const double Q46 = f0 * xhd * params.F(1, 0) * params.F(1, 2);
		const double Q47 = f0 * yh * params.F(1, 0) * params.F(2, 0);
		const double Q56 = f0 * yhd * params.F(1, 1) * params.F(1, 2);
		const double Q58 = f0 * yh * params.F(1, 1) * params.F(2, 1);

		const double udotV0xiu = Q00 + Q11 + Q22 + Q44 + Q55 +
			2.0 *
				(Q12 + Q13 + Q14 + Q17 + Q23 + Q25 + Q28 + Q45 + Q46 + Q47 +
				 Q56 + Q58);

		ASSERT_(fabs(udotV0xiu) > 1e-5);

		const double C = udotxi / udotV0xiu;

		xt = C *
			(params.F(0, 0) * xhd + params.F(0, 1) * yhd + f0 * params.F(0, 2));
		yt = C *
			(params.F(1, 0) * xhd + params.F(1, 1) * yhd + f0 * params.F(1, 2));
		xtd = C *
			(params.F(0, 0) * xh + params.F(1, 0) * yh + f0 * params.F(2, 0));
		ytd = C *
			(params.F(0, 1) * xh + params.F(1, 1) * yh + f0 * params.F(2, 1));

		const double Jt = xt * xt + yt * yt + xtd * xtd + ytd * ytd;
		//        cout << "Jt:" << Jt << " and Jh: " << Jh << endl;
		if ((std::abs)(Jt - Jh) <= 1e-5)
		{
			nfx1 = xh / f0;
			nfy1 = yh / f0;
			nfx2 = xhd / f0;
			// nfy2 = yhd / f0;

			break;
		}
		else
		{
			Jh = Jt;  // J hat      = J tilde
			xh = x - xt;  // x hat      = x  - x tilde
			yh = y - yt;  // y hat      = y  - y tilde
			xhd = xd - xtd;	 // x hat dash = x' - x tilde dash
			yhd = yd - ytd;	 // y hat dash = y' - y tilde dash
		}
	}  // end for

	double disp = nfx1 - nfx2;
	double aux = params.baseline / disp;
	p3D.x = (nfx1 - params.K(0, 2)) * aux;
	p3D.y = (nfy1 - params.K(1, 2)) * aux;
	p3D.z = params.K(0, 0) * aux;
}  // end projectMatchedFeature

/*-------------------------------------------------------------
					projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
	CMatchedFeatureList& mfList, const TStereoSystemParams& param,
	CLandmarksMap& landmarks)
{
	MRPT_START

	landmarks.clear();	// Assert that the output CLandmarksMap is clear

	float stdPixel2 = square(param.stdPixel);
	float stdDisp2 = square(param.stdDisp);

	// Main loop
	for (auto itList = mfList.begin(); itList != mfList.end();)
	{
		const auto& pt1 = itList->first.keypoint.pt;
		const auto& pt2 = itList->second.keypoint.pt;

		const float disp = pt1.x - pt2.x;  // Disparity
		if (disp < 1e-9)
		{
			// Filter out too far points
			// Erase the match
			itList = mfList.erase(itList);
			continue;
		}

		float x3D = (pt1.x - param.K(0, 2)) * ((param.baseline)) / disp;
		float y3D = (pt1.y - param.K(1, 2)) * ((param.baseline)) / disp;
		float z3D = (param.K(0, 0)) * ((param.baseline)) / disp;

		// Filter out bad points
		if ((z3D < param.minZ) || (z3D > param.maxZ))
		{ itList = mfList.erase(itList); }
		else
		{
			TPoint3D p3D(x3D, y3D, z3D);

			// STORE THE OBTAINED LANDMARK
			CLandmark lm;

			TPoint3D norm3D = p3D;
			norm3D *= -1 / norm3D.norm();

			lm.normal = norm3D;
			lm.pose_mean = p3D;
			lm.ID = itList->first.keypoint.ID;

			// If the matched landmarks has a (SIFT or SURF) descriptor,
			// asign the left one to the landmark.
			// TO DO: Assign the mean value of the descriptor (between the
			// matches)
			lm.features.resize(1);
			lm.features[0] = (*itList).first;

			// Compute the covariance matrix for the landmark
			switch (param.uncPropagation)
			{
				case TStereoSystemParams::Prop_Linear:
				{
					float foc2 = square(param.K(0, 0));
					float c0 = param.K(0, 2);
					float r0 = param.K(1, 2);
					float base2 = square(param.baseline);
					float disp2 = square(disp);

					lm.pose_cov_11 = stdPixel2 * base2 / disp2 +
						stdDisp2 * base2 * square(pt1.x - c0) / square(disp2);
					lm.pose_cov_12 = stdDisp2 * base2 * (pt1.x - c0) *
						(pt1.y - r0) / square(disp2);
					lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) *
						(pt1.x - c0) / square(disp2);
					lm.pose_cov_22 = stdPixel2 * base2 / disp2 +
						stdDisp2 * base2 * square(pt1.y - r0) / square(disp2);
					lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) *
						(pt1.y - r0) / square(disp2);
					lm.pose_cov_33 = stdDisp2 * foc2 * base2 / square(disp2);
				}  // end case 'Prop_Linear'
				break;

				case TStereoSystemParams::Prop_UT:
				{
					// Parameters
					unsigned int Na = 3;
					unsigned int i;

					float k = param.factor_k;

					float w0 = k / (Na + k);
					float w1 = 1 / (2 * (Na + k));

					CMatrixF Pa(3, 3);
					CMatrixF L(3, 3);

					Pa.fill(0);
					Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
					Pa(2, 2) = (Na + k) * square(param.stdDisp);

					// Cholesky decomposition
					Pa.chol(L);	 // math::chol(Pa,L);

					vector<TPoint3D> B;	 // B group
					TPoint3D meanB;	 // Mean value of the B group
					CMatrixF Pb;  // Covariance of the B group

					B.resize(2 * Na + 1);  // Set of output values
					Pb.fill(0);	 // Reset the output covariance

					CVectorFloat vAux, myPoint;	 // Auxiliar vectors
					CVectorFloat meanA;	 // Mean value of the A group

					vAux.resize(3);	 // Set the variables size
					meanA.resize(3);
					myPoint.resize(3);

					// Mean input value: (c,r,d)
					meanA[0] = pt1.x;
					meanA[1] = pt1.y;
					meanA[2] = disp;

					// Output mean
					meanB.x = w0 * x3D;
					meanB.y = w0 * y3D;
					meanB.z = w0 * z3D;	 // Add to the mean
					B[0].x = x3D;
					B[0].y = y3D;
					B[0].z = z3D;  // Insert into B

					for (i = 1; i <= 2 * Na; i++)
					{
						// Form the Ai value
						if (i <= Na)
						{
							// Extract the proper row
							vAux.asEigen() = L.asEigen().row(i - 1);
							myPoint[0] = meanA[0] + vAux[0];
							myPoint[1] = meanA[1] + vAux[1];
							myPoint[2] = meanA[2] + vAux[2];
						}
						else
						{
							vAux.asEigen() = L.asEigen().row((i - Na) - 1);
							myPoint[0] = meanA[0] - vAux[0];
							myPoint[1] = meanA[1] - vAux[1];
							myPoint[2] = meanA[2] - vAux[2];
						}

						// Pass the Ai through the functions:
						x3D = (myPoint[0] - param.K(0, 2)) *
							((param.baseline)) / myPoint[2];
						y3D = (myPoint[1] - param.K(1, 2)) *
							((param.baseline)) / myPoint[2];
						z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

						// Add to the B mean computation and the B vector
						meanB.x = meanB.x + w1 * x3D;
						meanB.y = meanB.y + w1 * y3D;
						meanB.z = meanB.z + w1 * z3D;

						B[i].x = x3D;
						B[i].y = y3D;
						B[i].z = z3D;

					}  // end for 'i'

					// Output covariance
					for (i = 0; i <= 2 * Na; i++)
					{
						float weight = w1;
						CMatrixF v(3, 1);

						if (i == 0)	 // The weight for the mean value of A is w0
							weight = w0;

						v(0, 0) = B[i].x - meanB.x;
						v(1, 0) = B[i].y - meanB.y;
						v(2, 0) = B[i].z - meanB.z;

						Pb.asEigen() +=
							(weight * (v.asEigen() * v.transpose())).eval();
					}  // end for 'i'

					// Store it in the landmark
					lm.pose_cov_11 = Pb(0, 0);
					lm.pose_cov_12 = Pb(0, 1);
					lm.pose_cov_13 = Pb(0, 2);
					lm.pose_cov_22 = Pb(1, 1);
					lm.pose_cov_23 = Pb(1, 2);
					lm.pose_cov_33 = Pb(2, 2);
				}  // end case 'Prop_UT'
				break;

				case TStereoSystemParams::Prop_SUT:
				{
					// Parameters
					unsigned int Na = 3;
					unsigned int i;

					float a = param.factor_a;
					float b = param.factor_b;
					float k = param.factor_k;

					float lambda = square(a) * (Na + k) - Na;

					float w0_m = lambda / (Na + lambda);
					float w0_c = w0_m + (1 - square(a) + b);
					float w1 = 1 / (2 * (Na + lambda));

					CMatrixF Pa(3, 3);
					CMatrixF L(3, 3);

					Pa.fill(0);
					Pa(0, 0) = Pa(1, 1) =
						(Na + lambda) * square(param.stdPixel);
					Pa(2, 2) = (Na + lambda) * square(param.stdDisp);

					// Cholesky decomposition
					Pa.chol(L);	 // math::chol(Pa,L);

					vector<TPoint3D> B;	 // B group
					TPoint3D meanB;	 // Mean value of the B group
					CMatrixF Pb;  // Covariance of the B group

					B.resize(2 * Na + 1);  // Set of output values
					Pb.fill(0);	 // Reset the output covariance

					CVectorFloat vAux, myPoint;	 // Auxiliar vectors
					CVectorFloat meanA;	 // Mean value of the A group

					vAux.resize(3);	 // Set the variables size
					meanA.resize(3);
					myPoint.resize(3);

					// Mean input value: (c,r,d)
					meanA[0] = pt1.x;
					meanA[1] = pt1.y;
					meanA[2] = disp;

					// Output mean
					meanB.x = w0_m * x3D;
					meanB.y = w0_m * y3D;
					meanB.z = w0_m * z3D;  // Add to the mean
					B[0].x = x3D;
					B[0].y = y3D;
					B[0].z = z3D;  // Insert into B

					for (i = 1; i <= 2 * Na; i++)
					{
						// Form the Ai value
						if (i <= Na)
						{
							// Extract the proper row
							vAux.asEigen() = L.row(i - 1);
							myPoint = meanA + vAux;
							// myPoint[0] = meanA[0] + vAux[0];
							// myPoint[1] = meanA[1] + vAux[1];
							// myPoint[2] = meanA[2] + vAux[2];
						}
						else
						{
							vAux = L.row((i - Na) - 1);
							myPoint = meanA - vAux;
							// myPoint[0] = meanA[0] - vAux[0];
							// myPoint[1] = meanA[1] - vAux[1];
							// myPoint[2] = meanA[2] - vAux[2];
						}

						// Pass the Ai through the functions:
						x3D = (myPoint[0] - param.K(0, 2)) *
							((param.baseline)) / myPoint[2];
						y3D = (myPoint[1] - param.K(1, 2)) *
							((param.baseline)) / myPoint[2];
						z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

						// Add to the B mean computation and the B vector
						meanB.x = meanB.x + w1 * x3D;
						meanB.y = meanB.y + w1 * y3D;
						meanB.z = meanB.z + w1 * z3D;

						B[i].x = x3D;
						B[i].y = y3D;
						B[i].z = z3D;

					}  // end for 'i'

					// Output covariance
					for (i = 0; i <= 2 * Na; i++)
					{
						float weight = w1;
						CMatrixF v(3, 1);

						if (i == 0)	 // The weight for the mean value of A is w0
							weight = w0_c;

						v(0, 0) = B[i].x - meanB.x;
						v(1, 0) = B[i].y - meanB.y;
						v(2, 0) = B[i].z - meanB.z;

						Pb.asEigen() +=
							(weight * (v.asEigen() * v.transpose())).eval();
					}  // end for 'i'

					// Store it in the landmark
					lm.pose_cov_11 = Pb(0, 0);
					lm.pose_cov_12 = Pb(0, 1);
					lm.pose_cov_13 = Pb(0, 2);
					lm.pose_cov_22 = Pb(1, 1);
					lm.pose_cov_23 = Pb(1, 2);
					lm.pose_cov_33 = Pb(2, 2);
				}  // end case 'Prop_SUT'
				break;

			}  // end switch
			landmarks.landmarks.push_back(lm);
			itList++;
		}  // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
	}  // end for 'i'

	MRPT_END
}  // end of projectMatchedFeatures

/*-------------------------------------------------------------
					projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
	CFeatureList& leftList, CFeatureList& rightList,
	const vision::TStereoSystemParams& param,
	mrpt::maps::CLandmarksMap& landmarks)
{
	MRPT_START
	ASSERT_(leftList.size() == rightList.size());

	landmarks.clear();	// Assert that the output CLandmarksMap is clear

	CFeatureList::iterator itListL, itListR;
	float stdPixel2 = square(param.stdPixel);
	float stdDisp2 = square(param.stdDisp);

	// Main loop
	for (itListL = leftList.begin(), itListR = rightList.begin();
		 itListL != leftList.end();)
	{
		const auto& ptL = itListL->keypoint.pt;
		const auto& ptR = itListR->keypoint.pt;

		float disp = ptL.x - ptR.x;	 // Disparity
		if (disp < 1e-9)  // Filter out too far points
		{
			itListL = leftList.erase(itListL);
			itListR = rightList.erase(itListR);
			continue;
		}

		// Too much distant features are not taken into account
		float x3D = (ptL.x - param.K(0, 2)) * param.baseline / disp;
		float y3D = (ptL.y - param.K(1, 2)) * param.baseline / disp;
		float z3D = (param.K(0, 0)) * param.baseline / disp;

		// Filter out bad points
		if ((z3D < param.minZ) || (z3D > param.maxZ))
		{
			itListL = leftList.erase(itListL);
			itListR = rightList.erase(itListR);
		}
		else
		{
			TPoint3D p3D(x3D, y3D, z3D);

			// STORE THE OBTAINED LANDMARK
			CLandmark lm;

			TPoint3D norm3D = p3D;
			norm3D *= -1. / norm3D.norm();

			lm.normal = norm3D;
			lm.pose_mean = p3D;
			lm.ID = itListL->keypoint.ID;

			// If the matched landmarks has a (SIFT or SURF) descriptor,
			// asign the left one to the landmark.
			// TO DO: Assign the mean value of the descriptor (between the
			// matches)
			lm.features.resize(2);
			lm.features[0] = *itListL;
			lm.features[1] = *itListR;

			// Compute the covariance matrix for the landmark
			switch (param.uncPropagation)
			{
				case TStereoSystemParams::Prop_Linear:
				{
					float foc2 = square(param.K(0, 0));
					float c0 = param.K(0, 2);
					float r0 = param.K(1, 2);
					float base2 = square(param.baseline);
					float disp2 = square(ptL.x - ptR.x);

					lm.pose_cov_11 = stdPixel2 * base2 / disp2 +
						stdDisp2 * base2 * square(ptL.x - c0) / square(disp2);
					lm.pose_cov_12 = stdDisp2 * base2 * (ptL.x - c0) *
						(ptL.y - r0) / square(disp2);
					lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) *
						(ptL.x - c0) / square(disp2);
					lm.pose_cov_22 = stdPixel2 * base2 / disp2 +
						stdDisp2 * base2 * square(ptL.y - r0) / square(disp2);
					lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) *
						(ptL.y - r0) / square(disp2);
					lm.pose_cov_33 = stdDisp2 * foc2 * base2 / square(disp2);
				}  // end case 'Prop_Linear'
				break;

				case TStereoSystemParams::Prop_UT:
				{
					// Parameters
					unsigned int Na = 3;
					unsigned int i;

					float k = param.factor_k;

					float w0 = k / (Na + k);
					float w1 = 1 / (2 * (Na + k));

					CMatrixF Pa(3, 3);
					CMatrixF L(3, 3);

					Pa.fill(0);
					Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
					Pa(2, 2) = (Na + k) * square(param.stdDisp);

					// Cholesky decomposition
					Pa.chol(L);	 // math::chol(Pa,L);

					vector<TPoint3D> B;	 // B group
					TPoint3D meanB;	 // Mean value of the B group
					CMatrixF Pb;  // Covariance of the B group

					B.resize(2 * Na + 1);  // Set of output values
					Pb.fill(0);	 // Reset the output covariance

					CVectorFloat vAux, myPoint;	 // Auxiliar vectors
					CVectorFloat meanA;	 // Mean value of the A group

					vAux.resize(3);	 // Set the variables size
					meanA.resize(3);
					myPoint.resize(3);

					// Mean input value: (c,r,d)
					meanA[0] = ptL.x;
					meanA[1] = ptL.y;
					meanA[2] = disp;

					// Output mean
					meanB.x = w0 * x3D;
					meanB.y = w0 * y3D;
					meanB.z = w0 * z3D;	 // Add to the mean
					B[0].x = x3D;
					B[0].y = y3D;
					B[0].z = z3D;  // Insert into B

					for (i = 1; i <= 2 * Na; i++)
					{
						// Form the Ai value
						if (i <= Na)
						{
							vAux.asEigen() = L.col(i - 1);
							myPoint[0] = meanA[0] + vAux[0];
							myPoint[1] = meanA[1] + vAux[1];
							myPoint[2] = meanA[2] + vAux[2];
						}
						else
						{
							vAux = L.col((i - Na) - 1);
							myPoint[0] = meanA[0] - vAux[0];
							myPoint[1] = meanA[1] - vAux[1];
							myPoint[2] = meanA[2] - vAux[2];
						}

						// Pass the Ai through the functions:
						x3D = (myPoint[0] - param.K(0, 2)) *
							((param.baseline)) / myPoint[2];
						y3D = (myPoint[1] - param.K(1, 2)) *
							((param.baseline)) / myPoint[2];
						z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

						// Add to the B mean computation and the B vector
						meanB.x = meanB.x + w1 * x3D;
						meanB.y = meanB.y + w1 * y3D;
						meanB.z = meanB.z + w1 * z3D;

						B[i].x = x3D;
						B[i].y = y3D;
						B[i].z = z3D;

					}  // end for 'i'

					// Output covariance
					for (i = 0; i <= 2 * Na; i++)
					{
						float weight = w1;
						CMatrixF v(3, 1);

						if (i == 0)	 // The weight for the mean value of A is w0
							weight = w0;

						v(0, 0) = B[i].x - meanB.x;
						v(1, 0) = B[i].y - meanB.y;
						v(2, 0) = B[i].z - meanB.z;

						Pb.asEigen() +=
							(weight * (v.asEigen() * v.transpose())).eval();
					}  // end for 'i'

					// Store it in the landmark
					lm.pose_cov_11 = Pb(0, 0);
					lm.pose_cov_12 = Pb(0, 1);
					lm.pose_cov_13 = Pb(0, 2);
					lm.pose_cov_22 = Pb(1, 1);
					lm.pose_cov_23 = Pb(1, 2);
					lm.pose_cov_33 = Pb(2, 2);
				}  // end case 'Prop_UT'
				break;

				case TStereoSystemParams::Prop_SUT:
				{
					// Parameters
					unsigned int Na = 3;
					unsigned int i;

					float a = param.factor_a;
					float b = param.factor_b;
					float k = param.factor_k;

					float lambda = square(a) * (Na + k) - Na;

					float w0_m = lambda / (Na + lambda);
					float w0_c = w0_m + (1 - square(a) + b);
					float w1 = 1 / (2 * (Na + lambda));

					CMatrixF Pa(3, 3);
					CMatrixF L(3, 3);

					Pa.fill(0);
					Pa(0, 0) = Pa(1, 1) =
						(Na + lambda) * square(param.stdPixel);
					Pa(2, 2) = (Na + lambda) * square(param.stdDisp);

					// Cholesky decomposition
					Pa.chol(L);	 // math::chol(Pa,L);

					vector<TPoint3D> B;	 // B group
					TPoint3D meanB;	 // Mean value of the B group
					CMatrixF Pb;  // Covariance of the B group

					B.resize(2 * Na + 1);  // Set of output values
					Pb.fill(0);	 // Reset the output covariance

					CVectorFloat vAux, myPoint;	 // Auxiliar vectors
					CVectorFloat meanA;	 // Mean value of the A group

					vAux.resize(3);	 // Set the variables size
					meanA.resize(3);
					myPoint.resize(3);

					// Mean input value: (c,r,d)
					meanA[0] = ptL.x;
					meanA[1] = ptL.y;
					meanA[2] = disp;

					// Output mean
					meanB.x = w0_m * x3D;
					meanB.y = w0_m * y3D;
					meanB.z = w0_m * z3D;  // Add to the mean
					B[0].x = x3D;
					B[0].y = y3D;
					B[0].z = z3D;  // Insert into B

					for (i = 1; i <= 2 * Na; i++)
					{
						// Form the Ai value
						if (i <= Na)
						{
							vAux = L.row(i - 1);
							myPoint = meanA + vAux;
						}
						else
						{
							vAux = L.col((i - Na) - 1);
							myPoint = meanA - vAux;
						}

						// Pass the Ai through the functions:
						x3D = (myPoint[0] - param.K(0, 2)) *
							((param.baseline)) / myPoint[2];
						y3D = (myPoint[1] - param.K(1, 2)) *
							((param.baseline)) / myPoint[2];
						z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

						// Add to the B mean computation and the B vector
						meanB.x = meanB.x + w1 * x3D;
						meanB.y = meanB.y + w1 * y3D;
						meanB.z = meanB.z + w1 * z3D;

						B[i].x = x3D;
						B[i].y = y3D;
						B[i].z = z3D;

					}  // end for 'i'

					// Output covariance
					for (i = 0; i <= 2 * Na; i++)
					{
						float weight = w1;
						CMatrixF v(3, 1);

						if (i == 0)	 // The weight for the mean value of A is w0
							weight = w0_c;

						v(0, 0) = B[i].x - meanB.x;
						v(1, 0) = B[i].y - meanB.y;
						v(2, 0) = B[i].z - meanB.z;

						Pb.asEigen() +=
							(weight * (v.asEigen() * v.transpose())).eval();
					}  // end for 'i'

					// Store it in the landmark
					lm.pose_cov_11 = Pb(0, 0);
					lm.pose_cov_12 = Pb(0, 1);
					lm.pose_cov_13 = Pb(0, 2);
					lm.pose_cov_22 = Pb(1, 1);
					lm.pose_cov_23 = Pb(1, 2);
					lm.pose_cov_33 = Pb(2, 2);
				}  // end case 'Prop_SUT'
				break;

			}  // end switch
			landmarks.landmarks.push_back(lm);
			itListL++;
			itListR++;
		}  // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
	}  // end for 'i'

	MRPT_END
}

/* -------------------------------------------------------
				StereoObs2RBObs #1
   ------------------------------------------------------- */
void vision::StereoObs2BRObs(
	const CMatchedFeatureList& inMatches,
	const CMatrixDouble33& intrinsicParams, double baseline,
	const CPose3D& sensorPose, const vector<double>& sg,
	CObservationBearingRange& outObs)
{
	// Compute the range and bearing
	double f = intrinsicParams(0, 0);  // Focal length in pixels
	double x0 = intrinsicParams(0, 2);	// Principal point column
	double y0 = intrinsicParams(1, 2);	// Principal point row
	double b = baseline;  // Stereo camera baseline
	double sg_c2 = square(sg[0]);  // Sigma of the column variable
	double sg_r2 = square(sg[1]);  // Sigma of the row variable
	double sg_d2 = square(sg[2]);  // Sigma of the disparity

	for (const auto& inMatche : inMatches)
	{
		double x = inMatche.first.keypoint.pt.x;  // Column of the feature
		double y = inMatche.first.keypoint.pt.y;  // Row of the feature

		double d = x - inMatche.second.keypoint.pt.x;  // Disparity
		double d2 = square(d);
		double k = square(b / d);

		// Projection equations according to a standard camera coordinate axis
		// (+Z forward & +Y downwards)
		// -------------------------------------------------------------------------------------------------------
		double X = (x - x0) * b / d;
		double Y = (y - y0) * b / d;
		double Z = f * b / d;

		// Projection equations according to a standard coordinate axis (+X
		// forward & +Z upwards)
		// -------------------------------------------------------------------------------------------------------
		// double X	= f * b / d;
		// double Y	= ( x0 - x ) * b / d;
		// double Z	= ( y0 - y ) * b / d;

		CObservationBearingRange::TMeasurement m;
		m.range = sqrt(square(X) + square(Y) + square(Z));
		m.yaw = atan2(Y, X);
		m.pitch = -asin(Z / m.range);
		m.landmarkID = inMatche.first.keypoint.ID;

		// Compute the covariance
		// Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
		//						\---------------------------------------/
		//											aux

		CMatrixDouble33 aux;

		// Jacobian equations according to a standard CAMERA coordinate axis (+Z
		// forward & +Y downwards)
		// -------------------------------------------------------------------------------------------------------
		aux(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
		aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
		aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);

		aux(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
		aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);

		aux(2, 2) = k * (sg_d2 * square(f) / d2);

		// Jacobian equations according to a standard coordinate axis (+X
		// forward & +Z upwards)
		// -------------------------------------------------------------------------------------------------------
		CMatrixDouble33 JG;
		JG(0, 0) = X / m.range;
		JG(0, 1) = Y / m.range;
		JG(0, 2) = Z / m.range;

		JG(1, 0) = -Y / (square(X) + square(Y));
		JG(1, 1) = X / (square(X) + square(Y));
		JG(1, 2) = 0;

		JG(2, 0) = Z * X / (square(m.range) * sqrt(square(X) + square(Y)));
		JG(2, 1) = Z * Y / (square(m.range) * sqrt(square(X) + square(Y)));
		JG(2, 2) = -sqrt(square(X) + square(Y)) / square(m.range);

		m.covariance = mrpt::math::multiply_HCHt(JG, aux);

		outObs.sensedData.push_back(m);

	}  // end for

	// Indicate that the covariances have been calculated (for compatibility
	// with earlier versions)
	outObs.validCovariances = true;
	outObs.setSensorPose(sensorPose);
}  // end-StereoObs2BRObs

/* -------------------------------------------------------
				StereoObs2RBObs #2
   ------------------------------------------------------- */
void vision::StereoObs2BRObs(
	const CObservationStereoImages& inObs, const vector<double>& sg,
	CObservationBearingRange& outObs)
{
	// Local variables
	CFeatureExtraction fExt;
	CFeatureList leftList, rightList;
	CMatchedFeatureList matchList;
	unsigned int id = 0;

	// Extract features
	fExt.detectFeatures(inObs.imageLeft, leftList);
	fExt.detectFeatures(inObs.imageRight, rightList);

	// DEBUG:
	// CDisplayWindow		win1, win2;
	// win1.showImageAndPoints( inObs.imageLeft, leftList );
	// win2.showImageAndPoints( inObs.imageRight, rightList );

	// Match features
	// size_t nMatches =
	vision::matchFeatures(leftList, rightList, matchList);

	// Compute the range and bearing
	double f = inObs.leftCamera.fx();  // Focal length in pixels
	double x0 = inObs.leftCamera.cx();	// Principal point column
	double y0 = inObs.leftCamera.cy();	// Principal point row
	double b = inObs.rightCameraPose.x();  // Stereo camera baseline
	double sg_c2 = square(sg[0]);  // Sigma of the column variable
	double sg_r2 = square(sg[1]);  // Sigma of the row variable
	double sg_d2 = square(sg[2]);  // Sigma of the disparity

	for (auto itMatchList = matchList.begin(); itMatchList != matchList.end();
		 itMatchList++, id++)
	{
		double x = itMatchList->first.keypoint.pt.x;  // Column of the feature
		double y = itMatchList->first.keypoint.pt.y;  // Row of the feature

		double d = x - itMatchList->second.keypoint.pt.x;  // Disparity
		double d2 = square(d);
		double k = square(b / d);

		// Projection equations according to a standard camera coordinate axis
		// (+Z forward & +Y downwards)
		// -------------------------------------------------------------------------------------------------------
		double X = (x - x0) * b / d;
		double Y = (y - y0) * b / d;
		double Z = f * b / d;

		// Projection equations according to a standard coordinate axis (+X
		// forward & +Z upwards)
		// -------------------------------------------------------------------------------------------------------
		// double X	= f * b / d;
		// double Y	= ( x0 - x ) * b / d;
		// double Z	= ( y0 - y ) * b / d;

		CObservationBearingRange::TMeasurement m;
		m.range = sqrt(square(X) + square(Y) + square(Z));
		// m.yaw	= atan2( Y,X );
		// m.pitch = -asin( Z/m.range );
		m.yaw = atan2(X, Z);
		m.pitch = atan2(Y, Z);

		// Compute the covariance
		// Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
		//						\---------------------------------------/
		//											aux

		CMatrixDouble33 aux;

		// Jacobian equations according to a standard CAMERA coordinate axis (+Z
		// forward & +Y downwards)
		// -------------------------------------------------------------------------------------------------------
		aux(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
		aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
		aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);

		aux(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
		aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);

		aux(2, 2) = k * (sg_d2 * square(f) / d2);

		// Jacobian equations according to a standard coordinate axis (+X
		// forward & +Z upwards)
		// -------------------------------------------------------------------------------------------------------
		CMatrixDouble33 JG;
		JG(0, 0) = X / m.range;
		JG(0, 1) = Y / m.range;
		JG(0, 2) = Z / m.range;

		JG(1, 0) = -Y / (square(X) + square(Y));
		JG(1, 1) = X / (square(X) + square(Y));
		JG(1, 2) = 0;

		JG(2, 0) = Z * X / (square(m.range) * sqrt(square(X) + square(Y)));
		JG(2, 1) = Z * Y / (square(m.range) * sqrt(square(X) + square(Y)));
		JG(2, 2) = -sqrt(square(X) + square(Y)) / square(m.range);

		// JF.multiply_HCHt( diag, aux );
		m.covariance = mrpt::math::multiply_HCHt(JG, aux);

		m.landmarkID = id;
		outObs.sensedData.push_back(m);
		outObs.fieldOfView_yaw = 2 * fabs(atan2(-x0, f));
		outObs.fieldOfView_pitch = 2 * fabs(atan2(-y0, f));

	}  // end for

	// Indicate that the covariances have been calculated (for compatibility
	// with earlier versions)
	outObs.validCovariances = true;
	outObs.setSensorPose(mrpt::poses::CPose3D(inObs.cameraPose));

}  // end StereoObs2BRObs

/* -------------------------------------------------------
				StereoObs2RBObs #3
   ------------------------------------------------------- */
void vision::StereoObs2BRObs(
	const CObservationVisualLandmarks& inObs, CObservationBearingRange& outObs)
{
	// For each of the 3D landmarks [X,Y,Z] we compute their range and bearing
	// representation.
	// The reference system is assumed to be that typical of cameras: +Z forward
	// and +X to the right.
	CLandmarksMap::TCustomSequenceLandmarks::const_iterator itCloud;
	for (itCloud = inObs.landmarks.landmarks.begin();
		 itCloud != inObs.landmarks.landmarks.end(); ++itCloud)
	{
		CObservationBearingRange::TMeasurement m;
		m.range = sqrt(
			square(itCloud->pose_mean.x) + square(itCloud->pose_mean.y) +
			square(itCloud->pose_mean.z));

		// The reference system is assumed to be that typical robot operation:
		// +X forward and +Z upwards.
		m.yaw = atan2(itCloud->pose_mean.y, itCloud->pose_mean.x);
		m.pitch = -sin(itCloud->pose_mean.z / m.range);
		m.landmarkID = itCloud->ID;

		outObs.sensedData.push_back(m);
	}  // end for
}  // end StereoObs2BRObs

/* -------------------------------------------------------
				computeStereoRectificationMaps
   ------------------------------------------------------- */
void vision::computeStereoRectificationMaps(
	const TCamera& cam1, const TCamera& cam2,
	const poses::CPose3D& rightCameraPose, void* outMap1x, void* outMap1y,
	void* outMap2x, void* outMap2y)
{
	ASSERT_(cam1.ncols == cam2.ncols && cam1.nrows == cam2.nrows);

#if MRPT_HAS_OPENCV

	cv::Mat *mapx1, *mapy1, *mapx2, *mapy2;
	mapx1 = static_cast<cv::Mat*>(outMap1x);
	mapy1 = static_cast<cv::Mat*>(outMap1y);
	mapx2 = static_cast<cv::Mat*>(outMap2x);
	mapy2 = static_cast<cv::Mat*>(outMap2y);

	const int resX = cam1.ncols;
	const int resY = cam1.nrows;

	CMatrixDouble44 hMatrix;
	rightCameraPose.getHomogeneousMatrix(hMatrix);

	double rcTrans[3];
	rcTrans[0] = hMatrix(0, 3);
	rcTrans[1] = hMatrix(1, 3);
	rcTrans[2] = hMatrix(2, 3);

	double m1[3][3];
	for (unsigned int i = 0; i < 3; ++i)
		for (unsigned int j = 0; j < 3; ++j)
			m1[i][j] = hMatrix(i, j);

	double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
	for (unsigned int i = 0; i < 3; ++i)
		for (unsigned int j = 0; j < 3; ++j)
		{
			ipl[i][j] = cam1.intrinsicParams(i, j);
			ipr[i][j] = cam2.intrinsicParams(i, j);
		}

	for (unsigned int i = 0; i < 5; ++i)
	{
		dpl[i] = cam1.dist[i];
		dpr[i] = cam2.dist[i];
	}

	cv::Mat R(3, 3, CV_64F, &m1);
	cv::Mat T(3, 1, CV_64F, &rcTrans);

	cv::Mat K1(3, 3, CV_64F, ipl);
	cv::Mat K2(3, 3, CV_64F, ipr);
	cv::Mat D1(1, 5, CV_64F, dpl);
	cv::Mat D2(1, 5, CV_64F, dpr);

	double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
	cv::Mat R1(3, 3, CV_64F, _R1);
	cv::Mat R2(3, 3, CV_64F, _R2);
	cv::Mat P1(3, 4, CV_64F, _P1);
	cv::Mat P2(3, 4, CV_64F, _P2);
	cv::Mat Q(4, 4, CV_64F, _Q);

	cv::Size nSize(resX, resY);
	double alpha = 0.0;	 // alpha value: 0.0 = zoom and crop the image so that
	// there's not black areas

	// OpenCV 2.3+ has this signature:
	cv::stereoRectify(
		K1, D1, K2, D2, nSize, R, T, R1, R2, P1, P2, Q,
		cv::CALIB_ZERO_DISPARITY, alpha);
	// Rest of arguments -> default

	cv::Size sz1, sz2;
	cv::initUndistortRectifyMap(
		K1, D1, R1, P1, cv::Size(resX, resY), CV_32FC1, *mapx1, *mapy1);
	cv::initUndistortRectifyMap(
		K2, D2, R2, P2, cv::Size(resX, resY), CV_32FC1, *mapx2, *mapy2);
/**/
#else
	THROW_EXCEPTION("The MRPT has been compiled without OPENCV!");
#endif
}  // end computeStereoRectificationMaps

TStereoSystemParams::TStereoSystemParams()

{
	K = defaultIntrinsicParamsMatrix(0, 640, 480);
	F.setZero();
	F(1, 2) = -1;
	F(2, 1) = 1;
}

/*-------------------------------------------------------------
			TStereoSystemParams: loadFromConfigFile
-------------------------------------------------------------*/
void TStereoSystemParams::loadFromConfigFile(
	const CConfigFileBase& iniFile, const string& section)
{
	int unc;
	unc = iniFile.read_int(section.c_str(), "uncPropagation", uncPropagation);
	switch (unc)
	{
		case 0: uncPropagation = Prop_Linear; break;
		case 1: uncPropagation = Prop_UT; break;
		case 2: uncPropagation = Prop_SUT; break;
	}  // end switch

	CVectorDouble k_vec(9);
	iniFile.read_vector(
		section.c_str(), "k_vec", CVectorDouble(), k_vec, false);
	for (unsigned int ii = 0; ii < 3; ++ii)
		for (unsigned int jj = 0; jj < 3; ++jj)
			K(ii, jj) = k_vec[ii * 3 + jj];

	CVectorDouble f_vec(9);
	iniFile.read_vector(
		section.c_str(), "f_vec", CVectorDouble(), f_vec, false);
	for (unsigned int ii = 0; ii < 3; ++ii)
		for (unsigned int jj = 0; jj < 3; ++jj)
			F(ii, jj) = f_vec[ii * 3 + jj];

	baseline = iniFile.read_float(section.c_str(), "baseline", baseline);
	stdPixel = iniFile.read_float(section.c_str(), "stdPixel", stdPixel);
	stdDisp = iniFile.read_float(section.c_str(), "stdDisp", stdDisp);
	maxZ = iniFile.read_float(section.c_str(), "maxZ", maxZ);
	minZ = iniFile.read_float(section.c_str(), "minZ", minZ);
	maxY = iniFile.read_float(section.c_str(), "maxY", maxY);
	factor_k = iniFile.read_float(section.c_str(), "factor_k", factor_k);
	factor_a = iniFile.read_float(section.c_str(), "factor_a", factor_a);
	factor_b = iniFile.read_float(section.c_str(), "factor_b", factor_b);
}  // end of loadFromConfigFile

/*---------------------------------------------------------------
					TStereoSystemParams: dumpToTextStream
  ---------------------------------------------------------------*/
void TStereoSystemParams::dumpToTextStream(std::ostream& out) const
{
	out << "\n----------- [vision::TStereoSystemParams] ------------ \n";
	out << "Method for 3D Uncert. \t= ";
	switch (uncPropagation)
	{
		case Prop_Linear: out << "Linear propagation\n"; break;
		case Prop_UT: out << "Unscented Transform\n"; break;
		case Prop_SUT: out << "Scaled Unscented Transform\n"; break;
	}  // end switch

	out << mrpt::format("K\t\t\t= [%f\t%f\t%f]\n", K(0, 0), K(0, 1), K(0, 2));
	out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", K(1, 0), K(1, 1), K(1, 2));
	out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", K(2, 0), K(2, 1), K(2, 2));

	out << mrpt::format("F\t\t\t= [%f\t%f\t%f]\n", F(0, 0), F(0, 1), F(0, 2));
	out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", F(1, 0), F(1, 1), F(1, 2));
	out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", F(2, 0), F(2, 1), F(2, 2));

	out << mrpt::format("Baseline \t\t= %f\n", baseline);
	out << mrpt::format("Pixel std \t\t= %f\n", stdPixel);
	out << mrpt::format("Disparity std\t\t= %f\n", stdDisp);
	out << mrpt::format("Z maximum\t\t= %f\n", maxZ);
	out << mrpt::format("Z minimum\t\t= %f\n", minZ);
	out << mrpt::format("Y maximum\t\t= %f\n", maxY);

	out << mrpt::format("k Factor [UT]\t\t= %f\n", factor_k);
	out << mrpt::format("a Factor [UT]\t\t= %f\n", factor_a);
	out << mrpt::format("b Factor [UT]\t\t= %f\n", factor_b);
	out << "-------------------------------------------------------- \n";
}

/*-------------------------------------------------------------
			TMatchingOptions: constructor
-------------------------------------------------------------*/
TMatchingOptions::TMatchingOptions()

	= default;	// end constructor TMatchingOptions

/*-------------------------------------------------------------
			TMatchingOptions: loadFromConfigFile
-------------------------------------------------------------*/
void TMatchingOptions::loadFromConfigFile(
	const CConfigFileBase& iniFile, const string& section)
{
	int mm =
		iniFile.read_int(section.c_str(), "matching_method", matching_method);
	switch (mm)
	{
		case 0: matching_method = mmCorrelation; break;
		case 1: matching_method = mmDescriptorSIFT; break;
		case 2: matching_method = mmDescriptorSURF; break;
		case 3: matching_method = mmSAD; break;
		case 4: matching_method = mmDescriptorORB; break;
	}  // end switch

	useEpipolarRestriction = iniFile.read_bool(
		section.c_str(), "useEpipolarRestriction", useEpipolarRestriction);
	hasFundamentalMatrix = iniFile.read_bool(
		section.c_str(), "hasFundamentalMatrix", hasFundamentalMatrix);
	parallelOpticalAxis = iniFile.read_bool(
		section.c_str(), "parallelOpticalAxis", parallelOpticalAxis);
	useXRestriction =
		iniFile.read_bool(section.c_str(), "useXRestriction", useXRestriction);
	addMatches = iniFile.read_bool(section.c_str(), "addMatches", addMatches);
	useDisparityLimits = iniFile.read_bool(
		section.c_str(), "useDisparityLimits", useDisparityLimits);

	min_disp = iniFile.read_float(section.c_str(), "min_disp", min_disp);
	max_disp = iniFile.read_float(section.c_str(), "max_disp", max_disp);

	epipolar_TH =
		iniFile.read_float(section.c_str(), "epipolar_TH", epipolar_TH);
	maxEDD_TH = iniFile.read_float(section.c_str(), "maxEDD_TH", maxEDD_TH);
	EDD_RATIO = iniFile.read_float(section.c_str(), "minDIF_TH", EDD_RATIO);
	minCC_TH = iniFile.read_float(section.c_str(), "minCC_TH", minCC_TH);
	minDCC_TH = iniFile.read_float(section.c_str(), "minDCC_TH", minDCC_TH);
	rCC_TH = iniFile.read_float(section.c_str(), "rCC_TH", rCC_TH);
	maxEDSD_TH = iniFile.read_float(section.c_str(), "maxEDSD_TH", maxEDSD_TH);
	EDSD_RATIO = iniFile.read_float(section.c_str(), "EDSD_RATIO", EDSD_RATIO);
	maxSAD_TH = iniFile.read_float(section.c_str(), "maxSAD_TH", maxSAD_TH);
	SAD_RATIO = iniFile.read_float(section.c_str(), "SAD_RATIO", SAD_RATIO);
	maxORB_dist =
		iniFile.read_float(section.c_str(), "maxORB_dist", maxORB_dist);

	estimateDepth =
		iniFile.read_bool(section.c_str(), "estimateDepth", estimateDepth);
	maxDepthThreshold = iniFile.read_float(
		section.c_str(), "maxDepthThreshold", maxDepthThreshold);
	//	fx                  = iniFile.read_float(section.c_str(),"fx",fx);
	//	cx                  = iniFile.read_float(section.c_str(),"cx",cx);
	//	cy                  = iniFile.read_float(section.c_str(),"cy",cy);
	//	baseline            =
	// iniFile.read_float(section.c_str(),"baseline",baseline);
}  // end TMatchingOptions::loadFromConfigFile

/*---------------------------------------------------------------
					TMatchingOptions: dumpToTextStream
  ---------------------------------------------------------------*/
void TMatchingOptions::dumpToTextStream(std::ostream& out) const
{
	out << "\n----------- [vision::TMatchingOptions] ------------ \n";
	out << "Matching method:                ";
	switch (matching_method)
	{
		case mmCorrelation:
			out << "Cross Correlation\n";
			out << mrpt::format(
				"· Min. CC. Threshold:           %f\n", minCC_TH);
			out << mrpt::format(
				"· Min. Dif. CC Threshold:       %f\n", minDCC_TH);
			out << mrpt::format("· Max. Ratio CC Threshold:      %f\n", rCC_TH);
			break;
		case mmDescriptorSIFT:
			out << "SIFT descriptor\n";
			out << mrpt::format(
				"· Max. EDD Threshold:           %f\n", maxEDD_TH);
			out << mrpt::format(
				"· EDD Ratio:                    %f\n", EDD_RATIO);
			break;
		case mmDescriptorSURF:
			out << "SURF descriptor\n";
			out << mrpt::format(
				"· EDD Ratio:                    %f\n", maxEDSD_TH);
			out << mrpt::format(
				"· Min. CC Threshold:            %f\n", EDSD_RATIO);
			break;
		case mmSAD:
			out << "SAD\n";
			out << mrpt::format(
				"· Max. Dif. SAD Threshold:      %f\n", maxSAD_TH);
			out << mrpt::format(
				"· Ratio SAD Threshold:          %f\n", SAD_RATIO);
			break;
		case mmDescriptorORB:
			out << "ORB\n";
			out << mrpt::format(
				"· Max. distance between desc:	%f\n", maxORB_dist);
			break;
	}  // end switch
	out << mrpt::format(
		"Epipolar Thres:                 %.2f px\n", epipolar_TH);
	out << "Using epipolar restriction?:    "
		<< (useEpipolarRestriction ? "Yes\n" : "No\n");
	out << "Has Fundamental Matrix?:        "
		<< (hasFundamentalMatrix ? "Yes\n" : "No\n");
	out << "Are camera axis parallel?:      "
		<< (parallelOpticalAxis ? "Yes\n" : "No\n");
	out << "Use X-coord restriction?:       "
		<< (useXRestriction ? "Yes\n" : "No\n");
	out << "Use disparity limits?:       "
		<< (useDisparityLimits ? "Yes\n" : "No\n");
	if (useDisparityLimits)
		out << mrpt::format(
			"· Min/max disp limits:          %.2f/%.2f px\n", min_disp,
			max_disp);
	out << "Estimate depth?:                "
		<< (estimateDepth ? "Yes\n" : "No\n");
	if (estimateDepth)
	{
		out << mrpt::format(
			"· Maximum depth allowed:        %f m\n", maxDepthThreshold);
	}
	out << "Add matches to list?:           ";
	out << (addMatches ? "Yes\n" : "No\n");
	out << "-------------------------------------------------------- \n";
}  // end TMatchingOptions::dumpToTextStream
