/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "rtabmap/core/util3d_motion_estimation.h"

#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d.h"

#include <pcl/common/common.h>

#include "opencv/solvepnp.h"

#ifdef RTABMAP_OPENGV
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac/MultiRansac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp>
#endif

namespace rtabmap
{

namespace util3d
{

Transform estimateMotion3DTo2D(
			const std::map<int, cv::Point3f> & words3A,
			const std::map<int, cv::KeyPoint> & words2B,
			const CameraModel & cameraModel,
			int minInliers,
			int iterations,
			double reprojError,
			int flagsPnP,
			int refineIterations,
			int varianceMedianRatio,
			float maxVariance,
			const Transform & guess,
			const std::map<int, cv::Point3f> & words3B,
			cv::Mat * covariance,
			std::vector<int> * matchesOut,
			std::vector<int> * inliersOut,
			bool splitLinearCovarianceComponents)
{
	UASSERT(cameraModel.isValidForProjection());
	UASSERT(!guess.isNull());
	UASSERT(varianceMedianRatio>1);
	Transform transform;
	std::vector<int> matches, inliers;

	if(covariance)
	{
		*covariance = cv::Mat::eye(6,6,CV_64FC1);
	}

	// find correspondences
	std::vector<int> ids = uKeys(words2B);
	std::vector<cv::Point3f> objectPoints(ids.size());
	std::vector<cv::Point2f> imagePoints(ids.size());
	int oi=0;
	matches.resize(ids.size());
	for(unsigned int i=0; i<ids.size(); ++i)
	{
		std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
		if(iter != words3A.end() && util3d::isFinite(iter->second))
		{
			const cv::Point3f & pt = iter->second;
			objectPoints[oi].x = pt.x;
			objectPoints[oi].y = pt.y;
			objectPoints[oi].z = pt.z;
			imagePoints[oi] = words2B.find(ids[i])->second.pt;
			matches[oi++] = ids[i];
		}
	}

	objectPoints.resize(oi);
	imagePoints.resize(oi);
	matches.resize(oi);

	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d",
			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
			guess.prettyPrint().c_str(), reprojError, iterations);

