g2o
Loading...
Searching...
No Matches
Public Member Functions | Static Public Member Functions | Public Attributes | Protected Attributes | List of all members
g2o::SE3Quat Class Reference

#include <se3quat.h>

Inheritance diagram for g2o::SE3Quat:
Inheritance graph
[legend]

Public Member Functions

 SE3Quat ()
 
 SE3Quat (const Matrix3 &R, const Vector3 &t)
 
 SE3Quat (const Quaternion &q, const Vector3 &t)
 
template<typename Derived >
 SE3Quat (const Eigen::MatrixBase< Derived > &v)
 
const Vector3 & translation () const
 
void setTranslation (const Vector3 &t_)
 
const Quaternion & rotation () const
 
void setRotation (const Quaternion &r_)
 
SE3Quat operator* (const SE3Quat &tr2) const
 
SE3Quat & operator*= (const SE3Quat &tr2)
 
Vector3 operator* (const Vector3 &v) const
 
SE3Quat inverse () const
 
double operator[] (int i) const
 
Vector7 toVector () const
 
void fromVector (const Vector7 &v)
 
Vector6 toMinimalVector () const
 
void fromMinimalVector (const Vector6 &v)
 
Vector6 log () const
 
Vector3 map (const Vector3 &xyz) const
 
Eigen::Matrix< double, 6, 6, Eigen::ColMajor > adj () const
 
Eigen::Matrix< double, 4, 4, Eigen::ColMajor > to_homogeneous_matrix () const
 
void normalizeRotation ()
 
 operator Isometry3 () const
 

Static Public Member Functions

static SE3Quat exp (const Vector6 &update)
 

Public Attributes

 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

Protected Attributes

Quaternion _r
 
Vector3 _t
 

Detailed Description

Definition at line 39 of file se3quat.h.

Constructor & Destructor Documentation

◆ SE3Quat() [1/4]

g2o::SE3Quat::SE3Quat ( )
inline

Definition at line 48 of file se3quat.h.

48 {
49 _r.setIdentity();
50 _t.setZero();
51 }
g2o::SE3Quat::_r
Quaternion _r
Definition se3quat.h:44
g2o::SE3Quat::_t
Vector3 _t
Definition se3quat.h:45

◆ SE3Quat() [2/4]

g2o::SE3Quat::SE3Quat ( const Matrix3 &  R,
const Vector3 &  t 
)
inline

Definition at line 53 of file se3quat.h.

53 : _r(Quaternion(R)), _t(t) {
54 normalizeRotation();
55 }
g2o::SE3Quat::normalizeRotation
void normalizeRotation()
Definition se3quat.h:251
g2o::Quaternion
Eigen::Quaternion< double > Quaternion
Definition eigen_types.h:81

◆ SE3Quat() [3/4]

g2o::SE3Quat::SE3Quat ( const Quaternion &  q,
const Vector3 &  t 
)
inline

Definition at line 57 of file se3quat.h.

57 : _r(q), _t(t) {
58 normalizeRotation();
59 }

◆ SE3Quat() [4/4]

template<typename Derived >
g2o::SE3Quat::SE3Quat ( const Eigen::MatrixBase< Derived > &  v)
inlineexplicit

templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6 or Map<Vector6>

Definition at line 66 of file se3quat.h.

66 {
67 assert((v.size() == 6 || v.size() == 7) &&
68 "Vector dimension does not match");
69 if (v.size() == 6) {
70 for (int i = 0; i < 3; i++) {
71 _t[i] = v[i];
72 _r.coeffs()(i) = v[i + 3];
73 }
74 _r.w() = 0.; // recover the positive w
75 if (_r.norm() > 1.) {
76 _r.normalize();
77 } else {
78 double w2 = cst(1.) - _r.squaredNorm();
79 _r.w() = (w2 < cst(0.)) ? cst(0.) : std::sqrt(w2);
80 }
81 } else if (v.size() == 7) {
82 int idx = 0;
83 for (int i = 0; i < 3; ++i, ++idx) _t(i) = v(idx);
84 for (int i = 0; i < 4; ++i, ++idx) _r.coeffs()(i) = v(idx);
85 normalizeRotation();
86 }
87 }
g2o::cst
constexpr double cst(long double v)
Definition misc.h:60

References g2o::cst().

Member Function Documentation

◆ adj()

