/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, 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/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/otherlibs/do_opencv_includes.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->other_x - (it->this_x * mat(0, 0) + it->this_y * mat(0, 1) +
							   it->this_z * mat(0, 2) + Rt.x());
		err.y = it->other_y - (it->this_x * mat(1, 0) + it->this_y * mat(1, 1) +
							   it->this_z * mat(1, 2) + Rt.y());
		err.z = it->other_z - (it->this_x * mat(2, 0) + it->this_y * mat(2, 1) +
							   it->this_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.this_idx = pair.other_idx = (unsigned int)itLand1->ID;

				pair.this_x = itLand1->pose_mean.x;
				pair.this_y = itLand1->pose_mean.y;
				pair.this_z = itLand1->pose_mean.z;

				pair.other_x = itLand2->pose_mean.x;
				pair.other_y = itLand2->pose_mean.y;
				pair.other_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, const 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);
	MRPT_UNUSED_PARAM(nMatches);

	// 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

/*-------------------------------------------------------------
					TROI Constructors
-------------------------------------------------------------*/
vision::TROI::TROI(float x1, float x2, float y1, float y2, float z1, float z2)
	: xMin(x1), xMax(x2), yMin(y1), yMax(y2), zMin(z1), zMax(z2)
{
}

/*-------------------------------------------------------------
					TImageROI Constructors
-------------------------------------------------------------*/
vision::TImageROI::TImageROI(float x1, float x2, float y1, float y2)
	: xMin(x1), xMax(x2), yMin(y1), yMax(y2)
{
}

/*-------------------------------------------------------------
			TStereoSystemParams: constructor
-------------------------------------------------------------*/
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


#include <cmath>
#include <iostream>

#include <boost/chrono.hpp>

using namespace std;

int
main ()
{
  // All code taken from the upstream documentation.
  //

  boost::chrono::system_clock::time_point start (
      boost::chrono::system_clock::now ());

  for (long i (0); i < 10000000; ++i)
    static_cast<void> (sqrt (123.456L)); // burn some time

  boost::chrono::duration<double> sec (boost::chrono::system_clock::now () -
                                       start);

  return 0;
}


#include "LanguageModel.h"

#include <iostream>
#include <math.h>
#include <random>
#include <sstream>

#include <boost/regex.hpp>

using namespace distrust;
using namespace Eigen;

LanguageModel::LanguageModel(const ModelInfo &model_info) :
  unif_(-1.0, 1.0),
  batch_size_(0) {

  window_size_ = model_info.window_size;
  wordvec_dim_ = model_info.wordvec_dim;
  hidden_dim_ = model_info.hidden_dim;
  start_token_index_ = model_info.start_token_index;
  end_token_index_ = model_info.end_token_index;
  unk_token_index_ = model_info.unk_token_index;
  vocab_size_ = model_info.vocab.size();
  for (unsigned int i = 0; i < vocab_size_; i++) {
    vocab_[model_info.vocab[i]] = i;
  }

  wordvec_w_.reserve(vocab_size_);
  wordvec_w_buf_.reserve(vocab_size_);
  input_hidden_w_.reserve(window_size_);
  input_hidden_w_buf_.reserve(window_size_);

  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_grad_.push_back(Matrix_t(hidden_dim_, wordvec_dim_));
  }
  input_hidden_b_grad_ = Vector_t(hidden_dim_);
  hidden_output_w_grad_ = Matrix_t(vocab_size_, hidden_dim_);
  hidden_output_b_grad_ = Vector_t(vocab_size_);

  // adagrad
  for (unsigned int i = 0; i < vocab_size_; i++) {
    wordvec_w_var_.push_back(ArrayXd::Zero(wordvec_dim_));
  }

  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_var_.push_back(MArray_t::Zero(hidden_dim_, wordvec_dim_));
  }

  input_hidden_b_var_ = ArrayXd::Zero(hidden_dim_);
  hidden_output_w_var_ = MArray_t::Zero(vocab_size_, hidden_dim_);
  hidden_output_b_var_ = ArrayXd::Zero(vocab_size_);
  zero_grad_params();
}

