/* +------------------------------------------------------------------------+
   |                     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 "math-precomp.h"  // Precompiled headers

#include <mrpt/core/exceptions.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CQuaternion.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/math/TLine2D.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject2D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/math/TPlane.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/TPolygon2D.h>
#include <mrpt/math/TPolygon3D.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/math/TPose3DQuat.h>
#include <mrpt/math/TSegment2D.h>
#include <mrpt/math/TSegment3D.h>
#include <mrpt/math/TTwist2D.h>
#include <mrpt/math/TTwist3D.h>
#include <mrpt/math/geometry.h>  // distance()
#include <mrpt/math/homog_matrices.h>
#include <mrpt/math/ops_containers.h>  // squareNorm()
#include <mrpt/serialization/CArchive.h>  // impl of << operator
#include <mrpt/serialization/stl_serialization.h>
#include <Eigen/Dense>

using namespace std;  // For min/max, etc...
using namespace mrpt::serialization;  // CArchive, << operator for STL

using mrpt::DEG2RAD;
using mrpt::RAD2DEG;

namespace mrpt::math
{
static_assert(std::is_trivial_v<TPoint2D_data>);
static_assert(std::is_trivially_copyable_v<TPoint2D>);
static_assert(std::is_trivial_v<TPoint3D_data>);
static_assert(std::is_trivially_copyable_v<TPoint3D>);
static_assert(std::is_trivially_copyable_v<TPose2D>);
static_assert(std::is_trivially_copyable_v<TPose3D>);
static_assert(std::is_trivially_copyable_v<TPlane3D>);
static_assert(std::is_trivially_copyable_v<TLine2D>);
static_assert(std::is_trivially_copyable_v<TLine3D>);
static_assert(std::is_trivially_copyable_v<TTwist2D>);
static_assert(std::is_trivially_copyable_v<TTwist3D>);

TPoint2D::TPoint2D(const TPose2D& p) : TPoint2D_data{p.x, p.y} {}
TPoint2D::TPoint2D(const TPoint3D& p) : TPoint2D_data{p.x, p.y} {}
TPoint2D::TPoint2D(const TPose3D& p) : TPoint2D_data{p.x, p.y} {}

bool TPoint2D::operator<(const TPoint2D& p) const
{
	if (x < p.x)
		return true;
	else if (x > p.x)
		return false;
	else
		return y < p.y;
}
void TPoint2D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 2, "Wrong size of vector in ::fromString");
	x = m(0, 0);
	y = m(0, 1);
}

TPose2D::TPose2D(const TPoint2D& p) : x(p.x), y(p.y), phi(0.0) {}
TPose2D::TPose2D(const TPoint3D& p) : x(p.x), y(p.y), phi(0.0) {}
TPose2D::TPose2D(const TPose3D& p) : x(p.x), y(p.y), phi(p.yaw) {}
void TPose2D::asString(std::string& s) const
{
	s = mrpt::format("[%f %f %f]", x, y, RAD2DEG(phi));
}
void TPose2D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
	x = m(0, 0);
	y = m(0, 1);
	phi = DEG2RAD(m(0, 2));
}
mrpt::math::TPose2D mrpt::math::TPose2D::operator+(
	const mrpt::math::TPose2D& b) const
{
	const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
	// Use temporary variables for the cases (A==this) or (B==this)
	const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
	const double new_y = this->y + b.x * A_sinphi + b.y * A_cosphi;
	const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);

	return mrpt::math::TPose2D(new_x, new_y, new_phi);
}

mrpt::math::TPose2D mrpt::math::TPose2D::operator-(
	const mrpt::math::TPose2D& b) const
{
	const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);

	const double new_x =
		(this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
	const double new_y =
		-(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
	const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);

	return mrpt::math::TPose2D(new_x, new_y, new_phi);
}
TPoint2D TPose2D::composePoint(const TPoint2D l) const
{
	const double ccos = ::cos(phi), csin = ::sin(phi);
	return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
}
mrpt::math::TPoint2D TPose2D::operator+(const mrpt::math::TPoint2D& b) const
{
	return this->composePoint(b);
}

mrpt::math::TPoint2D TPose2D::inverseComposePoint(const TPoint2D g) const
{
	const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi),
				 csin = ::sin(phi);
	return {Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos};
}

// ----
void TTwist2D::asString(std::string& s) const
{
	s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
}
void TTwist2D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
	vx = m(0, 0);
	vy = m(0, 1);
	omega = DEG2RAD(m(0, 2));
}
// Transform the (vx,vy) components for a counterclockwise rotation of `ang`
// radians
void TTwist2D::rotate(const double ang)
{
	const double nvx = vx * cos(ang) - vy * sin(ang);
	const double nvy = vx * sin(ang) + vy * cos(ang);
	vx = nvx;
	vy = nvy;
}
bool TTwist2D::operator==(const TTwist2D& o) const
{
	return vx == o.vx && vy == o.vy && omega == o.omega;
}
bool TTwist2D::operator!=(const TTwist2D& o) const { return !(*this == o); }
mrpt::math::TPose2D TTwist2D::operator*(const double dt) const
{
	return mrpt::math::TPose2D(vx * dt, vy * dt, omega * dt);
}

// ----
void TTwist3D::asString(std::string& s) const
{
	s = mrpt::format(
		"[%f %f %f  %f %f %f]", vx, vy, vz, RAD2DEG(wx), RAD2DEG(wy),
		RAD2DEG(wz));
}
void TTwist3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	for (int i = 0; i < 3; i++) (*this)[i] = m(0, i);
	for (int i = 0; i < 3; i++) (*this)[3 + i] = DEG2RAD(m(0, 3 + i));
}
// Transform all 6 components for a change of reference frame from "A" to
// another frame "B" whose rotation with respect to "A" is given by `rot`. The
// translational part of the pose is ignored
void TTwist3D::rotate(const TPose3D& rot)
{
	const TTwist3D t = *this;
	mrpt::math::CMatrixDouble33 R;
	rot.getRotationMatrix(R);
	vx = R(0, 0) * t.vx + R(0, 1) * t.vy + R(0, 2) * t.vz;
	vy = R(1, 0) * t.vx + R(1, 1) * t.vy + R(1, 2) * t.vz;
	vz = R(2, 0) * t.vx + R(2, 1) * t.vy + R(2, 2) * t.vz;

	wx = R(0, 0) * t.wx + R(0, 1) * t.wy + R(0, 2) * t.wz;
	wy = R(1, 0) * t.wx + R(1, 1) * t.wy + R(1, 2) * t.wz;
	wz = R(2, 0) * t.wx + R(2, 1) * t.wy + R(2, 2) * t.wz;
}
bool TTwist3D::operator==(const TTwist3D& o) const
{
	return vx == o.vx && vy == o.vy && vz == o.vz && wx == o.wx && wy == o.wy &&
		   wz == o.wz;
}
bool TTwist3D::operator!=(const TTwist3D& o) const { return !(*this == o); }
TPoint3D::TPoint3D(const TPoint2D& p) : TPoint3D_data{p.x, p.y, 0} {}
TPoint3D::TPoint3D(const TPose2D& p) : TPoint3D_data{p.x, p.y, 0} {}
TPoint3D::TPoint3D(const TPose3D& p) : TPoint3D_data{p.x, p.y, p.z} {}

bool TPoint3D::operator<(const TPoint3D& p) const
{
	if (x < p.x)
		return true;
	else if (x > p.x)
		return false;
	else if (y < p.y)
		return true;
	else if (y > p.y)
		return false;
	else
		return z < p.z;
}
void TPoint3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
	x = m(0, 0);
	y = m(0, 1);
	z = m(0, 2);
}

TPose3D::TPose3D(const TPoint2D& p)
	: x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
{
}
TPose3D::TPose3D(const TPose2D& p)
	: x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
{
}
TPose3D::TPose3D(const TPoint3D& p)
	: x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
{
}
void TPose3D::asString(std::string& s) const
{
	s = mrpt::format(
		"[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
		RAD2DEG(roll));
}
void TPose3D::getAsQuaternion(
	mrpt::math::CQuaternion<double>& q,
	mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 3>> out_dq_dr) const
{
	// See:
	// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
	const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
	const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);

	const double ccc = cr * cp * cy;
	const double ccs = cr * cp * sy;
	const double css = cr * sp * sy;
	const double sss = sr * sp * sy;
	const double scc = sr * cp * cy;
	const double ssc = sr * sp * cy;
	const double csc = cr * sp * cy;
	const double scs = sr * cp * sy;

	q[0] = ccc + sss;
	q[1] = scc - css;
	q[2] = csc + scs;
	q[3] = ccs - ssc;

	// Compute 4x3 Jacobian: for details, see technical report:
	//   Parameterizations of SE(3) transformations: equivalences, compositions
	//   and uncertainty, J.L. Blanco (2010).
	//   https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
	if (out_dq_dr)
	{
		alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[4 * 3] = {
			-0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
			-0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
			0.5 * q[1],  0.5 * (ccc - sss),  0.5 * q[3],
			0.5 * q[0],  0.5 * (-css - scc), -0.5 * q[2]};
		out_dq_dr.value().get().loadFromArray(nums);
	}
}
void TPose3D::composePoint(const TPoint3D& l, TPoint3D& g) const
{
	CMatrixDouble33 R;
	this->getRotationMatrix(R);
	TPoint3D res;
	res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
	res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
	res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;

	g = res;
}
TPoint3D TPose3D::composePoint(const TPoint3D& l) const
{
	TPoint3D g;
	composePoint(l, g);
	return g;
}

void TPose3D::inverseComposePoint(const TPoint3D& g, TPoint3D& l) const
{
	CMatrixDouble44 H;
	this->getInverseHomogeneousMatrix(H);
	TPoint3D res;
	res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
	res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
	res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);

	l = res;
}
TPoint3D TPose3D::inverseComposePoint(const TPoint3D& g) const
{
	TPoint3D l;
	inverseComposePoint(g, l);
	return l;
}

void TPose3D::getRotationMatrix(mrpt::math::CMatrixDouble33& R) const
{
	const double cy = cos(yaw);
	const double sy = sin(yaw);
	const double cp = cos(pitch);
	const double sp = sin(pitch);
	const double cr = cos(roll);
	const double sr = sin(roll);

	alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
		const double rot_vals[] = {cy * cp,
								   cy * sp * sr - sy * cr,
								   cy * sp * cr + sy * sr,
								   sy * cp,
								   sy * sp * sr + cy * cr,
								   sy * sp * cr - cy * sr,
								   -sp,
								   cp * sr,
								   cp * cr};
	R.loadFromArray(rot_vals);
}
void TPose3D::SO3_to_yaw_pitch_roll(
	const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
	double& roll)
{
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());

	// Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
	pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));

	// Roll:
	if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
		10 * std::numeric_limits<double>::epsilon())
	{
		// Gimbal lock between yaw and roll. This one is arbitrarily forced to
		// be zero.
		// Check
		// https://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
		// If cos(pitch)==0, the homogeneous matrix is:
		// When sin(pitch)==1:
		//  /0  cysr-sycr cycr+sysr x\   /0  sin(r-y) cos(r-y)  x\.
		//  |0  sysr+cycr sycr-cysr y| = |0  cos(r-y) -sin(r-y) y|
		//  |-1     0         0     z|   |-1    0         0     z|
		//  \0      0         0     1/   \0     0         0     1/
		//
		// And when sin(pitch)=-1:
		//  /0 -cysr-sycr -cycr+sysr x\   /0 -sin(r+y) -cos(r+y) x\.
		//  |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y)  -sin(r+y) y|
		//  |1      0          0     z|   |1    0          0     z|
		//  \0      0          0     1/   \0    0          0     1/
		//
		// Both cases are in a "gimbal lock" status. This happens because pitch
		// is vertical.

		roll = 0.0;
		if (pitch > 0)
			yaw = atan2(R(1, 2), R(0, 2));
		else
			yaw = atan2(-R(1, 2), -R(0, 2));
	}
	else
	{
		roll = atan2(R(2, 1), R(2, 2));
		// Yaw:
		yaw = atan2(R(1, 0), R(0, 0));
	}
}

void TPose3D::fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44& HG)
{
	SO3_to_yaw_pitch_roll(
		CMatrixDouble33(HG.blockCopy<3, 3>(0, 0)), yaw, pitch, roll);
	x = HG(0, 3);
	y = HG(1, 3);
	z = HG(2, 3);
}
void TPose3D::composePose(const TPose3D other, TPose3D& result) const
{
	CMatrixDouble44 me_H, o_H;
	this->getHomogeneousMatrix(me_H);
	other.getHomogeneousMatrix(o_H);
	result.fromHomogeneousMatrix(
		CMatrixDouble44(me_H.asEigen() * o_H.asEigen()));
}
void TPose3D::getHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const
{
	CMatrixDouble33 R;
	getRotationMatrix(R);
	HG.block<3, 3>(0, 0) = R.asEigen();
	HG(0, 3) = x;
	HG(1, 3) = y;
	HG(2, 3) = z;
	HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
	HG(3, 3) = 1.;
}
void TPose3D::getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const
{  // Get current HM & inverse in-place:
	this->getHomogeneousMatrix(HG);
	mrpt::math::homogeneousMatrixInverse(HG);
}

void TPose3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	x = m(0, 0);
	y = m(0, 1);
	z = m(0, 2);
	yaw = DEG2RAD(m(0, 3));
	pitch = DEG2RAD(m(0, 4));
	roll = DEG2RAD(m(0, 5));
}

void TPose3DQuat::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 7, "Wrong size of vector in ::fromString");
	for (int i = 0; i < m.cols(); i++) (*this)[i] = m(0, i);
}
TPose3D operator-(const TPose3D& p)
{
	CMatrixDouble44 H;
	p.getInverseHomogeneousMatrix(H);
	TPose3D ret;
	ret.fromHomogeneousMatrix(H);
	return ret;
}
TPose3D operator-(const TPose3D& b, const TPose3D& a)
{
	// b - a = A^{-1} * B
	CMatrixDouble44 Hainv, Hb;
	a.getInverseHomogeneousMatrix(Hainv);
	b.getHomogeneousMatrix(Hb);
	TPose3D ret;
	ret.fromHomogeneousMatrix(CMatrixDouble44(Hainv.asEigen() * Hb.asEigen()));
	return ret;
}

// Text streaming:
std::ostream& operator<<(std::ostream& o, const TPoint2D& p)
{
	return (o << p.asString());
}
std::ostream& operator<<(std::ostream& o, const TPoint3D& p)
{
	return (o << p.asString());
}
std::ostream& operator<<(std::ostream& o, const TPose2D& p)
{
	return (o << p.asString());
}
std::ostream& operator<<(std::ostream& o, const TPose3D& p)
{
	return (o << p.asString());
}
std::ostream& operator<<(std::ostream& o, const TPose3DQuat& p)
{
	return (o << p.asString());
}

CArchive& operator>>(CArchive& in, mrpt::math::TSegment2D& s)
{
	return in >> s.point1 >> s.point2;
}
CArchive& operator<<(CArchive& out, const mrpt::math::TSegment2D& s)
{
	return out << s.point1 << s.point2;
}
CArchive& operator>>(CArchive& in, mrpt::math::TLine2D& l)
{
	return in >> l.coefs[0] >> l.coefs[1] >> l.coefs[2];
}
CArchive& operator<<(CArchive& out, const mrpt::math::TLine2D& l)
{
	return out << l.coefs[0] << l.coefs[1] << l.coefs[2];
}

CArchive& operator>>(CArchive& in, mrpt::math::TSegment3D& s)
{
	return in >> s.point1 >> s.point2;
}
CArchive& operator<<(CArchive& out, const mrpt::math::TSegment3D& s)
{
	return out << s.point1 << s.point2;
}
CArchive& operator>>(CArchive& in, mrpt::math::TLine3D& l)
{
	return in >> l.pBase >> l.director[0] >> l.director[1] >> l.director[2];
}
CArchive& operator<<(CArchive& out, const mrpt::math::TLine3D& l)
{
	return out << l.pBase << l.director[0] << l.director[1] << l.director[2];
}
CArchive& operator>>(CArchive& in, mrpt::math::TPlane& p)
{
	return in >> p.coefs[0] >> p.coefs[1] >> p.coefs[2] >> p.coefs[3];
}
CArchive& operator<<(CArchive& out, const mrpt::math::TPlane& p)
{
	return out << p.coefs[0] << p.coefs[1] << p.coefs[2] << p.coefs[3];
}

double TSegment2D::length() const { return math::distance(point1, point2); }
double TSegment2D::distance(const TPoint2D& point) const
{
	return std::abs(signedDistance(point));
}
double TSegment2D::signedDistance(const TPoint2D& point) const
{
	// It is reckoned whether the perpendicular line to the TSegment2D which
	// passes through point crosses or not the referred segment,
	// or what is the same, whether point makes an obtuse triangle with the
	// segment or not (being the longest segment one between the point and
	// either end of TSegment2D).
	const double d1 = math::distance(point, point1);
	if (point1 == point2) return d1;

	const double d2 = math::distance(point, point2);
	const double d3 = length();
	const double ds1 = square(d1);
	const double ds2 = square(d2);
	const double ds3 = square(d3);
	if (ds1 > (ds2 + ds3) || ds2 > (ds1 + ds3))
		// Fix sign:
		return min(d1, d2) *
			   (TLine2D(*this).signedDistance(point) < 0 ? -1 : 1);
	else
		return TLine2D(*this).signedDistance(point);
}
bool TSegment2D::contains(const TPoint2D& point) const
{
	return abs(math::distance(point1, point) + math::distance(point2, point) -
			   math::distance(point1, point2)) < getEpsilon();
}
void TSegment2D::generate3DObject(TSegment3D& s) const
{
	s = TSegment3D(*this);
}
TSegment2D::TSegment2D(const TSegment3D& s)
{
	point1 = TPoint2D(s.point1);
	point2 = TPoint2D(s.point2);
	if (point1 == point2)
		throw std::logic_error("Segment is normal to projection plane");
}

bool TSegment2D::operator<(const TSegment2D& s) const
{
	if (point1 < s.point1)
		return true;
	else if (s.point1 < point1)
		return false;
	else
		return point2 < s.point2;
}

void TSegment3D::generate2DObject(TSegment2D& s) const
{
	s = TSegment2D(*this);
}

double TSegment3D::length() const { return math::distance(point1, point2); }
double TSegment3D::distance(const TPoint3D& point) const
{
	return min(
		min(math::distance(point, point1), math::distance(point, point2)),
		TLine3D(*this).distance(point));
}
double TSegment3D::distance(const TSegment3D& segment) const
{
	Eigen::Vector3d u, v, w;
	TPoint3D diff_vect = point2 - point1;
	diff_vect.asVector(u);
	diff_vect = segment.point2 - segment.point1;
	diff_vect.asVector(v);
	diff_vect = point1 - segment.point1;
	diff_vect.asVector(w);
	double a = u.dot(u);  // always >= 0
	double b = u.dot(v);
	double c = v.dot(v);  // always >= 0
	double d = u.dot(w);
	double e = v.dot(w);
	double D = a * c - b * b;  // always >= 0
	double sc, sN, sD = D;  // sc = sN / sD, default sD = D >= 0
	double tc, tN, tD = D;  // tc = tN / tD, default tD = D >= 0

	// compute the line parameters of the two closest points
	if (D < 0.00000001)
	{  // the lines are almost parallel
		sN = 0.0;  // force using point P0 on segment S1
		sD = 1.0;  // to prevent possible division by 0.0 later
		tN = e;
		tD = c;
	}
	else
	{  // get the closest points on the infinite lines
		sN = (b * e - c * d);
		tN = (a * e - b * d);
		if (sN < 0.0)
		{  // sc < 0 => the s=0 edge is visible
			sN = 0.0;
			tN = e;
			tD = c;
		}
		else if (sN > sD)
		{  // sc > 1 => the s=1 edge is visible
			sN = sD;
			tN = e + b;
			tD = c;
		}
	}

	if (tN < 0.0)
	{  // tc < 0 => the t=0 edge is visible
		tN = 0.0;
		// recompute sc for this edge
		if (-d < 0.0)
			sN = 0.0;
		else if (-d > a)
			sN = sD;
		else
		{
			sN = -d;
			sD = a;
		}
	}
	else if (tN > tD)
	{  // tc > 1 => the t=1 edge is visible
		tN = tD;
		// recompute sc for this edge
		if ((-d + b) < 0.0)
			sN = 0;
		else if ((-d + b) > a)
			sN = sD;
		else
		{
			sN = (-d + b);
			sD = a;
		}
	}
	// finally do the division to get sc and tc
	sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
	tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);

	// get the difference of the two closest points
	const auto dP = (w + (sc * u) - (tc * v)).eval();
	return dP.norm();  // return the closest distance
}
bool TSegment3D::contains(const TPoint3D& point) const
{
	// Not very intuitive, but very fast, method.
	return abs(math::distance(point1, point) + math::distance(point2, point) -
			   math::distance(point1, point2)) < getEpsilon();
}

bool TSegment3D::operator<(const TSegment3D& s) const
{
	if (point1 < s.point1)
		return true;
	else if (s.point1 < point1)
		return false;
	else
		return point2 < s.point2;
}

double TLine2D::evaluatePoint(const TPoint2D& point) const
{
	return coefs[0] * point.x + coefs[1] * point.y + coefs[2];
}
bool TLine2D::contains(const TPoint2D& point) const
{
	return abs(distance(point)) < getEpsilon();
}
double TLine2D::distance(const TPoint2D& point) const
{
	return abs(evaluatePoint(point)) /
		   sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
}
double TLine2D::signedDistance(const TPoint2D& point) const
{
	return evaluatePoint(point) /
		   sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
}
void TLine2D::getNormalVector(double (&vector)[2]) const
{
	vector[0] = coefs[0];
	vector[1] = coefs[1];
}
void TLine2D::unitarize()
{
	double s = sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
	for (double& coef : coefs) coef /= s;
}
void TLine2D::getDirectorVector(double (&vector)[2]) const
{
	vector[0] = -coefs[1];
	vector[1] = coefs[0];
}
void TLine2D::generate3DObject(TLine3D& l) const { l = TLine3D(*this); }
void TLine2D::getAsPose2D(TPose2D& outPose) const
{
	// Line's director vector is (-coefs[1],coefs[0]).
	// If line is horizontal, force x=0. Else, force y=0. In both cases, we'll
	// find a suitable point.
	outPose.phi = atan2(coefs[0], -coefs[1]);
	if (abs(coefs[0]) < getEpsilon())
	{
		outPose.x = 0;
		outPose.y = -coefs[2] / coefs[1];
	}
	else
	{
		outPose.x = -coefs[2] / coefs[0];
		outPose.y = 0;
	}
}
void TLine2D::getAsPose2DForcingOrigin(
	const TPoint2D& origin, TPose2D& outPose) const
{
	if (!contains(origin))
		throw std::logic_error("Base point is not contained in the line");
	outPose = origin;
	// Line's director vector is (-coefs[1],coefs[0]).
	outPose.phi = atan2(coefs[0], -coefs[1]);
}
TLine2D::TLine2D(const TPoint2D& p1, const TPoint2D& p2)
{
	if (p1 == p2) throw logic_error("Both points are the same");
	coefs[0] = p2.y - p1.y;
	coefs[1] = p1.x - p2.x;
	coefs[2] = p2.x * p1.y - p2.y * p1.x;
}
TLine2D::TLine2D(const TSegment2D& s)
{
	coefs[0] = s.point2.y - s.point1.y;
	coefs[1] = s.point1.x - s.point2.x;
	coefs[2] = s.point2.x * s.point1.y - s.point2.y * s.point1.x;
	// unitarize();	//¿?
}
TLine2D::TLine2D(const TLine3D& l)
{
	// Line's projection to Z plane may be a point.
	if (hypot(l.director[0], l.director[1]) < getEpsilon())
		throw std::logic_error("Line is normal to projection plane");
	coefs[0] = -l.director[1];
	coefs[1] = l.director[0];
	coefs[2] = l.pBase.x * l.director[1] - l.pBase.y * l.director[0];
}

void TLine3D::generate2DObject(TLine2D& l) const { l = TLine2D(*this); }

bool TLine3D::contains(const TPoint3D& point) const
{
	double dx = point.x - pBase.x;
	double dy = point.y - pBase.y;
	double dz = point.z - pBase.z;
	if (abs(dx) < getEpsilon() && abs(dy) < getEpsilon() &&
		abs(dz) < getEpsilon())
		return true;
	//       dx          dy          dz
	// if -----------=-----------=-----------, point is inside the line.
	//   director[0] director[1] director[2]
	return (abs(dx * director[1] - dy * director[0]) < getEpsilon()) &&
		   (abs(dx * director[2] - dz * director[0]) < getEpsilon()) &&
		   (abs(dy * director[2] - dz * director[1]) < getEpsilon());
}
double TLine3D::distance(const TPoint3D& point) const
{
	// Let d be line's base point minus the argument. Then,
	// d·director/(|d|·|director|) equals both vector's cosine.
	// So, d·director/|director| equals d's projection over director. Then,
	// distance is sqrt(|d|²-(d·director/|director|)²).
	double d[3] = {point.x - pBase.x, point.y - pBase.y, point.z - pBase.z};
	double dv = 0, d2 = 0, v2 = 0;
	for (size_t i = 0; i < 3; i++)
	{
		dv += d[i] * director[i];
		d2 += d[i] * d[i];
		v2 += director[i] * director[i];
	}
	return sqrt(d2 - (dv * dv) / v2);
}
void TLine3D::unitarize()
{
	double s = sqrt(squareNorm<3, double>(director));
	for (double& i : director) i /= s;
}
TLine3D::TLine3D(const TPoint3D& p1, const TPoint3D& p2)
{
	if (abs(math::distance(p1, p2)) < getEpsilon())
		throw logic_error("Both points are the same");
	pBase = p1;
	director[0] = p2.x - p1.x;
	director[1] = p2.y - p1.y;
	director[2] = p2.z - p1.z;
}
TLine3D::TLine3D(const TSegment3D& s)
{
	pBase = s.point1;
	director[0] = s.point2.x - s.point1.x;
	director[1] = s.point2.y - s.point1.y;
	director[2] = s.point2.z - s.point1.z;
}
TLine3D::TLine3D(const TLine2D& l)
{
	director[0] = -l.coefs[1];
	director[1] = l.coefs[0];
	director[2] = 0;
	// We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively,
	// either y or x can be used as free cordinate.
	if (abs(l.coefs[0]) >= getEpsilon())
	{
		pBase.x = -l.coefs[2] / l.coefs[0];
		pBase.y = 0;
	}
	else
	{
		pBase.x = 0;
		pBase.y = -l.coefs[1] / l.coefs[0];
	}
	pBase.z = 0;
}

double TPlane::evaluatePoint(const TPoint3D& point) const
{
	return dotProduct<3, double>(coefs, point) + coefs[3];
}
bool TPlane::contains(const TPoint3D& point) const
{
	return distance(point) < getEpsilon();
}
bool TPlane::contains(const TLine3D& line) const
{
	if (!contains(line.pBase)) return false;  // Base point must be contained
	return abs(getAngle(*this, line)) <
		   getEpsilon();  // Plane's normal must be normal to director vector
}
double TPlane::distance(const TPoint3D& point) const
{
	return abs(evaluatePoint(point)) / sqrt(squareNorm<3, double>(coefs));
}
double TPlane::distance(const TLine3D& line) const
{
	if (abs(getAngle(*this, line)) >= getEpsilon())
		return 0;  // Plane crosses with line
	else
		return distance(line.pBase);
}
void TPlane::getNormalVector(double (&vector)[3]) const
{
	for (int i = 0; i < 3; i++) vector[i] = coefs[i];
}
TVector3D TPlane::getNormalVector() const
{
	TVector3D v;
	for (int i = 0; i < 3; i++) v[i] = coefs[i];
	return v;
}

void TPlane::getUnitaryNormalVector(double (&vec)[3]) const
{
	const double s = sqrt(squareNorm<3, double>(coefs));
	ASSERT_ABOVE_(s, getEpsilon());
	const double k = 1.0 / s;
	for (int i = 0; i < 3; i++) vec[i] = coefs[i] * k;
}

void TPlane::unitarize()
{
	double s = sqrt(squareNorm<3, double>(coefs));
	for (double& coef : coefs) coef /= s;
}

// Returns a 6D pose such as its XY plane coincides with the plane
void TPlane::getAsPose3D(mrpt::math::TPose3D& outPose) const
{
	double normal[3];
	getUnitaryNormalVector(normal);
	CMatrixDouble44 AXIS;
	generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
	for (size_t i = 0; i < 3; i++)
		if (abs(coefs[i]) >= getEpsilon())
		{
			AXIS(i, 3) = -coefs[3] / coefs[i];
			break;
		}
	outPose.fromHomogeneousMatrix(AXIS);
}
void TPlane::getAsPose3DForcingOrigin(
	const TPoint3D& center, TPose3D& pose) const
{
	if (!contains(center))
		throw std::logic_error("Base point is not in the plane.");
	double normal[3];
	getUnitaryNormalVector(normal);
	CMatrixDouble44 AXIS;
	generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
	for (size_t i = 0; i < 3; i++) AXIS(i, 3) = center[i];
	pose.fromHomogeneousMatrix(AXIS);
}
TPlane::TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3)
{
	double dx1 = p2.x - p1.x;
	double dy1 = p2.y - p1.y;
	double dz1 = p2.z - p1.z;
	double dx2 = p3.x - p1.x;
	double dy2 = p3.y - p1.y;
	double dz2 = p3.z - p1.z;
	coefs[0] = dy1 * dz2 - dy2 * dz1;
	coefs[1] = dz1 * dx2 - dz2 * dx1;
	coefs[2] = dx1 * dy2 - dx2 * dy1;
	if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
		abs(coefs[2]) < getEpsilon())
		throw logic_error("Points are linearly dependent");
	coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
}
TPlane::TPlane(const TPoint3D& p1, const TLine3D& r2)
{
	double dx1 = p1.x - r2.pBase.x;
	double dy1 = p1.y - r2.pBase.y;
	double dz1 = p1.z - r2.pBase.z;
	coefs[0] = dy1 * r2.director[2] - dz1 * r2.director[1];
	coefs[1] = dz1 * r2.director[0] - dx1 * r2.director[2];
	coefs[2] = dx1 * r2.director[1] - dy1 * r2.director[0];
	if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
		abs(coefs[2]) < getEpsilon())
		throw logic_error("Point is contained in the line");
	coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
}
TPlane::TPlane(const TPoint3D& p1, const TVector3D& normal)
{
	const double normal_norm = normal.norm();
	ASSERT_ABOVE_(normal_norm, getEpsilon());

	// Ensure we have a unit vector:
	const auto n = normal * (1. / normal_norm);
	coefs[0] = n.x;
	coefs[1] = n.y;
	coefs[2] = n.z;
	coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
}
TPlane::TPlane(const TLine3D& r1, const TLine3D& r2)
{
	crossProduct3D(r1.director, r2.director, coefs);
	coefs[3] =
		-coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y - coefs[2] * r1.pBase.z;
	if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
		abs(coefs[2]) < getEpsilon())
	{
		// Lines are parallel
		if (r1.contains(r2.pBase)) throw std::logic_error("Lines are the same");
		// Use a line's director vector and both pBase's difference to create
		// the plane.
		double d[3];
		for (size_t i = 0; i < 3; i++) d[i] = r1.pBase[i] - r2.pBase[i];
		crossProduct3D(r1.director, d, coefs);
		coefs[3] = -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y -
				   coefs[2] * r1.pBase.z;
	}
	else if (abs(evaluatePoint(r2.pBase)) >= getEpsilon())
		throw logic_error("Lines do not intersect");
}

template <class T>
inline void removeUnusedVertices(T& poly)
{
	size_t N = poly.size();
	if (N < 3) return;
	std::vector<size_t> unused;
	if (abs(mrpt::math::distance(poly[N - 1], poly[0]) +
			mrpt::math::distance(poly[0], poly[1]) -
			mrpt::math::distance(poly[N - 1], poly[1])) <
		mrpt::math::getEpsilon())
		unused.push_back(0);
	for (size_t i = 1; i < N - 1; i++)
		if (abs(mrpt::math::distance(poly[i - 1], poly[i]) +
				mrpt::math::distance(poly[i], poly[i + 1]) -
				mrpt::math::distance(poly[i - 1], poly[i + 1])) <
			mrpt::math::getEpsilon())
			unused.push_back(i);
	if (abs(mrpt::math::distance(poly[N - 2], poly[N - 1]) +
			mrpt::math::distance(poly[N - 1], poly[0]) -
			mrpt::math::distance(poly[N - 2], poly[0])) <
		mrpt::math::getEpsilon())
		unused.push_back(N - 1);
	unused.push_back(N);
	size_t diff = 1;
	for (size_t i = 0; i < unused.size() - 1; i++)
	{
		size_t last = unused[i + 1];
		for (size_t j = unused[i] + 1 - diff; j < last - diff; j++)
			poly[j] = poly[j + diff];
	}
	poly.resize(N + 1 - unused.size());
}
template <class T>
inline void removeRepVertices(T& poly)
{
	size_t N = poly.size();
	if (N < 3) return;
	std::vector<size_t> rep;
	for (size_t i = 0; i < N - 1; i++)
		if (mrpt::math::distance(poly[i], poly[i + 1]) < getEpsilon())
			rep.push_back(i);
	if (mrpt::math::distance(poly[N - 1], poly[0]) < getEpsilon())
		rep.push_back(N - 1);
	rep.push_back(N);
	size_t diff = 1;
	for (size_t i = 0; i < rep.size() - 1; i++)
	{
		size_t last = rep[i + 1];
		for (size_t j = rep[i] + 1 - diff; j < last - diff; j++)
			poly[j] = poly[j + diff];
	}
	poly.resize(N + 1 - rep.size());
}

double TPolygon2D::distance(const TPoint2D& point) const
{
	if (contains(point)) return 0;
	std::vector<TSegment2D> sgs;
	getAsSegmentList(sgs);

	if (sgs.empty())
		THROW_EXCEPTION("Cannot compute distance to an empty polygon.");

	double distance = std::numeric_limits<double>::max();

	for (auto it = sgs.begin(); it != sgs.end(); ++it)
	{
		double d = (*it).distance(point);
		if (d < distance) distance = d;
	}
	return distance;
}

void TPolygon2D::getBoundingBox(
	TPoint2D& min_coords, TPoint2D& max_coords) const
{
	ASSERTMSG_(!this->empty(), "getBoundingBox() called on an empty polygon!");
	min_coords.x = min_coords.y = std::numeric_limits<double>::max();
	max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
	for (size_t i = 0; i < size(); i++)
	{
		mrpt::keep_min(min_coords.x, (*this)[i].x);
		mrpt::keep_min(min_coords.y, (*this)[i].y);
		mrpt::keep_max(max_coords.x, (*this)[i].x);
		mrpt::keep_max(max_coords.y, (*this)[i].y);
	}
}

// isLeft(): tests if a point is Left|On|Right of an infinite line.
//    Input:  three points P0, P1, and P2
//    Return: >0 for P2 left of the line through P0 and P1
//            =0 for P2  on the line
//            <0 for P2  right of the line
//    See: Algorithm 1 "Area of Triangles and Polygons"
inline double isLeft(
	const mrpt::math::TPoint2D& P0, const mrpt::math::TPoint2D& P1,
	const mrpt::math::TPoint2D& P2)
{
	return ((P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y));
}

bool TPolygon2D::contains(const TPoint2D& P) const
{
	int wn = 0;  // the  winding number counter

	// loop through all edges of the polygon
	const size_t n = this->size();
	for (size_t i = 0; i < n; i++)  // edge from V[i] to  V[i+1]
	{
		if ((*this)[i].y <= P.y)
		{
			// start y <= P.y
			if ((*this)[(i + 1) % n].y > P.y)  // an upward crossing
				if (isLeft((*this)[i], (*this)[(i + 1) % n], P) >
					0)  // P left of  edge
					++wn;  // have  a valid up intersect
		}
		else
		{
			// start y > P.y (no test needed)
			if ((*this)[(i + 1) % n].y <= P.y)  // a downward crossing
				if (isLeft((*this)[i], (*this)[(i + 1) % n], P) <
					0)  // P right of  edge
					--wn;  // have  a valid down intersect
		}
	}

	return wn != 0;
}
void TPolygon2D::getAsSegmentList(vector<TSegment2D>& v) const
{
	size_t N = size();
	v.resize(N);
	for (size_t i = 0; i < N - 1; i++)
		v[i] = TSegment2D(operator[](i), operator[](i + 1));
	v[N - 1] = TSegment2D(operator[](N - 1), operator[](0));
}

void TPolygon2D::generate3DObject(TPolygon3D& p) const
{
	p = TPolygon3D(*this);
}
// Auxiliar functor class to compute polygon's center
template <class T, int N>
class FAddPoint
{
   public:
	T& object;
	FAddPoint(T& o) : object(o)
	{
		for (size_t i = 0; i < N; i++) object[i] = 0.0;
	}
	void operator()(const T& o)
	{
		for (size_t i = 0; i < N; i++) object[i] += o[i];
	}
};
void TPolygon2D::getCenter(TPoint2D& p) const
{
	for_each(begin(), end(), FAddPoint<TPoint2D, 2>(p));
	size_t N = size();
	p.x /= N;
	p.y /= N;
}
bool TPolygon2D::isConvex() const
{
	size_t N = size();
	if (N <= 3) return true;
	vector<TSegment2D> sgms;
	getAsSegmentList(sgms);
	for (size_t i = 0; i < N; i++)
	{
		char s = 0;
		auto l = TLine2D(sgms[i]);
		for (size_t j = 0; j < N; j++)
		{
			double d = l.evaluatePoint(operator[](j));
			if (abs(d) < getEpsilon())
				continue;
			else if (!s)
				s = (d > 0) ? 1 : -1;
			else if (s != ((d > 0) ? 1 : -1))
				return false;
		}
	}
	return true;
}
void TPolygon2D::removeRepeatedVertices() { removeRepVertices(*this); }
void TPolygon2D::removeRedundantVertices()
{
	removeRepeatedVertices();
	removeUnusedVertices(*this);
}
void TPolygon2D::getPlotData(
	std::vector<double>& x, std::vector<double>& y) const
{
	size_t N = size();
	x.resize(N + 1);
	y.resize(N + 1);
	for (size_t i = 0; i < N; i++)
	{
		x[i] = operator[](i).x;
		y[i] = operator[](i).y;
	}
	x[N] = operator[](0).x;
	y[N] = operator[](0).y;
}
TPolygon2D::TPolygon2D(const TPolygon3D& p) : std::vector<TPoint2D>()
{
	size_t N = p.size();
	resize(N);
	for (size_t i = 0; i < N; i++) operator[](i) = TPoint2D(p[i]);
}
void TPolygon2D::createRegularPolygon(
	size_t numEdges, double radius, TPolygon2D& poly)
{
	if (numEdges < 3 || abs(radius) < getEpsilon())
		throw std::logic_error(
			"Invalid arguments for regular polygon creations");
	poly.resize(numEdges);
	for (size_t i = 0; i < numEdges; i++)
	{
		double angle = i * M_PI * 2 / numEdges;
		poly[i] = TPoint2D(radius * cos(angle), radius * sin(angle));
	}
}
inline void TPolygon2D::createRegularPolygon(
	size_t numEdges, double radius, TPolygon2D& poly, const TPose2D& pose)
{
	createRegularPolygon(numEdges, radius, poly);
	for (size_t i = 0; i < numEdges; i++) poly[i] = pose.composePoint(poly[i]);
}

void TPolygon3D::generate2DObject(TPolygon2D& p) const
{
	p = TPolygon2D(*this);
}

double TPolygon3D::distance(const TPoint3D& point) const
{
	TPlane pl;
	if (!getPlane(pl))
		throw std::logic_error("Polygon does not conform a plane");
	TPoint3D newPoint;
	TPolygon3D newPoly;
	TPose3D pose;
	pl.getAsPose3DForcingOrigin(operator[](0), pose);
	project3D(point, pose, newPoint);
	project3D(*this, pose, newPoly);
	double distance2D = TPolygon2D(newPoly).distance(TPoint2D(newPoint));
	return sqrt(newPoint.z * newPoint.z + distance2D * distance2D);
}
bool TPolygon3D::contains(const TPoint3D& point) const
{
	TPoint3D pMin, pMax;
	getPrismBounds(*this, pMin, pMax);
	if (point.x + getEpsilon() < pMin.x || point.y + getEpsilon() < pMin.y ||
		point.z + getEpsilon() < pMin.z || point.x > pMax.x + getEpsilon() ||
		point.y > pMax.y + getEpsilon() || point.z > pMax.z + getEpsilon())
		return false;
	TPlane plane;
	if (!getPlane(plane))
		throw std::logic_error("Polygon does not conform a plane");
	TPolygon3D projectedPoly;
	TPoint3D projectedPoint;
	TPose3D pose;
	// plane.getAsPose3DForcingOrigin(operator[](0),pose);
	plane.getAsPose3D(pose);
	CMatrixDouble44 P_inv;
	pose.getInverseHomogeneousMatrix(P_inv);
	pose.fromHomogeneousMatrix(P_inv);
	project3D(point, pose, projectedPoint);
	if (abs(projectedPoint.z) >= getEpsilon())
		return false;  // Point is not inside the polygon's plane.
	project3D(*this, pose, projectedPoly);
	return TPolygon2D(projectedPoly).contains(TPoint2D(projectedPoint));
}
void TPolygon3D::getAsSegmentList(vector<TSegment3D>& v) const
{
	size_t N = size();
	v.resize(N);
	for (size_t i = 0; i < N - 1; i++)
		v[i] = TSegment3D(operator[](i), operator[](i + 1));
	v[N - 1] = TSegment3D(operator[](N - 1), operator[](0));
}
bool TPolygon3D::getPlane(TPlane& p) const { return conformAPlane(*this, p); }
void TPolygon3D::getBestFittingPlane(TPlane& p) const
{
	getRegressionPlane(*this, p);
}
void TPolygon3D::getCenter(TPoint3D& p) const
{
	for_each(begin(), end(), FAddPoint<TPoint3D, 3>(p));
	size_t N = size();
	p.x /= N;
	p.y /= N;
	p.z /= N;
}
bool TPolygon3D::isSkew() const { return !mrpt::math::conformAPlane(*this); }
void TPolygon3D::removeRepeatedVertices() { removeRepVertices(*this); }
void TPolygon3D::removeRedundantVertices()
{
	removeRepeatedVertices();
	removeUnusedVertices(*this);
}
TPolygon3D::TPolygon3D(const TPolygon2D& p) : std::vector<TPoint3D>()
{
	size_t N = p.size();
	resize(N);
	for (size_t i = 0; i < N; i++) operator[](i) = p[i];
}
void TPolygon3D::createRegularPolygon(
	size_t numEdges, double radius, TPolygon3D& poly)
{
	if (numEdges < 3 || abs(radius) < getEpsilon())
		throw std::logic_error(
			"Invalid arguments for regular polygon creations");
	poly.resize(numEdges);
	for (size_t i = 0; i < numEdges; i++)
	{
		double angle = i * 2 * M_PI / numEdges;
		poly[i] = TPoint3D(radius * cos(angle), radius * sin(angle), 0);
	}
}
inline void TPolygon3D::createRegularPolygon(
	size_t numEdges, double radius, TPolygon3D& poly, const TPose3D& pose)
{
	createRegularPolygon(numEdges, radius, poly);
	for (size_t i = 0; i < numEdges; i++) pose.composePoint(poly[i], poly[i]);
}

void TObject2D::generate3DObject(TObject3D& obj) const
{
	switch (type)
	{
		case GEOMETRIC_TYPE_POINT:
			obj = TPoint3D(data.point);
			break;
		case GEOMETRIC_TYPE_SEGMENT:
			obj = TSegment3D(data.segment);
			break;
		case GEOMETRIC_TYPE_LINE:
			obj = TLine3D(data.line);
			break;
		case GEOMETRIC_TYPE_POLYGON:
			obj = TPolygon3D(*(data.polygon));
			break;
		default:
			obj = TObject3D();
			break;
	}
}
void TObject2D::getPoints(
	const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts)
{
	for (const auto& obj : objs)
		if (obj.isPoint()) pnts.push_back(obj.data.point);
}
void TObject2D::getSegments(
	const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms)
{
	for (const auto& obj : objs)
		if (obj.isSegment()) sgms.push_back(obj.data.segment);
}
void TObject2D::getLines(
	const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins)
{
	for (const auto& obj : objs)
		if (obj.isLine()) lins.push_back(obj.data.line);
}
void TObject2D::getPolygons(
	const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys)
{
	for (const auto& obj : objs)
		if (obj.isPolygon()) polys.push_back(*(obj.data.polygon));
}
void TObject2D::getPoints(
	const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts,
	std::vector<TObject2D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isPoint())
			pnts.push_back(obj.data.point);
		else
			remainder.push_back(obj);
}
void TObject2D::getSegments(
	const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms,
	std::vector<TObject2D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isSegment())
			sgms.push_back(obj.data.segment);
		else
			remainder.push_back(obj);
}
void TObject2D::getLines(
	const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins,
	std::vector<TObject2D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isLine())
			lins.push_back(obj.data.line);
		else
			remainder.push_back(obj);
}
void TObject2D::getPolygons(
	const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys,
	vector<TObject2D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isPolygon())
			polys.push_back(*(obj.data.polygon));
		else
			remainder.push_back(obj);
}

void TObject3D::generate2DObject(TObject2D& obj) const
{
	switch (type)
	{
		case GEOMETRIC_TYPE_POINT:
			obj = TPoint2D(data.point);
			break;
		case GEOMETRIC_TYPE_SEGMENT:
			obj = TSegment2D(data.segment);
			break;
		case GEOMETRIC_TYPE_LINE:
			obj = TLine2D(data.line);
			break;
		case GEOMETRIC_TYPE_POLYGON:
			obj = TPolygon2D(*(data.polygon));
			break;
		case GEOMETRIC_TYPE_PLANE:
			throw std::logic_error("Too many dimensions");
		default:
			obj = TObject2D();
			break;
	}
}

void TObject3D::getPoints(
	const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts)
{
	for (const auto& obj : objs)
		if (obj.isPoint()) pnts.push_back(obj.data.point);
}
void TObject3D::getSegments(
	const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms)
{
	for (const auto& obj : objs)
		if (obj.isSegment()) sgms.push_back(obj.data.segment);
}
void TObject3D::getLines(
	const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins)
{
	for (const auto& obj : objs)
		if (obj.isLine()) lins.push_back(obj.data.line);
}
void TObject3D::getPlanes(
	const std::vector<TObject3D>& objs, std::vector<TPlane>& plns)
{
	for (const auto& obj : objs)
		if (obj.isPlane()) plns.push_back(obj.data.plane);
}
void TObject3D::getPolygons(
	const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
{
	for (const auto& obj : objs)
		if (obj.isPolygon()) polys.push_back(*(obj.data.polygon));
}
void TObject3D::getPoints(
	const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts,
	std::vector<TObject3D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isPoint())
			pnts.push_back(obj.data.point);
		else
			remainder.push_back(obj);
}
void TObject3D::getSegments(
	const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms,
	std::vector<TObject3D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isSegment())
			sgms.push_back(obj.data.segment);
		else
			remainder.push_back(obj);
}
void TObject3D::getLines(
	const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins,
	std::vector<TObject3D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isLine())
			lins.push_back(obj.data.line);
		else
			remainder.push_back(obj);
}
void TObject3D::getPlanes(
	const std::vector<TObject3D>& objs, std::vector<TPlane>& plns,
	std::vector<TObject3D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isPlane())
			plns.push_back(obj.data.plane);
		else
			remainder.push_back(obj);
}
void TObject3D::getPolygons(
	const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
	vector<TObject3D>& remainder)
{
	for (const auto& obj : objs)
		if (obj.isPolygon())
			polys.push_back(*(obj.data.polygon));
		else
			remainder.push_back(obj);
}

CArchive& operator>>(CArchive& in, mrpt::math::TTwist2D& o)
{
	for (size_t i = 0; i < o.size(); i++) in >> o[i];
	return in;
}
CArchive& operator<<(CArchive& out, const mrpt::math::TTwist2D& o)
{
	for (size_t i = 0; i < o.size(); i++) out << o[i];
	return out;
}

CArchive& operator>>(CArchive& in, mrpt::math::TTwist3D& o)
{
	for (size_t i = 0; i < o.size(); i++) in >> o[i];
	return in;
}
CArchive& operator<<(CArchive& out, const mrpt::math::TTwist3D& o)
{
	for (size_t i = 0; i < o.size(); i++) out << o[i];
	return out;
}

CArchive& operator>>(CArchive& in, mrpt::math::TObject2D& o)
{
	uint16_t type;
	in >> type;
	switch (static_cast<unsigned char>(type))
	{
		case GEOMETRIC_TYPE_POINT:
		{
			TPoint2D p;
			in >> p;
			o = p;
		}
		break;
		case GEOMETRIC_TYPE_SEGMENT:
		{
			TSegment2D s;
			in >> s;
			o = s;
		}
		break;
		case GEOMETRIC_TYPE_LINE:
		{
			TLine2D l;
			in >> l;
			o = l;
		}
		break;
		case GEOMETRIC_TYPE_POLYGON:
		{
			TPolygon2D p;
			in >> p;
			o = p;
		}
		break;
		case GEOMETRIC_TYPE_UNDEFINED:
		{
			o = TObject2D();
		}
		break;
		default:
			throw std::logic_error(
				"Unknown TObject2D type found while reading stream");
	}
	return in;
}
CArchive& operator<<(CArchive& out, const mrpt::math::TObject2D& o)
{
	out << static_cast<uint16_t>(o.getType());
	switch (o.getType())
	{
		case GEOMETRIC_TYPE_POINT:
		{
			TPoint2D p;
			o.getPoint(p);
			return out << p;
		};
		case GEOMETRIC_TYPE_SEGMENT:
		{
			TSegment2D s;
			o.getSegment(s);
			return out << s;
		};
		case GEOMETRIC_TYPE_LINE:
		{
			TLine2D l;
			o.getLine(l);
			return out << l;
		};
		case GEOMETRIC_TYPE_POLYGON:
		{
			TPolygon2D p;
			o.getPolygon(p);
			return out << p;
		};
	}
	return out;
}

CArchive& operator>>(CArchive& in, mrpt::math::TObject3D& o)
{
	uint16_t type;
	in >> type;
	switch (static_cast<unsigned char>(type))
	{
		case GEOMETRIC_TYPE_POINT:
		{
			TPoint3D p;
			in >> p;
			o = p;
		}
		break;
		case GEOMETRIC_TYPE_SEGMENT:
		{
			TSegment3D s;
			in >> s;
			o = s;
		}
		break;
		case GEOMETRIC_TYPE_LINE:
		{
			TLine3D l;
			in >> l;
			o = l;
		}
		break;
		case GEOMETRIC_TYPE_PLANE:
		{
			TPlane p;
			in >> p;
			o = p;
		}
		break;
		case GEOMETRIC_TYPE_POLYGON:
		{
			TPolygon3D p;
			in >> p;
			o = p;
		}
		break;
		case GEOMETRIC_TYPE_UNDEFINED:
		{
			o = TObject3D();
		}
		break;
		default:
			throw std::logic_error(
				"Unknown TObject3D type found while reading stream");
	}
	return in;
}
CArchive& operator<<(CArchive& out, const mrpt::math::TObject3D& o)
{
	out << static_cast<uint16_t>(o.getType());
	switch (o.getType())
	{
		case GEOMETRIC_TYPE_POINT:
		{
			TPoint3D p;
			o.getPoint(p);
			return out << p;
		};
		case GEOMETRIC_TYPE_SEGMENT:
		{
			TSegment3D s;
			o.getSegment(s);
			return out << s;
		};
		case GEOMETRIC_TYPE_LINE:
		{
			TLine3D l;
			o.getLine(l);
			return out << l;
		};
		case GEOMETRIC_TYPE_PLANE:
		{
			TPlane p;
			o.getPlane(p);
			return out << p;
		};
		case GEOMETRIC_TYPE_POLYGON:
		{
			TPolygon3D p;
			o.getPolygon(p);
			return out << p;
		};
	}
	return out;
}
}  // namespace mrpt::math


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_reconstruction_estimator.h"

#include <Eigen/Core>
#include <memory>
#include <sstream>  // NOLINT

#include "theia/sfm/bundle_adjustment/bundle_adjustment.h"
#include "theia/sfm/estimate_track.h"
#include "theia/sfm/extract_maximally_parallel_rigid_subgraph.h"
#include "theia/sfm/filter_view_graph_cycles_by_rotation.h"
#include "theia/sfm/filter_view_pairs_from_orientation.h"
#include "theia/sfm/filter_view_pairs_from_relative_translation.h"
#include "theia/sfm/global_pose_estimation/least_unsquared_deviation_position_estimator.h"
#include "theia/sfm/global_pose_estimation/linear_position_estimator.h"
#include "theia/sfm/global_pose_estimation/linear_rotation_estimator.h"
#include "theia/sfm/global_pose_estimation/nonlinear_position_estimator.h"
#include "theia/sfm/global_pose_estimation/nonlinear_rotation_estimator.h"
#include "theia/sfm/global_pose_estimation/position_estimator.h"
#include "theia/sfm/global_pose_estimation/robust_rotation_estimator.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/reconstruction_estimator_options.h"
#include "theia/sfm/reconstruction_estimator_utils.h"
#include "theia/sfm/set_camera_intrinsics_from_priors.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/view_graph/orientations_from_maximum_spanning_tree.h"
#include "theia/sfm/view_graph/remove_disconnected_view_pairs.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/random.h"
#include "theia/util/timer.h"

namespace theia {

using Eigen::Vector3d;

namespace {

// All times are given in seconds.
struct GlobalReconstructionEstimatorTimings {
  double initial_view_graph_filtering_time = 0.0;
  double camera_intrinsics_calibration_time = 0.0;
  double rotation_estimation_time = 0.0;
  double rotation_filtering_time = 0.0;
  double relative_translation_optimization_time = 0.0;
  double relative_translation_filtering_time = 0.0;
  double position_estimation_time = 0.0;
};

FilterViewPairsFromRelativeTranslationOptions
SetRelativeTranslationFilteringOptions(
    const ReconstructionEstimatorOptions& options) {
  FilterViewPairsFromRelativeTranslationOptions fvpfrt_options;
  fvpfrt_options.num_threads = options.num_threads;
  fvpfrt_options.num_iterations = options.translation_filtering_num_iterations;
  fvpfrt_options.translation_projection_tolerance =
      options.translation_filtering_projection_tolerance;
  return fvpfrt_options;
}

void SetUnderconstrainedAsUnestimated(Reconstruction* reconstruction) {
  int num_underconstrained_views = -1;
  int num_underconstrained_tracks = -1;
  while (num_underconstrained_views != 0 && num_underconstrained_tracks != 0) {
    num_underconstrained_views =
        SetUnderconstrainedViewsToUnestimated(reconstruction);
    num_underconstrained_tracks =
        SetUnderconstrainedTracksToUnestimated(reconstruction);
  }
}

}  // namespace

GlobalReconstructionEstimator::GlobalReconstructionEstimator(
    const ReconstructionEstimatorOptions& options) {
  options_ = options;
  translation_filter_options_ = SetRelativeTranslationFilteringOptions(options);
  options_.nonlinear_position_estimator_options.num_threads =
      options_.num_threads;
  options_.linear_triplet_position_estimator_options.num_threads =
      options_.num_threads;
  ransac_params_ = SetRansacParameters(options);
}

// The pipeline for estimating camera poses and structure is as follows:
//   1) Filter potentially bad pairwise geometries by enforcing a loop
//      constaint on rotations that form a triplet.
//   2) Initialize focal lengths.
//   3) Estimate the global rotation for each camera.
//   4) Remove any pairwise geometries where the relative rotation is not
//      consistent with the global rotation.
//   5) Optimize the relative translation given the known rotations.
//   6) Filter potentially bad relative translations.
//   7) Estimate positions.
//   8) Estimate structure.
//   9) Bundle adjustment.
//   10) Retriangulate, and bundle adjust.
//
// After each filtering step we remove any views which are no longer connected
// to the largest connected component in the view graph.
ReconstructionEstimatorSummary GlobalReconstructionEstimator::Estimate(
    ViewGraph* view_graph, Reconstruction* reconstruction) {
  CHECK_NOTNULL(reconstruction);
  reconstruction_ = reconstruction;
  view_graph_ = view_graph;
  orientations_.clear();
  positions_.clear();

  ReconstructionEstimatorSummary summary;
  GlobalReconstructionEstimatorTimings global_estimator_timings;
  Timer total_timer;
  Timer timer;

  // Step 1. Filter the initial view graph and remove any bad two view
  // geometries.
  LOG(INFO) << "Filtering the intial view graph.";
  timer.Reset();
  if (!FilterInitialViewGraph()) {
    LOG(INFO) << "Insufficient view pairs to perform estimation.";
    return summary;
  }
  global_estimator_timings.initial_view_graph_filtering_time =
      timer.ElapsedTimeInSeconds();

  // Step 2. Calibrate any uncalibrated cameras.
  LOG(INFO) << "Calibrating any uncalibrated cameras.";
  timer.Reset();
  CalibrateCameras();
  summary.camera_intrinsics_calibration_time = timer.ElapsedTimeInSeconds();

  // Step 3. Estimate global rotations.
  LOG(INFO) << "Estimating the global rotations of all cameras.";
  timer.Reset();
  if (!EstimateGlobalRotations()) {
    LOG(WARNING) << "Rotation estimation failed!";
    summary.success = false;
    return summary;
  }
  global_estimator_timings.rotation_estimation_time =
      timer.ElapsedTimeInSeconds();

  // Step 4. Filter bad rotations.
  LOG(INFO) << "Filtering any bad rotation estimations.";
  timer.Reset();
  FilterRotations();
  global_estimator_timings.rotation_filtering_time =
      timer.ElapsedTimeInSeconds();

  // Step 5. Optimize relative translations.
  LOG(INFO) << "Optimizing the pairwise translation estimations.";
  timer.Reset();
  OptimizePairwiseTranslations();
  global_estimator_timings.relative_translation_optimization_time =
      timer.ElapsedTimeInSeconds();

  // Step 6. Filter bad relative translations.
  LOG(INFO) << "Filtering any bad relative translations.";
  timer.Reset();
  FilterRelativeTranslation();
  global_estimator_timings.relative_translation_filtering_time =
      timer.ElapsedTimeInSeconds();

  // Step 7. Estimate global positions.
  LOG(INFO) << "Estimating the positions of all cameras.";
  timer.Reset();
  if (!EstimatePosition()) {
    LOG(WARNING) << "Position estimation failed!";
    summary.success = false;
    return summary;
  }
  LOG(INFO) << positions_.size()
            << " camera positions were estimated successfully.";
  global_estimator_timings.position_estimation_time =
      timer.ElapsedTimeInSeconds();

  summary.pose_estimation_time =
      global_estimator_timings.rotation_estimation_time +
      global_estimator_timings.rotation_filtering_time +
      global_estimator_timings.relative_translation_optimization_time +
      global_estimator_timings.relative_translation_filtering_time +
      global_estimator_timings.position_estimation_time;

  // Set the poses in the reconstruction object.
  SetReconstructionFromEstimatedPoses(orientations_,
                                      positions_,
                                      reconstruction_);

  // Always triangulate once, then retriangulate and remove outliers depending
  // on the reconstruciton estimator options.
  for (int i = 0; i < options_.num_retriangulation_iterations + 1; i++) {
    // Step 8. Triangulate features.
    LOG(INFO) << "Triangulating all features.";
    timer.Reset();
    EstimateStructure();
    summary.triangulation_time += timer.ElapsedTimeInSeconds();

    SetUnderconstrainedAsUnestimated(reconstruction_);

    // Step 9. Bundle Adjustment.
    LOG(INFO) << "Performing bundle adjustment.";
    timer.Reset();
    if (!BundleAdjustment()) {
      summary.success = false;
      LOG(WARNING) << "Bundle adjustment failed!";
      return summary;
    }
    summary.bundle_adjustment_time += timer.ElapsedTimeInSeconds();

    int num_points_removed = RemoveOutlierFeatures(
        options_.max_reprojection_error_in_pixels,
        options_.min_triangulation_angle_degrees,
        reconstruction_);
    LOG(INFO) << num_points_removed << " outlier points were removed.";
  }

  // Set the output parameters.
  GetEstimatedViewsFromReconstruction(*reconstruction_,
                                      &summary.estimated_views);
  GetEstimatedTracksFromReconstruction(*reconstruction_,
                                       &summary.estimated_tracks);
  summary.success = true;
  summary.total_time = total_timer.ElapsedTimeInSeconds();

  // Output some timing statistics.
  std::ostringstream string_stream;
  string_stream
      << "Global Reconstruction Estimator timings:"
      << "\n\tInitial view graph filtering time = "
      << global_estimator_timings.initial_view_graph_filtering_time
      << "\n\tCamera intrinsic calibration time = "
      << summary.camera_intrinsics_calibration_time
      << "\n\tRotation estimation time = "
      << global_estimator_timings.rotation_estimation_time
      << "\n\tRotation filtering time = "
      << global_estimator_timings.rotation_filtering_time
      << "\n\tRelative translation optimization time = "
      << global_estimator_timings.relative_translation_optimization_time
      << "\n\tRelative translation filtering time = "
      << global_estimator_timings.relative_translation_filtering_time
      << "\n\tPosition estimation time = "
      << global_estimator_timings.position_estimation_time;
  summary.message = string_stream.str();

  return summary;
}

bool GlobalReconstructionEstimator::FilterInitialViewGraph() {
  // Remove any view pairs that do not have a sufficient number of inliers.
  std::unordered_set<ViewIdPair> view_pairs_to_remove;
  const auto& view_pairs = view_graph_->GetAllEdges();
  for (const auto& view_pair : view_pairs) {
    if (view_pair.second.num_verified_matches <
        options_.min_num_two_view_inliers) {
      view_pairs_to_remove.insert(view_pair.first);
    }
  }
  for (const ViewIdPair view_id_pair : view_pairs_to_remove) {
    view_graph_->RemoveEdge(view_id_pair.first, view_id_pair.second);
  }

  // Only reconstruct the largest connected component.
  RemoveDisconnectedViewPairs(view_graph_);
  return view_graph_->NumEdges() >= 1;
}

void GlobalReconstructionEstimator::CalibrateCameras() {
  SetCameraIntrinsicsFromPriors(reconstruction_);
}

bool GlobalReconstructionEstimator::EstimateGlobalRotations() {
  const auto& view_pairs = view_graph_->GetAllEdges();

  // Choose the global rotation estimation type.
  std::unique_ptr<RotationEstimator> rotation_estimator;
  switch (options_.global_rotation_estimator_type) {
    case GlobalRotationEstimatorType::ROBUST_L1L2: {
      // Initialize the orientation estimations by walking along the maximum
      // spanning tree.
      OrientationsFromMaximumSpanningTree(*view_graph_, &orientations_);
      RobustRotationEstimator::Options robust_rotation_estimator_options;
      rotation_estimator.reset(
          new RobustRotationEstimator(robust_rotation_estimator_options));
      break;
    }
    case GlobalRotationEstimatorType::NONLINEAR: {
      // Initialize the orientation estimations by walking along the maximum
      // spanning tree.
      OrientationsFromMaximumSpanningTree(*view_graph_, &orientations_);
      rotation_estimator.reset(new NonlinearRotationEstimator());
      break;
    }
    case GlobalRotationEstimatorType::LINEAR: {
      // Set the constructor variable to true to weigh each term by the inlier
      // count.
      rotation_estimator.reset(new LinearRotationEstimator(false));
      break;
    }
    default: {
      LOG(FATAL) << "Invalid type of global rotation estimation chosen.";
      break;
    }
  }

  return rotation_estimator->EstimateRotations(view_pairs, &orientations_);
}

void GlobalReconstructionEstimator::FilterRotations() {
  // Filter view pairs based on the relative rotation and the estimated global
  // orientations.
  FilterViewPairsFromOrientation(
      orientations_,
      options_.rotation_filtering_max_difference_degrees,
      view_graph_);
  // Remove any disconnected views from the estimation.
  const std::unordered_set<ViewId> removed_views =
      RemoveDisconnectedViewPairs(view_graph_);
  for (const ViewId removed_view : removed_views) {
    orientations_.erase(removed_view);
  }
}

void GlobalReconstructionEstimator::OptimizePairwiseTranslations() {
  if (options_.refine_relative_translations_after_rotation_estimation) {
    RefineRelativeTranslationsWithKnownRotations(*reconstruction_,
                                                 orientations_,
                                                 options_.num_threads,
                                                 view_graph_);
  }
}

void GlobalReconstructionEstimator::FilterRelativeTranslation() {
  if (options_.extract_maximal_rigid_subgraph) {
    LOG(INFO) << "Extracting maximal rigid component of viewing graph to "
                 "determine which cameras are well-constrained for position "
                 "estimation.";
    ExtractMaximallyParallelRigidSubgraph(orientations_, view_graph_);
  }

  // Filter potentially bad relative translations.
  if (options_.filter_relative_translations_with_1dsfm) {
    LOG(INFO) << "Filtering relative translations with 1DSfM filter.";
    FilterViewPairsFromRelativeTranslation(translation_filter_options_,
                                           orientations_,
                                           view_graph_);
  }
  // Remove any disconnected views from the estimation.
  const std::unordered_set<ViewId> removed_views =
      RemoveDisconnectedViewPairs(view_graph_);
  for (const ViewId removed_view : removed_views) {
    orientations_.erase(removed_view);
  }
}

bool GlobalReconstructionEstimator::EstimatePosition() {
  // Estimate position.
  const auto& view_pairs = view_graph_->GetAllEdges();
  std::unique_ptr<PositionEstimator> position_estimator;

  // Choose the global position estimation type.
  switch (options_.global_position_estimator_type) {
    case GlobalPositionEstimatorType::NONLINEAR: {
      position_estimator.reset(new NonlinearPositionEstimator(
          options_.nonlinear_position_estimator_options, *reconstruction_));
      break;
    }
    case GlobalPositionEstimatorType::LINEAR_TRIPLET: {
      position_estimator.reset(new LinearPositionEstimator(
          options_.linear_triplet_position_estimator_options,
          *reconstruction_));
      break;
    }
    case GlobalPositionEstimatorType::LEAST_UNSQUARED_DEVIATION: {
      position_estimator.reset(new LeastUnsquaredDeviationPositionEstimator(
          options_.least_unsquared_deviation_position_estimator_options));
      break;
    }
    default: {
      LOG(FATAL) << "Invalid type of global position estimation chosen.";
      break;
    }
  }

  return position_estimator->EstimatePositions(view_pairs,
                                               orientations_,
                                               &positions_);
}

void GlobalReconstructionEstimator::EstimateStructure() {
  // Estimate all tracks.
  TrackEstimator::Options triangulation_options;
  triangulation_options.max_acceptable_reprojection_error_pixels =
      options_.triangulation_max_reprojection_error_in_pixels;
  triangulation_options.min_triangulation_angle_degrees =
      options_.min_triangulation_angle_degrees;
  triangulation_options.bundle_adjustment = options_.bundle_adjust_tracks;
  triangulation_options.ba_options = SetBundleAdjustmentOptions(options_, 0);
  triangulation_options.ba_options.verbose = false;
  triangulation_options.num_threads = options_.num_threads;
  TrackEstimator track_estimator(triangulation_options, reconstruction_);
  const TrackEstimator::Summary summary = track_estimator.EstimateAllTracks();
}

bool GlobalReconstructionEstimator::BundleAdjustment() {
  // Bundle adjustment.
  bundle_adjustment_options_ =
      SetBundleAdjustmentOptions(options_, positions_.size());
  const auto& bundle_adjustment_summary =
      BundleAdjustReconstruction(bundle_adjustment_options_, reconstruction_);
  return bundle_adjustment_summary.success;
}

}  // namespace theia


#include <iostream>
#include <caffe/caffe.hpp>
#include <boost/lexical_cast.hpp>
// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <fstream>
#include <unistd.h>

// definitions
#define NO_OPERATION      0
#define MEAN_SUBTRACTION  1
#define SCALE_DATA        2

using namespace caffe;
using namespace std;
using std::string;

void WriteCVMatToFile(string fileName, cv::Mat data);
cv::Mat ReadCVMatFromFile(string filename);
int CheckForRequest(string filename);
void SetProcessDone(string filename, bool flag);
/*************************************************/
class CaffeForward {
 public:
  CaffeForward( const string& model_file,const string& weight_file,const string& prep_op,const string& prep_file);

