34 #include <visp3/core/vpExponentialMap.h>
76 const unsigned int v_size = 6;
77 const unsigned int index_0 = 0;
78 const unsigned int index_1 = 1;
79 const unsigned int index_2 = 2;
80 const unsigned int index_3 = 3;
81 const unsigned int index_4 = 4;
82 const unsigned int index_5 = 5;
84 if (v.
size() != v_size) {
86 "Cannot compute direct exponential map from a %d-dim velocity vector. Should be 6-dim.",
89 double theta, si, co, sinc, mcosc, msinc;
96 u[index_0] = v_dt[index_3];
97 u[index_1] = v_dt[index_4];
98 u[index_2] = v_dt[index_5];
101 theta = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2]));
108 dt[index_0] = ((v_dt[index_0] * (sinc + (u[index_0] * u[index_0] * msinc))) +
109 (v_dt[index_1] * ((u[index_0] * u[index_1] * msinc) - (u[index_2] * mcosc)))) +
110 (v_dt[index_2] * ((u[index_0] * u[index_2] * msinc) + (u[index_1] * mcosc)));
112 dt[index_1] = ((v_dt[index_0] * ((u[index_0] * u[index_1] * msinc) + (u[index_2] * mcosc))) +
113 (v_dt[index_1] * (sinc + (u[index_1] * u[index_1] * msinc)))) +
114 (v_dt[index_2] * ((u[index_1] * u[index_2] * msinc) - (u[index_0] * mcosc)));
116 dt[index_2] = ((v_dt[index_0] * ((u[index_0] * u[index_2] * msinc) - (u[index_1] * mcosc))) +
117 (v_dt[index_1] * ((u[index_1] * u[index_2] * msinc) + (u[index_0] * mcosc)))) +
118 (v_dt[index_2] * (sinc + (u[index_2] * u[index_2] * msinc)));
164 double theta, si, co, sinc, mcosc, msinc, det;
168 const unsigned int index_0 = 0;
169 const unsigned int index_1 = 1;
170 const unsigned int index_2 = 2;
171 const unsigned int index_3 = 3;
172 const unsigned int val_3 = 3;
176 for (i = 0; i < val_3; ++i) {
177 v[index_3 + i] = u[i];
180 theta = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2]));
191 a[index_0][index_0] = sinc + (u[index_0] * u[index_0] * msinc);
192 a[index_0][index_1] = (u[index_0] * u[index_1] * msinc) - (u[index_2] * mcosc);
193 a[index_0][index_2] = (u[index_0] * u[index_2] * msinc) + (u[index_1] * mcosc);
195 a[index_1][index_0] = (u[index_0] * u[index_1] * msinc) + (u[index_2] * mcosc);
196 a[index_1][index_1] = sinc + (u[index_1] * u[index_1] * msinc);
197 a[index_1][index_2] = (u[index_1] * u[index_2] * msinc) - (u[index_0] * mcosc);
199 a[index_2][index_0] = (u[index_0] * u[index_2] * msinc) - (u[index_1] * mcosc);
200 a[index_2][index_1] = (u[index_1] * u[index_2] * msinc) + (u[index_0] * mcosc);
201 a[index_2][index_2] = sinc + (u[index_2] * u[index_2] * msinc);
203 det = (((((a[index_0][index_0] * a[index_1][index_1] * a[index_2][index_2]) + (a[index_1][index_0] * a[index_2][index_1] * a[index_0][index_2])) + (a[index_0][index_1] * a[index_1][index_2] * a[index_2][index_0])) -
204 (a[index_2][index_0] * a[index_1][index_1] * a[index_0][index_2])) - (a[index_1][index_0] * a[index_0][index_1] * a[index_2][index_2])) - (a[index_0][index_0] * a[index_2][index_1] * a[index_1][index_2]);
206 if (fabs(det) > 1.e-5) {
207 v[index_0] = ((((((M[index_0][index_3] * a[index_1][index_1] * a[index_2][index_2]) + (M[index_1][index_3] * a[index_2][index_1] * a[index_0][index_2])) + (M[index_2][index_3] * a[index_0][index_1] * a[index_1][index_2])) -
208 (M[index_2][index_3] * a[index_1][index_1] * a[index_0][index_2])) - (M[index_1][index_3] * a[index_0][index_1] * a[index_2][index_2])) - (M[index_0][index_3] * a[index_2][index_1] * a[index_1][index_2])) /
210 v[index_1] = ((((((a[index_0][index_0] * M[index_1][index_3] * a[index_2][index_2]) + (a[index_1][index_0] * M[index_2][index_3] * a[index_0][index_2])) + (M[index_0][index_3] * a[index_1][index_2] * a[index_2][index_0])) -
211 (a[index_2][index_0] * M[index_1][index_3] * a[index_0][index_2])) - (a[index_1][index_0] * M[index_0][index_3] * a[index_2][index_2])) - (a[index_0][index_0] * M[index_2][index_3] * a[index_1][index_2])) /
213 v[index_2] = ((((((a[index_0][index_0] * a[index_1][index_1] * M[index_2][index_3]) + (a[index_1][index_0] * a[index_2][index_1] * M[index_0][index_3])) + (a[index_0][index_1] * M[index_1][index_3] * a[index_2][index_0])) -
214 (a[index_2][index_0] * a[index_1][index_1] * M[index_0][index_3])) - (a[index_1][index_0] * a[index_0][index_1] * M[index_2][index_3])) - (a[index_0][index_0] * a[index_2][index_1] * M[index_1][index_3])) /
218 v[index_0] = M[index_0][index_3];
219 v[index_1] = M[index_1][index_3];
220 v[index_2] = M[index_2][index_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.