LanguageModel::LanguageModel(const LanguageModel &model) :
  window_size_(model.window_size_),
  wordvec_dim_(model.wordvec_dim_),
  hidden_dim_(model.hidden_dim_),
  start_token_index_(model.start_token_index_),
  end_token_index_(model.end_token_index_),
  unk_token_index_(model.unk_token_index_),
  vocab_size_(model.vocab_size_),
  vocab_(model.vocab_),
  wordvec_w_buf_(model.wordvec_w_buf_),
  input_hidden_w_buf_(model.input_hidden_w_buf_),
  input_hidden_b_buf_(model.input_hidden_b_buf_),
  hidden_output_w_buf_(model.hidden_output_w_buf_),
  hidden_output_b_buf_(model.hidden_output_b_buf_),
  input_hidden_w_grad_(model.input_hidden_w_grad_),
  input_hidden_b_grad_(model.input_hidden_b_grad_),
  hidden_output_w_grad_(model.hidden_output_w_grad_),
  hidden_output_b_grad_(model.hidden_output_b_grad_) {

  zero_grad_params();
  wrap_buffers();
}

double
LanguageModel::sample() {
  return unif_(re_);
}

void
LanguageModel::wrap_buffers() {
  wordvec_w_.clear();
  for (unsigned int i = 0; i < vocab_size_; i++) {
    wordvec_w_.push_back(Map<Vector_t>(&wordvec_w_buf_[i][0], wordvec_dim_));
  }

  input_hidden_w_.clear();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_.push_back(Map<Matrix_t>(&input_hidden_w_buf_[i][0], hidden_dim_, wordvec_dim_));
  }

  input_hidden_b_ = std::unique_ptr<Map<Vector_t>>(
    new Map<Vector_t>(&input_hidden_b_buf_[0], hidden_dim_));
  hidden_output_w_ = std::unique_ptr<Map<Matrix_t>>(
    new Map<Matrix_t>(&hidden_output_w_buf_[0], vocab_size_, hidden_dim_));
  hidden_output_b_ = std::unique_ptr<Map<Vector_t>>(
    new Map<Vector_t>(&hidden_output_b_buf_[0], vocab_size_));
}

void
LanguageModel::random_init() {
  wordvec_w_buf_.clear();
  for (unsigned int i = 0; i < vocab_size_; i++) {
    wordvec_w_buf_.push_back(std::vector<double>(wordvec_dim_));
    for (unsigned int j = 0; j < wordvec_dim_; j++) {
      wordvec_w_buf_[i][j] = 0.05 * sample();
    }
  }

  input_hidden_w_buf_.clear();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_buf_.push_back(std::vector<double>(hidden_dim_ * wordvec_dim_));
    for (unsigned int j = 0; j < hidden_dim_ * wordvec_dim_; j++) {
      input_hidden_w_buf_[i][j] = 0.05 * sample();
    }
  }

  input_hidden_b_buf_ = std::vector<double>(hidden_dim_, 0.0);
  hidden_output_w_buf_ = std::vector<double>(vocab_size_ * hidden_dim_, 0.0);
  hidden_output_b_buf_ = std::vector<double>(vocab_size_, 0.0);
  wrap_buffers();
}

void
LanguageModel::set_params(Params &params) {
  wordvec_w_buf_ = std::move(params.wordvec_w);
  input_hidden_w_buf_ = std::move(params.input_hidden_w);
  input_hidden_b_buf_ = std::move(params.input_hidden_b);
  hidden_output_w_buf_ = std::move(params.hidden_output_w);
  hidden_output_b_buf_ = std::move(params.hidden_output_b);
  wrap_buffers();
}

void
LanguageModel::update_params(const ParamUpdate &update) {
  for (auto itr = update.wordvec_w.begin(); itr != update.wordvec_w.end(); itr++) {
    uint32_t idx = itr->first;
    for (unsigned int i = 0; i < wordvec_dim_; i++) {
      wordvec_w_buf_[idx][i] += itr->second[i];
    }
  }

  for (unsigned int i = 0; i < window_size_; i++) {
    for (unsigned int j = 0; j < wordvec_dim_ * hidden_dim_; j++) {
      input_hidden_w_buf_[i][j] += update.input_hidden_w[i][j];
    }
  }

  for (unsigned int i = 0; i < hidden_dim_; i++) {
    input_hidden_b_buf_[i] += update.input_hidden_b[i];
  }

  for (unsigned int i = 0; i < hidden_dim_ * vocab_size_; i++) {
    hidden_output_w_buf_[i] += update.hidden_output_w[i];
  }

  for (unsigned int i = 0; i < vocab_size_; i++) {
    hidden_output_b_buf_[i] += update.hidden_output_b[i];
  }
}

