/* ========================================================================
 * PROJECT: ARToolKitPlus
 * ========================================================================
 *
 * The robust pose estimator algorithm has been provided by G. Schweighofer
 * and A. Pinz (Inst.of El.Measurement and Measurement Signal Processing,
 * Graz University of Technology). Details about the algorithm are given in
 * a Technical Report: TR-EMT-2005-01, available at:
 * http://www.emt.tu-graz.ac.at/publications/index.htm
 *
 * Ported from MATLAB to C by T.Pintaric (Vienna University of Technology).
 *
 * Copyright of the derived and new portions of this work
 *     (C) 2006 Graz University of Technology
 *
 * This framework is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This framework is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * For further information please contact 
 *   Dieter Schmalstieg
 *   <schmalstieg@icg.tu-graz.ac.at>
 *   Graz University of Technology, 
 *   Institut for Computer Graphics and Vision,
 *   Inffeldgasse 16a, 8010 Graz, Austria.
 * ========================================================================
 ** @author   Thomas Pintaric
 *
 * $Id: rpp.cpp 162 2006-04-19 21:28:10Z grabner $
 * @file
 * ======================================================================== */


#ifndef _NO_LIBRPP_


#include <vector>
#include "assert.h"

#include "rpp.h"
#include "rpp_const.h"
#include "rpp_vecmat.h"

