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))) +
105 (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))) +
109 (v_dt[1] * (sinc + (u[1] * u[1] * msinc)))) +
110 (v_dt[2] * ((u[1] * u[2] * msinc) - (u[0] * mcosc)));
112 dt[2] = ((v_dt[0] * ((u[0] * u[2] * msinc) - (u[1] * mcosc))) +
113 (v_dt[1] * ((u[1] * u[2] * msinc) + (u[0] * mcosc)))) +
114 (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;
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)))));
158 for (i = 0; i < 3; ++i) {
159 for (j = 0; j < 3; ++j) {
174 for (i = 0; i < 4; ++i) {
175 for (j = 0; j < 4; ++j) {
176 if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) {
182 printf(
"pb vpHomogeneousMatrix::expMap\n");
183 std::cout <<
" Delta : " << std::endl << Delta << std::endl;
184 std::cout <<
" Delta_old : " << std::endl << Delta_old << std::endl;
229 double theta, si, co, sinc, mcosc, msinc, det;
236 for (i = 0; i < 3; ++i) {
240 theta = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]));
251 a[0][0] = sinc + (u[0] * u[0] * msinc);
252 a[0][1] = (u[0] * u[1] * msinc) - (u[2] * mcosc);
253 a[0][2] = (u[0] * u[2] * msinc) + (u[1] * mcosc);
255 a[1][0] = (u[0] * u[1] * msinc) + (u[2] * mcosc);
256 a[1][1] = sinc + (u[1] * u[1] * msinc);
257 a[1][2] = (u[1] * u[2] * msinc) - (u[0] * mcosc);
259 a[2][0] = (u[0] * u[2] * msinc) - (u[1] * mcosc);
260 a[2][1] = (u[1] * u[2] * msinc) + (u[0] * mcosc);
261 a[2][2] = sinc + (u[2] * u[2] * msinc);
263 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])) -
264 (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]);
266 if (fabs(det) > 1.e-5) {
267 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])) -
268 (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])) /
270 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])) -
271 (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])) /
273 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])) -
274 (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.