void
LanguageModel::get_params(Params &ret) {
  ret.wordvec_w = wordvec_w_buf_;
  ret.input_hidden_w = input_hidden_w_buf_;
  ret.input_hidden_b = input_hidden_b_buf_;
  ret.hidden_output_w = hidden_output_w_buf_;
  ret.hidden_output_b = hidden_output_b_buf_;
}

void
LanguageModel::get_update(ParamUpdate &ret, const double learn_rate) {
  double *ptr;
  for (auto itr = wordvec_w_grad_.begin(); itr != wordvec_w_grad_.end(); itr++) {
    uint32_t idx = itr->first;
    //ArrayXd a = itr->second.array() * (-learn_rate);
    ArrayXd a = itr->second.array();
    wordvec_w_var_[idx] += a.square();
    a *= -learn_rate / (wordvec_w_var_[idx].sqrt() + 1e-4);
    ptr = a.data();
    ret.wordvec_w[idx] = std::vector<double>(ptr, ptr + wordvec_dim_);
  }

  for (unsigned int i = 0; i < window_size_; i++) {
    //MArray_t a = input_hidden_w_grad_[i].array() * (-learn_rate);
    MArray_t a = input_hidden_w_grad_[i].array();
    input_hidden_w_var_[i] += a.square();
    a *= -learn_rate / (input_hidden_w_var_[i].sqrt() + 1e-4);
    ptr = a.data();
    ret.input_hidden_w.push_back(std::vector<double>(ptr, ptr + hidden_dim_ * wordvec_dim_));
  }

  //ArrayXd a_ihb = input_hidden_b_grad_.array() * (-learn_rate);
  ArrayXd a_ihb = input_hidden_b_grad_.array();
  input_hidden_b_var_ += a_ihb.square();
  a_ihb *= -learn_rate / (input_hidden_b_var_.sqrt() + 1e-4);
  ptr = a_ihb.data();
  ret.input_hidden_b = std::vector<double>(ptr, ptr + hidden_dim_);

  //MArray_t a_how = hidden_output_w_grad_.array() * (-learn_rate);
  MArray_t a_how = hidden_output_w_grad_.array();
  hidden_output_w_var_ += a_how.square();
  a_how *= -learn_rate / (hidden_output_w_var_.sqrt() + 1e-4);
  ptr = a_how.data();
  ret.hidden_output_w = std::vector<double>(ptr, ptr + vocab_size_ * hidden_dim_);

  //ArrayXd a_hob = hidden_output_b_grad_.array() * (-learn_rate);
  ArrayXd a_hob = hidden_output_b_grad_.array();
  hidden_output_b_var_ += a_hob.square();
  a_hob *= -learn_rate / (hidden_output_b_var_.sqrt() + 1e-4);
  ptr = a_hob.data();
  ret.hidden_output_b = std::vector<double>(ptr, ptr + vocab_size_);
}

VectorXd
LanguageModel::tanh(const VectorXd &v) {
  ArrayXd a = (v.array() * 2).exp();
  return ((a - 1) / (a + 1)).matrix();
}

double
LanguageModel::logZ(const VectorXd &v) {
  return log(v.array().exp().sum());
}

uint32_t
LanguageModel::word_index(const std::string &word) {
  boost::regex re("[0-9]");
  std::string token = boost::regex_replace(word, re, "0");
  uint32_t index = unk_token_index_;
  auto itr = vocab_.find(token);
  if (itr != vocab_.end()) {
    index = itr->second;
  }
  return index;
}

std::vector<uint32_t>
LanguageModel::tokenize(const std::string &line) {
  std::vector<uint32_t> tokens;
  for (unsigned int i = 0; i < window_size_; i++) {
    tokens.push_back(start_token_index_);
  }

  std::string word;
  std::stringstream ss(line);
  while (std::getline(ss, word, ' ')) {
    tokens.push_back(word_index(word));
  }
  tokens.push_back(end_token_index_);
  return tokens;
}

std::vector<double>
LanguageModel::forward(const std::vector<uint32_t> &input) {
  if (input.size() != window_size_) {
    throw std::invalid_argument("forward: input size does not equal window size");
  }

  hidden_ = *input_hidden_b_;
  for (unsigned int i = 0; i < window_size_; i++) {
    hidden_ += input_hidden_w_[i] * wordvec_w_[input[i]];
  }
  hidden_tanh_ = tanh(hidden_);
  output_ = (*hidden_output_w_) * hidden_tanh_ + (*hidden_output_b_);
  logZ_ = logZ(output_);
  output_normed_ = (output_.array() - logZ_).matrix();
  double *ptr = output_normed_.data();
  return std::vector<double>(ptr, ptr + vocab_size_);
}

