se3quat.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 H. Strasdat
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #ifndef G2O_SE3QUAT_H_
28 #define G2O_SE3QUAT_H_
29 
30 #include "se3_ops.h"
31 
32 #include <Eigen/Core>
33 #include <Eigen/Geometry>
34 
35 namespace g2o {
36  using namespace Eigen;
37 
38  typedef Matrix<double, 6, 1> Vector6d;
39  typedef Matrix<double, 7, 1> Vector7d;
40 
41  class SE3Quat {
42  public:
44 
45 
46  protected:
47 
48  Quaterniond _r;
49  Vector3d _t;
50 
51 
52  public:
54  _r.setIdentity();
55  _t.setZero();
56  }
57 
58  SE3Quat(const Matrix3d& R, const Vector3d& t):_r(Quaterniond(R)),_t(t){
59  normalizeRotation();
60  }
61 
62  SE3Quat(const Quaterniond& q, const Vector3d& t):_r(q),_t(t){
63  normalizeRotation();
64  }
65 
69  template <typename Derived>
70  explicit SE3Quat(const MatrixBase<Derived>& v)
71  {
72  assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
73  if (v.size() == 6) {
74  for (int i=0; i<3; i++){
75  _t[i]=v[i];
76  _r.coeffs()(i)=v[i+3];
77  }
78  _r.w() = 0.; // recover the positive w
79  if (_r.norm()>1.){
80  _r.normalize();
81  } else {
82  double w2=1.-_r.squaredNorm();
83  _r.w()= (w2<0.) ? 0. : sqrt(w2);
84  }
85  }
86  else if (v.size() == 7) {
87  int idx = 0;
88  for (int i=0; i<3; ++i, ++idx)
89  _t(i) = v(idx);
90  for (int i=0; i<4; ++i, ++idx)
91  _r.coeffs()(i) = v(idx);
92  normalizeRotation();
93  }
94  }
95 
96  inline const Vector3d& translation() const {return _t;}
97 
98  inline void setTranslation(const Vector3d& t_) {_t = t_;}
99 
100  inline const Quaterniond& rotation() const {return _r;}
101 
102  void setRotation(const Quaterniond& r_) {_r=r_;}
103 
104  inline SE3Quat operator* (const SE3Quat& tr2) const{
105  SE3Quat result(*this);
106  result._t += _r*tr2._t;
107  result._r*=tr2._r;
108  result.normalizeRotation();
109  return result;
110  }
111 
112  inline SE3Quat& operator*= (const SE3Quat& tr2){
113  _t+=_r*tr2._t;
114  _r*=tr2._r;
115  normalizeRotation();
116  return *this;
117  }
118 
119  inline Vector3d operator* (const Vector3d& v) const {
120  return _t+_r*v;
121  }
122 
123  inline SE3Quat inverse() const{
124  SE3Quat ret;
125  ret._r=_r.conjugate();
126  ret._t=ret._r*(_t*-1.);
127  return ret;
128  }
129 
130  inline double operator [](int i) const {
131  assert(i<7);
132  if (i<3)
133  return _t[i];
134  return _r.coeffs()[i-3];
135  }
136 
137 
138  inline Vector7d toVector() const{
139  Vector7d v;
140  v[0]=_t(0);
141  v[1]=_t(1);
142  v[2]=_t(2);
143  v[3]=_r.x();
144  v[4]=_r.y();
145  v[5]=_r.z();
146  v[6]=_r.w();
147  return v;
148  }
149 
150  inline void fromVector(const Vector7d& v){
151  _r=Quaterniond(v[6], v[3], v[4], v[5]);
152  _t=Vector3d(v[0], v[1], v[2]);
153  }
154 
155  inline Vector6d toMinimalVector() const{
156  Vector6d v;
157  v[0]=_t(0);
158  v[1]=_t(1);
159  v[2]=_t(2);
160  v[3]=_r.x();
161  v[4]=_r.y();
162  v[5]=_r.z();
163  return v;
164  }
165 
166  inline void fromMinimalVector(const Vector6d& v){
167  double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];
168  if (w>0){
169  _r=Quaterniond(sqrt(w), v[3], v[4], v[5]);
170  } else {
171  _r=Quaterniond(0, -v[3], -v[4], -v[5]);
172  }
173  _t=Vector3d(v[0], v[1], v[2]);
174  }
175 
176 
177 
178  Vector6d log() const {
179  Vector6d res;
180  Matrix3d _R = _r.toRotationMatrix();
181  double d = 0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1);
182  Vector3d omega;
183  Vector3d upsilon;
184 
185 
186  Vector3d dR = deltaR(_R);
187  Matrix3d V_inv;
188 
189  if (d>0.99999)
190  {
191 
192  omega=0.5*dR;
193  Matrix3d Omega = skew(omega);
194  V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
195  }
196  else
197  {
198  double theta = acos(d);
199  omega = theta/(2*sqrt(1-d*d))*dR;
200  Matrix3d Omega = skew(omega);
201  V_inv = ( Matrix3d::Identity() - 0.5*Omega
202  + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
203  }
204 
205  upsilon = V_inv*_t;
206  for (int i=0; i<3;i++){
207  res[i]=omega[i];
208  }
209  for (int i=0; i<3;i++){
210  res[i+3]=upsilon[i];
211  }
212 
213  return res;
214 
215  }
216 
217  Vector3d map(const Vector3d & xyz) const
218  {
219  return _r*xyz + _t;
220  }
221 
222 
223  static SE3Quat exp(const Vector6d & update)
224  {
225  Vector3d omega;
226  for (int i=0; i<3; i++)
227  omega[i]=update[i];
228  Vector3d upsilon;
229  for (int i=0; i<3; i++)
230  upsilon[i]=update[i+3];
231 
232  double theta = omega.norm();
233  Matrix3d Omega = skew(omega);
234 
235  Matrix3d R;
236  Matrix3d V;
237  if (theta<0.00001)
238  {
239  //TODO: CHECK WHETHER THIS IS CORRECT!!!
240  R = (Matrix3d::Identity() + Omega + Omega*Omega);
241 
242  V = R;
243  }
244  else
245  {
246  Matrix3d Omega2 = Omega*Omega;
247 
248  R = (Matrix3d::Identity()
249  + sin(theta)/theta *Omega
250  + (1-cos(theta))/(theta*theta)*Omega2);
251 
252  V = (Matrix3d::Identity()
253  + (1-cos(theta))/(theta*theta)*Omega
254  + (theta-sin(theta))/(pow(theta,3))*Omega2);
255  }
256  return SE3Quat(Quaterniond(R),V*upsilon);
257  }
258 
259  Matrix<double, 6, 6> adj() const
260  {
261  Matrix3d R = _r.toRotationMatrix();
262  Matrix<double, 6, 6> res;
263  res.block(0,0,3,3) = R;
264  res.block(3,3,3,3) = R;
265  res.block(3,0,3,3) = skew(_t)*R;
266  res.block(0,3,3,3) = Matrix3d::Zero(3,3);
267  return res;
268  }
269 
270  Matrix<double,4,4> to_homogeneous_matrix() const
271  {
272  Matrix<double,4,4> homogeneous_matrix;
273  homogeneous_matrix.setIdentity();
274  homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
275  homogeneous_matrix.col(3).head(3) = translation();
276 
277  return homogeneous_matrix;
278  }
279 
281  if (_r.w()<0){
282  _r.coeffs() *= -1;
283  }
284  _r.normalize();
285  }
286 
290  operator Eigen::Isometry3d() const
291  {
292  Eigen::Isometry3d result = (Eigen::Isometry3d) rotation();
293  result.translation() = translation();
294  return result;
295  }
296  };
297 
298  inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)
299  {
300  out_str << se3.to_homogeneous_matrix() << std::endl;
301  return out_str;
302  }
303 
304 } // end namespace
305 
306 #endif
d
Matrix< double, 7, 1 > Vector7d
Definition: se3quat.h:39
std::ostream & operator<<(std::ostream &os, const G2OBatchStatistics &st)
Definition: batch_stats.cpp:48
void setRotation(const Quaterniond &r_)
Definition: se3quat.h:102
Vector3d deltaR(const Matrix3d &R)
Definition: se3_ops.h:41
Vector7d toVector() const
Definition: se3quat.h:138
Matrix< double, 6, 1 > Vector6d
Definition: se3quat.h:38
Vector6d log() const
Definition: se3quat.h:178
SE3Quat inverse() const
Definition: se3quat.h:123
Quaterniond _r
Definition: se3quat.h:48
void setTranslation(const Vector3d &t_)
Definition: se3quat.h:98
static SE3Quat exp(const Vector6d &update)
Definition: se3quat.h:223
Vector6d toMinimalVector() const
Definition: se3quat.h:155
SE3Quat(const MatrixBase< Derived > &v)
Definition: se3quat.h:70
void fromMinimalVector(const Vector6d &v)
Definition: se3quat.h:166
SE3Quat(const Quaterniond &q, const Vector3d &t)
Definition: se3quat.h:62
void normalizeRotation()
Definition: se3quat.h:280
void fromVector(const Vector7d &v)
Definition: se3quat.h:150
Vector3d _t
Definition: se3quat.h:49
const Vector3d & translation() const
Definition: se3quat.h:96
TFSIMD_FORCE_INLINE const tfScalar & w() const
Matrix3d skew(const Vector3d &v)
Definition: se3_ops.h:28
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: se3quat.h:43
Matrix< double, 4, 4 > to_homogeneous_matrix() const
Definition: se3quat.h:270
const Quaterniond & rotation() const
Definition: se3quat.h:100
Vector3d map(const Vector3d &xyz) const
Definition: se3quat.h:217
SE3Quat(const Matrix3d &R, const Vector3d &t)
Definition: se3quat.h:58
Matrix< double, 6, 6 > adj() const
Definition: se3quat.h:259


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47