g2o
Loading...
Searching...
No Matches
Public Member Functions | Protected Attributes | List of all members
g2o::Sim3 Struct Reference

#include <sim3.h>

Public Member Functions

 Sim3 ()
 
 Sim3 (const Quaternion &r, const Vector3 &t, double s)
 
 Sim3 (const Matrix3 &R, const Vector3 &t, double s)
 
 Sim3 (const Vector7 &update)
 
Vector3 map (const Vector3 &xyz) const
 
Vector7 log () const
 
Sim3 inverse () const
 
double operator[] (int i) const
 
double & operator[] (int i)
 
Sim3 operator* (const Sim3 &other) const
 
Sim3 & operator*= (const Sim3 &other)
 
void normalizeRotation ()
 
const Vector3 & translation () const
 
Vector3 & translation ()
 
const Quaternion & rotation () const
 
Quaternion & rotation ()
 
const double & scale () const
 
double & scale ()
 

Protected Attributes

Quaternion r
 
Vector3 t
 
double s
 

Detailed Description

Definition at line 37 of file sim3.h.

Constructor & Destructor Documentation

◆ Sim3() [1/4]

g2o::Sim3::Sim3 ( )
inline

Definition at line 46 of file sim3.h.

46 {
47 r.setIdentity();
48 t.fill(0.);
49 s = 1.;
50 }
g2o::Sim3::s
double s
Definition sim3.h:43
g2o::Sim3::r
Quaternion r
Definition sim3.h:41
g2o::Sim3::t
Vector3 t
Definition sim3.h:42

References r, s, and t.

Referenced by inverse().

◆ Sim3() [2/4]

g2o::Sim3::Sim3 ( const Quaternion &  r,
const Vector3 &  t,
double  s 
)
inline

Definition at line 52 of file sim3.h.

52 : r(r), t(t), s(s) {
53 normalizeRotation();
54 }
g2o::Sim3::normalizeRotation
void normalizeRotation()
Definition sim3.h:239

References normalizeRotation().

◆ Sim3() [3/4]

g2o::Sim3::Sim3 ( const Matrix3 &  R,
const Vector3 &  t,
double  s 
)
inline

Definition at line 56 of file sim3.h.

57 : r(Quaternion(R)), t(t), s(s) {
58 normalizeRotation();
59 }
g2o::Quaternion
Eigen::Quaternion< double > Quaternion
Definition eigen_types.h:81

References normalizeRotation().

◆ Sim3() [4/4]

g2o::Sim3::Sim3 ( const Vector7 &  update)
inline

Definition at line 61 of file sim3.h.

61 {
62 Vector3 omega;
63 for (int i = 0; i < 3; i++) omega[i] = update[i];
64
65 Vector3 upsilon;
66 for (int i = 0; i < 3; i++) upsilon[i] = update[i + 3];
67
68 double sigma = update[6];
69 double theta = omega.norm();
70 Matrix3 Omega = skew(omega);
71 s = std::exp(sigma);
72 Matrix3 Omega2 = Omega * Omega;
73 Matrix3 I;
74 I.setIdentity();
75 Matrix3 R;
76
77 double eps = cst(0.00001);
78 double A, B, C;
79 if (fabs(sigma) < eps) {
80 C = 1;
81 if (theta < eps) {
82 A = cst(1. / 2.);
83 B = cst(1. / 6.);
84 R = (I + Omega +
85 Omega * Omega /
86 2); // R=I+(1-cos(theta))*a^a^+sin(theta)*a^~=(omit
87 // O(theta^3))=I+theta^2/2*a^a^+theta*a^
88 } else {
89 double theta2 = theta * theta;
90 A = (1 - std::cos(theta)) / (theta2);
91 B = (theta - std::sin(theta)) / (theta2 * theta);
92 R = I + std::sin(theta) / theta * Omega +
93 (1 - std::cos(theta)) / (theta * theta) * Omega2;
94 }
95 } else {
96 C = (s - 1) / sigma;
97 if (theta < eps) {
98 double sigma2 = sigma * sigma;
99 A = ((sigma - 1) * s + 1) / sigma2;
100 B = ((cst(0.5) * sigma2 - sigma + 1) * s - 1) /
101 (sigma2 *
102 sigma); // B=[C-((s*cos(theta)-1)*sigma+s*sin(theta)*theta)/(sigma^2+theta^2)]/theta^2~=(omit
103 // O(theta^2))=
104 //(1/2*s*sigma-s)/(sigma^2)+[C-(s-1)*sigma/(sigma^2+theta^2)]/theta^2~=(0.5*sigma^2*s-s*sigma)/sigma^3+[s-1]/sigma^3=[s*(0.5*sigma^2-sigma+1)-1]/sigma^3
105 R = (I + Omega +
106 Omega2 /
107 2); // R=I+(1-cos(theta))*a^a^+sin(theta)*a^~=I+theta^2/2*a^a^+theta*a^
108 } else {
109 R = I + std::sin(theta) / theta * Omega +
110 (1 - std::cos(theta)) / (theta * theta) * Omega2;
111 double a = s * std::sin(theta);
112 double b = s * std::cos(theta);
113 double theta2 = theta * theta;
114 double sigma2 = sigma * sigma;
115 double c = theta2 + sigma2;
116 A = (a * sigma + (1 - b) * theta) / (theta * c);
117 B = (C - ((b - 1) * sigma + a * theta) / (c)) * 1 / (theta2);
118 }
119 }
120 r = Quaternion(R);
121
122 Matrix3 W = A * Omega + B * Omega2 + C * I;
123 t = W * upsilon;
124 }
g2o::Vector3
VectorN< 3 > Vector3
Definition eigen_types.h:51
g2o::Matrix3
MatrixN< 3 > Matrix3
Definition eigen_types.h:72
g2o::skew
G2O_TYPES_SLAM3D_API Matrix3 skew(const Vector3 &v)
g2o::cst
constexpr double cst(long double v)
Definition misc.h:60