  std::vector<std::vector<float> > ForwardProcess(std::vector <cv::Mat> &inputs);
  void SaveChannels(std::vector<std::vector<float> > outputs , const string& output_dir);
  private:
  void WrapInputLayer(std::vector<cv::Mat>* input_channels, int input_idx);
  void SetMean(const string& mean_file);
  void Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels, int input_idx);

 public:
 	int num_outputs_, num_inputs_;
 private:
  shared_ptr<Net<float> > net_;
  std::vector<cv::Size> input_geometry_;
  std::vector<int> num_channels_;
  cv::Mat mean_;
  double scale_;
  unsigned char prep_op_;
};
/*************************************************/
/* CAFFE FORWARD */
CaffeForward::CaffeForward( const string& model_file,const string& weight_file,const string& prep_op,const string& prep_file)
{
  #ifdef CPU_ONLY
    Caffe::set_mode(Caffe::CPU);
  #else
    Caffe::set_mode(Caffe::GPU);
  #endif
	/* Load the network. */
  net_.reset(new Net<float>(model_file, TEST));
  net_->CopyTrainedLayersFrom(weight_file);

  num_outputs_ = net_->num_outputs();
  num_inputs_ = net_->num_inputs();
	cout << "net inputs x outputs dimension: " << num_inputs_ << "x" << num_outputs_ << endl;

  for(int i = 0; i < num_inputs_; i++)
  {
    Blob<float>* input_layer = net_->input_blobs()[i];
    num_channels_.push_back(input_layer->channels());
    input_geometry_.push_back(cv::Size(input_layer->width(), input_layer->height()));
    cout << "input " << i << " (channels x width x height): " << num_channels_[i] <<"x"<<input_geometry_[i].width<<"x"<<input_geometry_[i].height << endl;
    CHECK(num_channels_[i] == 3 || num_channels_[i] == 1) << "Input layer should have 1 or 3 channels.";
  }
  for(int i = 0; i < num_outputs_; i++)
  {
    Blob<float>* output_layer = net_->output_blobs()[i];
    cout << "output " << i << " (channels x width x height): " << output_layer->channels()<< "x" << output_layer->width() << "x" << output_layer->height() << endl;
  }

  if ( prep_op == "MEAN_SUBTRACTION" )
  {
    SetMean(prep_file);
    prep_op_ = MEAN_SUBTRACTION;
  }
  else if( prep_op == "SCALE_DATA" )
  {
    scale_  = boost::lexical_cast<double>(prep_file);
    prep_op_ = SCALE_DATA;
  }
  else
    prep_op_ = NO_OPERATION;
}

