00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
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)
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 }
00799
00800
00801 #endif //_NO_LIBRPP_
