40 #include <visp3/core/vpExponentialMap.h>
83 "Cannot compute direct exponential map from a %d-dim velocity vector. Should be 6-dim.",
86 double theta, si, co, sinc, mcosc, msinc;
98 theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
105 dt[0] = v_dt[0] * (sinc + u[0] * u[0] * msinc) + v_dt[1] * (u[0] * u[1] * msinc - u[2] * mcosc) +
106 v_dt[2] * (u[0] * u[2] * msinc + u[1] * mcosc);
108 dt[1] = v_dt[0] * (u[0] * u[1] * msinc + u[2] * mcosc) + v_dt[1] * (sinc + u[1] * u[1] * msinc) +
109 v_dt[2] * (u[1] * u[2] * msinc - u[0] * mcosc);
111 dt[2] = v_dt[0] * (u[0] * u[2] * msinc - u[1] * mcosc) + v_dt[1] * (u[1] * u[2] * msinc + u[0] * mcosc) +
112 v_dt[2] * (sinc + u[2] * u[2] * msinc);
128 s = sqrt(v_dt[3] * v_dt[3] + v_dt[4] * v_dt[4] + v_dt[5] * v_dt[5]);
130 for (i = 0; i < 3; i++)
131 u[i] = v_dt[3 + i] / s;
132 double sinu = sin(s);
133 double cosi = cos(s);
134 double mcosi = 1 - cosi;
135 rd[0][0] = cosi + mcosi * u[0] * u[0];
136 rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
137 rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
138 rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
139 rd[1][1] = cosi + mcosi * u[1] * u[1];
140 rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
141 rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
142 rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
143 rd[2][2] = cosi + mcosi * u[2] * u[2];
145 dt[0] = v_dt[0] * (sinu / s + u[0] * u[0] * (1 - sinu / s)) +
146 v_dt[1] * (u[0] * u[1] * (1 - sinu / s) - u[2] * mcosi / s) +
147 v_dt[2] * (u[0] * u[2] * (1 - sinu / s) + u[1] * mcosi / s);
149 dt[1] = v_dt[0] * (u[0] * u[1] * (1 - sinu / s) + u[2] * mcosi / s) +
150 v_dt[1] * (sinu / s + u[1] * u[1] * (1 - sinu / s)) +
151 v_dt[2] * (u[1] * u[2] * (1 - sinu / s) - u[0] * mcosi / s);
153 dt[2] = v_dt[0] * (u[0] * u[2] * (1 - sinu / s) - u[1] * mcosi / s) +
154 v_dt[1] * (u[1] * u[2] * (1 - sinu / s) + u[0] * mcosi / s) +
155 v_dt[2] * (sinu / s + u[2] * u[2] * (1 - sinu / s));
157 for (i = 0; i < 3; i++) {
158 for (j = 0; j < 3; j++)
172 for (i = 0; i < 4; i++) {
173 for (j = 0; j < 4; j++)
174 if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5)
178 printf(
"pb vpHomogeneousMatrix::expMap\n");
179 std::cout <<
" Delta : " << std::endl << Delta << std::endl;
180 std::cout <<
" Delta_old : " << std::endl << Delta_old << std::endl;
225 double theta, si, co, sinc, mcosc, msinc, det;
232 for (i = 0; i < 3; i++)
235 theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
246 a[0][0] = sinc + u[0] * u[0] * msinc;
247 a[0][1] = u[0] * u[1] * msinc - u[2] * mcosc;
248 a[0][2] = u[0] * u[2] * msinc + u[1] * mcosc;
250 a[1][0] = u[0] * u[1] * msinc + u[2] * mcosc;
251 a[1][1] = sinc + u[1] * u[1] * msinc;
252 a[1][2] = u[1] * u[2] * msinc - u[0] * mcosc;
254 a[2][0] = u[0] * u[2] * msinc - u[1] * mcosc;
255 a[2][1] = u[1] * u[2] * msinc + u[0] * mcosc;
256 a[2][2] = sinc + u[2] * u[2] * msinc;
258 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] -
259 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];
261 if (fabs(det) > 1.e-5) {
262 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] -
263 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]) /
265 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] -
266 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]) /
268 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] -
269 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 emited 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.