symmetric3.hpp 15.5 KB
Newer Older
jcarpent's avatar
jcarpent committed
1
//
2
// Copyright (c) 2014-2017 CNRS
jcarpent's avatar
jcarpent committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
//
18
19
20
// This file is originally copied from metapod/tools/spatial/lti.hh.
// Authors: Olivier Stasse (LAAS, CNRS) and Sébastien Barthélémy (Aldebaran Robotics)
// The file was modified in pinocchio by Nicolas Mansard (LAAS, CNRS)
jcarpent's avatar
jcarpent committed
21
//
22
23
24
25
// metapod is free software, distributed under the terms of the GNU Lesser
// General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
26

27
28
#ifndef __se3_symmetric3_hpp__
#define __se3_symmetric3_hpp__
29

30
#include "pinocchio/macros.hpp"
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

namespace se3
{

  template<typename _Scalar, int _Options>
  class Symmetric3Tpl
  {
  public:
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
    typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;
46
47
    
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48
49

  public:    
50
    Symmetric3Tpl(): data_() {}
51
52
53
54
55
56
57
58
59
60
    
//    template<typename D>
//    explicit Symmetric3Tpl(const Eigen::MatrixBase<D> & I)
//    {
//      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
//      assert( (I-I.transpose()).isMuchSmallerThan(I) );
//      data_(0) = I(0,0);
//      data_(1) = I(1,0); data_(2) = I(1,1);
//      data_(3) = I(2,0); data_(4) = I(2,1); data_(5) = I(2,2);
//    }
Nicolas Mansard's avatar
Nicolas Mansard committed
61
62
    template<typename Sc,int N,int Opt>
    explicit Symmetric3Tpl(const Eigen::Matrix<Sc,N,N,Opt> & I)
63
    {
64
      EIGEN_STATIC_ASSERT(N==3,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
65
      assert( (I-I.transpose()).isMuchSmallerThan(I) );
66
67
68
      data_(0) = I(0,0);
      data_(1) = I(1,0); data_(2) = I(1,1);
      data_(3) = I(2,0); data_(4) = I(2,1); data_(5) = I(2,2);
69
    }
70
71
72
73
74
    
    explicit Symmetric3Tpl(const Vector6 & I) : data_(I) {}
    
    Symmetric3Tpl(const Scalar & a0, const Scalar & a1, const Scalar & a2,
		  const Scalar & a3, const Scalar & a4, const Scalar & a5)
75
    { data_ << a0,a1,a2,a3,a4,a5; }
76

77
    static Symmetric3Tpl Zero()     { return Symmetric3Tpl(Vector6::Zero());  }
78
    void setZero() { data_.setZero(); }
79
    
80
81
    static Symmetric3Tpl Random()   { return RandomPositive();  }
    void setRandom()
82
    {
83
84
85
86
87
88
89
      Scalar
      a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
90
91
      
      data_ << a, b, c, d, e, f;
92
93
    }
    
94
    static Symmetric3Tpl Identity() { return Symmetric3Tpl(1, 0, 1, 0, 0, 1);  }
95
    void setIdentity() { data_ << 1, 0, 1, 0, 0, 1; }
96
97

    /* Requiered by Inertia::operator== */
98
    bool operator== (const Symmetric3Tpl & S2) const { return data_ == S2.data_; }
99
    
100
101
102
103
    bool isApprox(const Symmetric3Tpl & other,
                  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    { return data_.isApprox(other.data_,prec); }
    
104
    void fill(const Scalar value) { data_.fill(value); }
105
    
106
107
108
109
110
111
    struct SkewSquare
    {
      const Vector3 & v;
      SkewSquare( const Vector3 & v ) : v(v) {}
      operator Symmetric3Tpl () const 
      {
112
113
114
115
        const Scalar & x = v[0], & y = v[1], & z = v[2];
        return Symmetric3Tpl( -y*y-z*z,
                             x*y    ,  -x*x-z*z,
                             x*z    ,   y*z    ,  -x*x-y*y );
116
      }
117
118
    }; // struct SkewSquare
    
119
120
    Symmetric3Tpl operator- (const SkewSquare & v) const
    {
121
122
123
124
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
      return Symmetric3Tpl(data_[0]+y*y+z*z,
                           data_[1]-x*y,data_[2]+x*x+z*z,
                           data_[3]-x*z,data_[4]-y*z,data_[5]+x*x+y*y);
125
    }
126
    
127
128
    Symmetric3Tpl& operator-= (const SkewSquare & v)
    {
129
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
130
      data_[0]+=y*y+z*z;
131
132
      data_[1]-=x*y; data_[2]+=x*x+z*z;
      data_[3]-=x*z; data_[4]-=y*z; data_[5]+=x*x+y*y;
133
      return *this;
134
    }
jcarpent's avatar
jcarpent committed
135
136
137
138
139
140
141
142
143
144
    
    template<typename D>
    friend Matrix3 operator- (const Symmetric3Tpl & S, const Eigen::MatrixBase <D> & M)
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
      Matrix3 result (S.matrix());
      result -= M;
      
      return result;
    }
145
146
147

    struct AlphaSkewSquare
    {
148
149
150
151
      const Scalar & m;
      const Vector3 & v;
      
      AlphaSkewSquare(const Scalar & m, const SkewSquare & v) : m(m),v(v.v) {}
152
153
      AlphaSkewSquare(const Scalar & m, const Vector3 & v) : m(m),v(v) {}
      
154
155
      operator Symmetric3Tpl () const 
      {
156
157
158
159
        const Scalar & x = v[0], & y = v[1], & z = v[2];
        return Symmetric3Tpl(-m*(y*y+z*z),
                             m* x*y,-m*(x*x+z*z),
                             m* x*z,m* y*z,-m*(x*x+y*y));
160
161
      }
    };
162
163
    
    friend AlphaSkewSquare operator* (const Scalar & m, const SkewSquare & sk)
164
    { return AlphaSkewSquare(m,sk); }
165
    
166
167
    Symmetric3Tpl operator- (const AlphaSkewSquare & v) const
    {
168
169
170
171
172
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
      return Symmetric3Tpl(data_[0]+v.m*(y*y+z*z),
                           data_[1]-v.m* x*y, data_[2]+v.m*(x*x+z*z),
                           data_[3]-v.m* x*z, data_[4]-v.m* y*z,
                           data_[5]+v.m*(x*x+y*y));
173
    }
174
    
175
176
    Symmetric3Tpl& operator-= (const AlphaSkewSquare & v)
    {
177
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
178
      data_[0]+=v.m*(y*y+z*z);
179
180
      data_[1]-=v.m* x*y; data_[2]+=v.m*(x*x+z*z);
      data_[3]-=v.m* x*z; data_[4]-=v.m* y*z; data_[5]+=v.m*(x*x+y*y);
181
182
183
      return *this;
    }

184
185
186
    const Vector6 & data () const {return data_;}
    Vector6 & data () {return data_;}
    
187
188
    // static Symmetric3Tpl SkewSq( const Vector3 & v )
    // { 
189
190
191
192
    //   const Scalar & x = v[0], & y = v[1], & z = v[2];
    //   return Symmetric3Tpl(-y*y-z*z,
    // 			    x*y, -x*x-z*z,
    // 			    x*z, y*z, -x*x-y*y );
193
194
    // }

195
196
197
    /* Shoot a positive definite matrix. */
    static Symmetric3Tpl RandomPositive() 
    { 
198
199
200
201
202
203
204
      Scalar
      a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
205
206
207
208
      return Symmetric3Tpl(a*a+b*b+d*d,
			   a*b+b*c+d*e, b*b+c*c+e*e,
			   a*d+b*e+d*f, b*d+c*e+e*f,  d*d+e*e+f*f );
    }
209
    
210
211
212
    Matrix3 matrix() const
    {
      Matrix3 res;
213
214
215
      res(0,0) = data_(0); res(0,1) = data_(1); res(0,2) = data_(3);
      res(1,0) = data_(1); res(1,1) = data_(2); res(1,2) = data_(4);
      res(2,0) = data_(3); res(2,1) = data_(4); res(2,2) = data_(5);
216
217
218
      return res;
    }
    operator Matrix3 () const { return matrix(); }
jcarpent's avatar
jcarpent committed
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
    
    Scalar vtiv (const Vector3 & v) const
    {
      const Scalar & x = v[0];
      const Scalar & y = v[1];
      const Scalar & z = v[2];
      
      const Scalar xx = x*x;
      const Scalar xy = x*y;
      const Scalar xz = x*z;
      const Scalar yy = y*y;
      const Scalar yz = y*z;
      const Scalar zz = z*z;
      
      return data_(0)*xx + data_(2)*yy + data_(5)*zz + 2.*(data_(1)*xy + data_(3)*xz + data_(4)*yz);
    }
235
236
    
    ///
237
238
    /// \brief Performs the operation \f$ M = [v]_{\cross} S_{3} \f$.
    ///        This operation is equivalent to applying the cross product of v on each column of S.
239
240
241
242
243
244
245
246
    ///
    /// \tparam Vector3, Matrix3
    ///
    /// \param[in]  v  a vector of dimension 3.
    /// \param[in]  S3 a symmetric matrix of dimension 3x3.
    /// \param[out] M  an output matrix of dimension 3x3.
    ///
    template<typename Vector3, typename Matrix3>
247
248
249
    static void vxs(const Eigen::MatrixBase<Vector3> & v,
                    const Symmetric3Tpl & S3,
                    const Eigen::MatrixBase<Matrix3> & M)
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
      
      const Scalar & a = S3.data()[0];
      const Scalar & b = S3.data()[1];
      const Scalar & c = S3.data()[2];
      const Scalar & d = S3.data()[3];
      const Scalar & e = S3.data()[4];
      const Scalar & f = S3.data()[5];
      
      
      const typename Vector3::RealScalar & v0 = v[0];
      const typename Vector3::RealScalar & v1 = v[1];
      const typename Vector3::RealScalar & v2 = v[2];
      
      Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
      M_(0,0) = d * v1 - b * v2;
      M_(1,0) = a * v2 - d * v0;
      M_(2,0) = b * v0 - a * v1;
      
      M_(0,1) = e * v1 - c * v2;
      M_(1,1) = b * v2 - e * v0;
      M_(2,1) = c * v0 - b * v1;
      
      M_(0,2) = f * v1 - e * v2;
      M_(1,2) = d * v2 - f * v0;
      M_(2,2) = e * v0 - d * v1;
    }
    