// double
// LanguageModel::forward(const std::vector<uint32_t> &tokens) {
//   uint32_t size = tokens.size() - window_size_;
//   Matrix<uint32_t, Dynamic, Dynamic> input(size, window_size_);
//   for (unsigned int i = 0; i < size; i++) {
//     for (unsigned int j = 0; j < window_size_; j++) {
//       input(i, j) = tokens[i + j];
//     }
//   }

// }

// double
// LanguageModel::forward(const std::vector<uint32_t> &input, const uint32_t target) {
//   if (input.size() != window_size_) {
//     throw std::invalid_argument("forward: input size does not equal window size");
//   }

//   hidden_ = *input_hidden_b_;
//   for (unsigned int i = 0; i < window_size_; i++) {
//     hidden_ += input_hidden_w_[i] * wordvec_w_[input[i]];
//   }
//   hidden_tanh_ = tanh(hidden_);
//   output_ = (*hidden_output_w_) * hidden_tanh_ + (*hidden_output_b_);
//   logZ_ = logZ(output_);
//   output_normed_ = (output_.array() - logZ_).matrix();
//   return output_normed_(target);
// }

// std::vector<double>
// LanguageModel::forward(const std::vector<uint32_t> &input, uint32_t target_idx) {
//   hidden_ = *input_hidden_b_;
//   for (unsigned int i = 0; i < window_size_; i++) {
//     hidden_ += input_hidden_w_[i] * wordvec_w_[input[target_idx - window_size_ + i]];
//   }
//   hidden_tanh_ = tanh(hidden_);
//   output_ = (*hidden_output_w_) * hidden_tanh_ + (*hidden_output_b_);
//   logZ_ = logZ(output_);
//   output_normed_ = (output_.array() - logZ_).matrix();

//   double *ptr = output_normed_.data();
//   return std::vector<double>(ptr, ptr + vocab_size_);
// }

void
LanguageModel::backward(
  const std::vector<uint32_t> &input,
  const uint32_t target) {

  batch_size_++;

  // hidden-output gradients
  Vector_t output_grad = output_normed_.array().exp().matrix();
  output_grad(target) -= 1;
  hidden_output_w_grad_ += output_grad * hidden_tanh_.transpose();
  hidden_output_b_grad_ += output_grad;

  // input-hidden gradients
  Vector_t hidden_grad = output_grad.transpose() * (*hidden_output_w_);
  hidden_grad = (hidden_grad.array() *
    (1 - hidden_tanh_.array().square()))
    .matrix();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_grad_[i] += hidden_grad * wordvec_w_[input[i]].transpose();
  }
  input_hidden_b_grad_ += hidden_grad;

  // word vector gradients
  for (unsigned int i = 0; i < window_size_; i++) {
    uint32_t idx = input[i];
    Vector_t input_grad = hidden_grad.transpose() * input_hidden_w_[i];
    auto itr = wordvec_w_grad_.find(idx);
    if (itr == wordvec_w_grad_.end()) {
      wordvec_w_grad_[idx] = input_grad;
    } else {
      wordvec_w_grad_[idx] += input_grad;
    }
  }
}

void
LanguageModel::zero_grad_params() {
  batch_size_ = 0;
  wordvec_w_grad_.clear();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_grad_[i].setZero();
  }
  input_hidden_b_grad_.setZero();
  hidden_output_w_grad_.setZero();
  hidden_output_b_grad_.setZero();
}


#ifndef JHMI_LIVER_PARSE_VESSEL_HPP_NRC_20150829
#define JHMI_LIVER_PARSE_VESSEL_HPP_NRC_20150829

#include "liver/physical_vessel.hpp"
#include "liver/constants.hpp"
#include <boost/algorithm/string.hpp>
#include <range/v3/algorithm.hpp>
#include <range/v3/getlines.hpp>
#include <range/v3/range_for.hpp>
#include <fmt/ostream.h>