	if((int)matches.size() >= minInliers)
	{
		//PnPRansac
		cv::Mat K = cameraModel.K();
		cv::Mat D = cameraModel.D();
		Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
		cv::Mat R = (cv::Mat_<double>(3,3) <<
				(double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
				(double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(),
				(double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33());

		cv::Mat rvec(3,1, CV_64FC1);
		cv::Rodrigues(R, rvec);
		cv::Mat tvec = (cv::Mat_<double>(3,1) <<
				(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());

		util3d::solvePnPRansac(
				objectPoints,
				imagePoints,
				K,
				D,
				rvec,
				tvec,
				true,
				iterations,
				reprojError,
				minInliers, // min inliers
				inliers,
				flagsPnP,
				refineIterations);

		if((int)inliers.size() >= minInliers)
		{
			cv::Rodrigues(rvec, R);
			Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
						   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
						   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));

			transform = (cameraModel.localTransform() * pnp).inverse();

			// compute variance (like in PCL computeVariance() method of sac_model.h)
			if(covariance && (!words3B.empty() || cameraModel.imageSize() != cv::Size()))
			{
				std::vector<float> errorSqrdDists(inliers.size());
				std::vector<float> errorSqrdX;
				std::vector<float> errorSqrdY;
				std::vector<float> errorSqrdZ;
				if(splitLinearCovarianceComponents)
				{
					errorSqrdX.resize(inliers.size());
					errorSqrdY.resize(inliers.size());
					errorSqrdZ.resize(inliers.size());
				}
				std::vector<float> errorSqrdAngles(inliers.size());
				Transform localTransformInv = cameraModel.localTransform().inverse();
				Transform transformCameraFrame = transform * cameraModel.localTransform();
				Transform transformCameraFrameInv = transformCameraFrame.inverse();
				for(unsigned int i=0; i<inliers.size(); ++i)
				{
					cv::Point3f objPt = objectPoints[inliers[i]];

					// Get 3D point from cameraB base frame in cameraA base frame
					std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
					cv::Point3f newPt;
					if(iter!=words3B.end() && util3d::isFinite(iter->second))
					{
						newPt = util3d::transformPoint(iter->second, transform);
					}
					else
					{

						// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
						cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformCameraFrameInv);

						//compute from projection
						Eigen::Vector3f ray = projectDepthTo3DRay(
								cameraModel.imageSize(),
								imagePoints.at(inliers[i]).x,
								imagePoints.at(inliers[i]).y,
								cameraModel.cx(),
								cameraModel.cy(),
								cameraModel.fx(),
								cameraModel.fy());
						// transform in camera B frame
						newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error

						//transform back into cameraA base frame
						newPt = util3d::transformPoint(newPt, transformCameraFrame);
					}

					if(splitLinearCovarianceComponents)
					{
						double errorX = objPt.x-newPt.x;
						double errorY = objPt.y-newPt.y;
						double errorZ = objPt.z-newPt.z;
						errorSqrdX[i] = errorX * errorX;
						errorSqrdY[i] = errorY * errorY;
						errorSqrdZ[i] = errorZ * errorZ;
					}

					errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);

					Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
					Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
					errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
				}

				std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
				//divide by 4 instead of 2 to ignore very very far features (stereo)
				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
				UASSERT(uIsFinite(median_error_sqr_lin));
				(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
				std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
				UASSERT(uIsFinite(median_error_sqr_ang));
				(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;

				if(splitLinearCovarianceComponents)
				{
					std::sort(errorSqrdX.begin(), errorSqrdX.end());
					double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
					std::sort(errorSqrdY.begin(), errorSqrdY.end());
					double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
					std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
					double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
					
					UASSERT(uIsFinite(median_error_sqr_x));
					UASSERT(uIsFinite(median_error_sqr_y));
					UASSERT(uIsFinite(median_error_sqr_z));
					covariance->at<double>(0,0) = median_error_sqr_x;
					covariance->at<double>(1,1) = median_error_sqr_y;
					covariance->at<double>(2,2) = median_error_sqr_z;

					median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
				}

				if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
				{
					UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
					*covariance = cv::Mat::eye(6,6,CV_64FC1);
					transform.setNull();
				}
			}
			else if(covariance)
			{
				// compute variance, which is the rms of reprojection errors
				std::vector<cv::Point2f> imagePointsReproj;
				cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePointsReproj);
				float err = 0.0f;
				for(unsigned int i=0; i<inliers.size(); ++i)
				{
					err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
				}
				UASSERT(uIsFinite(err));
				*covariance *= std::sqrt(err/float(inliers.size()));
			}
		}
	}

	if(matchesOut)
	{
		*matchesOut = matches;
	}
	if(inliersOut)
	{
		inliersOut->resize(inliers.size());
		for(unsigned int i=0; i<inliers.size(); ++i)
		{
			inliersOut->at(i) = matches[inliers[i]];
		}
	}

	return transform;
}

Transform estimateMotion3DTo2D(
	const std::map<int, cv::Point3f> & words3A,
	const std::map<int, cv::KeyPoint> & words2B,
	const std::vector<CameraModel> & cameraModels,
	unsigned int samplingPolicy,
	int minInliers,
	int iterations,
	double reprojError,
	int flagsPnP,
	int refineIterations,
	int varianceMedianRatio,
	float maxVariance,
	const Transform & guess,
	const std::map<int, cv::Point3f> & words3B,
	cv::Mat * covariance,
	std::vector<int> * matchesOut,
	std::vector<int> * inliersOut,
	bool splitLinearCovarianceComponents)
{
	std::vector<std::vector<int> > matchesPerCamera;
	std::vector<std::vector<int> > inliersPerCamera;
	Transform t = estimateMotion3DTo2D(
		words3A,
		words2B,
		cameraModels,
		samplingPolicy,
		minInliers,
		iterations,
		reprojError,
		flagsPnP,
		refineIterations,
		varianceMedianRatio,
		maxVariance,
		guess,
		words3B,
		covariance,
		matchesOut?&matchesPerCamera:0,
		inliersOut?&inliersPerCamera:0,
		splitLinearCovarianceComponents);
	if(matchesOut)
	{
		for(size_t i=0; i<matchesPerCamera.size(); ++i)
		{
			matchesOut->insert(matchesOut->end(), matchesPerCamera[i].begin(), matchesPerCamera[i].end());
		}
	}
	if(inliersOut)
	{
		for(size_t i=0; i<inliersPerCamera.size(); ++i)
		{
			inliersOut->insert(inliersOut->end(), inliersPerCamera[i].begin(), inliersPerCamera[i].end());
		}
	}
	return t;
}

Transform estimateMotion3DTo2D(
			const std::map<int, cv::Point3f> & words3A,
			const std::map<int, cv::KeyPoint> & words2B,
			const std::vector<CameraModel> & cameraModels,
			unsigned int samplingPolicy,
			int minInliers,
			int iterations,
			double reprojError,
			int flagsPnP,
			int refineIterations,
			int varianceMedianRatio,
			float maxVariance,
			const Transform & guess,
			const std::map<int, cv::Point3f> & words3B,
			cv::Mat * covariance,
			std::vector<std::vector<int> > * matchesOut,
			std::vector<std::vector<int> > * inliersOut,
			bool splitLinearCovarianceComponents)
{
	Transform transform;
#ifndef RTABMAP_OPENGV
	UERROR("This function is only available if rtabmap is built with OpenGV dependency.");
#else
	UASSERT(!cameraModels.empty() && cameraModels[0].imageWidth() > 0);
	int subImageWidth = cameraModels[0].imageWidth();
	for(size_t i=0; i<cameraModels.size(); ++i)
	{
		UASSERT(cameraModels[i].isValidForProjection());
		UASSERT(subImageWidth  == cameraModels[i].imageWidth());
	}

	UASSERT(!guess.isNull());
	UASSERT(varianceMedianRatio > 1);

	std::vector<int> matches, inliers;

	if(covariance)
	{
		*covariance = cv::Mat::eye(6,6,CV_64FC1);
	}

	// find correspondences
	std::vector<int> ids = uKeys(words2B);
	std::vector<cv::Point3f> objectPoints(ids.size());
	std::vector<cv::Point2f> imagePoints(ids.size());
	int oi=0;
	matches.resize(ids.size());
	std::vector<int> cameraIndexes(ids.size());
	for(unsigned int i=0; i<ids.size(); ++i)
	{
		std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
		if(iter != words3A.end() && util3d::isFinite(iter->second))
		{
			const cv::Point2f & kpt = words2B.find(ids[i])->second.pt;
			int cameraIndex = int(kpt.x / subImageWidth);
			UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)cameraModels.size(),
					uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
							cameraIndex, (int)cameraModels.size(), kpt.x, subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());

			const cv::Point3f & pt = iter->second;
			objectPoints[oi] = pt;
			imagePoints[oi] = kpt;
			// convert in image space
			imagePoints[oi].x = imagePoints[oi].x - (cameraIndex*subImageWidth);
			cameraIndexes[oi] = cameraIndex;
			matches[oi++] = ids[i];
		}
	}