/*************************************************/
/* SET MEAN */
void CaffeForward::SetMean(const string& mean_file) {
  /*cv::Mat mean_img = cv::imread(mean_file.c_str(), -1);
  cv::resize(mean_img, mean_, input_geometry_);
  mean_.convertTo(mean_, CV_32FC3);
  return;*/
  /*
  BlobProto blob_proto;
  ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);
  // Convert from BlobProto to Blob<float>
  Blob<float> mean_blob;
  mean_blob.FromProto(blob_proto);
  CHECK_EQ(mean_blob.channels(), num_channels_)
    << "Number of channels of mean file doesn't match input layer.";
  // The format of the mean file is planar 32-bit float BGR or grayscale.
  std::vector<cv::Mat> channels;
  float* data = mean_blob.mutable_cpu_data();
  for (int i = 0; i < num_channels_; ++i) {
    // Extract an individual channel.
    cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
    channels.push_back(channel);
    data += mean_blob.height() * mean_blob.width();
  }
  // Merge the separate channels into a single image.
  cv::Mat mean;
  cv::merge(channels, mean);
  cv::imwrite("/home/ali/Codes/Caffe/mean.PNG", mean);
  // Compute the global mean pixel value and create a mean image filled with this value.
  cv::Scalar channel_mean = cv::mean(mean);
  mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean);
  mean_ = mean;
  cv::imwrite("/home/ali/Codes/Caffe/mean2.PNG", mean_);*/
}