    ///
    /// \brief Performs the operation \f$ [v]_{\cross} S \f$.
282
    ///        This operation is equivalent to applying the cross product of v on each column of S.
283
284
285
286
287
288
289
290
    ///
    /// \tparam Vector3
    ///
    /// \param[in]  v  a vector of dimension 3.
    ///
    /// \returns the result \f$ [v]_{\cross} S \f$.
    ///
    template<typename Vector3>
291
    Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
292
293
    {
      Matrix3 M;
294
      vxs(v,*this,M);
295
296
      return M;
    }
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
    
    ///
    /// \brief Performs the operation \f$ M = S_{3} [v]_{\cross \f$.
    ///
    /// \tparam Vector3, Matrix3
    ///
    /// \param[in]  v  a vector of dimension 3.
    /// \param[in]  S3 a symmetric matrix of dimension 3x3.
    /// \param[out] M  an output matrix of dimension 3x3.
    ///
    template<typename Vector3, typename Matrix3>
    static void svx(const Eigen::MatrixBase<Vector3> & v,
                    const Symmetric3Tpl & S3,
                    const Eigen::MatrixBase<Matrix3> & M)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
      
      const Scalar & a = S3.data()[0];
      const Scalar & b = S3.data()[1];
      const Scalar & c = S3.data()[2];
      const Scalar & d = S3.data()[3];
      const Scalar & e = S3.data()[4];
      const Scalar & f = S3.data()[5];
      