	objectPoints.resize(oi);
	imagePoints.resize(oi);
	cameraIndexes.resize(oi);
	matches.resize(oi);

	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld",
			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
			guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy);

	if((int)matches.size() >= minInliers)
	{
		if(samplingPolicy == 0 || samplingPolicy == 2)
		{
			std::vector<int> cc;
			cc.resize(cameraModels.size());
			std::fill(cc.begin(), cc.end(),0);
			for(size_t i=0; i<cameraIndexes.size(); ++i)
			{
				cc[cameraIndexes[i]] = cc[cameraIndexes[i]] + 1;
			}

			for (size_t i=0; i<cameraModels.size(); ++i)
			{
				UDEBUG("Matches in Camera %d: %d", i, cc[i]);
				// opengv multi ransac needs at least 2 matches/camera
				if (cc[i] < 2)
				{
					if(samplingPolicy==2) {
						UERROR("Not enough matches in camera %ld to do "
							  "homogenoeus random sampling, returning null "
							  "transform. Consider using AUTO sampling "
							  "policy to fallback to ANY policy.", i);
						return Transform();
					}
					else { // samplingPolicy==0
						samplingPolicy = 1;
						UWARN("Not enough matches in camera %ld to do "
							  "homogenoeus random sampling, falling back to ANY policy.", i);
						break;
					}
				}
			}
		}

		if(samplingPolicy == 0)
		{
			samplingPolicy = 2;
		}

		// convert cameras
		opengv::translations_t camOffsets;
		opengv::rotations_t camRotations;
		for(size_t i=0; i<cameraModels.size(); ++i)
		{
			camOffsets.push_back(opengv::translation_t(
					cameraModels[i].localTransform().x(),
					cameraModels[i].localTransform().y(),
					cameraModels[i].localTransform().z()));
			camRotations.push_back(cameraModels[i].localTransform().toEigen4d().block<3,3>(0, 0));
		}

		Transform pnp;
		if(samplingPolicy == 2) // Homogenoeus random sampling
		{
			// convert 3d points
			std::vector<std::shared_ptr<opengv::points_t>> multiPoints;
			multiPoints.resize(cameraModels.size());
			// convert 2d-3d correspondences into bearing vectors
			std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
			multiBearingVectors.resize(cameraModels.size());
			for(size_t i=0; i<cameraModels.size();++i)
			{
				multiPoints[i] = std::make_shared<opengv::points_t>();
				multiBearingVectors[i] = std::make_shared<opengv::bearingVectors_t>();
			}

			for(size_t i=0; i<objectPoints.size(); ++i)
			{
				int cameraIndex = cameraIndexes[i];
				multiPoints[cameraIndex]->push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
				cv::Vec3f pt;
				cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
				pt = cv::normalize(pt);
				multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
			}

			//create a non-central absolute multi adapter
			opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
					multiBearingVectors,
					multiPoints,
					camOffsets,
					camRotations );

			adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
			adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));

			//Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac
			//The method is set to GP3P
			opengv::sac::MultiRansac<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
			std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
					new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));

			ransac.sac_model_ = absposeproblem_ptr;
			ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
			ransac.max_iterations_ = iterations;
			UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);

			//Run the experiment
			ransac.computeModel();

			pnp = Transform::fromEigen3d(ransac.model_coefficients_);

			UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
			UDEBUG("Ransac iterations done: %d", ransac.iterations_);
			for (size_t i=0; i < cameraModels.size(); ++i)
			{
				inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
			}
		}
		else
		{
			// convert 3d points
			opengv::points_t points;

			// convert 2d-3d correspondences into bearing vectors
			opengv::bearingVectors_t bearingVectors;
			opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences;

			for(size_t i=0; i<objectPoints.size(); ++i)
			{
				int cameraIndex = cameraIndexes[i];
				points.push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
				cv::Vec3f pt;
				cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
				pt = cv::normalize(pt);
				bearingVectors.push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
				camCorrespondences.push_back(cameraIndex);
			}

			//create a non-central absolute adapter
			opengv::absolute_pose::NoncentralAbsoluteAdapter adapter(
					bearingVectors,
					camCorrespondences,
					points,
					camOffsets,
					camRotations );

			adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
			adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));

			//Create a AbsolutePoseSacProblem and Ransac
			//The method is set to GP3P
			opengv::sac::Ransac<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
			std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
					new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));

			ransac.sac_model_ = absposeproblem_ptr;
			ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
			ransac.max_iterations_ = iterations;
			UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);

			//Run the experiment
			ransac.computeModel();

			pnp = Transform::fromEigen3d(ransac.model_coefficients_);

			UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
			UDEBUG("Ransac iterations done: %d", ransac.iterations_);
			inliers = ransac.inliers_;
		}

		UDEBUG("Ransac inliers: %ld", inliers.size());

		if((int)inliers.size() >= minInliers && !pnp.isNull())
		{
			transform = pnp;

			// compute variance (like in PCL computeVariance() method of sac_model.h)
			if(covariance)
			{
				std::vector<float> errorSqrdX;
				std::vector<float> errorSqrdY;
				std::vector<float> errorSqrdZ;
				if(splitLinearCovarianceComponents)
				{
					errorSqrdX.resize(inliers.size());
					errorSqrdY.resize(inliers.size());
					errorSqrdZ.resize(inliers.size());
				}
				std::vector<float> errorSqrdDists(inliers.size());
				std::vector<float> errorSqrdAngles(inliers.size());

				std::vector<Transform> transformsCameraFrame(cameraModels.size());
				std::vector<Transform> transformsCameraFrameInv(cameraModels.size());
				for(size_t i=0; i<cameraModels.size(); ++i)
				{
					transformsCameraFrame[i] = transform * cameraModels[i].localTransform();
					transformsCameraFrameInv[i] = transformsCameraFrame[i].inverse();
				}

				for(unsigned int i=0; i<inliers.size(); ++i)
				{
					cv::Point3f objPt = objectPoints[inliers[i]];

					// Get 3D point from cameraB base frame in cameraA base frame
					std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
					cv::Point3f newPt;
					if(iter!=words3B.end() && util3d::isFinite(iter->second))
					{
						newPt = util3d::transformPoint(iter->second, transform);
					}
					else
					{
						int cameraIndex = cameraIndexes[inliers[i]];

						// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
						cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformsCameraFrameInv[cameraIndex]);

						//compute from projection
						Eigen::Vector3f ray = projectDepthTo3DRay(
								cameraModels[cameraIndex].imageSize(),
								imagePoints.at(inliers[i]).x,
								imagePoints.at(inliers[i]).y,
								cameraModels[cameraIndex].cx(),
								cameraModels[cameraIndex].cy(),
								cameraModels[cameraIndex].fx(),
								cameraModels[cameraIndex].fy());
						// transform in camera B frame
						newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error

						//transfor back into cameraA base frame
						newPt = util3d::transformPoint(newPt, transformsCameraFrame[cameraIndex]);
					}

					if(splitLinearCovarianceComponents)
					{
						double errorX = objPt.x-newPt.x;
						double errorY = objPt.y-newPt.y;
						double errorZ = objPt.z-newPt.z;
						errorSqrdX[i] = errorX * errorX;
						errorSqrdY[i] = errorY * errorY;
						errorSqrdZ[i] = errorZ * errorZ;
					}

					errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);

					Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
					Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
					errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
				}

				std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
				//divide by 4 instead of 2 to ignore very very far features (stereo)
				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
				UASSERT(uIsFinite(median_error_sqr_lin));
				(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
				std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
				UASSERT(uIsFinite(median_error_sqr_ang));
				(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;

				if(splitLinearCovarianceComponents)
				{
					std::sort(errorSqrdX.begin(), errorSqrdX.end());
					double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
					std::sort(errorSqrdY.begin(), errorSqrdY.end());
					double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
					std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
					double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
					
					UASSERT(uIsFinite(median_error_sqr_x));
					UASSERT(uIsFinite(median_error_sqr_y));
					UASSERT(uIsFinite(median_error_sqr_z));
					covariance->at<double>(0,0) = median_error_sqr_x;
					covariance->at<double>(1,1) = median_error_sqr_y;
					covariance->at<double>(2,2) = median_error_sqr_z;

					median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
				}

				if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
				{
					UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
					*covariance = cv::Mat::eye(6,6,CV_64FC1);
					transform.setNull();
				}
			}
		}
	}

	if(matchesOut)
	{
		matchesOut->resize(cameraModels.size());
		UASSERT(matches.size() == cameraIndexes.size());
		for(size_t i=0; i<matches.size(); ++i)
		{
			UASSERT(cameraIndexes[i]>=0 && cameraIndexes[i] < (int)cameraModels.size());
			matchesOut->at(cameraIndexes[i]).push_back(matches[i]);
		}
	}
	if(inliersOut)
	{
		inliersOut->resize(cameraModels.size());
		for(unsigned int i=0; i<inliers.size(); ++i)
		{
			UASSERT(inliers[i]>=0 && inliers[i] < (int)cameraIndexes.size());
			UASSERT(cameraIndexes[inliers[i]]>=0 && cameraIndexes[inliers[i]] < (int)cameraModels.size());
			inliersOut->at(cameraIndexes[inliers[i]]).push_back(matches[inliers[i]]);
		}
	}
