rpp.cpp
Go to the documentation of this file.
00001 /* ========================================================================
00002  * PROJECT: ARToolKitPlus
00003  * ========================================================================
00004  *
00005  * The robust pose estimator algorithm has been provided by G. Schweighofer
00006  * and A. Pinz (Inst.of El.Measurement and Measurement Signal Processing,
00007  * Graz University of Technology). Details about the algorithm are given in
00008  * a Technical Report: TR-EMT-2005-01, available at:
00009  * http://www.emt.tu-graz.ac.at/publications/index.htm
00010  *
00011  * Ported from MATLAB to C by T.Pintaric (Vienna University of Technology).
00012  *
00013  * Copyright of the derived and new portions of this work
00014  *     (C) 2006 Graz University of Technology
00015  *
00016  * This framework is free software; you can redistribute it and/or modify
00017  * it under the terms of the GNU General Public License as published by
00018  * the Free Software Foundation; either version 2 of the License, or
00019  * (at your option) any later version.
00020  *
00021  * This framework is distributed in the hope that it will be useful,
00022  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00023  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00024  * GNU General Public License for more details.
00025  *
00026  * You should have received a copy of the GNU General Public License
00027  * along with this framework; if not, write to the Free Software
00028  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00029  *
00030  * For further information please contact 
00031  *   Dieter Schmalstieg
00032  *   <schmalstieg@icg.tu-graz.ac.at>
00033  *   Graz University of Technology, 
00034  *   Institut for Computer Graphics and Vision,
00035  *   Inffeldgasse 16a, 8010 Graz, Austria.
00036  * ========================================================================
00037  ** @author   Thomas Pintaric
00038  *
00039  * $Id: rpp.cpp 162 2006-04-19 21:28:10Z grabner $
00040  * @file
00041  * ======================================================================== */
00042 
00043 
00044 #ifndef _NO_LIBRPP_
00045 
00046 
00047 #include <vector>
00048 #include <string.h>
00049 #include "assert.h"
00050 
00051 #include "rpp/rpp.h"
00052 #include "rpp/rpp_const.h"
00053 #include "rpp/rpp_vecmat.h"
00054 
00055 namespace rpp {
00056 
00057 // ===========================================================================================
00058 void Quaternion_byAngleAndVector(quat_t &Q, const real_t &q_angle, const vec3_t &q_vector)
00059 {
00060         vec3_t rotation_axis;
00061         normRv(rotation_axis, q_vector);
00062         real_t f = _sin(q_angle/2.0f);
00063         vec3_copy(Q.v,rotation_axis);
00064         vec3_mult(Q.v,f);
00065         Q.s = _cos(q_angle/2.0f);
00066         quat_mult(Q, 1.0f / quat_norm(Q));
00067 }
00068 
00069 // ===========================================================================================
00070 
00071 void GetRotationbyVector(mat33_t &R, const vec3_t &v1, const vec3_t &v2)
00072 {
00073         real_t winkel = _acos(vec3_dot(v1,v2));
00074         quat_t QU;
00075         vec3_t vc;
00076         vec3_cross(vc,v2,v1);
00077         Quaternion_byAngleAndVector(QU,winkel,vc);
00078         mat33_from_quat(R,QU);
00079 }
00080 
00081 // ===========================================================================================
00082 void xform(vec3_array &Q, const vec3_array &P, const mat33_t &R, const vec3_t &t)
00083 {
00084         const unsigned int n = (unsigned int) P.size();
00085         for(unsigned int i=0; i<n; i++)
00086         {
00087                 vec3_mult(Q.at(i),R,P.at(i));
00088                 vec3_add(Q.at(i),t);
00089         }
00090 }
00091 // ===========================================================================================
00092 
00093 void xformproj(vec3_array &Qp, const vec3_array &P, const mat33_t &R, const vec3_t &t)
00094 {
00095         const unsigned int n = (unsigned int) P.size();
00096         vec3_array Q;
00097         Q.resize(n);
00098 
00099         for(unsigned int i=0; i<n; i++)
00100         {
00101                 vec3_mult(Q.at(i),R,P.at(i));
00102                 vec3_add(Q.at(i),t);
00103                 Qp.at(i).v[0] = Q.at(i).v[0] / Q.at(i).v[2]; 
00104                 Qp.at(i).v[1] = Q.at(i).v[1] / Q.at(i).v[2]; 
00105                 Qp.at(i).v[2] = 1.0;
00106         }
00107 }
00108 
00109 // ===========================================================================================
00110 void rpyMat(mat33_t &R, const vec3_t &rpy) // rpy: roll,pitch,yaw
00111 {
00112         const real_t cosA = _cos(rpy.v[2]);
00113         const real_t sinA = _sin(rpy.v[2]);
00114         const real_t cosB = _cos(rpy.v[1]);
00115         const real_t sinB = _sin(rpy.v[1]);
00116         const real_t cosC = _cos(rpy.v[0]);
00117         const real_t sinC = _sin(rpy.v[0]);
00118         const real_t cosAsinB = cosA * sinB;
00119         const real_t sinAsinB = sinA * sinB;
00120 
00121         R.m[0][0] = cosA*cosB;
00122         R.m[0][1] = cosAsinB*sinC-sinA*cosC;
00123         R.m[0][2] = cosAsinB*cosC+sinA*sinC;
00124 
00125         R.m[1][0] = sinA*cosB;
00126         R.m[1][1] = sinAsinB*sinC+cosA*cosC;
00127         R.m[1][2] = sinAsinB*cosC-cosA*sinC;
00128 
00129         R.m[2][0] = -sinB;
00130         R.m[2][1] = cosB*sinC;
00131         R.m[2][2] = cosB*cosC;
00132 }
00133 // ===========================================================================================
00134 void rpyAng(vec3_t &angs, const mat33_t &R)
00135 {
00136         const real_t sinB = -(R.m[2][0]);
00137         const real_t cosB = _sqrt(R.m[0][0]*R.m[0][0] + R.m[1][0]*R.m[1][0]);
00138 
00139         if(_abs(cosB) > real_t(1E-15))
00140         {
00141                 const real_t sinA = R.m[1][0] / cosB;
00142                 const real_t cosA = R.m[0][0] / cosB;
00143                 const real_t sinC = R.m[2][1] / cosB;
00144                 const real_t cosC = R.m[2][2] / cosB;
00145                 vec3_assign(angs,_atan2(sinC,cosC),_atan2(sinB,cosB),_atan2(sinA,cosA));
00146         }
00147         else
00148         {
00149                 const real_t sinC = (R.m[0][1] - R.m[1][2]) / 2.0f;
00150                 const real_t cosC = (R.m[1][1] - R.m[0][2]) / 2.0f;
00151                 vec3_assign(angs,_atan2(sinC,cosC),CONST_PI_OVER_2, 0.0f);
00152         }
00153 }
00154 
00155 // ===========================================================================================
00156 
00157 void rpyAng_X(vec3_t &ang_zyx, const mat33_t &R)
00158 {
00159         rpyAng(ang_zyx,R);
00160 
00161         if(_abs(ang_zyx.v[0]) > CONST_PI_OVER_2)
00162         {
00163                 while(_abs(ang_zyx.v[0]) > CONST_PI_OVER_2)
00164                 {
00165                         if(ang_zyx.v[0] > 0)
00166                         {
00167                                 vec3_assign(ang_zyx, ang_zyx.v[0]+CONST_PI,
00168                                                          3*CONST_PI-ang_zyx.v[1],
00169                                                                          ang_zyx.v[2]+CONST_PI);
00170                                 vec3_sub(ang_zyx,CONST_2_PI);
00171                         }
00172                         else
00173                         {
00174                                 vec3_assign(ang_zyx, ang_zyx.v[0]+CONST_PI,
00175                                                                          3*CONST_PI-ang_zyx.v[1],
00176                                                                          ang_zyx.v[2]+CONST_PI);
00177                         }
00178                 }
00179         }
00180 }
00181 
00182 // ===========================================================================================
00183 void decomposeR(mat33_t &Rz, const mat33_t &R)
00184 {
00185         real_t cl = _atan2(R.m[2][1],R.m[2][0]);
00186         vec3_t rpy;
00187         vec3_assign(rpy,0,0,cl);
00188         rpyMat(Rz,rpy);
00189 }
00190 
00191 // ===========================================================================================
00192 void abskernel(mat33_t &R, vec3_t &t, vec3_array &Qout, real_t &err2, 
00193                            const vec3_array _P, const vec3_array _Q, 
00194                            const mat33_array F, const mat33_t G)
00195 
00196 {
00197         unsigned i,j;
00198 
00199         vec3_array P(_P.begin(),_P.end());
00200         vec3_array Q(_Q.begin(),_Q.end());
00201         const unsigned int n = (unsigned int) P.size();
00202 
00203         for(i=0; i<n; i++)
00204         {
00205                 vec3_t _q;
00206                 vec3_mult(_q,F.at(i),_Q.at(i));
00207                 vec3_copy(Q.at(i),_q);
00208         }
00209 
00210         vec3_t pbar;
00211         vec3_array_sum(pbar,P);
00212         vec3_div(pbar,real_t(n));
00213         vec3_array_sub(P,pbar);
00214 
00215         vec3_t qbar;
00216         vec3_array_sum(qbar,Q);
00217         vec3_div(qbar,real_t(n));
00218         vec3_array_sub(Q,qbar);
00219 
00220         mat33_t M;
00221         mat33_clear(M);
00222         for(j=0; j<n; j++)
00223         {
00224                 mat33_t _m;
00225                 vec3_mul_vec3trans(_m,P.at(j),Q.at(j));
00226                 mat33_add(M,_m);
00227         }
00228 
00229         mat33_t _U;
00230         mat33_t _S;
00231         mat33_t _V;
00232         mat33_clear(_U);
00233         mat33_clear(_S);
00234         mat33_clear(_V);
00235         mat33_svd2(_U,_S,_V,M);
00236 
00237         mat33_t _Ut;
00238         mat33_transpose(_Ut,_U);
00239         mat33_mult(R,_V,_Ut);
00240 
00241         vec3_t _sum;
00242         vec3_clear(_sum);
00243         for(i=0; i<n; i++)
00244         {
00245                 vec3_t _v1,_v2;
00246                 vec3_mult(_v1,R,P.at(i));
00247                 vec3_mult(_v2,F.at(i),_v1);
00248                 vec3_add(_sum,_v2);
00249         }
00250 
00251         vec3_mult(t,G,_sum);
00252         xform(Qout,P,R,t);
00253         err2 = 0;
00254         for(i=0; i<n; i++)
00255         {
00256                 mat33_t _m1;
00257                 vec3_t _v1;
00258                 mat33_eye(_m1);
00259                 mat33_sub(_m1,F.at(i));
00260                 vec3_mult(_v1,_m1,Qout.at(i));
00261                 err2 += vec3_dot(_v1,_v1);
00262         }
00263 }
00264 // ===========================================================================================
00265 
00266 void objpose(mat33_t &R, vec3_t &t, unsigned int &it, real_t &obj_err, real_t &img_err,
00267                          bool calc_img_err, const vec3_array _P, const vec3_array Qp, const options_t options)
00268 {
00269         unsigned int i,j;
00270         vec3_array P(_P.begin(),_P.end());
00271 
00272         const unsigned int n = (unsigned int) P.size();
00273         vec3_t pbar;
00274         vec3_array_sum(pbar,P);
00275         vec3_div(pbar,real_t(n));
00276         vec3_array_sub(P,pbar);
00277         vec3_array Q(Qp.begin(),Qp.end());
00278         vec3_t ones;
00279         ones.v[0] = 1;
00280         ones.v[1] = 1;
00281         ones.v[2] = 1;
00282         const bool mask_z[3] = {0,0,1};
00283         vec3_array_set(Q,ones,mask_z);
00284         mat33_array F;
00285         F.resize(n);
00286         vec3_t V;
00287         for(i=0; i<n; i++)
00288         {
00289                 V.v[0] = Q.at(i).v[0] / Q.at(i).v[2];
00290                 V.v[1] = Q.at(i).v[1] / Q.at(i).v[2];
00291                 V.v[2] = 1.0;
00292                 mat33_t _m;
00293                 vec3_mul_vec3trans(_m,V,V);
00294                 mat33_div(_m,vec3trans_mul_vec3(V,V));
00295                 F.at(i) = _m;
00296         }
00297 
00298         mat33_t tFactor;
00299         {
00300                 mat33_t _m1,_m2,_m3;
00301                 mat33_eye(_m1);
00302                 mat33_array_sum(_m2,F);
00303                 mat33_div(_m2,real_t(n));
00304                 mat33_sub(_m3,_m1,_m2);
00305                 mat33_inv(tFactor,_m3);
00306                 mat33_div(tFactor,real_t(n));
00307         }
00308 
00309         it = 0;
00310         bool initR_approximate = mat33_all_zeros(options.initR);
00311         mat33_t Ri;
00312         vec3_t ti;
00313         vec3_array Qi; Qi.resize(n);
00314         real_t old_err, new_err;
00315 
00316         // ----------------------------------------------------------------------------------------
00317         if(!initR_approximate)
00318         {
00319                 mat33_copy(Ri,options.initR);
00320                 vec3_t _sum;
00321                 vec3_clear(_sum);
00322                 for(j=0; j<n; j++)
00323                 {
00324                         vec3_t _v1, _v2;
00325                         mat33_t _m1,_m2;
00326                         mat33_eye(_m1);              
00327                         mat33_sub(_m2,F.at(j),_m1);
00328                         vec3_mult(_v1,Ri,P.at(j));
00329                         vec3_mult(_v2,_m2,_v1);
00330                         vec3_add(_sum,_v2);
00331                 }
00332                 vec3_mult(ti,tFactor,_sum);
00333                 xform(Qi,P,Ri,ti);
00334                 old_err = 0;
00335                 vec3_t _v;
00336                 for(j=0; j<n; j++)
00337                 {
00338                         mat33_t _m1,_m2;
00339                         mat33_eye(_m1);
00340                         mat33_sub(_m2,F.at(j),_m1);
00341                         vec3_mult(_v,_m2,Qi.at(j));
00342                         old_err += vec3_dot(_v,_v);
00343                 }
00344         // ----------------------------------------------------------------------------------------
00345         }
00346         else
00347         {
00348                 abskernel(Ri,ti,Qi,old_err,P,Q,F,tFactor);
00349                 it = 1;
00350         }
00351         // ----------------------------------------------------------------------------------------
00352 
00353         abskernel(Ri,ti,Qi,new_err,P,Qi,F,tFactor);
00354         it = it + 1;
00355 
00356         while((_abs((old_err-new_err)/old_err) > options.tol) && (new_err > options.epsilon) &&
00357                   (options.max_iter == 0 || it<options.max_iter))
00358         {
00359                 old_err = new_err;
00360                 abskernel(Ri,ti,Qi,new_err,P,Qi,F,tFactor);
00361                 it = it + 1;
00362         }
00363 
00364 
00365         mat33_copy(R,Ri);
00366         vec3_copy(t,ti);
00367         obj_err = _sqrt(new_err/real_t(n));
00368 
00369         if(calc_img_err)
00370         {
00371                 vec3_array Qproj; Qproj.resize(n);
00372                 xformproj(Qproj, P, Ri, ti);
00373                 img_err = 0;
00374 
00375                 vec3_t _v;
00376                 for(unsigned int j=0; j<n; j++)
00377                 {
00378                         vec3_sub(_v,Qproj.at(j),Qp.at(j));
00379                         img_err += vec3_dot(_v,_v);
00380                 }
00381                 img_err = _sqrt(img_err/real_t(n));
00382         }
00383 
00384         if(t.v[2] < 0)
00385         {
00386                 mat33_mult(R,-1.0);
00387                 vec3_mult(t,-1.0);
00388         }
00389 
00390         vec3_t _ts;
00391         vec3_mult(_ts,Ri,pbar);
00392         vec3_sub(t,_ts);
00393 }
00394 
00395 // =====================================================================================
00396 
00397 void getRotationY_wrtT(scalar_array &al_ret, vec3_array &tnew, const vec3_array &v,
00398                                            const vec3_array &p, const vec3_t &t, const real_t &DB,
00399                                            const mat33_t &Rz)
00400 {
00401         unsigned int i,j;
00402         const unsigned int n = (unsigned int) v.size();
00403         mat33_array V;
00404         V.resize(n);
00405         for(i=0; i<n; i++)
00406         {
00407                 vec3_mul_vec3trans(V.at(i),v.at(i),v.at(i));
00408                 mat33_div(V.at(i), vec3trans_mul_vec3(v.at(i),v.at(i)));
00409         }
00410 
00411         mat33_t G, _g1, _g2, _g3;
00412         mat33_array_sum(_g1,V);
00413         mat33_eye(_g2);
00414         mat33_div(_g1,real_t(n));
00415         mat33_sub(_g3,_g2,_g1);
00416         mat33_inv(G, _g3);
00417         mat33_div(G,real_t(n));
00418         mat33_t _opt_t;
00419         mat33_clear(_opt_t);
00420 
00421         for(i=0; i<n; i++)
00422         {
00423                 const real_t v11 = V.at(i).m[0][0]; 
00424                 const real_t v21 = V.at(i).m[1][0];
00425                 const real_t v31 = V.at(i).m[2][0];
00426                 const real_t v12 = V.at(i).m[0][1]; 
00427                 const real_t v22 = V.at(i).m[1][1];
00428                 const real_t v32 = V.at(i).m[2][1];
00429                 const real_t v13 = V.at(i).m[0][2]; 
00430                 const real_t v23 = V.at(i).m[1][2];
00431                 const real_t v33 = V.at(i).m[2][2];
00432                 const real_t px = p.at(i).v[0];
00433                 const real_t py = p.at(i).v[1];
00434                 const real_t pz = p.at(i).v[2];
00435                 const real_t r1 = Rz.m[0][0];
00436                 const real_t r2 = Rz.m[0][1];
00437                 const real_t r3 = Rz.m[0][2];
00438                 const real_t r4 = Rz.m[1][0];
00439                 const real_t r5 = Rz.m[1][1];
00440                 const real_t r6 = Rz.m[1][2];
00441                 const real_t r7 = Rz.m[2][0];
00442                 const real_t r8 = Rz.m[2][1];
00443                 const real_t r9 = Rz.m[2][2];
00444 
00445                 mat33_t _o;
00446                 _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);
00447                 _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);
00448                 _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;
00449 
00450                 _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);
00451                 _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);
00452                 _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;
00453 
00454                 _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);
00455                 _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);
00456                 _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;
00457 
00458                 mat33_add(_opt_t,_o);
00459         }
00460 
00461         mat33_t opt_t;
00462         mat33_mult(opt_t,G,_opt_t);
00463         real_t E_2[5] = {0,0,0,0,0};
00464         for(i=0; i<n; i++)
00465         {
00466 #if 0     
00467                 const real_t v11 = V.at(i).m[0][0]; 
00468                 const real_t v21 = V.at(i).m[1][0];
00469                 const real_t v31 = V.at(i).m[2][0];
00470                 const real_t v12 = V.at(i).m[0][1]; 
00471                 const real_t v22 = V.at(i).m[1][1];
00472                 const real_t v32 = V.at(i).m[2][1];
00473                 const real_t v13 = V.at(i).m[0][2]; 
00474                 const real_t v23 = V.at(i).m[1][2];
00475                 const real_t v33 = V.at(i).m[2][2];
00476 #endif
00477                 const real_t px = p.at(i).v[0];
00478                 const real_t py = p.at(i).v[1];
00479                 const real_t pz = p.at(i).v[2];
00480 
00481                 mat33_t Rpi;
00482                 mat33_assign(Rpi,-px,real_t(2)*pz,px,py,real_t(0),py,-pz,-real_t(2)*px,pz);
00483 
00484                 mat33_t E,_e1,_e2;
00485                 mat33_eye(_e1);
00486                 mat33_sub(_e1,V.at(i));
00487                 mat33_mult(_e2,Rz,Rpi);
00488                 mat33_add(_e2,opt_t);
00489                 mat33_mult(E,_e1,_e2);
00490                 vec3_t e2,e1,e0;
00491                 mat33_to_col_vec3(e2,e1,e0,E);
00492                 vec3_t _E2_0,_E2_1,_E2_2,_E2_3,_E2_4;
00493                 vec3_copy(_E2_0,e2);
00494                 vec3_mult(_E2_0,e2);
00495                 vec3_copy(_E2_1,e1);
00496                 vec3_mult(_E2_1,e2);
00497                 vec3_mult(_E2_1,2.0f);
00498                 vec3_copy(_E2_2,e0);
00499                 vec3_mult(_E2_2,e2);
00500                 vec3_mult(_E2_2,2.0f);
00501                 vec3_t _e1_sq;
00502                 vec3_copy(_e1_sq,e1);
00503                 vec3_mult(_e1_sq,e1);
00504                 vec3_add(_E2_2,_e1_sq);
00505                 vec3_copy(_E2_3,e0);
00506                 vec3_mult(_E2_3,e1);
00507                 vec3_mult(_E2_3,2.0f);
00508                 vec3_copy(_E2_4,e0);
00509                 vec3_mult(_E2_4,e0);
00510                 E_2[0] += vec3_sum(_E2_0);
00511                 E_2[1] += vec3_sum(_E2_1);
00512                 E_2[2] += vec3_sum(_E2_2);
00513                 E_2[3] += vec3_sum(_E2_3);
00514                 E_2[4] += vec3_sum(_E2_4);
00515         }
00516 
00517         scalar_array _a;
00518         _a.resize(5);
00519         _a[4] = -E_2[1];
00520         _a[3] = real_t(4)*E_2[0] - real_t(2)*E_2[2];
00521         _a[2] = -real_t(3)*E_2[3] + real_t(3)*E_2[1];
00522         _a[1] = -real_t(4)*E_2[4] + real_t(2)*E_2[2];
00523         _a[0] = E_2[3];
00524 
00525         scalar_array at_sol;
00526         int num_sol = solve_polynomial(at_sol, _a);
00527         scalar_array e;
00528         e.resize(num_sol);
00529         scalar_array_clear(e);
00530         scalar_array at;
00531         scalar_array_add(e,_a[0]);
00532         at.clear();
00533         at.assign(at_sol.begin(),at_sol.end());
00534         scalar_array_mult(at,_a[1]);
00535         scalar_array_add(e,at);
00536 
00537         for(j=2; j<=4; j++)
00538         {
00539                 at.clear();
00540                 at.assign(at_sol.begin(),at_sol.end());
00541                 scalar_array_pow(at,real_t(j));
00542                 scalar_array_mult(at,_a[j]);
00543                 scalar_array_add(e,at);
00544         }
00545 
00546         scalar_array at_;
00547         at_.clear();
00548         for(i=0; i<at.size(); i++)
00549         {
00550                 if(_abs(e[i]) < real_t(1e-3)) at_.push_back(at_sol[i]);
00551         }
00552 
00553         scalar_array p1(at_.begin(),at_.end());
00554         scalar_array_pow(p1,2);
00555         scalar_array_add(p1,1);
00556         scalar_array_pow(p1,3);
00557 
00558         at.clear();
00559         for(i=0; i<at_.size(); i++)
00560         {
00561                 if(_abs(p1[i]) > real_t(0.1f)) at.push_back(at_[i]);
00562         }
00563 
00564         scalar_array sa(at.begin(),at.end());
00565         scalar_array_mult(sa,2);
00566         scalar_array _ca1(at.begin(),at.end());
00567         scalar_array_pow(_ca1,2);
00568         scalar_array_add(_ca1,1);
00569         scalar_array ca(at.begin(),at.end());
00570         scalar_array_pow(ca,2);
00571         scalar_array_negate(ca);
00572         scalar_array_add(ca,1);
00573         scalar_array_div(ca,_ca1);
00574         scalar_array_div(sa,_ca1);
00575         scalar_array al;
00576         scalar_array_atan2(al,sa,ca);
00577         scalar_array_mult(al,real_t(180./CONST_PI));
00578         scalar_array _c_tMaxMin;
00579         _c_tMaxMin.resize(at.size());
00580         scalar_array_clear(_c_tMaxMin);
00581         scalar_array_add(_c_tMaxMin,_a[1]);
00582         scalar_array _at;
00583         _at.clear();
00584         _at.assign(at.begin(),at.end());
00585         scalar_array_mult(_at,_a[2]);
00586         scalar_array_mult(_at,2);
00587         scalar_array_add(_c_tMaxMin,_at);
00588 
00589         for(j=3; j<=4; j++)
00590         {
00591                 _at.clear();
00592                 _at.assign(at.begin(),at.end());
00593                 scalar_array_pow(_at,(real_t)real_t(j)-real_t(1.0f));
00594                 scalar_array_mult(_at,_a[j]);
00595                 scalar_array_mult(_at,real_t(j));
00596                 scalar_array_add(_c_tMaxMin,_at);
00597         }
00598 
00599         scalar_array tMaxMin(_c_tMaxMin.begin(),_c_tMaxMin.end());
00600         scalar_array al_;
00601         al_.clear();
00602         for(i=0; i<tMaxMin.size(); i++)
00603         {
00604                 if(tMaxMin.at(i) > 0) al_.push_back(al.at(i));
00605         }
00606 
00607         tnew.resize(al_.size());
00608         for(unsigned int a=0; a<al_.size(); a++)
00609         {
00610                 vec3_t rpy;
00611                 vec3_assign(rpy,real_t(0),real_t(al_[a] * CONST_PI / real_t(180)), real_t(0));
00612                 mat33_t R,Ry_;
00613                 rpyMat(Ry_,rpy);
00614                 mat33_mult(R,Rz,Ry_);
00615                 vec3_t t_opt;
00616                 vec3_clear(t_opt);
00617 
00618                 for(i=0; i<n; i++)
00619                 {
00620                         mat33_t _m1,_eye3;
00621                         mat33_eye(_eye3);
00622                         mat33_copy(_m1,V.at(i));
00623                         mat33_sub(_m1,_eye3);
00624                         vec3_t _v1,_v2;
00625                         vec3_mult(_v1,R,p.at(i));
00626                         vec3_mult(_v2,_m1,_v1);
00627                         vec3_add(t_opt,_v2);
00628                 }
00629 
00630                 vec3_t t_opt_;
00631                 vec3_mult(t_opt_,G,t_opt);
00632                 tnew.at(a) = t_opt_;
00633         }
00634         al_ret.assign(al_.begin(),al_.end());
00635 }
00636 
00637 // =====================================================================================
00638 
00639 void getRfor2ndPose_V_Exact(pose_vec &sol, const vec3_array &v, const vec3_array &P,
00640                                                 const mat33_t R, const vec3_t t, const real_t DB)
00641 {
00642 
00643         mat33_t RzN;
00644         decomposeR(RzN, R);
00645         mat33_t R_;
00646         mat33_mult(R_,R,RzN);
00647         mat33_t RzN_tr;
00648         mat33_transpose(RzN_tr,RzN);
00649         vec3_array P_;
00650         vec3_array_mult(P_,RzN_tr,P);
00651         vec3_t ang_zyx;
00652         rpyAng_X(ang_zyx,R_);
00653         vec3_t rpy;
00654         mat33_t Ry,Rz;
00655         vec3_assign(rpy,0,ang_zyx.v[1],0);
00656         rpyMat(Ry,rpy);
00657         vec3_assign(rpy,0,0,ang_zyx.v[2]);
00658         rpyMat(Rz,rpy);
00659         scalar_array bl;
00660         vec3_array Tnew;
00661         getRotationY_wrtT(bl,Tnew, v ,P_, t, DB, Rz);
00662         scalar_array_div(bl,180.0f/CONST_PI);
00663         const unsigned int n = (unsigned int) v.size();
00664         mat33_array V;
00665         V.resize(n);
00666         for(unsigned int i=0; i<n; i++)
00667         {
00668                 vec3_mul_vec3trans(V.at(i),v.at(i),v.at(i));
00669                 mat33_div(V.at(i),vec3trans_mul_vec3(v.at(i),v.at(i)));
00670         }
00671 
00672         sol.clear();
00673         sol.resize(bl.size());
00674 
00675         for(unsigned int j=0; j<(unsigned int)bl.size(); j++)
00676         {
00677                 mat33_clear(Ry);
00678                 vec3_assign(rpy,0,bl[j],0);
00679                 rpyMat(Ry,rpy);
00680                 mat33_t _m1;
00681                 mat33_mult(_m1,Rz,Ry);
00682                 mat33_mult(sol[j].R,_m1,RzN_tr);
00683                 vec3_copy(sol[j].t,Tnew[j]);
00684                 real_t E = 0;
00685                 for(unsigned int i=0; i<n; i++)
00686                 {
00687                         mat33_t _m2;
00688                         mat33_eye(_m2);
00689                         mat33_sub(_m2,V.at(i));
00690                         vec3_t _v1;
00691                         vec3_mult(_v1,sol[j].R,P.at(i));
00692                         vec3_add(_v1,sol[j].t);
00693                         vec3_t _v2;
00694                         vec3_mult(_v2,_m2,_v1);
00695                         vec3_mult(_v2,_v2);
00696                         E += vec3_sum(_v2);
00697                 }
00698                 sol[j].E = E;
00699         }
00700 }
00701 
00702 // =====================================================================================
00703 
00704 void get2ndPose_Exact(pose_vec &sol, const vec3_array &v, const vec3_array &P,
00705                                           const mat33_t R, const vec3_t t, const real_t DB)
00706 {
00707         vec3_t cent, _v1;
00708         vec3_array _va1;
00709         normRv(_va1,v);
00710         vec3_array_mean(_v1,_va1);
00711         normRv(cent,_v1);
00712         mat33_t Rim;
00713         vec3_clear(_v1);
00714         _v1.v[2] = 1.0f;
00715         GetRotationbyVector(Rim,_v1,cent);
00716         vec3_array v_;
00717         vec3_array_mult(v_,Rim,v);
00718         normRv(_va1,v_);
00719         vec3_array_mean(_v1,_va1);
00720         normRv(cent,_v1);
00721         mat33_t R_;
00722         vec3_t  t_;
00723         mat33_mult(R_,Rim,R);
00724         vec3_mult(t_,Rim,t);
00725         getRfor2ndPose_V_Exact(sol,v_,P,R_,t_,DB);
00726         mat33_t Rim_tr;
00727         mat33_transpose(Rim_tr,Rim);
00728         for(unsigned int i=0; i<sol.size(); i++)
00729         {
00730                 vec3_t _t;
00731                 mat33_t _R;
00732                 vec3_mult(_t,Rim_tr,sol[i].t);
00733                 mat33_mult(_R,Rim_tr,sol[i].R);
00734 
00735                 vec3_copy(sol[i].t,_t);
00736                 mat33_copy(sol[i].R,_R);
00737         }
00738 }
00739 
00740 // =====================================================================================
00741 void robust_pose(real_t &err, mat33_t &R, vec3_t &t,
00742                          const vec3_array &_model, const vec3_array &_iprts,
00743                          const options_t _options)
00744 {
00745         mat33_t Rlu_;
00746         vec3_t tlu_;
00747         unsigned int it1_;
00748         real_t obj_err1_;
00749         real_t img_err1_;
00750 
00751         vec3_array model(_model.begin(),_model.end());
00752         vec3_array iprts(_iprts.begin(),_iprts.end());
00753         options_t options;
00754         memcpy(&options,&_options,sizeof(options_t));
00755 
00756         mat33_clear(Rlu_);
00757         vec3_clear(tlu_);
00758         it1_ = 0;
00759         obj_err1_ = 0;
00760         img_err1_ = 0;
00761 
00762         objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);
00763 
00764         pose_vec sol;
00765         sol.clear();
00766         get2ndPose_Exact(sol,iprts,model,Rlu_,tlu_,0);
00767         int min_err_idx = (-1);
00768         real_t min_err = MAX_FLOAT;
00769         for(unsigned int i=0; i<sol.size(); i++)
00770         {
00771                 mat33_copy(options.initR,sol[i].R);
00772                 objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);
00773                 mat33_copy(sol[i].PoseLu_R,Rlu_);
00774                 vec3_copy(sol[i].PoseLu_t,tlu_);
00775                 sol[i].obj_err = obj_err1_;
00776                 if(sol[i].obj_err < min_err)
00777                 {
00778                         min_err = sol[i].obj_err;
00779                         min_err_idx = i;
00780                 }
00781         }
00782 
00783         if(min_err_idx >= 0)
00784         {
00785                 mat33_copy(R,sol[min_err_idx].PoseLu_R);
00786                 vec3_copy(t,sol[min_err_idx].PoseLu_t);
00787                 err = sol[min_err_idx].obj_err;
00788         }
00789         else
00790         {
00791                 mat33_clear(R);
00792                 vec3_clear(t);
00793                 err = MAX_FLOAT;
00794         }
00795 }
00796 
00797 // ----------------------------------------
00798 } // namespace rpp
00799 
00800 
00801 #endif //_NO_LIBRPP_


aruco_pose
Author(s): Julian Brunner
autogenerated on Mon Oct 6 2014 08:32:34