References g2o::cst(), r, s, g2o::skew(), and t.

Member Function Documentation

◆ inverse()

Sim3 g2o::Sim3::inverse ( ) const
inline

Definition at line 200 of file sim3.h.

200 {
201 return Sim3(r.conjugate(), r.conjugate() * ((-1 / s) * t), 1 / s);
202 }
g2o::Sim3::Sim3
Sim3()
Definition sim3.h:46

References r, s, Sim3(), and t.

Referenced by g2o::EdgeSim3::computeError(), g2o::EdgeInverseSim3ProjectXYZ::computeError(), g2o::EdgeSim3::read(), and ToVertexSE3().

◆ log()

Vector7 g2o::Sim3::log ( ) const
inline

Definition at line 128 of file sim3.h.

128 {
129 Vector7 res;
130 double sigma = std::log(s);
131
132 Vector3 omega;
133 Vector3 upsilon;
134
135 Matrix3 R = r.toRotationMatrix();
136 double d = cst(0.5) * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
137
138 Matrix3 Omega;
139
140 double eps = cst(0.00001);
141 Matrix3 I = Matrix3::Identity();
142
143 double A, B, C;
144 if (fabs(sigma) < eps) {
145 C = 1;
146 if (d > 1 - eps) {
147 omega = 0.5 * deltaR(R);
148 Omega = skew(omega);
149 A = cst(1. / 2.);
150 B = cst(1. / 6.);
151 } else {
152 double theta = std::acos(d);
153 double theta2 = theta * theta;
154 omega = theta / (2 * std::sqrt(1 - d * d)) * deltaR(R);
155 Omega = skew(omega);
156 A = (1 - std::cos(theta)) / (theta2);
157 B = (theta - std::sin(theta)) / (theta2 * theta);
158 }
159 } else {
160 C = (s - 1) / sigma;
161 if (d > 1 - eps) {
162 double sigma2 = sigma * sigma;
163 omega = cst(0.5) * deltaR(R);
164 Omega = skew(omega);
165 A = ((sigma - 1) * s + 1) / (sigma2);
166 B = ((cst(0.5) * sigma2 - sigma + 1) * s - 1) /
167 (sigma2 *
168 sigma); // B=[C-((s*cos(theta)-1)*sigma+s*sin(theta)*theta)/(sigma^2+theta^2)]/theta^2
169 // use
170 // limit(theta->0)(B)=limit(theta->0){[(sigma2+theta2)*(s*sigma*sin(theta)-s*sin(theta)-s*theta*cos(theta))+(s*cos(theta)*sigma-sigma+s*sin(theta)*theta)*2*theta]/(2*theta)}=
171 //=limit(theta->0)(s*sigma-s)*sin(theta)/(2*(sigma2+theta2)*theta)+limit(theta->0)[-s*cos(theta)/(2*(sigma2+theta2))+(s*cos(theta)*sigma-sigma+s*sin(theta)*theta)/(sigma2+theta2)^2]=
172 //=limit(theta->0)(s*sigma-s)*cos(theta)/(2*(sigma2+3*theta2))+-s/(2*sigma2)+(s-1)/sigma^3=
173 //=(s*sigma-s)/2/sigma2-s/2/sigma2+(s-1)/sigma^3=[(0.5*sigma2-sigma+1)*s-1]/sigma^3
174 } else {
175 double theta = std::acos(d);
176 omega = theta / (2 * std::sqrt(1 - d * d)) * deltaR(R);
177 Omega = skew(omega);
178 double theta2 = theta * theta;
179 double a = s * std::sin(theta);
180 double b = s * std::cos(theta);
181 double c = theta2 + sigma * sigma;
182 A = (a * sigma + (1 - b) * theta) / (theta * c);
183 B = (C - ((b - 1) * sigma + a * theta) / (c)) * 1 / (theta2);
184 }
185 }
186
187 Matrix3 W = A * Omega + B * Omega * Omega + C * I;
188
189 upsilon = W.lu().solve(t);
190
191 for (int i = 0; i < 3; i++) res[i] = omega[i];
192
193 for (int i = 0; i < 3; i++) res[i + 3] = upsilon[i];
194
195 res[6] = sigma;
196
197 return res;
198 }
g2o::Vector7
VectorN< 7 > Vector7
Definition eigen_types.h:54
g2o::deltaR
G2O_TYPES_SLAM3D_API Vector3 deltaR(const Matrix3 &R)