#endif
	return transform;
}

Transform estimateMotion3DTo3D(
			const std::map<int, cv::Point3f> & words3A,
			const std::map<int, cv::Point3f> & words3B,
			int minInliers,
			double inliersDistance,
			int iterations,
			int refineIterations,
			cv::Mat * covariance,
			std::vector<int> * matchesOut,
			std::vector<int> * inliersOut)
{
	Transform transform;
	std::vector<cv::Point3f> inliers1; // previous
	std::vector<cv::Point3f> inliers2; // new

	std::vector<int> matches;
	util3d::findCorrespondences(
			words3A,
			words3B,
			inliers1,
			inliers2,
			0,
			&matches);
	UASSERT(inliers1.size() == inliers2.size());
	UDEBUG("Unique correspondences = %d", (int)inliers1.size());

	if(covariance)
	{
		*covariance = cv::Mat::eye(6,6,CV_64FC1);
	}

	std::vector<int> inliers;
	if((int)inliers1.size() >= minInliers)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2cloud(new pcl::PointCloud<pcl::PointXYZ>);
		inliers1cloud->resize(inliers1.size());
		inliers2cloud->resize(inliers1.size());
		for(unsigned int i=0; i<inliers1.size(); ++i)
		{
			(*inliers1cloud)[i].x = inliers1[i].x;
			(*inliers1cloud)[i].y = inliers1[i].y;
			(*inliers1cloud)[i].z = inliers1[i].z;
			(*inliers2cloud)[i].x = inliers2[i].x;
			(*inliers2cloud)[i].y = inliers2[i].y;
			(*inliers2cloud)[i].z = inliers2[i].z;
		}
		Transform t = util3d::transformFromXYZCorrespondences(
				inliers2cloud,
				inliers1cloud,
				inliersDistance,
				iterations,
				refineIterations,
				3.0,
				&inliers,
				covariance);

		if(!t.isNull() && (int)inliers.size() >= minInliers)
		{
			transform = t;
		}
	}

	if(matchesOut)
	{
		*matchesOut = matches;
	}
	if(inliersOut)
	{
		inliersOut->resize(inliers.size());
		for(unsigned int i=0; i<inliers.size(); ++i)
		{
			inliersOut->at(i) = matches[inliers[i]];
		}
	}

	return transform;
}