/*************************************************/
/* FORWARD PROCESS */
std::vector <std::vector<float> > CaffeForward::ForwardProcess(std::vector <cv::Mat> & inputs)
{
  for (int i = 0; i < inputs.size(); i++)
  {
    Blob<float>* input_layer = net_->input_blobs()[i];
    input_layer->Reshape(1, input_layer->channels(), input_layer->height(), input_layer->width());
  }
  net_->Reshape();
  for (int i = 0; i < inputs.size(); i++)
  {
    std::vector<cv::Mat> input_channels;
    WrapInputLayer(&input_channels, i);
    Preprocess(inputs[i], &input_channels, i);
  }

  net_->Forward();

  std::vector<std::vector<float> > outputs;

  for (int i = 0; i < num_outputs_; i++)
  {
    Blob<float>* output_layer = net_->output_blobs()[i];
    // cout << output_layer->width() << "x"<< output_layer->height() << "x" <<output_layer->channels() << endl;
  	const float* begin = output_layer->cpu_data();
  	const float* end = begin + output_layer->channels()*output_layer->width()*output_layer->height();
		outputs.push_back(std::vector<float>(begin, end));
  }

  return outputs;
}

/*************************************************/
/* SAVE CHANNELS */
void CaffeForward::SaveChannels(std::vector<std::vector<float> > outputs, const string& output_dir )
{
  for (int i = 0; i < outputs.size(); i++)
  {
    Blob<float>* output_layer = net_->output_blobs()[i];
    char filename[100];
    for (int ch = 0; ch < output_layer->channels(); ch++)
    {
      std::vector<float> channel;
      int p_start = ch*output_layer->width()*output_layer->height();
      int length = output_layer->width()*output_layer->height();
      channel.assign(outputs[i].begin()+p_start, outputs[i].begin()+p_start+length);
      cv::Mat img = cv::Mat(channel).reshape(0,output_layer->height());
      sprintf(filename,"%s/o_%02d_ch_%02d.txt", output_dir.c_str(),i,ch);
      WriteCVMatToFile(filename, img);
      double min, max;
      cv::minMaxLoc(img, &min, &max);
      img = img - min;
      img = img / (max - min);
      img.convertTo(img, CV_8UC1, 255.0);
      sprintf(filename,"%s/o_%02d_ch_%02d.jpg", output_dir.c_str(),i,ch);
      cv::imwrite(filename, img);
    }
  }
}
 /*************************************************/
 /* WRAP INPUT LAYER */
