39 #include <visp3/core/vpExponentialMap.h>
88 double theta,si,co,sinc,mcosc,msinc;
100 theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
107 dt[0] = v_dt[0]*(sinc + u[0]*u[0]*msinc)
108 + v_dt[1]*(u[0]*u[1]*msinc - u[2]*mcosc)
109 + v_dt[2]*(u[0]*u[2]*msinc + u[1]*mcosc);
111 dt[1] = v_dt[0]*(u[0]*u[1]*msinc + u[2]*mcosc)
112 + v_dt[1]*(sinc + u[1]*u[1]*msinc)
113 + v_dt[2]*(u[1]*u[2]*msinc - u[0]*mcosc);
115 dt[2] = v_dt[0]*(u[0]*u[2]*msinc - u[1]*mcosc)
116 + v_dt[1]*(u[1]*u[2]*msinc + u[0]*mcosc)
117 + v_dt[2]*(sinc + u[2]*u[2]*msinc);
130 double sinu,cosi,mcosi,s;
135 s = sqrt(v_dt[3]*v_dt[3] + v_dt[4]*v_dt[4] + v_dt[5]*v_dt[5]);
138 for (i=0;i<3;i++) u[i] = v_dt[3+i]/s;
142 rd[0][0] = cosi + mcosi*u[0]*u[0];
143 rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
144 rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
145 rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
146 rd[1][1] = cosi + mcosi*u[1]*u[1];
147 rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
148 rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
149 rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
150 rd[2][2] = cosi + mcosi*u[2]*u[2];
152 dt[0] = v_dt[0]*(sinu/s + u[0]*u[0]*(1-sinu/s))
153 + v_dt[1]*(u[0]*u[1]*(1-sinu/s)-u[2]*mcosi/s)
154 + v_dt[2]*(u[0]*u[2]*(1-sinu/s)+u[1]*mcosi/s);
156 dt[1] = v_dt[0]*(u[0]*u[1]*(1-sinu/s)+u[2]*mcosi/s)
157 + v_dt[1]*(sinu/s + u[1]*u[1]*(1-sinu/s))
158 + v_dt[2]*(u[1]*u[2]*(1-sinu/s)-u[0]*mcosi/s);
160 dt[2] = v_dt[0]*(u[0]*u[2]*(1-sinu/s)-u[1]*mcosi/s)
161 + v_dt[1]*(u[1]*u[2]*(1-sinu/s)+u[0]*mcosi/s)
162 + v_dt[2]*(sinu/s + u[2]*u[2]*(1-sinu/s));
168 for(j=0;j<3;j++) rd[i][j] = 0.0;
184 if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) pb = 1;
188 printf(
"pb vpHomogeneousMatrix::expMap\n");
189 std::cout <<
" Delta : " << std::endl << Delta << std::endl;
190 std::cout <<
" Delta_old : " << std::endl << Delta_old << std::endl;
242 double theta,si,co,sinc,mcosc,msinc,det;
249 for (i=0;i<3;i++) v[3+i] = u[i];
251 theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
262 a[0][0] = sinc + u[0]*u[0]*msinc;
263 a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
264 a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;
266 a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
267 a[1][1] = sinc + u[1]*u[1]*msinc;
268 a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;
270 a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
271 a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
272 a[2][2] = sinc + u[2]*u[2]*msinc;
274 det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
275 + a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
276 - a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];
278 if (fabs(det) > 1.e-5)
280 v[0] = (M[0][3]*a[1][1]*a[2][2]
281 + M[1][3]*a[2][1]*a[0][2]
282 + M[2][3]*a[0][1]*a[1][2]
283 - M[2][3]*a[1][1]*a[0][2]
284 - M[1][3]*a[0][1]*a[2][2]
285 - M[0][3]*a[2][1]*a[1][2])/det;
286 v[1] = (a[0][0]*M[1][3]*a[2][2]
287 + a[1][0]*M[2][3]*a[0][2]
288 + M[0][3]*a[1][2]*a[2][0]
289 - a[2][0]*M[1][3]*a[0][2]
290 - a[1][0]*M[0][3]*a[2][2]
291 - a[0][0]*M[2][3]*a[1][2])/det;
292 v[2] = (a[0][0]*a[1][1]*M[2][3]
293 + a[1][0]*a[2][1]*M[0][3]
294 + a[0][1]*M[1][3]*a[2][0]
295 - a[2][0]*a[1][1]*M[0][3]
296 - a[1][0]*a[0][1]*M[2][3]
297 - a[0][0]*a[2][1]*M[1][3])/det;
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
static double mcosc(double cosx, double x)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
void extract(vpRotationMatrix &R) const
Implementation of column vector and the associated operations.
static vpHomogeneousMatrix direct(const vpColVector &v)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
static double msinc(double sinx, double x)