sim3.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_SIM_3
28 #define G2O_SIM_3
29 
30 #include "se3_ops.h"
31 #include <Eigen/Geometry>
32 
33 namespace g2o
34 {
35  using namespace Eigen;
36 
37  typedef Matrix <double, 7, 1> Vector7d;
38  typedef Matrix <double, 7, 7> Matrix7d;
39 
40 
41  struct Sim3
42  {
43  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 
45  protected:
46  Quaterniond r;
47  Vector3d t;
48  double s;
49 
50 
51 public:
52  Sim3()
53  {
54  r.setIdentity();
55  t.fill(0.);
56  s=1.;
57  }
58 
59  Sim3(const Quaterniond & r, const Vector3d & t, double s)
60  : r(r),t(t),s(s)
61  {
62  }
63 
64  Sim3(const Matrix3d & R, const Vector3d & t, double s)
65  : r(Quaterniond(R)),t(t),s(s)
66  {
67  }
68 
69 
70  Sim3(const Vector7d & update)
71  {
72 
73  Vector3d omega;
74  for (int i=0; i<3; i++)
75  omega[i]=update[i];
76 
77  Vector3d upsilon;
78  for (int i=0; i<3; i++)
79  upsilon[i]=update[i+3];
80 
81  double sigma = update[6];
82  double theta = omega.norm();
83  Matrix3d Omega = skew(omega);
84  s = std::exp(sigma);
85  Matrix3d Omega2 = Omega*Omega;
86  Matrix3d I;
87  I.setIdentity();
88  Matrix3d R;
89 
90  double eps = 0.00001;
91  double A,B,C;
92  if (fabs(sigma)<eps)
93  {
94  C = 1;
95  if (theta<eps)
96  {
97  A = 1./2.;
98  B = 1./6.;
99  R = (I + Omega + Omega*Omega);
100  }
101  else
102  {
103  double theta2= theta*theta;
104  A = (1-cos(theta))/(theta2);
105  B = (theta-sin(theta))/(theta2*theta);
106  R = I + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2;
107  }
108  }
109  else
110  {
111  C=(s-1)/sigma;
112  if (theta<eps)
113  {
114  double sigma2= sigma*sigma;
115  A = ((sigma-1)*s+1)/sigma2;
116  B= ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
117  R = (I + Omega + Omega2);
118  }
119  else
120  {
121  R = I + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2;
122 
123 
124 
125  double a=s*sin(theta);
126  double b=s*cos(theta);
127  double theta2= theta*theta;
128  double sigma2= sigma*sigma;
129 
130  double c=theta2+sigma2;
131  A = (a*sigma+ (1-b)*theta)/(theta*c);
132  B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
133 
134  }
135  }
136  r = Quaterniond(R);
137 
138 
139 
140  Matrix3d W = A*Omega + B*Omega2 + C*I;
141  t = W*upsilon;
142  }
143 
144  Vector3d map (const Vector3d& xyz) const {
145  return s*(r*xyz) + t;
146  }
147 
148  Vector7d log() const
149  {
150  Vector7d res;
151  double sigma = std::log(s);
152 
153 
154 
155 
156  Vector3d omega;
157  Vector3d upsilon;
158 
159 
160  Matrix3d R = r.toRotationMatrix();
161  double d = 0.5*(R(0,0)+R(1,1)+R(2,2)-1);
162 
163  Matrix3d Omega;
164 
165  double eps = 0.00001;
166  Matrix3d I = Matrix3d::Identity();
167 
168  double A,B,C;
169  if (fabs(sigma)<eps)
170  {
171  C = 1;
172  if (d>1-eps)
173  {
174  omega=0.5*deltaR(R);
175  Omega = skew(omega);
176  A = 1./2.;
177  B = 1./6.;
178  }
179  else
180  {
181  double theta = acos(d);
182  double theta2 = theta*theta;
183  omega = theta/(2*sqrt(1-d*d))*deltaR(R);
184  Omega = skew(omega);
185  A = (1-cos(theta))/(theta2);
186  B = (theta-sin(theta))/(theta2*theta);
187  }
188  }
189  else
190  {
191  C=(s-1)/sigma;
192  if (d>1-eps)
193  {
194 
195  double sigma2 = sigma*sigma;
196  omega=0.5*deltaR(R);
197  Omega = skew(omega);
198  A = ((sigma-1)*s+1)/(sigma2);
199  B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
200  }
201  else
202  {
203  double theta = acos(d);
204  omega = theta/(2*sqrt(1-d*d))*deltaR(R);
205  Omega = skew(omega);
206  double theta2 = theta*theta;
207  double a=s*sin(theta);
208  double b=s*cos(theta);
209  double c=theta2 + sigma*sigma;
210  A = (a*sigma+ (1-b)*theta)/(theta*c);
211  B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
212  }
213  }
214 
215  Matrix3d W = A*Omega + B*Omega*Omega + C*I;
216 
217  upsilon = W.lu().solve(t);
218 
219 
220  for (int i=0; i<3; i++)
221  res[i] = omega[i];
222 
223  for (int i=0; i<3; i++)
224  res[i+3] = upsilon[i];
225 
226  res[6] = sigma;
227 
228  return res;
229 
230  }
231 
232 
233  Sim3 inverse() const
234  {
235  return Sim3(r.conjugate(), r.conjugate()*((-1./s)*t), 1./s);
236  }
237 
238 
239  double operator[](int i) const
240  {
241  assert(i<8);
242  if (i<4){
243 
244  return r.coeffs()[i];
245  }
246  if (i<7){
247  return t[i-4];
248  }
249  return s;
250  }
251 
252  double& operator[](int i)
253  {
254  assert(i<8);
255  if (i<4){
256 
257  return r.coeffs()[i];
258  }
259  if (i<7)
260  {
261  return t[i-4];
262  }
263  return s;
264  }
265 
266  Sim3 operator *(const Sim3& other) const {
267  Sim3 ret;
268  ret.r = r*other.r;
269  ret.t=s*(r*other.t)+t;
270  ret.s=s*other.s;
271  return ret;
272  }
273 
274  Sim3& operator *=(const Sim3& other){
275  Sim3 ret=(*this)*other;
276  *this=ret;
277  return *this;
278  }
279 
280  inline const Vector3d& translation() const {return t;}
281 
282  inline Vector3d& translation() {return t;}
283 
284  inline const Quaterniond& rotation() const {return r;}
285 
286  inline Quaterniond& rotation() {return r;}
287 
288  inline const double& scale() const {return s;}
289 
290  inline double& scale() {return s;}
291 
292  };
293 
294  inline std::ostream& operator <<(std::ostream& out_str,
295  const Sim3& sim3)
296  {
297  out_str << sim3.rotation().coeffs() << std::endl;
298  out_str << sim3.translation() << std::endl;
299  out_str << sim3.scale() << std::endl;
300 
301  return out_str;
302  }
303 
304 } // end namespace
305 
306 
307 #endif
d
Definition: sim3.h:41
Matrix< double, 7, 1 > Vector7d
Definition: se3quat.h:39
Vector3d & translation()
Definition: sim3.h:282
std::ostream & operator<<(std::ostream &os, const G2OBatchStatistics &st)
Definition: batch_stats.cpp:48
Sim3(const Quaterniond &r, const Vector3d &t, double s)
Definition: sim3.h:59
Vector3d deltaR(const Matrix3d &R)
Definition: se3_ops.h:41
Sim3 inverse() const
Definition: sim3.h:233
Quaterniond r
Definition: sim3.h:46
const Vector3d & translation() const
Definition: sim3.h:280
double & scale()
Definition: sim3.h:290
Sim3(const Matrix3d &R, const Vector3d &t, double s)
Definition: sim3.h:64
double s
Definition: sim3.h:48
Sim3()
Definition: sim3.h:52
Matrix< double, 7, 7 > Matrix7d
Definition: sim3.h:38
double operator[](int i) const
Definition: sim3.h:239
double & operator[](int i)
Definition: sim3.h:252
Sim3(const Vector7d &update)
Definition: sim3.h:70
const double & scale() const
Definition: sim3.h:288
Vector7d log() const
Definition: sim3.h:148
Matrix3d skew(const Vector3d &v)
Definition: se3_ops.h:28
Vector3d t
Definition: sim3.h:47
Vector3d map(const Vector3d &xyz) const
Definition: sim3.h:144
Quaterniond & rotation()
Definition: sim3.h:286
const Quaterniond & rotation() const
Definition: sim3.h:284


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