void CaffeForward::WrapInputLayer(std::vector<cv::Mat>* input_channels, int input_idx)
{
  Blob<float>* input_layer = net_->input_blobs()[input_idx];
  int width = input_layer->width();
  int height = input_layer->height();
  float* input_data = input_layer->mutable_cpu_data();
  for (int i = 0; i < input_layer->channels(); ++i) {
    cv::Mat channel(height, width, CV_32FC1, input_data);
    input_channels->push_back(channel);
    input_data += width * height;
  }
}
/*************************************************/
/* PREPROCESS */
void CaffeForward::Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels, int input_idx)
{
  cv::Mat sample;
  if (img.channels() == 3 && num_channels_[input_idx] == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
  else if (img.channels() == 4 && num_channels_[input_idx] == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
  else if (img.channels() == 4 && num_channels_[input_idx] == 3)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
  else if (img.channels() == 1 && num_channels_[input_idx] == 3)
    cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
  else
    sample = img;
  cv::Mat sample_resized;
  if (sample.size() != input_geometry_[input_idx])
    cv::resize(sample, sample_resized, input_geometry_[input_idx]);
  else
    sample_resized = sample;

  cv::Mat sample_float;
  if (num_channels_[input_idx] == 3)
    sample_resized.convertTo(sample_float, CV_32FC3);
  else
    sample_resized.convertTo(sample_float, CV_32FC1);

  cv::Mat sample_preprocessed;

  if (prep_op_ == SCALE_DATA)
    sample_preprocessed = (1.0 / 256.0) * sample_float;
  else if(prep_op_ == MEAN_SUBTRACTION)
    cv::subtract(sample_float, mean_, sample_preprocessed);
  else
    sample_preprocessed = sample_float;

  cv::split(sample_preprocessed, *input_channels);
  CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
        == net_->input_blobs()[input_idx]->cpu_data())
    << "Input channels are not wrapping the input layer of the network.";
}

/*************************************************/
/* MAIN */
int main(int argc, char** argv)
{
	std::cout << "\n\n\nProgram is started" << std::endl;

	::google::InitGoogleLogging(argv[0]);

  string model_file   = argv[1];    // deploy file
  string weight_file  = argv[2];    // weights
  string prep_op      = argv[3];    // preprocess operation
  string prep_file    = argv[4];    // preprocess file
  string root_data    = argv[5];    // input directory
  string output_dir   = argv[6];    // output directory to save the files

  CaffeForward CaffeForward(model_file, weight_file, prep_op, prep_file);

  string filename;
  int request = 0;
  while (request >= 0)
  {
    filename = root_data + "/initiate_process.txt";
    request = CheckForRequest(filename);

    if(request > 0)
    {
      filename = root_data + "/process_done.txt";
      SetProcessDone(filename, 0);
      cout << "request is registered" << endl;
      std::vector<cv::Mat> inputs;
      filename = root_data + "/img.jpg";
      cv::Mat img = cv::imread(filename, -1);
      CHECK(!img.empty()) << "Unable to decode image " << filename;
      inputs.push_back(img);

      filename = root_data + "/joint.txt";
      cv::Mat state = ReadCVMatFromFile(filename);
      CHECK(!state.empty()) << "Unable to decode joint values " << filename;
      inputs.push_back(state);

      std::vector< std::vector<float> > outputs = CaffeForward.ForwardProcess(inputs);
      CaffeForward.SaveChannels(outputs, output_dir);

      filename = root_data + "/process_done.txt";
      SetProcessDone(filename, 1);
      cout << "process done" << endl;
    }
    usleep(10000);
  }
  return 0;
}

int CheckForRequest(string filename)
{
  ifstream reader(filename.c_str());
  if (reader.is_open())
  {
    string line;
    getline(reader, line);
    reader.close();
    ofstream writer(filename.c_str());
    writer << 0 << endl;
    writer.close();
    return boost::lexical_cast<int> (line);
  }
  return 0;
}

void WriteCVMatToFile(string fileName, cv::Mat data)
{
    int nRow = data.size().height;
    int nCol = data.size().width;
    ofstream fileWriter;
    fileWriter.open(fileName.c_str());

    fileWriter << nRow << endl;
    fileWriter << nCol << endl;

    for(int i = 0; i < nRow; i++)
    {
      for(int j = 0; j < nCol; j++)
      {
        fileWriter << std::fixed << std::setprecision(20) << data.at<float>(i,j) << endl;
      }
    }
    fileWriter.close();
}
cv::Mat ReadCVMatFromFile(string filename)
{
  ifstream reader(filename.c_str());
  int rows, cols;
  string line;

  getline(reader, line);
  rows = boost::lexical_cast<int>(line);
  getline(reader, line);
  cols = boost::lexical_cast<int>(line);

  cv::Mat im( cv::Size(rows, cols), CV_32FC1, cv::Scalar(255) );

  for(int i = 0; i < rows; i++)
  {
    for(int j = 0; j < cols; j++)
    {
      getline(reader, line);
      im.at<float>(j, i) = boost::lexical_cast<float>(line);
    }
  }

  reader.close();
  return im;
}

void SetProcessDone(string filename, bool flag)
{
  ofstream writer(filename.c_str());
  if (flag)
    writer << 1 << endl;
  else
    writer << 0 << endl;
  writer.close();
}