text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          http://www.mrpt.org/                          |
     |                                                                        |
     | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
     | See: http://www.mrpt.org/Authors - All rights reserved.                |
     | Released under BSD License. See details in http://www.mrpt.org/License |
     +------------------------------------------------------------------------+ */
  
  #include "poses-precomp.h"  // Precompiled headers
  
  #include <mrpt/poses/CPose3DQuatPDFGaussian.h>
  #include <mrpt/math/transform_gaussian.h>
  #include <mrpt/math/distributions.h>
  #include <mrpt/math/matrix_serialization.h>
  #include <mrpt/poses/CPose3DPDFGaussian.h>
  #include <mrpt/poses/CPose3D.h>
  #include <mrpt/poses/CPose3DQuat.h>
  #include <mrpt/poses/CPose3DQuatPDF.h>
  #include <mrpt/system/os.h>
  #include <mrpt/serialization/CArchive.h>
  
  using namespace mrpt;
  using namespace mrpt::poses;
  using namespace mrpt::math;
  using namespace mrpt::random;
  using namespace mrpt::system;
  using namespace std;
  
  bool USE_SUT_EULER2QUAT_CONVERSION_value = false;
  
  bool mrpt::global_settings::USE_SUT_EULER2QUAT_CONVERSION()
  {
  	return USE_SUT_EULER2QUAT_CONVERSION_value;
  }
  void mrpt::global_settings::USE_SUT_EULER2QUAT_CONVERSION(bool value)
  {
  	USE_SUT_EULER2QUAT_CONVERSION_value = value;
  }
  
  IMPLEMENTS_SERIALIZABLE(CPose3DQuatPDFGaussian, CPose3DQuatPDF, mrpt::poses)
  
  /** Default constructor - set all values to zero. */
  CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian() : mean(), cov() {}
  // Un-initialized constructor:
  CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian(
  	TConstructorFlags_Quaternions constructor_dummy_param)
  	: mean(UNINITIALIZED_QUATERNION), cov(UNINITIALIZED_MATRIX)
  {
  	MRPT_UNUSED_PARAM(constructor_dummy_param);
  }
  
  /** Constructor from a default mean value, covariance equals to zero. */
  CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian(const CPose3DQuat& init_Mean)
  	: mean(init_Mean), cov()
  {
  }
  
  /** Constructor with mean and covariance. */
  CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian(
  	const CPose3DQuat& init_Mean, const CMatrixDouble77& init_Cov)
  	: mean(init_Mean), cov(init_Cov)
  {
  }
  
  /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables).
   */
  CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian(const CPosePDFGaussian& o)
  	: mean(UNINITIALIZED_QUATERNION), cov(UNINITIALIZED_MATRIX)
  {
  	this->copyFrom(CPose3DPDFGaussian(o));
  }
  
  /** Constructor from an equivalent Gaussian in Euler angles representation. */
  CPose3DQuatPDFGaussian::CPose3DQuatPDFGaussian(const CPose3DPDFGaussian& o)
  	: mean(UNINITIALIZED_QUATERNION), cov(UNINITIALIZED_MATRIX)
  {
  	this->copyFrom(o);
  }
  
  /*---------------------------------------------------------------
  						getMean
    Returns an estimate of the pose, (the mean, or mathematical expectation of the
   PDF)
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::getMean(CPose3DQuat& p) const { p = mean; }
  /*---------------------------------------------------------------
  						getCovarianceAndMean
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::getCovarianceAndMean(
  	CMatrixDouble77& C, CPose3DQuat& p) const
  {
  	C = cov;
  	p = mean;
  }
  
  uint8_t CPose3DQuatPDFGaussian::serializeGetVersion() const { return 0; }
  void CPose3DQuatPDFGaussian::serializeTo(
  	mrpt::serialization::CArchive& out) const
  {
  	out << mean;
  	mrpt::math::serializeSymmetricMatrixTo(cov, out);
  }
  void CPose3DQuatPDFGaussian::serializeFrom(
  	mrpt::serialization::CArchive& in, uint8_t version)
  {
  	switch (version)
  	{
  		case 0:
  		{
  			in >> mean;
  			mrpt::math::deserializeSymmetricMatrixFrom(cov, in);
  		}
  		break;
  		default:
  			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  	};
  }
  
  void CPose3DQuatPDFGaussian::copyFrom(const CPose3DQuatPDF& o)
  {
  	if (this == &o) return;  // It may be used sometimes
  
  	// Convert to gaussian pdf:
  	o.getCovarianceAndMean(cov, mean);
  }
  
  void CPose3DQuatPDFGaussian::copyFrom(const CPosePDF& o)
  {
  	CPose3DPDFGaussian aux;
  	aux.copyFrom(o);
  	this->copyFrom(aux);
  }
  
  void aux_poseypr2posequat(
  	const CArrayDouble<6>& x, const double& dummy, CArrayDouble<7>& y)
  {
  	MRPT_UNUSED_PARAM(dummy);
  	y[0] = x[0];
  	y[1] = x[1];
  	y[2] = x[2];
  
  	CPose3D p(0, 0, 0, x[3], x[4], x[5]);
  	CQuaternionDouble q(UNINITIALIZED_QUATERNION);
  	p.getAsQuaternion(q);
  	y[3] = q[0];
  	y[4] = q[1];
  	y[5] = q[2];
  	y[6] = q[3];
  }
  
  void CPose3DQuatPDFGaussian::copyFrom(const CPose3DPDFGaussian& o)
  {
  	if (!USE_SUT_EULER2QUAT_CONVERSION_value)
  	{  // Use Jacobians
  		CMatrixFixedNumeric<double, 4, 3> dq_dr_sub(UNINITIALIZED_MATRIX);
  
  		// Mean:
  		mean.x(o.mean.x());
  		mean.y(o.mean.y());
  		mean.z(o.mean.z());
  
  		o.mean.getAsQuaternion(mean.quat(), &dq_dr_sub);
  
  // Cov:
  #if 1
  		CMatrixFixedNumeric<double, 7, 6> dq_dr;
  		dq_dr.get_unsafe(0, 0) = dq_dr.get_unsafe(1, 1) =
  			dq_dr.get_unsafe(2, 2) = 1;
  		dq_dr.insertMatrix(3, 3, dq_dr_sub);
  		// Now for the covariance:
  		dq_dr.multiply_HCHt(o.cov, this->cov);
  #else
  		CMatrixDouble33 cov_R(UNINITIALIZED_MATRIX);
  		CMatrixDouble33 cov_T(UNINITIALIZED_MATRIX);
  		CMatrixDouble33 cov_TR(UNINITIALIZED_MATRIX);
  		o.cov.extractMatrix(3, 3, cov_R);
  		o.cov.extractMatrix(0, 0, cov_T);
  		o.cov.extractMatrix(0, 3, cov_TR);
  
  		// [        S_T       |   S_TR * H^t    ]
  		// [ -----------------+---------------- ]
  		// [  (S_TR * H^t)^t  |  H * S_R * H^t  ]
  
  		// top-left:
  		this->cov.insertMatrix(0, 0, cov_T);
  
  		// diagonals:
  		CMatrixFixedNumeric<double, 3, 4> cov_TQ(UNINITIALIZED_MATRIX);
  		cov_TQ.multiply_ABt(cov_TR, dq_dr_sub);
  		this->cov.insertMatrix(0, 3, cov_TQ);
  		this->cov.insertMatrixTranspose(3, 0, cov_TQ);
  
  		// bottom-right:
  		CMatrixDouble44 cov_q(UNINITIALIZED_MATRIX);
  		dq_dr_sub.multiply_HCHt(cov_R, cov_q);
  		this->cov.insertMatrix(3, 3, cov_q);
  #endif
  	}
  	else
  	{
  		// Use UT transformation:
  		//   f: R^6 => R^7
  		const CArrayDouble<6> x_mean(o.mean);
  
  		static const double dummy = 0;
  		mrpt::math::transform_gaussian_unscented(
  			x_mean, o.cov, &aux_poseypr2posequat, dummy, this->mean, this->cov);
  	}
  }
  
  /*---------------------------------------------------------------
  					saveToTextFile
    ---------------------------------------------------------------*/
  bool CPose3DQuatPDFGaussian::saveToTextFile(const string& file) const
  {
  	FILE* f = os::fopen(file.c_str(), "wt");
  	if (!f) return false;
  
  	os::fprintf(
  		f, "%e %e %e %e %e %e %e\n", mean.x(), mean.y(), mean.z(),
  		mean.quat()[0], mean.quat()[1], mean.quat()[2], mean.quat()[3]);
  
  	for (unsigned int i = 0; i < 7; i++)
  		os::fprintf(
  			f, "%e %e %e %e %e %e %e\n", cov(i, 0), cov(i, 1), cov(i, 2),
  			cov(i, 3), cov(i, 4), cov(i, 5), cov(i, 6));
  
  	os::fclose(f);
  	return true;
  }
  
  /*---------------------------------------------------------------
  						changeCoordinatesReference
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::changeCoordinatesReference(
  	const CPose3D& newReferenceBase)
  {
  	MRPT_START
  	changeCoordinatesReference(CPose3DQuat(newReferenceBase));
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  						changeCoordinatesReference
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::changeCoordinatesReference(
  	const CPose3DQuat& newReferenceBaseQuat)
  {
  	MRPT_START
  
  	// COV:
  	const CMatrixDouble77 OLD_COV = this->cov;
  	CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);
  
  	CPose3DQuatPDF::jacobiansPoseComposition(
  		newReferenceBaseQuat,  // x
  		this->mean,  // u
  		df_dx, df_du,
  		&this->mean  // Output:  newReferenceBaseQuat + this->mean;
  	);
  
  	// this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
  	// df_dx: not used, since its COV are all zeros... // df_dx.multiply_HCHt(
  	// OLD_COV, cov );
  	df_du.multiply_HCHt(OLD_COV, cov);
  
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  					drawSingleSample
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::drawSingleSample(CPose3DQuat& outPart) const
  {
  	MRPT_START
  	getRandomGenerator().drawGaussianMultivariate(outPart, cov, &mean);
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  					drawManySamples
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::drawManySamples(
  	size_t N, vector<CVectorDouble>& outSamples) const
  {
  	MRPT_START
  
  	getRandomGenerator().drawGaussianMultivariateMany(outSamples, N, cov);
  
  	for (auto& outSample : outSamples)
  		for (unsigned int k = 0; k < 7; k++) outSample[k] += mean[k];
  
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  					inverse
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::inverse(CPose3DQuatPDF& o) const
  {
  	ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DQuatPDFGaussian));
  	auto& out = dynamic_cast<CPose3DQuatPDFGaussian&>(o);
  
  	// COV:
  	CMatrixFixedNumeric<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
  	double lx, ly, lz;
  	mean.inverseComposePoint(0, 0, 0, lx, ly, lz, nullptr, &df_dpose);
  
  	CMatrixFixedNumeric<double, 7, 7> jacob;
  	jacob.insertMatrix(0, 0, df_dpose);
  	jacob.set_unsafe(3, 3, 1);
  	jacob.set_unsafe(4, 4, -1);
  	jacob.set_unsafe(5, 5, -1);
  	jacob.set_unsafe(6, 6, -1);
  
  	// C(0:2,0:2): H C H^t
  	jacob.multiply_HCHt(this->cov, out.cov);
  
  	// Mean:
  	out.mean.x(lx);
  	out.mean.y(ly);
  	out.mean.z(lz);
  	this->mean.quat().conj(out.mean.quat());
  }
  
  /*---------------------------------------------------------------
  							+=
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::operator+=(const CPose3DQuat& Ap)
  {
  	// COV:
  	const CMatrixDouble77 OLD_COV = this->cov;
  	CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);
  
  	CPose3DQuatPDF::jacobiansPoseComposition(
  		this->mean,  // x
  		Ap,  // u
  		df_dx, df_du,
  		&this->mean  // Output: this->mean + Ap;
  	);
  
  	// this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
  	df_dx.multiply_HCHt(OLD_COV, cov);
  	// df_du: Nothing to do, since COV(Ap) = zeros
  }
  
  /*---------------------------------------------------------------
  							+=
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::operator+=(const CPose3DQuatPDFGaussian& Ap)
  {
  	// COV:
  	const CMatrixDouble77 OLD_COV = this->cov;
  	CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);
  
  	CPose3DQuatPDF::jacobiansPoseComposition(
  		this->mean,  // x
  		Ap.mean,  // u
  		df_dx, df_du,
  		&this->mean  // Output:  this->mean + Ap.mean;
  	);
  
  	// this->cov = H1*this->cov*~H1 + H2*Ap.cov*~H2;
  	df_dx.multiply_HCHt(OLD_COV, cov);
  	df_du.multiply_HCHt(Ap.cov, cov, true);  // Accumulate result
  }
  
  /*---------------------------------------------------------------
  							-=
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::operator-=(const CPose3DQuatPDFGaussian& Ap)
  {
  	// THIS = THIS (-) Ap             -->
  	// THIS = inverse(Ap) (+) THIS
  	CPose3DQuatPDFGaussian inv_Ap = -Ap;
  	*this = inv_Ap + *this;
  }
  
  /*---------------------------------------------------------------
  						evaluatePDF
   ---------------------------------------------------------------*/
  double CPose3DQuatPDFGaussian::evaluatePDF(const CPose3DQuat& x) const
  {
  	return mrpt::math::normalPDF(
  		CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov);
  }
  
  /*---------------------------------------------------------------
  						evaluateNormalizedPDF
   ---------------------------------------------------------------*/
  double CPose3DQuatPDFGaussian::evaluateNormalizedPDF(const CPose3DQuat& x) const
  {
  	return mrpt::math::normalPDF(
  		CMatrixDouble71(x), CMatrixDouble71(this->mean), this->cov, true);
  }
  
  /*---------------------------------------------------------------
  						assureSymmetry
   ---------------------------------------------------------------*/
  void CPose3DQuatPDFGaussian::assureSymmetry()
  {
  	// Differences, when they exist, appear in the ~15'th significant
  	//  digit, so... just take one of them arbitrarily!
  	for (int i = 0; i < cov.rows() - 1; i++)
  		for (int j = i + 1; j < cov.rows(); j++)
  			cov.get_unsafe(i, j) = cov.get_unsafe(j, i);
  }
  
  /*---------------------------------------------------------------
  						mahalanobisDistanceTo
   ---------------------------------------------------------------*/
  double CPose3DQuatPDFGaussian::mahalanobisDistanceTo(
  	const CPose3DQuatPDFGaussian& theOther)
  {
  	MRPT_START
  	const CMatrixDouble77 COV2 = cov + theOther.cov;
  	return mrpt::math::mahalanobisDistance(
  		CMatrixDouble71(this->mean) - CMatrixDouble71(theOther.mean), COV2);
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  						operator <<
   ---------------------------------------------------------------*/
  ostream& mrpt::poses::operator<<(
  	ostream& out, const CPose3DQuatPDFGaussian& obj)
  {
  	out << "Mean: " << obj.mean << "\n";
  	out << "Covariance:\n" << obj.cov << "\n";
  	return out;
  }
  
  bool mrpt::poses::operator==(
  	const CPose3DQuatPDFGaussian& p1, const CPose3DQuatPDFGaussian& p2)
  {
  	return p1.mean == p2.mean && p1.cov == p2.cov;
  }
  
  CPose3DQuatPDFGaussian mrpt::poses::operator+(
  	const CPose3DQuatPDFGaussian& x, const CPose3DQuatPDFGaussian& u)
  {
  	CPose3DQuatPDFGaussian res(x);
  	res += u;
  	return res;
  }
  
  CPose3DQuatPDFGaussian mrpt::poses::operator-(
  	const CPose3DQuatPDFGaussian& x, const CPose3DQuatPDFGaussian& u)
  {
  	CPose3DQuatPDFGaussian res(x);
  	res -= u;
  	return res;
  }
  