      const typename Vector3::RealScalar & v0 = v[0];
      const typename Vector3::RealScalar & v1 = v[1];
      const typename Vector3::RealScalar & v2 = v[2];
      
      Matrix3 & M_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
      M_(0,0) = b * v2 - d * v1;
      M_(1,0) = c * v2 - e * v1;
      M_(2,0) = e * v2 - f * v1;
      
      M_(0,1) = d * v0 - a * v2;
      M_(1,1) = e * v0 - b * v2;
      M_(2,1) = f * v0 - d * v2;
      
      M_(0,2) = a * v1 - b * v0;
      M_(1,2) = b * v1 - c * v0;
      M_(2,2) = d * v1 - e * v0;
    }
    
    /// \brief Performs the operation \f$ M = S_{3} [v]_{\cross \f$.
    ///
    /// \tparam Vector3
    ///
    /// \param[in]  v  a vector of dimension 3.
    ///
    /// \returns the result \f$ S [v]_{\cross} \f$.
    ///
    template<typename Vector3>
    Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
    {
      Matrix3 M;
      svx(v,*this,M);
      return M;
    }
355
356
357

    Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
    {
358
      return Symmetric3Tpl((data_+s2.data_).eval());
359
360
361
362
    }

    Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
    {
363
      data_ += s2.data_; return *this;
364
365
    }

366
367
368
369
    template<typename V3in, typename V3out>
    static void rhsMult(const Symmetric3Tpl & S3,
                        const Eigen::MatrixBase<V3in> & vin,
                        const Eigen::MatrixBase<V3out> & vout)
370
    {
371
372
373
374
375
376
377
378
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in,Vector3);
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out,Vector3);
      
      Eigen::MatrixBase<V3out> & vout_ = const_cast<Eigen::MatrixBase<V3out>&>(vout);
      
      vout_[0] = S3.data_(0) * vin[0] + S3.data_(1) * vin[1] + S3.data_(3) * vin[2];
      vout_[1] = S3.data_(1) * vin[0] + S3.data_(2) * vin[1] + S3.data_(4) * vin[2];
      vout_[2] = S3.data_(3) * vin[0] + S3.data_(4) * vin[1] + S3.data_(5) * vin[2];
