/*
 * icp6Ddual implementation
 *
 * Copyright (C) Andreas Nuechter, Alexandru-Eugen Ichim
 *
 * Released under the GPL version 3.
 *
 */

/** @file 
 *  @brief Implementation of the ICP error function minimization
 *  via dual quaternions
 *
 *  @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
 *  @author Alexandru-Eugen Ichim. Jacobs University Bremen gGmbH, Germany
 *
 */

#include "slam6d/icp6Ddual.h"
#include "slam6d/globals.icc"

#include <iomanip>
using std::ios;
using std::resetiosflags;
using std::setiosflags;

#include "newmat/newmat.h"
#include "newmat/newmatap.h"

using namespace NEWMAT;

/**
 * computes the rotation matrix consisting
 * of a rotation and translation that
 * minimizes the root-mean-square error of the
 * point pairs using dual quaternions
 * @param pairs Vector of point pairs (pairs of corresponding points)
 * @param *alignfx The resulting transformation matrix
 * @return Error estimation of the matching (rms)
 */
double icp6D_DUAL::Point_Point_Align(const vector<PtPair>& pairs, double *alignfx,
		const double centroid_m[3], const double centroid_d[3])
{
	double error = 0;
	double sum = 0.0;

	for(unsigned int i = 0; i <  pairs.size(); i++){
		sum += sqr(pairs[i].p1.x - pairs[i].p2.x)
			 + sqr(pairs[i].p1.y - pairs[i].p2.y)
			 + sqr(pairs[i].p1.z - pairs[i].p2.z) ;
	}

	error = sqrt(sum / (double)pairs.size());

	if (!quiet) {
		cout.setf(ios::basefield);
		cout << "DUALQUAT RMS point-to-point error = "
				<< resetiosflags(ios::adjustfield) << setiosflags(ios::internal)
				<< resetiosflags(ios::floatfield) << setiosflags(ios::fixed)
				<< std::setw(10) << std::setprecision(7)
		<< error
		<< "  using " << std::setw(6) << (int)pairs.size() << " points" << endl;
	}



/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////

	Matrix C1(4, 4); C1 = 0.0;
	Matrix C2(4, 4); C2 = 0.0;

	/// build up matrices C1 and C2
	for(unsigned int i = 0; i < pairs.size(); ++i) {
		ColumnVector m(3), d(3);
		m << pairs[i].p1.x << pairs[i].p1.y << pairs[i].p1.z;
		d << pairs[i].p2.x << pairs[i].p2.y << pairs[i].p2.z;
		Matrix Cm(3, 3); Cm << 0 << -m(3) << m(2) << m(3) << 0 << -m(1) << -m(2) << m(1) << 0;
		Matrix Cd(3, 3); Cd << 0 << -d(3) << d(2) << d(3) << 0 << -d(1) << -d(2) << d(1) << 0;

		C1.SubMatrix(1, 1, 1, 1) += m.t()*d;
		C1.SubMatrix(1, 1, 2, 4) += -m.t()*Cd;
		C1.SubMatrix(2, 4, 1, 1) += -Cm*d;
		C1.SubMatrix(2, 4, 2, 4) += m*d.t() + Cm*Cd;

		C2.SubMatrix(1, 1, 2, 4) += -d.t() + m.t();
		C2.SubMatrix(2, 4, 1, 1) += d - m;
		C2.SubMatrix(2, 4, 2, 4) += -Cd - Cm;
	}

	/// the sums need to be multiplied by a scalar
	C1 = C1 * (-2);
	C2 = C2 * 2;

	/// matrix A from C1 and C2
	Matrix A = (C2.t()*C2 * 1.0/(2*pairs.size()) - C1 - C1.t()) * 0.5;

	/// the quaternion qdot is the eigenvector of matrix A corresponding to the largest eigenvalue,
	/// which is always found in the first column of matrix U
	Matrix U(4, 4); DiagonalMatrix D(4);
	SVD(A, D, U);
	ColumnVector qdot = U.Column(1);
	ColumnVector q(3); q << qdot(2) << qdot(3) << qdot(4);
	Matrix Cq(3, 3); Cq << 0 << -q(3) << q(2) << q(3) << 0 << -q(1) << -q(2) << q(1) << 0;

	ColumnVector s = C2*qdot * (-1.0)/(2*pairs.size());

	Matrix Q(4, 4);
	Q(1, 1) = qdot(1);
	Q.SubMatrix(1, 1, 2, 4) = q.t();
	Q.SubMatrix(2, 4, 1, 1) = -q;
	Q.SubMatrix(2, 4, 2, 4) = IdentityMatrix(3)*qdot(1) + Cq;

	/// get the translation from the double quaternion
	ColumnVector p = Q*s;
	ColumnVector translation(3); translation << p(2) << p(3) << p(4);

	/// compute the rotation matrix from the quaternion
	ColumnVector qtqx = q.t()*q;
	Matrix R = IdentityMatrix(3) * (qdot(1)*qdot(1) - qtqx(1)) + q*q.t()*2 + Cq*qdot(1)*2;

	/// create the final transformation matrix (OpenGL column-wise ordering)
	alignfx[0] = R(1, 1);
	alignfx[1] = R(2, 1);
	alignfx[2] = R(3, 1);
	alignfx[3] = 0.0;
	alignfx[4] = R(1, 2);
	alignfx[5] = R(2, 2);
	alignfx[6] = R(3, 2);
	alignfx[7] = 0.0;
	alignfx[8] = R(1, 3);
	alignfx[9] = R(2, 3);
	alignfx[10]= R(3, 3);
	alignfx[11]= 0.0;
	alignfx[12]= translation(1);
	alignfx[13]= translation(2);
	alignfx[14]= translation(3);
	alignfx[15]= 1.0;

/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////

	return error;
}