Eigen::Matrix< double, 6, 6, Eigen::ColMajor > g2o::SE3Quat::adj ( ) const
inline

Definition at line 232 of file se3quat.h.

232 {
233 Matrix3 R = _r.toRotationMatrix();
234 Eigen::Matrix<double, 6, 6, Eigen::ColMajor> res;
235 res.block(0, 0, 3, 3) = R;
236 res.block(3, 3, 3, 3) = R;
237 res.block(3, 0, 3, 3) = skew(_t) * R;
238 res.block(0, 3, 3, 3) = Matrix3::Zero(3, 3);
239 return res;
240 }
g2o::Matrix3
MatrixN< 3 > Matrix3
Definition eigen_types.h:72
g2o::skew
G2O_TYPES_SLAM3D_API Matrix3 skew(const Vector3 &v)

References skew().

Referenced by g2o::EdgeSE3Expmap::linearizeOplus().

◆ exp()

static SE3Quat g2o::SE3Quat::exp ( const Vector6 &  update)
inlinestatic

Definition at line 202 of file se3quat.h.

202 {
203 Vector3 omega;
204 for (int i = 0; i < 3; i++) omega[i] = update[i];
205 Vector3 upsilon;
206 for (int i = 0; i < 3; i++) upsilon[i] = update[i + 3];
207
208 double theta = omega.norm();
209 Matrix3 Omega = skew(omega);
210
211 Matrix3 R;
212 Matrix3 V;
213 if (theta < cst(0.00001)) {
214 Matrix3 Omega2 = Omega * Omega;
215
216 R = (Matrix3::Identity() + Omega + cst(0.5) * Omega2);
217
218 V = (Matrix3::Identity() + cst(0.5) * Omega + cst(1.) / cst(6.) * Omega2);
219 } else {
220 Matrix3 Omega2 = Omega * Omega;
221
222 R = (Matrix3::Identity() + std::sin(theta) / theta * Omega +
223 (1 - std::cos(theta)) / (theta * theta) * Omega2);
224
225 V = (Matrix3::Identity() +
226 (1 - std::cos(theta)) / (theta * theta) * Omega +
227 (theta - std::sin(theta)) / (std::pow(theta, 3)) * Omega2);
228 }
229 return SE3Quat(Quaternion(R), V * upsilon);
230 }
g2o::Vector3
VectorN< 3 > Vector3
Definition eigen_types.h:51

References g2o::cst(), and skew().

Referenced by g2o::VertexSE3Expmap::oplusImpl().

◆ fromMinimalVector()

void g2o::SE3Quat::fromMinimalVector ( const Vector6 &  v)
inline

Definition at line 155 of file se3quat.h.

155 {
156 double w = cst(1.) - v[3] * v[3] - v[4] * v[4] - v[5] * v[5];
157 if (w > 0) {
158 _r = Quaternion(std::sqrt(w), v[3], v[4], v[5]);
159 } else {
160 _r = Quaternion(0, -v[3], -v[4], -v[5]);
161 }
162 _t = Vector3(v[0], v[1], v[2]);
163 }

References g2o::cst().

◆ fromVector()

void g2o::SE3Quat::fromVector ( const Vector7 &  v)
inline

Definition at line 139 of file se3quat.h.

139 {
140 _r = Quaternion(v[6], v[3], v[4], v[5]);
141 _t = Vector3(v[0], v[1], v[2]);
142 }

◆ inverse()

SE3Quat g2o::SE3Quat::inverse ( ) const
inline

Definition at line 114 of file se3quat.h.

114 {
115 SE3Quat ret;
116 ret._r = _r.conjugate();
117 ret._t = ret._r * (_t * -cst(1.));
118 return ret;
119 }

References _r, _t, and g2o::cst().

Referenced by g2o::EdgeProjectPSI2UV::computeError(), g2o::EdgeSBACam::computeError(), g2o::EdgeSE3Expmap::computeError(), g2o::EdgeSBAScale::initialEstimate(), g2o::EdgeProjectPSI2UV::linearizeOplus(), g2o::EdgeSE3Expmap::linearizeOplus(), main(), g2o::EdgeSBACam::setMeasurement(), g2o::EdgeSBACam::setMeasurementData(), and g2o::EdgeSBACam::setMeasurementFromState().

◆ log()

Vector6 g2o::SE3Quat::log ( ) const
inline

Definition at line 165 of file se3quat.h.

165 {
166 Vector6 res;
167 Matrix3 _R = _r.toRotationMatrix();
168 double d = cst(0.5) * (_R(0, 0) + _R(1, 1) + _R(2, 2) - 1);
169 Vector3 omega;
170 Vector3 upsilon;
171
172 Vector3 dR = deltaR(_R);
173 Matrix3 V_inv;
174
175 if (std::abs(d) > cst(0.99999)) {
176 omega = 0.5 * dR;
177 Matrix3 Omega = skew(omega);
178 V_inv = Matrix3::Identity() - cst(0.5) * Omega +
179 (cst(1.) / cst(12.)) * (Omega * Omega);
180 } else {
181 double theta = std::acos(d);
182 omega = theta / (2 * std::sqrt(1 - d * d)) * dR;
183 Matrix3 Omega = skew(omega);
184 V_inv = (Matrix3::Identity() - cst(0.5) * Omega +
185 (1 - theta / (2 * std::tan(theta / 2))) / (theta * theta) *
186 (Omega * Omega));
187 }
188
189 upsilon = V_inv * _t;
190 for (int i = 0; i < 3; i++) {
191 res[i] = omega[i];
192 }
193 for (int i = 0; i < 3; i++) {
194 res[i + 3] = upsilon[i];
195 }
196
197 return res;
198 }
g2o::deltaR
G2O_TYPES_SLAM3D_API Vector3 deltaR(const Matrix3 &R)
g2o::Vector6
VectorN< 6 > Vector6
Definition eigen_types.h:53

References g2o::cst(), deltaR(), and skew().

Referenced by g2o::EdgeSE3Expmap::computeError().

◆ map()

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

Definition at line 200 of file se3quat.h.

200{ return _r * xyz + _t; }

Referenced by g2o::EdgeStereoSE3ProjectXYZ::computeError(), g2o::EdgeStereoSE3ProjectXYZOnlyPose::computeError(), g2o::EdgeSE3ProjectXYZ::computeError(), g2o::EdgeProjectXYZ2UV::computeError(), g2o::EdgeProjectXYZ2UVU::computeError(), g2o::EdgeSE3ProjectXYZOnlyPose::computeError(), g2o::EdgeStereoSE3ProjectXYZ::isDepthPositive(), g2o::EdgeStereoSE3ProjectXYZOnlyPose::isDepthPositive(), g2o::EdgeSE3ProjectXYZ::isDepthPositive(), g2o::EdgeSE3ProjectXYZOnlyPose::isDepthPositive(), g2o::EdgeStereoSE3ProjectXYZ::linearizeOplus(), g2o::EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus(), g2o::EdgeSE3ProjectXYZ::linearizeOplus(), g2o::EdgeProjectXYZ2UV::linearizeOplus(), and g2o::EdgeSE3ProjectXYZOnlyPose::linearizeOplus().

◆ normalizeRotation()

void g2o::SE3Quat::normalizeRotation ( )
inline

Definition at line 251 of file se3quat.h.

251 {
252 if (_r.w() < 0) {
253 _r.coeffs() *= -1;
254 }
255 _r.normalize();
256 }

Referenced by operator*().

◆ operator Isometry3()

g2o::SE3Quat::operator Isometry3 ( ) const
inline

cast SE3Quat into an Isometry3

Definition at line 261 of file se3quat.h.

261 {
262 Isometry3 result = (Isometry3)rotation();
263 result.translation() = translation();
264 return result;
265 }
g2o::SE3Quat::rotation
const Quaternion & rotation() const
Definition se3quat.h:93
g2o::SE3Quat::translation
const Vector3 & translation() const
Definition se3quat.h:89
g2o::Isometry3
Eigen::Transform< double, 3, Eigen::Isometry, Eigen::ColMajor > Isometry3
Definition eigen_types.h:77

◆ operator*() [1/2]

SE3Quat g2o::SE3Quat::operator* ( const SE3Quat &  tr2) const
inline

Definition at line 97 of file se3quat.h.

97 {
98 SE3Quat result(*this);
99 result._t += _r * tr2._t;
100 result._r *= tr2._r;
101 result.normalizeRotation();
102 return result;
103 }

References _r, _t, and normalizeRotation().

◆ operator*() [2/2]

Vector3 g2o::SE3Quat::operator* ( const Vector3 &  v) const
inline

Definition at line 112 of file se3quat.h.

112{ return _t + _r * v; }

◆ operator*=()

SE3Quat & g2o::SE3Quat::operator*= ( const SE3Quat &  tr2)
inline

Definition at line 105 of file se3quat.h.

105 {
106 _t += _r * tr2._t;
107 _r *= tr2._r;
108 normalizeRotation();
109 return *this;
110 }

References _r, and _t.

◆ operator[]()

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

Definition at line 121 of file se3quat.h.

121 {
122 assert(i < 7);
123 if (i < 3) return _t[i];
124 return _r.coeffs()[i - 3];
125 }

◆ rotation()

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

Definition at line 93 of file se3quat.h.

93{ return _r; }

Referenced by g2o::EdgeSBACam::computeError(), g2o::internal::d_Tinvpsi_d_psi(), g2o::internal::fromSE3Quat(), g2o::EdgeProjectPSI2UV::linearizeOplus(), g2o::EdgeStereoSE3ProjectXYZ::linearizeOplus(), g2o::EdgeSE3ProjectXYZ::linearizeOplus(), g2o::EdgeProjectXYZ2UV::linearizeOplus(), main(), g2o::operator<<(), and g2o::VertexCam::write().

◆ setRotation()

void g2o::SE3Quat::setRotation ( const Quaternion &  r_)
inline

Definition at line 95 of file se3quat.h.

95{ _r = r_; }

◆ setTranslation()

void g2o::SE3Quat::setTranslation ( const Vector3 &  t_)
inline

Definition at line 91 of file se3quat.h.

91{ _t = t_; }

Referenced by g2o::EdgeSBAScale::initialEstimate().

◆ to_homogeneous_matrix()

Eigen::Matrix< double, 4, 4, Eigen::ColMajor > g2o::SE3Quat::to_homogeneous_matrix ( ) const
inline

Definition at line 242 of file se3quat.h.

242 {
243 Eigen::Matrix<double, 4, 4, Eigen::ColMajor> homogeneous_matrix;
244 homogeneous_matrix.setIdentity();
245 homogeneous_matrix.block(0, 0, 3, 3) = _r.toRotationMatrix();
246 homogeneous_matrix.col(3).head(3) = translation();
247
248 return homogeneous_matrix;
249 }

Referenced by g2o::operator<<().

◆ toMinimalVector()

Vector6 g2o::SE3Quat::toMinimalVector ( ) const
inline

Definition at line 144 of file se3quat.h.

144 {
145 Vector6 v;
146 v[0] = _t(0);
147 v[1] = _t(1);
148 v[2] = _t(2);
149 v[3] = _r.x();
150 v[4] = _r.y();
151 v[5] = _r.z();
152 return v;
153 }

◆ toVector()

Vector7 g2o::SE3Quat::toVector ( ) const
inline

Definition at line 127 of file se3quat.h.

127 {
128 Vector7 v;
129 v[0] = _t(0);
130 v[1] = _t(1);
131 v[2] = _t(2);
132 v[3] = _r.x();
133 v[4] = _r.y();
134 v[5] = _r.z();
135 v[6] = _r.w();
136 return v;
137 }
g2o::Vector7
VectorN< 7 > Vector7
Definition eigen_types.h:54

◆ translation()

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

Definition at line 89 of file se3quat.h.

89{ return _t; }

Referenced by g2o::EdgeSBACam::computeError(), g2o::EdgeSBAScale::computeError(), g2o::internal::fromSE3Quat(), g2o::EdgeSBAScale::initialEstimate(), g2o::EdgeProjectP2MC::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), main(), g2o::operator<<(), and g2o::VertexCam::write().

Member Data Documentation

◆ _r

Quaternion g2o::SE3Quat::_r
protected

Definition at line 44 of file se3quat.h.

Referenced by inverse(), operator*(), operator*=(), and g2o::SBACam::update().

◆ _t

Vector3 g2o::SE3Quat::_t
protected

Definition at line 45 of file se3quat.h.

Referenced by inverse(), operator*(), operator*=(), and g2o::SBACam::update().

◆ EIGEN_MAKE_ALIGNED_OPERATOR_NEW

g2o::SE3Quat::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 41 of file se3quat.h.

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