References g2o::cst(), g2o::deltaR(), r, s, g2o::skew(), and t.

Referenced by g2o::EdgeSim3::computeError(), g2o::VertexSim3Expmap::write(), and g2o::EdgeSim3::write().

◆ map()

Vector3 g2o::Sim3::map ( const Vector3 &  xyz) const
inline

Definition at line 126 of file sim3.h.

126{ return s * (r * xyz) + t; }

References r, s, and t.

Referenced by g2o::EdgeSim3ProjectXYZ::computeError(), and g2o::EdgeInverseSim3ProjectXYZ::computeError().

◆ normalizeRotation()

void g2o::Sim3::normalizeRotation ( )
inline

Definition at line 239 of file sim3.h.

239 {
240 if (r.w() < 0) {
241 r.coeffs() *= -1;
242 }
243 r.normalize();
244 }

References r.

Referenced by Sim3(), and Sim3().

◆ operator*()

Sim3 g2o::Sim3::operator* ( const Sim3 &  other) const
inline

Definition at line 226 of file sim3.h.

226 {
227 Sim3 ret;
228 ret.r = r * other.r;
229 ret.t = s * (r * other.t) + t;
230 ret.s = s * other.s;
231 return ret;
232 }

References r, s, and t.

◆ operator*=()

Sim3 & g2o::Sim3::operator*= ( const Sim3 &  other)
inline

Definition at line 234 of file sim3.h.

234 {
235 Sim3 ret = (*this) * other;
236 *this = ret;
237 return *this;
238 }

◆ operator[]() [1/2]

double & g2o::Sim3::operator[] ( int  i)
inline

Definition at line 215 of file sim3.h.

215 {
216 assert(i < 8);
217 if (i < 4) {
218 return r.coeffs()[i];
219 }
220 if (i < 7) {
221 return t[i - 4];
222 }
223 return s;
224 }

References r, s, and t.

◆ operator[]() [2/2]

double g2o::Sim3::operator[] ( int  i) const
inline

Definition at line 204 of file sim3.h.

204 {
205 assert(i < 8);
206 if (i < 4) {
207 return r.coeffs()[i];
208 }
209 if (i < 7) {
210 return t[i - 4];
211 }
212 return s;
213 }

References r, s, and t.

◆ rotation() [1/2]

Quaternion & g2o::Sim3::rotation ( )
inline

Definition at line 251 of file sim3.h.

251{ return r; }

References r.

◆ rotation() [2/2]

const Quaternion & g2o::Sim3::rotation ( ) const
inline

Definition at line 249 of file sim3.h.

249{ return r; }

References r.

Referenced by g2o::operator<<(), and ToVertexSE3().

◆ scale() [1/2]

double & g2o::Sim3::scale ( )
inline

Definition at line 255 of file sim3.h.

255{ return s; }

References s.

◆ scale() [2/2]

const double & g2o::Sim3::scale ( ) const
inline

Definition at line 253 of file sim3.h.

253{ return s; }

References s.

Referenced by g2o::operator<<().

◆ translation() [1/2]

Vector3 & g2o::Sim3::translation ( )
inline

Definition at line 247 of file sim3.h.

247{ return t; }

References t.

◆ translation() [2/2]

const Vector3 & g2o::Sim3::translation ( ) const
inline

Definition at line 245 of file sim3.h.

245{ return t; }

References t.

Referenced by g2o::operator<<(), and ToVertexSE3().

Member Data Documentation

◆ r

Quaternion g2o::Sim3::r
protected

Definition at line 41 of file sim3.h.

Referenced by inverse(), log(), map(), normalizeRotation(), operator*(), operator[](), operator[](), rotation(), rotation(), Sim3(), and Sim3().

◆ s

double g2o::Sim3::s
protected

Definition at line 43 of file sim3.h.

Referenced by inverse(), log(), map(), operator*(), operator[](), operator[](), scale(), scale(), Sim3(), and Sim3().

◆ t

Vector3 g2o::Sim3::t
protected

Definition at line 42 of file sim3.h.

Referenced by inverse(), log(), map(), operator*(), operator[](), operator[](), Sim3(), Sim3(), translation(), and translation().

The documentation for this struct was generated from the following file:
Generated on Tue Nov 11 2025 05:18:45 for g2o by doxygen 1.9.8