379
380
    }

381
382
383
384
385
386
387
388
    template<typename V3>
    Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
    {
      Vector3 res;
      rhsMult(*this,v,res);
      return res;
    }
    
389
390
391
392
393
    // Matrix3 operator*(const Matrix3 &a) const
    // {
    //   Matrix3 r;
    //   for(unsigned int i=0; i<3; ++i)
    //     {
394
395
396
    //       r(0,i) = data_(0) * a(0,i) + data_(1) * a(1,i) + data_(3) * a(2,i);
    //       r(1,i) = data_(1) * a(0,i) + data_(2) * a(1,i) + data_(4) * a(2,i);
    //       r(2,i) = data_(3) * a(0,i) + data_(4) * a(1,i) + data_(5) * a(2,i);
397
398
399
400
401
402
    //     }
    //   return r;
    // }

    const Scalar& operator()(const int &i,const int &j) const
    {
403
      return ((i!=2)&&(j!=2)) ? data_[i+j] : data_[i+j+1];
404
405
406
407
408
    }

    Symmetric3Tpl operator-(const Matrix3 &S) const
    {
      assert( (S-S.transpose()).isMuchSmallerThan(S) );
409
410
411
      return Symmetric3Tpl( data_(0)-S(0,0),
			    data_(1)-S(1,0), data_(2)-S(1,1),
			    data_(3)-S(2,0), data_(4)-S(2,1), data_(5)-S(2,2) );
412
413
414
415
416
    }

    Symmetric3Tpl operator+(const Matrix3 &S) const
    {
      assert( (S-S.transpose()).isMuchSmallerThan(S) );
417
418
419
      return Symmetric3Tpl( data_(0)+S(0,0),
			    data_(1)+S(1,0), data_(2)+S(1,1),
			    data_(3)+S(2,0), data_(4)+S(2,1), data_(5)+S(2,2) );
420
421
422
423
424
425
426
427
428
429
430
    }

    /* --- Symmetric R*S*R' and R'*S*R products --- */
  public: //private:
    
    /** \brief Computes L for a symmetric matrix A.
     */
    Matrix32  decomposeltI() const
    {
      Matrix32 L;
      L << 
431
432
433
	data_(0) - data_(5),    data_(1),
	data_(1),              data_(2) - data_(5),
	2*data_(3),            data_(4) + data_(4);
434
435
436
437
438
      return L;
    }

    /* R*S*R' */
    template<typename D>
439
    Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
440
    {
441
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
442
443
      assert( (R.transpose()*R).isApprox(Matrix3::Identity()) );

444
445
446
      Symmetric3Tpl Sres;
      
      // 4 a
447
      const Matrix32 L( decomposeltI() );
448
449
      
      // Y = R' L   ===> (12 m + 8 a)
450
      const Matrix2 Y( R.template block<2,3>(1,0) * L );
451
	
452
      // Sres= Y R  ===> (16 m + 8a)
453
454
455
456
457
      Sres.data_(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
      Sres.data_(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
      Sres.data_(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
      Sres.data_(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
      Sres.data_(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
458
459

      // r=R' v ( 6m + 3a)
460
461
462
      const Vector3 r(-R(0,0)*data_(4) + R(0,1)*data_(3),
                      -R(1,0)*data_(4) + R(1,1)*data_(3),
                      -R(2,0)*data_(4) + R(2,1)*data_(3));
463
464

      // Sres_11 (3a)
465
      Sres.data_(0) = L(0,0) + L(1,1) - Sres.data_(2) - Sres.data_(5);
466
	
467
      // Sres + D + (Ev)x ( 9a)
468
      Sres.data_(0) += data_(5); 
469
470
      Sres.data_(1) += r(2); Sres.data_(2)+= data_(5);
      Sres.data_(3) +=-r(1); Sres.data_(4)+= r(0); Sres.data_(5) += data_(5);
471

472
      return Sres;
473
474
    }

jcarpent's avatar
jcarpent committed
475
476
477
  protected:
    Vector6 data_;
    
478
479
480
481
  };

} // namespace se3

482
#endif // ifndef __se3_symmetric3_hpp__
483