namespace jhmi {
  namespace jhmi_detail {
    struct flat_vessel {
      physical_vessel v;
      vessel_id parent_id, left_id, right_id;
    };
    flat_vessel parse_vessel(std::vector<std::string> const& m, int idx, float scalar) {
      using boost::units::pow;
      if (m.size() != 13)
        throw std::runtime_error(fmt::format("Invalid v1 vessel file format: {}",m.size()));
      return {physical_vessel{scalar * dbl3{stod(m[0]),stod(m[1]),stod(m[2])}*mm,
                     scalar * dbl3{stod(m[3]),stod(m[4]),stod(m[5])}*mm,
                     scalar * stod(m[6])*mm,
                     cell_id::invalid(),//no connected macrocells, initially
                     cubic_meters_per_second{1 * pow<3>(meters) / seconds},
                     25_mmHg,
                     vessel_id{idx},
                     true},
              //parent, left, right
              vessel_id{stoi(m[7])}, vessel_id{stoi(m[8])},
              vessel_id{stoi(m[9])}};
    }

    auto make_vessel_map(std::istream&& in, float scalar) {
      vidx_to<flat_vessel> vns;
      std::vector<std::string> m;

      int idx = 0;
      RANGES_FOR (auto const& line, ranges::getlines(in)) {
        if (!line.empty() && line[0] != '#') {
          boost::split(m, line, boost::is_any_of(" \t"), boost::token_compress_on);
          auto vn = parse_vessel(m, idx++, scalar);
          vns.insert(std::make_pair(vn.v.id(), vn));
        }
      }
      return vns;
    }
    /**@return the ID of the parent vessel, throws a std::runtime_error if not present*/
    vessel_id find_parent_vessel(vidx_to<flat_vessel> const& vns) {
      auto it = ranges::find_if(vns, [](auto const& v) {
          return !v.second.parent_id.valid() && v.second.v.id().valid();
      });
      if (it == vns.end())
        throw std::runtime_error("No parent vessel found");
      return it->first;
    }

  }//jhmi_detail
}//jhmi

#endif


#include <iostream>
#include <time.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <stdlib.h>
#include "graph.h"
#include "alignmentUtils.h"