namespace rpp {

// ===========================================================================================
void Quaternion_byAngleAndVector(quat_t &Q, const real_t &q_angle, const vec3_t &q_vector)
{
	vec3_t rotation_axis;
	normRv(rotation_axis, q_vector);
	real_t f = _sin(q_angle/2.0f);
	vec3_copy(Q.v,rotation_axis);
	vec3_mult(Q.v,f);
	Q.s = _cos(q_angle/2.0f);
	quat_mult(Q, 1.0f / quat_norm(Q));
}

// ===========================================================================================

void GetRotationbyVector(mat33_t &R, const vec3_t &v1, const vec3_t &v2)
{
	real_t winkel = _acos(vec3_dot(v1,v2));
	quat_t QU;
	vec3_t vc;
	vec3_cross(vc,v2,v1);
	Quaternion_byAngleAndVector(QU,winkel,vc);
	mat33_from_quat(R,QU);
}

// ===========================================================================================
void xform(vec3_array &Q, const vec3_array &P, const mat33_t &R, const vec3_t &t)
{
	const unsigned int n = (unsigned int) P.size();
	for(unsigned int i=0; i<n; i++)
	{
		vec3_mult(Q.at(i),R,P.at(i));
		vec3_add(Q.at(i),t);
	}
}
// ===========================================================================================

void xformproj(vec3_array &Qp, const vec3_array &P, const mat33_t &R, const vec3_t &t)
{
	const unsigned int n = (unsigned int) P.size();
	vec3_array Q;
	Q.resize(n);

	for(unsigned int i=0; i<n; i++)
	{
		vec3_mult(Q.at(i),R,P.at(i));
		vec3_add(Q.at(i),t);
		Qp.at(i).v[0] = Q.at(i).v[0] / Q.at(i).v[2]; 
		Qp.at(i).v[1] = Q.at(i).v[1] / Q.at(i).v[2]; 
		Qp.at(i).v[2] = 1.0;
	}
}

// ===========================================================================================
void rpyMat(mat33_t &R, const vec3_t &rpy) // rpy: roll,pitch,yaw
{
	const real_t cosA = _cos(rpy.v[2]);
	const real_t sinA = _sin(rpy.v[2]);
	const real_t cosB = _cos(rpy.v[1]);
	const real_t sinB = _sin(rpy.v[1]);
	const real_t cosC = _cos(rpy.v[0]);
	const real_t sinC = _sin(rpy.v[0]);
	const real_t cosAsinB = cosA * sinB;
	const real_t sinAsinB = sinA * sinB;

	R.m[0][0] = cosA*cosB;
	R.m[0][1] = cosAsinB*sinC-sinA*cosC;
	R.m[0][2] = cosAsinB*cosC+sinA*sinC;

	R.m[1][0] = sinA*cosB;
	R.m[1][1] = sinAsinB*sinC+cosA*cosC;
	R.m[1][2] = sinAsinB*cosC-cosA*sinC;

	R.m[2][0] = -sinB;
	R.m[2][1] = cosB*sinC;
	R.m[2][2] = cosB*cosC;
}
// ===========================================================================================
void rpyAng(vec3_t &angs, const mat33_t &R)
{
	const real_t sinB = -(R.m[2][0]);
	const real_t cosB = _sqrt(R.m[0][0]*R.m[0][0] + R.m[1][0]*R.m[1][0]);

	if(_abs(cosB) > real_t(1E-15))
	{
		const real_t sinA = R.m[1][0] / cosB;
		const real_t cosA = R.m[0][0] / cosB;
		const real_t sinC = R.m[2][1] / cosB;
		const real_t cosC = R.m[2][2] / cosB;
		vec3_assign(angs,_atan2(sinC,cosC),_atan2(sinB,cosB),_atan2(sinA,cosA));
	}
	else
	{
		const real_t sinC = (R.m[0][1] - R.m[1][2]) / 2.0f;
		const real_t cosC = (R.m[1][1] - R.m[0][2]) / 2.0f;
		vec3_assign(angs,_atan2(sinC,cosC),CONST_PI_OVER_2, 0.0f);
	}
}

// ===========================================================================================

void rpyAng_X(vec3_t &ang_zyx, const mat33_t &R)
{
	rpyAng(ang_zyx,R);

	if(_abs(ang_zyx.v[0]) > CONST_PI_OVER_2)
	{
		while(_abs(ang_zyx.v[0]) > CONST_PI_OVER_2)
		{
			if(ang_zyx.v[0] > 0)
			{
				vec3_assign(ang_zyx, ang_zyx.v[0]+CONST_PI,
					                 3*CONST_PI-ang_zyx.v[1],
									 ang_zyx.v[2]+CONST_PI);
				vec3_sub(ang_zyx,CONST_2_PI);
			}
			else
			{
				vec3_assign(ang_zyx, ang_zyx.v[0]+CONST_PI,
									 3*CONST_PI-ang_zyx.v[1],
									 ang_zyx.v[2]+CONST_PI);
			}
		}
	}
}

// ===========================================================================================
void decomposeR(mat33_t &Rz, const mat33_t &R)
{
	real_t cl = _atan2(R.m[2][1],R.m[2][0]);
	vec3_t rpy;
	vec3_assign(rpy,0,0,cl);
	rpyMat(Rz,rpy);
}

// ===========================================================================================
void abskernel(mat33_t &R, vec3_t &t, vec3_array &Qout, real_t &err2, 
			   const vec3_array _P, const vec3_array _Q, 
			   const mat33_array F, const mat33_t G)

{
	unsigned i,j;

	vec3_array P(_P.begin(),_P.end());
	vec3_array Q(_Q.begin(),_Q.end());
	const unsigned int n = (unsigned int) P.size();

	for(i=0; i<n; i++)
	{
		vec3_t _q;
		vec3_mult(_q,F.at(i),_Q.at(i));
		vec3_copy(Q.at(i),_q);
	}

	vec3_t pbar;
	vec3_array_sum(pbar,P);
	vec3_div(pbar,real_t(n));
	vec3_array_sub(P,pbar);

	vec3_t qbar;
	vec3_array_sum(qbar,Q);
	vec3_div(qbar,real_t(n));
	vec3_array_sub(Q,qbar);

	mat33_t M;
	mat33_clear(M);
	for(j=0; j<n; j++)
	{
		mat33_t _m;
		vec3_mul_vec3trans(_m,P.at(j),Q.at(j));
		mat33_add(M,_m);
	}

	mat33_t _U;
	mat33_t _S;
	mat33_t _V;
	mat33_clear(_U);
	mat33_clear(_S);
	mat33_clear(_V);
	mat33_svd2(_U,_S,_V,M);

	mat33_t _Ut;
	mat33_transpose(_Ut,_U);
	mat33_mult(R,_V,_Ut);

	vec3_t _sum;
	vec3_clear(_sum);
	for(i=0; i<n; i++)
	{
		vec3_t _v1,_v2;
		vec3_mult(_v1,R,P.at(i));
		vec3_mult(_v2,F.at(i),_v1);
		vec3_add(_sum,_v2);
	}

	vec3_mult(t,G,_sum);
	xform(Qout,P,R,t);
	err2 = 0;
	for(i=0; i<n; i++)
	{
		mat33_t _m1;
		vec3_t _v1;
		mat33_eye(_m1);
		mat33_sub(_m1,F.at(i));
		vec3_mult(_v1,_m1,Qout.at(i));
		err2 += vec3_dot(_v1,_v1);
	}
}
// ===========================================================================================

void objpose(mat33_t &R, vec3_t &t, unsigned int &it, real_t &obj_err, real_t &img_err,
			 bool calc_img_err, const vec3_array _P, const vec3_array Qp, const options_t options)
{
	unsigned int i,j;
	vec3_array P(_P.begin(),_P.end());

	const unsigned int n = (unsigned int) P.size();
	vec3_t pbar;
	vec3_array_sum(pbar,P);
	vec3_div(pbar,real_t(n));
	vec3_array_sub(P,pbar);
	vec3_array Q(Qp.begin(),Qp.end());
	vec3_t ones;
	ones.v[0] = 1;
	ones.v[1] = 1;
	ones.v[2] = 1;
	const bool mask_z[3] = {0,0,1};
	vec3_array_set(Q,ones,mask_z);
	mat33_array F;
	F.resize(n);
	vec3_t V;
	for(i=0; i<n; i++)
	{
		V.v[0] = Q.at(i).v[0] / Q.at(i).v[2];
		V.v[1] = Q.at(i).v[1] / Q.at(i).v[2];
		V.v[2] = 1.0;
		mat33_t _m;
		vec3_mul_vec3trans(_m,V,V);
		mat33_div(_m,vec3trans_mul_vec3(V,V));
		F.at(i) = _m;
	}

	mat33_t tFactor;
	{
		mat33_t _m1,_m2,_m3;
		mat33_eye(_m1);
		mat33_array_sum(_m2,F);
		mat33_div(_m2,real_t(n));
		mat33_sub(_m3,_m1,_m2);
		mat33_inv(tFactor,_m3);
		mat33_div(tFactor,real_t(n));
	}

	it = 0;
	bool initR_approximate = mat33_all_zeros(options.initR);
	mat33_t Ri;
	vec3_t ti;
	vec3_array Qi; Qi.resize(n);
	real_t old_err, new_err;

	// ----------------------------------------------------------------------------------------
	if(!initR_approximate)
	{
		mat33_copy(Ri,options.initR);
		vec3_t _sum;
		vec3_clear(_sum);
		for(j=0; j<n; j++)
		{
			vec3_t _v1, _v2;
			mat33_t _m1,_m2;
			mat33_eye(_m1);              
			mat33_sub(_m2,F.at(j),_m1);
			vec3_mult(_v1,Ri,P.at(j));
			vec3_mult(_v2,_m2,_v1);
			vec3_add(_sum,_v2);
		}
		vec3_mult(ti,tFactor,_sum);
		xform(Qi,P,Ri,ti);
		old_err = 0;
		vec3_t _v;
		for(j=0; j<n; j++)
		{
			mat33_t _m1,_m2;
			mat33_eye(_m1);
			mat33_sub(_m2,F.at(j),_m1);
			vec3_mult(_v,_m2,Qi.at(j));
			old_err += vec3_dot(_v,_v);
		}
	// ----------------------------------------------------------------------------------------
	}
	else
	{
		abskernel(Ri,ti,Qi,old_err,P,Q,F,tFactor);
		it = 1;
	}
	// ----------------------------------------------------------------------------------------

	abskernel(Ri,ti,Qi,new_err,P,Qi,F,tFactor);
	it = it + 1;

	while((_abs((old_err-new_err)/old_err) > options.tol) && (new_err > options.epsilon) &&
		  (options.max_iter == 0 || it<options.max_iter))
	{
		old_err = new_err;
		abskernel(Ri,ti,Qi,new_err,P,Qi,F,tFactor);
		it = it + 1;
	}


	mat33_copy(R,Ri);
	vec3_copy(t,ti);
	obj_err = _sqrt(new_err/real_t(n));

	if(calc_img_err)
	{
		vec3_array Qproj; Qproj.resize(n);
		xformproj(Qproj, P, Ri, ti);
		img_err = 0;

		vec3_t _v;
		for(unsigned int j=0; j<n; j++)
		{
			vec3_sub(_v,Qproj.at(j),Qp.at(j));
			img_err += vec3_dot(_v,_v);
		}
		img_err = _sqrt(img_err/real_t(n));
	}

	if(t.v[2] < 0)
	{
		mat33_mult(R,-1.0);
		vec3_mult(t,-1.0);
	}

	vec3_t _ts;
	vec3_mult(_ts,Ri,pbar);
	vec3_sub(t,_ts);
}

// =====================================================================================

void getRotationY_wrtT(scalar_array &al_ret, vec3_array &tnew, const vec3_array &v,
					   const vec3_array &p, const vec3_t &t, const real_t &DB,
					   const mat33_t &Rz)
{
	unsigned int i,j;
	const unsigned int n = (unsigned int) v.size();
	mat33_array V;
	V.resize(n);
	for(i=0; i<n; i++)
	{
		vec3_mul_vec3trans(V.at(i),v.at(i),v.at(i));
		mat33_div(V.at(i), vec3trans_mul_vec3(v.at(i),v.at(i)));
	}

	mat33_t G, _g1, _g2, _g3;
	mat33_array_sum(_g1,V);
	mat33_eye(_g2);
	mat33_div(_g1,real_t(n));
	mat33_sub(_g3,_g2,_g1);
	mat33_inv(G, _g3);
	mat33_div(G,real_t(n));
	mat33_t _opt_t;
	mat33_clear(_opt_t);

	for(i=0; i<n; i++)
	{
		const real_t v11 = V.at(i).m[0][0]; 
		const real_t v21 = V.at(i).m[1][0];
		const real_t v31 = V.at(i).m[2][0];
		const real_t v12 = V.at(i).m[0][1]; 
		const real_t v22 = V.at(i).m[1][1];
		const real_t v32 = V.at(i).m[2][1];
		const real_t v13 = V.at(i).m[0][2]; 
		const real_t v23 = V.at(i).m[1][2];
		const real_t v33 = V.at(i).m[2][2];
		const real_t px = p.at(i).v[0];
		const real_t py = p.at(i).v[1];
		const real_t pz = p.at(i).v[2];
		const real_t r1 = Rz.m[0][0];
		const real_t r2 = Rz.m[0][1];
		const real_t r3 = Rz.m[0][2];
		const real_t r4 = Rz.m[1][0];
		const real_t r5 = Rz.m[1][1];
		const real_t r6 = Rz.m[1][2];
		const real_t r7 = Rz.m[2][0];
		const real_t r8 = Rz.m[2][1];
		const real_t r9 = Rz.m[2][2];

		mat33_t _o;
		_o.m[0][0] = (((v11-real_t(1))*r2+v12*r5+v13*r8)*py+(-(v11-real_t(1))*r1-v12*r4-v13*r7)*px+(-(v11-real_t(1))*r3-v12*r6-v13*r9)*pz);
		_o.m[0][1] = ((real_t(2)*(v11-real_t(1))*r1+real_t(2)*v12*r4+real_t(2)*v13*r7)*pz+(-real_t(2)*(v11-real_t(1))*r3-real_t(2)*v12*r6-real_t(2)*v13*r9)*px);
		_o.m[0][2] = ((v11-real_t(1))*r1+v12*r4+v13*r7)*px+((v11-real_t(1))*r3+v12*r6+v13*r9)*pz+((v11-real_t(1))*r2+v12*r5+v13*r8)*py;

		_o.m[1][0] = ((v21*r2+(v22-real_t(1))*r5+v23*r8)*py+(-v21*r1-(v22-real_t(1))*r4-v23*r7)*px+(-v21*r3-(v22-real_t(1))*r6-v23*r9)*pz);
		_o.m[1][1] = ((real_t(2)*v21*r1+real_t(2)*(v22-real_t(1))*r4+real_t(2)*v23*r7)*pz+(-real_t(2)*v21*r3-real_t(2)*(v22-real_t(1))*r6-real_t(2)*v23*r9)*px);
		_o.m[1][2] = (v21*r1+(v22-real_t(1))*r4+v23*r7)*px+(v21*r3+(v22-real_t(1))*r6+v23*r9)*pz+(v21*r2+(v22-real_t(1))*r5+v23*r8)*py;

		_o.m[2][0] = ((v31*r2+v32*r5+(v33-real_t(1))*r8)*py+(-v31*r1-v32*r4-(v33-real_t(1))*r7)*px+(-v31*r3-v32*r6-(v33-real_t(1))*r9)*pz);
		_o.m[2][1] = ((real_t(2)*v31*r1+real_t(2)*v32*r4+real_t(2)*(v33-real_t(1))*r7)*pz+(-real_t(2)*v31*r3-real_t(2)*v32*r6-real_t(2)*(v33-real_t(1))*r9)*px);
		_o.m[2][2] = (v31*r1+v32*r4+(v33-real_t(1))*r7)*px+(v31*r3+v32*r6+(v33-real_t(1))*r9)*pz+(v31*r2+v32*r5+(v33-real_t(1))*r8)*py;

		mat33_add(_opt_t,_o);
	}

	mat33_t opt_t;
	mat33_mult(opt_t,G,_opt_t);
	real_t E_2[5] = {0,0,0,0,0};
	for(i=0; i<n; i++)
	{
#if 0	  
		const real_t v11 = V.at(i).m[0][0]; 
		const real_t v21 = V.at(i).m[1][0];
		const real_t v31 = V.at(i).m[2][0];
		const real_t v12 = V.at(i).m[0][1]; 
		const real_t v22 = V.at(i).m[1][1];
		const real_t v32 = V.at(i).m[2][1];
		const real_t v13 = V.at(i).m[0][2]; 
		const real_t v23 = V.at(i).m[1][2];
		const real_t v33 = V.at(i).m[2][2];
#endif
		const real_t px = p.at(i).v[0];
		const real_t py = p.at(i).v[1];
		const real_t pz = p.at(i).v[2];

		mat33_t Rpi;
		mat33_assign(Rpi,-px,real_t(2)*pz,px,py,real_t(0),py,-pz,-real_t(2)*px,pz);

		mat33_t E,_e1,_e2;
		mat33_eye(_e1);
		mat33_sub(_e1,V.at(i));
		mat33_mult(_e2,Rz,Rpi);
		mat33_add(_e2,opt_t);
		mat33_mult(E,_e1,_e2);
		vec3_t e2,e1,e0;
		mat33_to_col_vec3(e2,e1,e0,E);
		vec3_t _E2_0,_E2_1,_E2_2,_E2_3,_E2_4;
		vec3_copy(_E2_0,e2);
		vec3_mult(_E2_0,e2);
		vec3_copy(_E2_1,e1);
		vec3_mult(_E2_1,e2);
		vec3_mult(_E2_1,2.0f);
		vec3_copy(_E2_2,e0);
		vec3_mult(_E2_2,e2);
		vec3_mult(_E2_2,2.0f);
		vec3_t _e1_sq;
		vec3_copy(_e1_sq,e1);
		vec3_mult(_e1_sq,e1);
		vec3_add(_E2_2,_e1_sq);
		vec3_copy(_E2_3,e0);
		vec3_mult(_E2_3,e1);
		vec3_mult(_E2_3,2.0f);
		vec3_copy(_E2_4,e0);
		vec3_mult(_E2_4,e0);
		E_2[0] += vec3_sum(_E2_0);
		E_2[1] += vec3_sum(_E2_1);
		E_2[2] += vec3_sum(_E2_2);
		E_2[3] += vec3_sum(_E2_3);
		E_2[4] += vec3_sum(_E2_4);
	}

	scalar_array _a;
	_a.resize(5);
	_a[4] = -E_2[1];
	_a[3] = real_t(4)*E_2[0] - real_t(2)*E_2[2];
	_a[2] = -real_t(3)*E_2[3] + real_t(3)*E_2[1];
	_a[1] = -real_t(4)*E_2[4] + real_t(2)*E_2[2];
	_a[0] = E_2[3];

	scalar_array at_sol;
	int num_sol = solve_polynomial(at_sol, _a);
	scalar_array e;
	e.resize(num_sol);
	scalar_array_clear(e);
	scalar_array at;
	scalar_array_add(e,_a[0]);
	at.clear();
	at.assign(at_sol.begin(),at_sol.end());
	scalar_array_mult(at,_a[1]);
	scalar_array_add(e,at);

	for(j=2; j<=4; j++)
	{
		at.clear();
		at.assign(at_sol.begin(),at_sol.end());
		scalar_array_pow(at,real_t(j));
		scalar_array_mult(at,_a[j]);
		scalar_array_add(e,at);
	}

	scalar_array at_;
	at_.clear();
	for(i=0; i<at.size(); i++)
	{
		if(_abs(e[i]) < real_t(1e-3)) at_.push_back(at_sol[i]);
	}

	scalar_array p1(at_.begin(),at_.end());
	scalar_array_pow(p1,2);
	scalar_array_add(p1,1);
	scalar_array_pow(p1,3);

	at.clear();
	for(i=0; i<at_.size(); i++)
	{
		if(_abs(p1[i]) > real_t(0.1f)) at.push_back(at_[i]);
	}

	scalar_array sa(at.begin(),at.end());
	scalar_array_mult(sa,2);
	scalar_array _ca1(at.begin(),at.end());
	scalar_array_pow(_ca1,2);
	scalar_array_add(_ca1,1);
	scalar_array ca(at.begin(),at.end());
	scalar_array_pow(ca,2);
	scalar_array_negate(ca);
	scalar_array_add(ca,1);
	scalar_array_div(ca,_ca1);
	scalar_array_div(sa,_ca1);
	scalar_array al;
	scalar_array_atan2(al,sa,ca);
	scalar_array_mult(al,real_t(180./CONST_PI));
	scalar_array _c_tMaxMin;
	_c_tMaxMin.resize(at.size());
	scalar_array_clear(_c_tMaxMin);
	scalar_array_add(_c_tMaxMin,_a[1]);
	scalar_array _at;
	_at.clear();
	_at.assign(at.begin(),at.end());
	scalar_array_mult(_at,_a[2]);
	scalar_array_mult(_at,2);
	scalar_array_add(_c_tMaxMin,_at);

	for(j=3; j<=4; j++)
	{
		_at.clear();
		_at.assign(at.begin(),at.end());
		scalar_array_pow(_at,(real_t)real_t(j)-real_t(1.0f));
		scalar_array_mult(_at,_a[j]);
		scalar_array_mult(_at,real_t(j));
		scalar_array_add(_c_tMaxMin,_at);
	}

	scalar_array tMaxMin(_c_tMaxMin.begin(),_c_tMaxMin.end());
	scalar_array al_;
	al_.clear();
	for(i=0; i<tMaxMin.size(); i++)
	{
		if(tMaxMin.at(i) > 0) al_.push_back(al.at(i));
	}

	tnew.resize(al_.size());
	for(unsigned int a=0; a<al_.size(); a++)
	{
		vec3_t rpy;
		vec3_assign(rpy,real_t(0),real_t(al_[a] * CONST_PI / real_t(180)), real_t(0));
		mat33_t R,Ry_;
		rpyMat(Ry_,rpy);
		mat33_mult(R,Rz,Ry_);
		vec3_t t_opt;
		vec3_clear(t_opt);

		for(i=0; i<n; i++)
		{
			mat33_t _m1,_eye3;
			mat33_eye(_eye3);
			mat33_copy(_m1,V.at(i));
			mat33_sub(_m1,_eye3);
			vec3_t _v1,_v2;
			vec3_mult(_v1,R,p.at(i));
			vec3_mult(_v2,_m1,_v1);
			vec3_add(t_opt,_v2);
		}

		vec3_t t_opt_;
		vec3_mult(t_opt_,G,t_opt);
		tnew.at(a) = t_opt_;
	}
	al_ret.assign(al_.begin(),al_.end());
}

// =====================================================================================

void getRfor2ndPose_V_Exact(pose_vec &sol, const vec3_array &v, const vec3_array &P,
					        const mat33_t R, const vec3_t t, const real_t DB)
{

	mat33_t RzN;
	decomposeR(RzN, R);
	mat33_t R_;
	mat33_mult(R_,R,RzN);
	mat33_t RzN_tr;
	mat33_transpose(RzN_tr,RzN);
	vec3_array P_;
	vec3_array_mult(P_,RzN_tr,P);
	vec3_t ang_zyx;
	rpyAng_X(ang_zyx,R_);
	vec3_t rpy;
	mat33_t Ry,Rz;
	vec3_assign(rpy,0,ang_zyx.v[1],0);
	rpyMat(Ry,rpy);
	vec3_assign(rpy,0,0,ang_zyx.v[2]);
	rpyMat(Rz,rpy);
	scalar_array bl;
	vec3_array Tnew;
	getRotationY_wrtT(bl,Tnew, v ,P_, t, DB, Rz);
	scalar_array_div(bl,180.0f/CONST_PI);
	const unsigned int n = (unsigned int) v.size();
	mat33_array V;
	V.resize(n);
	for(unsigned int i=0; i<n; i++)
	{
		vec3_mul_vec3trans(V.at(i),v.at(i),v.at(i));
		mat33_div(V.at(i),vec3trans_mul_vec3(v.at(i),v.at(i)));
	}

	sol.clear();
	sol.resize(bl.size());

	for(unsigned int j=0; j<(unsigned int)bl.size(); j++)
	{
		mat33_clear(Ry);
		vec3_assign(rpy,0,bl[j],0);
		rpyMat(Ry,rpy);
		mat33_t _m1;
		mat33_mult(_m1,Rz,Ry);
		mat33_mult(sol[j].R,_m1,RzN_tr);
		vec3_copy(sol[j].t,Tnew[j]);
		real_t E = 0;
		for(unsigned int i=0; i<n; i++)
		{
			mat33_t _m2;
			mat33_eye(_m2);
			mat33_sub(_m2,V.at(i));
			vec3_t _v1;
			vec3_mult(_v1,sol[j].R,P.at(i));
			vec3_add(_v1,sol[j].t);
			vec3_t _v2;
			vec3_mult(_v2,_m2,_v1);
			vec3_mult(_v2,_v2);
			E += vec3_sum(_v2);
		}
		sol[j].E = E;
	}
}

// =====================================================================================

void get2ndPose_Exact(pose_vec &sol, const vec3_array &v, const vec3_array &P,
					  const mat33_t R, const vec3_t t, const real_t DB)
{
	vec3_t cent, _v1;
	vec3_array _va1;
	normRv(_va1,v);
	vec3_array_mean(_v1,_va1);
	normRv(cent,_v1);
	mat33_t Rim;
	vec3_clear(_v1);
	_v1.v[2] = 1.0f;
	GetRotationbyVector(Rim,_v1,cent);
	vec3_array v_;
	vec3_array_mult(v_,Rim,v);
	normRv(_va1,v_);
	vec3_array_mean(_v1,_va1);
	normRv(cent,_v1);
	mat33_t R_;
	vec3_t  t_;
	mat33_mult(R_,Rim,R);
	vec3_mult(t_,Rim,t);
	getRfor2ndPose_V_Exact(sol,v_,P,R_,t_,DB);
	mat33_t Rim_tr;
	mat33_transpose(Rim_tr,Rim);
	for(unsigned int i=0; i<sol.size(); i++)
	{
		vec3_t _t;
		mat33_t _R;
		vec3_mult(_t,Rim_tr,sol[i].t);
		mat33_mult(_R,Rim_tr,sol[i].R);

		vec3_copy(sol[i].t,_t);
		mat33_copy(sol[i].R,_R);
	}
}

// =====================================================================================
void robust_pose(real_t &err, mat33_t &R, vec3_t &t,
		         const vec3_array &_model, const vec3_array &_iprts,
		         const options_t _options)
{
	mat33_t Rlu_;
	vec3_t tlu_;
	unsigned int it1_;
	real_t obj_err1_;
	real_t img_err1_;

	vec3_array model(_model.begin(),_model.end());
	vec3_array iprts(_iprts.begin(),_iprts.end());
	options_t options;
	memcpy(&options,&_options,sizeof(options_t));

	mat33_clear(Rlu_);
	vec3_clear(tlu_);
	it1_ = 0;
	obj_err1_ = 0;
	img_err1_ = 0;

	objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);

	pose_vec sol;
	sol.clear();
	get2ndPose_Exact(sol,iprts,model,Rlu_,tlu_,0);
	int min_err_idx = (-1);
	real_t min_err = MAX_FLOAT;
	for(unsigned int i=0; i<sol.size(); i++)
	{
		mat33_copy(options.initR,sol[i].R);
		objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);
		mat33_copy(sol[i].PoseLu_R,Rlu_);
		vec3_copy(sol[i].PoseLu_t,tlu_);
		sol[i].obj_err = obj_err1_;
		if(sol[i].obj_err < min_err)
		{
			min_err = sol[i].obj_err;
			min_err_idx = i;
		}
	}

	if(min_err_idx >= 0)
	{
		mat33_copy(R,sol[min_err_idx].PoseLu_R);
		vec3_copy(t,sol[min_err_idx].PoseLu_t);
		err = sol[min_err_idx].obj_err;
	}
	else
	{
		mat33_clear(R);
		vec3_clear(t);
		err = MAX_FLOAT;
	}
}

// ----------------------------------------
} // namespace rpp


#endif //_NO_LIBRPP_