std::vector<float> computeReprojErrors(
		std::vector<cv::Point3f> opoints,
		std::vector<cv::Point2f> ipoints,
		const cv::Mat & cameraMatrix,
		const cv::Mat & distCoeffs,
		const cv::Mat & rvec,
		const cv::Mat & tvec,
		float reprojErrorThreshold,
		std::vector<int> & inliers)
{
	UASSERT(opoints.size() == ipoints.size());
	int count = (int)opoints.size();

	std::vector<cv::Point2f> projpoints;
	projectPoints(opoints, rvec, tvec, cameraMatrix, distCoeffs, projpoints);

	inliers.resize(count,0);
	std::vector<float> err(count);
	int oi=0;
	for (int i = 0; i < count; ++i)
	{
		float e = (float)cv::norm( ipoints[i] - projpoints[i]);
		if(e <= reprojErrorThreshold)
		{
			inliers[oi] = i;
			err[oi++] = e;
		}
	}
	inliers.resize(oi);
	err.resize(oi);
	return err;
}

void solvePnPRansac(
		const std::vector<cv::Point3f> & objectPoints,
		const std::vector<cv::Point2f> & imagePoints,
		const cv::Mat & cameraMatrix,
		const cv::Mat & distCoeffs,
		cv::Mat & rvec,
		cv::Mat & tvec,
		bool useExtrinsicGuess,
        int iterationsCount,
        float reprojectionError,
        int minInliersCount,
        std::vector<int> & inliers,
        int flags,
        int refineIterations,
        float refineSigma)
{
	if(minInliersCount < 4)
	{
		minInliersCount = 4;
	}

	// Use OpenCV3 version of solvePnPRansac in OpenCV2.
	// FIXME: we should use this version of solvePnPRansac in newer 3.3.1 too, which seems a lot less stable!?!? Why!?
	cv3::solvePnPRansac(
			objectPoints,
			imagePoints,
			cameraMatrix,
			distCoeffs,
			rvec,
			tvec,
			useExtrinsicGuess,
			iterationsCount,
			reprojectionError,
			0.99, // confidence
			inliers,
			flags);

	float inlierThreshold = reprojectionError;
	if((int)inliers.size() >= minInliersCount && refineIterations>0)
	{
		float error_threshold = inlierThreshold;
		int refine_iterations = 0;
		bool inlier_changed = false, oscillating = false;
		std::vector<int> new_inliers, prev_inliers = inliers;
		std::vector<size_t> inliers_sizes;
		//Eigen::VectorXf new_model_coefficients = model_coefficients;
		cv::Mat new_model_rvec = rvec;
		cv::Mat new_model_tvec = tvec;

		do
		{
			// Get inliers from the current model
			std::vector<cv::Point3f> opoints_inliers(prev_inliers.size());
			std::vector<cv::Point2f> ipoints_inliers(prev_inliers.size());
			for(unsigned int i=0; i<prev_inliers.size(); ++i)
			{
				opoints_inliers[i] = objectPoints[prev_inliers[i]];
				ipoints_inliers[i] = imagePoints[prev_inliers[i]];
			}

			UDEBUG("inliers=%d refine_iterations=%d, rvec=%f,%f,%f tvec=%f,%f,%f", (int)prev_inliers.size(), refine_iterations,
					*new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
					*new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));

			// Optimize the model coefficients
			cv::solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, true, flags);
			inliers_sizes.push_back(prev_inliers.size());

			UDEBUG("rvec=%f,%f,%f tvec=%f,%f,%f",
					*new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
					*new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));

			// Select the new inliers based on the optimized coefficients and new threshold
			std::vector<float> err = computeReprojErrors(objectPoints, imagePoints, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, error_threshold, new_inliers);
			UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
					(int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);

			if ((int)new_inliers.size() < minInliersCount)
			{
				++refine_iterations;
				if (refine_iterations >= refineIterations)
				{
					break;
				}
				continue;
			}

			// Estimate the variance and the new threshold
			float m = uMean(err.data(), err.size());
			float variance = uVariance(err.data(), err.size());
			error_threshold = std::min(inlierThreshold, refineSigma * float(sqrt(variance)));

			UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.",
				  error_threshold, variance, m, refine_iterations, refineIterations);
			inlier_changed = false;
			std::swap (prev_inliers, new_inliers);

			// If the number of inliers changed, then we are still optimizing
			if (new_inliers.size () != prev_inliers.size ())
			{
				// Check if the number of inliers is oscillating in between two values
				if ((int)inliers_sizes.size () >= minInliersCount)
				{
					if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
					inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
					{
						oscillating = true;
						break;
					}
				}
				inlier_changed = true;
				continue;
			}

			// Check the values of the inlier set
			for (size_t i = 0; i < prev_inliers.size (); ++i)
			{
				// If the value of the inliers changed, then we are still optimizing
				if (prev_inliers[i] != new_inliers[i])
				{
					inlier_changed = true;
					break;
				}
			}
		}
		while (inlier_changed && ++refine_iterations < refineIterations);

		// If the new set of inliers is empty, we didn't do a good job refining
		if ((int)prev_inliers.size() < minInliersCount)
		{
			UWARN ("RANSAC refineModel: Refinement failed: got very low inliers (%d)!", (int)prev_inliers.size());
		}

		if (oscillating)
		{
			UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
		}

		std::swap (inliers, new_inliers);
		rvec = new_model_rvec;
		tvec = new_model_tvec;
	}

}

}

}