int main(int argc, char* argv[]) {
  if (argc < 5) {
    cout << "Needs 4 arguments - ontologyComputed, referenceOntology, minimum similarity value accepted, version of semantic verification (options are 'criss_cross', 'strict_hierarchy', 'sib_sib', and 'none')" << endl;
    cout << "Optional 5th argument - allow multiple mappings of same node when scores are identical.  1 will allow multiple mappings, 0 will not (default is 0 if no 5th argument is given" << endl;
    cout << "Optional 6th argument - treat genes as terms.  Default is 1 (yes).  0 is no." << endl;
    cout << "Optional 7th argument - category name of terminal nodes (i.e. gene, patient, etc.). Default is gene" << endl;
    cout << "similarity value can range from 0 to 1 (1 means nodes in ontology are identical)" << endl;
    return 0;
  }
  map<string, unsigned> geneNamesToIDs;
  string ontology1 = argv[1];
  string ontology2 = argv[2];
  string terminalName = "gene";
  if (argc >= 8) {
    terminalName = argv[7];
  }
  cout << "Getting ontology 1" << endl;
  DAGraph g1(ontology1, geneNamesToIDs, true, terminalName);
  cout << "Getting ontology 2" << endl;
  DAGraph g2(ontology2, geneNamesToIDs, true, terminalName);

  bool allowMultiples = false;
  if ((argc >= 6) && (atoi(argv[5]) == 1)) {
    allowMultiples = true;
  }

  bool genesAsTerms = true;
  if ((argc >=7) && (atoi(argv[6]) == 0)) {
    genesAsTerms = false;
  }

  vector<string> geneIDsToNames(geneNamesToIDs.size());
  for (map<string, unsigned>::iterator it = geneNamesToIDs.begin(); it != geneNamesToIDs.end(); ++it) {
    geneIDsToNames[it->second] = it->first;
  }

  float threshold = 0.01;
  float minSimilarityForPrealignment = atof(argv[3]);
  cout << "Minimum similarity value accepted: " << minSimilarityForPrealignment << endl;

  int verificationMode = 0;
  string verMode = argv[4];
  if (verMode == "criss_cross") {
    cout << "No parent child criss cross allowed" << endl;
    verificationMode = 1;
  } else if (verMode == "strict_hierarchy") {
    cout << "Strict hierarchy maintained in mappings" << endl;
    verificationMode = 2;
  } else if (verMode == "none") {
    cout << "No hierarchy information enforced in mappings" << endl;
  } else if (verMode == "sib_sib") {
    cout << "No parent child criss cross allowed and no sibling-sibling to parent-child mapping allowed" << endl;
    verificationMode = 3;
  } else {
    cout << "Options for semantic verification (argument 4) are 'criss_cross', 'strict_hierarchy', 'sib_sib', and 'none'" << endl;
    return 1;
  }

  cout << "Terms in first ontology: " << g1.numNodes() << "\t" << "Nodes in second ontology: " << g2.numNodes() << endl;
  boost::numeric::ublas::matrix<float> extSim(g1.numNodes(),g2.numNodes());
  cout << "Calculating extsim" << endl;
  time_t start, end;
  time (&start);
  alignmentUtils::calculateExtSim(extSim, g1, g2);
  time (&end);
  double dif = difftime(end,start);
  cout << "calculateExtSim took " << dif << " seconds" << endl;

  boost::numeric::ublas::matrix<float> lastSim = extSim;

  //removalListType removalList(g1.numNodes(), g2.numNodes());
  vector< alignmentType > verifiedAlignments;
  alignmentType finalAlignment;
  //boost::numeric::ublas::matrix<bool> foreverRemoved(g1.numNodes(), g2.numNodes());

  // Mike Yu edit: limit the number of iterations
  int max_iter = 10;
  int curr_iter = 1;
  cout << "Max iterations: " << max_iter << endl;

  bool repeatAlignmentFound = false;
  while (!repeatAlignmentFound) {

    // Mike Yu: print out iteration number
    //cout << "Starting iteration" << endl;
    cout << "Starting iteration " << curr_iter << endl;

    boost::numeric::ublas::matrix<float> simMat(extSim);
    cout << "Adding relSim to simMat" << endl;

    time (&start);
    alignmentUtils::addRelSimToSimMat(simMat, lastSim, g1, g2, genesAsTerms);
    time (&end);
    double dif = difftime(end,start);
    cout << "addRelSimToSimMat took " << dif << " seconds" << endl;

    //for (int i = 0; i < relSim.size1(); ++i) {
    //  cout << g1.getName(i) << "\t" << extSim(i,i) << "\t" << relSim(i,i) << endl;
    //}
    //return 0;

    alignmentType alignment;
    time(&start);
    cout << "Aligning" << endl;
    alignmentUtils::getAlignment(simMat,alignment,threshold,
				 g1,g2,minSimilarityForPrealignment, verificationMode, allowMultiples);
    time (&end);
    dif = difftime(end,start);
    cout << "Alignment took " << dif << " seconds" << endl;
    cout << "Got an alignment" << endl;

    for (vector<alignmentType>::iterator it = verifiedAlignments.begin();
	 it != verifiedAlignments.end(); ++it) {
      if (alignment == *it) {
	repeatAlignmentFound = true;
	finalAlignment = alignment;
	cout << "Matched" << endl;
      }
    }
    lastSim = simMat;

    /*
    for (int i = 0; i < lastSim.size1(); ++i) {
      for (int j = 0; j < lastSim.size2(); ++j) {
	if (removalList.isInRemovalList(make_pair(i,j))) {
	  foreverRemoved(i,j) = true;
	}
      }
    }
    */
    verifiedAlignments.push_back(alignment);

    // End early
    //repeatAlignmentFound = true;
    //finalAlignment = verifiedAlignment;
    //cout << "Matched" << endl;

    // Mike Yu edit: limit the number of iterations
    curr_iter += 1;
    if (curr_iter > max_iter) {
      repeatAlignmentFound = true;
      finalAlignment = alignment;
      cout << "Matched" << endl;
    }
  }

  for (alignmentType::iterator it = finalAlignment.begin(); it != finalAlignment.end(); ++it) {

    if (!g1.isGene(it->first)) {
      cout << g1.getName(it->first) << "\t" << g2.getName(it->second) << "\t" << lastSim(it->first,it->second) << endl;
    }

    /* THE BELOW CODE WILL PRINT DESCENDENTS OF EACH TERM
    cout << g1.getName(it->first) << "\t" << g2.getName(it->second) << "\t" << lastSim(it->first,it->second) << "\t";
    for (set<unsigned>::iterator genesIt = g1.getGenesBegin(it->first); genesIt != g1.getGenesEnd(it->first); ++genesIt) {
      cout << geneIDsToNames[*genesIt] << ",";
    }
    cout << "\t";
    for (set<unsigned>::iterator genesIt = g2.getGenesBegin(it->second); genesIt != g2.getGenesEnd(it->second); ++genesIt) {
      cout << geneIDsToNames[*genesIt] << ",";
    }
    cout << endl;
    */
  }

  return 1;
}