39 #include <visp3/core/vpExponentialMap.h>
82 "Cannot compute direct exponential map from a %d-dim velocity vector. Should be 6-dim.",
85 double theta, si, co, sinc, mcosc, msinc;
97 theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
104 dt[0] = v_dt[0] * (sinc + u[0] * u[0] * msinc) + v_dt[1] * (u[0] * u[1] * msinc - u[2] * mcosc) +
105 v_dt[2] * (u[0] * u[2] * msinc + u[1] * mcosc);
107 dt[1] = v_dt[0] * (u[0] * u[1] * msinc + u[2] * mcosc) + v_dt[1] * (sinc + u[1] * u[1] * msinc) +
108 v_dt[2] * (u[1] * u[2] * msinc - u[0] * mcosc);
110 dt[2] = v_dt[0] * (u[0] * u[2] * msinc - u[1] * mcosc) + v_dt[1] * (u[1] * u[2] * msinc + u[0] * mcosc) +
111 v_dt[2] * (sinc + u[2] * u[2] * msinc);
127 s = sqrt(v_dt[3] * v_dt[3] + v_dt[4] * v_dt[4] + v_dt[5] * v_dt[5]);
129 for (i = 0; i < 3; i++)
130 u[i] = v_dt[3 + i] / s;
131 double sinu = sin(s);
132 double cosi = cos(s);
133 double mcosi = 1 - cosi;
134 rd[0][0] = cosi + mcosi * u[0] * u[0];
135 rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
136 rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
137 rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
138 rd[1][1] = cosi + mcosi * u[1] * u[1];
139 rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
140 rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
141 rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
142 rd[2][2] = cosi + mcosi * u[2] * u[2];
144 dt[0] = v_dt[0] * (sinu / s + u[0] * u[0] * (1 - sinu / s)) +
145 v_dt[1] * (u[0] * u[1] * (1 - sinu / s) - u[2] * mcosi / s) +
146 v_dt[2] * (u[0] * u[2] * (1 - sinu / s) + u[1] * mcosi / s);
148 dt[1] = v_dt[0] * (u[0] * u[1] * (1 - sinu / s) + u[2] * mcosi / s) +
149 v_dt[1] * (sinu / s + u[1] * u[1] * (1 - sinu / s)) +
150 v_dt[2] * (u[1] * u[2] * (1 - sinu / s) - u[0] * mcosi / s);
152 dt[2] = v_dt[0] * (u[0] * u[2] * (1 - sinu / s) - u[1] * mcosi / s) +
153 v_dt[1] * (u[1] * u[2] * (1 - sinu / s) + u[0] * mcosi / s) +
154 v_dt[2] * (sinu / s + u[2] * u[2] * (1 - sinu / s));
156 for (i = 0; i < 3; i++) {
157 for (j = 0; j < 3; j++)
171 for (i = 0; i < 4; i++) {
172 for (j = 0; j < 4; j++)
173 if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5)
177 printf(
"pb vpHomogeneousMatrix::expMap\n");
178 std::cout <<
" Delta : " << std::endl << Delta << std::endl;
179 std::cout <<
" Delta_old : " << std::endl << Delta_old << std::endl;
224 double theta, si, co, sinc, mcosc, msinc, det;
231 for (i = 0; i < 3; i++)
234 theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
245 a[0][0] = sinc + u[0] * u[0] * msinc;
246 a[0][1] = u[0] * u[1] * msinc - u[2] * mcosc;
247 a[0][2] = u[0] * u[2] * msinc + u[1] * mcosc;
249 a[1][0] = u[0] * u[1] * msinc + u[2] * mcosc;
250 a[1][1] = sinc + u[1] * u[1] * msinc;
251 a[1][2] = u[1] * u[2] * msinc - u[0] * mcosc;
253 a[2][0] = u[0] * u[2] * msinc - u[1] * mcosc;
254 a[2][1] = u[1] * u[2] * msinc + u[0] * mcosc;
255 a[2][2] = sinc + u[2] * u[2] * msinc;
257 det = a[0][0] * a[1][1] * a[2][2] + a[1][0] * a[2][1] * a[0][2] + a[0][1] * a[1][2] * a[2][0] -
258 a[2][0] * a[1][1] * a[0][2] - a[1][0] * a[0][1] * a[2][2] - a[0][0] * a[2][1] * a[1][2];
260 if (fabs(det) > 1.e-5) {
261 v[0] = (M[0][3] * a[1][1] * a[2][2] + M[1][3] * a[2][1] * a[0][2] + M[2][3] * a[0][1] * a[1][2] -
262 M[2][3] * a[1][1] * a[0][2] - M[1][3] * a[0][1] * a[2][2] - M[0][3] * a[2][1] * a[1][2]) /
264 v[1] = (a[0][0] * M[1][3] * a[2][2] + a[1][0] * M[2][3] * a[0][2] + M[0][3] * a[1][2] * a[2][0] -
265 a[2][0] * M[1][3] * a[0][2] - a[1][0] * M[0][3] * a[2][2] - a[0][0] * M[2][3] * a[1][2]) /
267 v[2] = (a[0][0] * a[1][1] * M[2][3] + a[1][0] * a[2][1] * M[0][3] + a[0][1] * M[1][3] * a[2][0] -
268 a[2][0] * a[1][1] * M[0][3] - a[1][0] * a[0][1] * M[2][3] - a[0][0] * a[2][1] * M[1][3]) /
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
static vpHomogeneousMatrix direct(const vpColVector &v)
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
void insert(const vpRotationMatrix &R)
static double msinc(double sinx, double x)
static double sinc(double x)
static double mcosc(double cosx, double x)
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.