g2o
Public Member Functions | Public Attributes | Protected Attributes | List of all members
g2o::ParameterSE3Offset Class Reference

offset for an SE3 More...

#include <parameter_se3_offset.h>

Inheritance diagram for g2o::ParameterSE3Offset:
Inheritance graph
[legend]
Collaboration diagram for g2o::ParameterSE3Offset:
Collaboration graph
[legend]

Public Member Functions

 ParameterSE3Offset ()
 
virtual bool read (std::istream &is)
 read the data from a stream More...
 
virtual bool write (std::ostream &os) const
 write the data to a stream More...
 
void setOffset (const Isometry3D &offset_=Isometry3D::Identity())
 
const Isometry3Doffset () const
 rotation of the offset as 3x3 rotation matrix More...
 
const Isometry3DinverseOffset () const
 rotation of the inverse offset as 3x3 rotation matrix More...
 
- Public Member Functions inherited from g2o::Parameter
 Parameter ()
 
virtual ~Parameter ()
 
int id () const
 
void setId (int id_)
 
virtual HyperGraph::HyperGraphElementType elementType () const
 
- Public Member Functions inherited from g2o::HyperGraph::HyperGraphElement
virtual ~HyperGraphElement ()
 
HyperGraphElementclone () const
 

Public Attributes

 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

Protected Attributes

Isometry3D _offset
 
Isometry3D _inverseOffset
 
- Protected Attributes inherited from g2o::Parameter
int _id
 

Detailed Description

offset for an SE3

Definition at line 45 of file parameter_se3_offset.h.

Constructor & Destructor Documentation

g2o::ParameterSE3Offset::ParameterSE3Offset ( )

Definition at line 38 of file parameter_se3_offset.cpp.

References setOffset().

38  {
39  setOffset();
40  }
void setOffset(const Isometry3D &offset_=Isometry3D::Identity())

Member Function Documentation

const Isometry3D& g2o::ParameterSE3Offset::inverseOffset ( ) const
inline

rotation of the inverse offset as 3x3 rotation matrix

Definition at line 64 of file parameter_se3_offset.h.

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

64 { return _inverseOffset;}
const Isometry3D& g2o::ParameterSE3Offset::offset ( ) const
inline
bool g2o::ParameterSE3Offset::read ( std::istream &  is)
virtual

read the data from a stream

Implements g2o::Parameter.

Reimplemented in g2o::ParameterCamera, and g2o::ParameterStereoCamera.

Definition at line 47 of file parameter_se3_offset.cpp.

References g2o::internal::fromVectorQT(), g2o::internal::normalize(), and setOffset().

47  {
48  Vector7d off;
49  for (int i=0; i<7; i++) {
50  is >> off[i];
51  }
52  // normalize the quaternion to recover numerical precision lost by storing as human readable text
53  Vector4D::MapType(off.data()+3).normalize();
55  return is.good();
56  }
Isometry3D fromVectorQT(const Vector7d &v)
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
void setOffset(const Isometry3D &offset_=Isometry3D::Identity())
Eigen::Quaterniond & normalize(Eigen::Quaterniond &q)
void g2o::ParameterSE3Offset::setOffset ( const Isometry3D offset_ = Isometry3D::Identity())

update the offset to a new value. re-calculates the different representations, e.g., the rotation matrix

Definition at line 42 of file parameter_se3_offset.cpp.

References _inverseOffset, and _offset.

Referenced by main(), ParameterSE3Offset(), and read().

42  {
43  _offset = offset_;
44  _inverseOffset = _offset.inverse();
45  }
bool g2o::ParameterSE3Offset::write ( std::ostream &  os) const
virtual

write the data to a stream

Implements g2o::Parameter.

Reimplemented in g2o::ParameterCamera, and g2o::ParameterStereoCamera.

Definition at line 58 of file parameter_se3_offset.cpp.

References _offset, and g2o::internal::toVectorQT().

58  {
60  for (int i=0; i<7; i++)
61  os << off[i] << " ";
62  return os.good();
63  }
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
Vector7d toVectorQT(const Isometry3D &t)

Member Data Documentation

Isometry3D g2o::ParameterSE3Offset::_inverseOffset
protected

Definition at line 68 of file parameter_se3_offset.h.

Referenced by setOffset().

Isometry3D g2o::ParameterSE3Offset::_offset
protected

Definition at line 67 of file parameter_se3_offset.h.

Referenced by setOffset(), and write().

g2o::ParameterSE3Offset::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 48 of file parameter_se3_offset.h.


The documentation for this class was generated from the following files: