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

parameters for a camera More...

#include <parameter_camera.h>

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

Public Member Functions

 ParameterCamera ()
 
void setKcam (double fx, double fy, double cx, double cy)
 
void setOffset (const Isometry3D &offset_=Isometry3D::Identity())
 
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...
 
const Matrix3DKcam () const
 
const Matrix3DinvKcam () const
 
const Matrix3DKcam_inverseOffsetR () const
 
- Public Member Functions inherited from g2o::ParameterSE3Offset
 ParameterSE3Offset ()
 
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
 
- Public Attributes inherited from g2o::ParameterSE3Offset
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 

Protected Attributes

Matrix3D _Kcam
 
Matrix3D _invKcam
 
Matrix3D _Kcam_inverseOffsetR
 
- Protected Attributes inherited from g2o::ParameterSE3Offset
Isometry3D _offset
 
Isometry3D _inverseOffset
 
- Protected Attributes inherited from g2o::Parameter
int _id
 

Detailed Description

parameters for a camera

Definition at line 38 of file parameter_camera.h.

Constructor & Destructor Documentation

g2o::ParameterCamera::ParameterCamera ( )

Definition at line 40 of file parameter_camera.cpp.

40  {
41  setId(-1);
42  setKcam(1,1,0.5,0.5);
43  setOffset();
44  }
void setOffset(const Isometry3D &offset_=Isometry3D::Identity())
void setId(int id_)
Definition: parameter.cpp:35
void setKcam(double fx, double fy, double cx, double cy)

Member Function Documentation

const Matrix3D& g2o::ParameterCamera::invKcam ( ) const
inline
const Matrix3D& g2o::ParameterCamera::Kcam ( ) const
inline

Definition at line 48 of file parameter_camera.h.

48 { return _Kcam;}
const Matrix3D& g2o::ParameterCamera::Kcam_inverseOffsetR ( ) const
inline
bool g2o::ParameterCamera::read ( std::istream &  is)
virtual

read the data from a stream

Reimplemented from g2o::ParameterSE3Offset.

Reimplemented in g2o::ParameterStereoCamera.

Definition at line 63 of file parameter_camera.cpp.

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

63  {
64  Vector7d off;
65  for (int i=0; i<7; i++)
66  is >> off[i];
67  // normalize the quaternion to recover numerical precision lost by storing as human readable text
68  Vector4D::MapType(off.data()+3).normalize();
70  double fx,fy,cx,cy;
71  is >> fx >> fy >> cx >> cy;
72  setKcam(fx,fy,cx,cy);
73  return is.good();
74  }
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 setKcam(double fx, double fy, double cx, double cy)
void g2o::ParameterCamera::setKcam ( double  fx,
double  fy,
double  cx,
double  cy 
)

Definition at line 51 of file parameter_camera.cpp.

Referenced by main().

51  {
52  _Kcam.setZero();
53  _Kcam(0,0) = fx;
54  _Kcam(1,1) = fy;
55  _Kcam(0,2) = cx;
56  _Kcam(1,2) = cy;
57  _Kcam(2,2) = 1.0;
58  _invKcam = _Kcam.inverse();
59  _Kcam_inverseOffsetR = _Kcam * inverseOffset().rotation();
60  }
const Isometry3D & inverseOffset() const
rotation of the inverse offset as 3x3 rotation matrix
void g2o::ParameterCamera::setOffset ( const Isometry3D offset_ = Isometry3D::Identity())

Definition at line 46 of file parameter_camera.cpp.

46  {
48  _Kcam_inverseOffsetR = _Kcam * inverseOffset().rotation();
49  }
const Isometry3D & inverseOffset() const
rotation of the inverse offset as 3x3 rotation matrix
void setOffset(const Isometry3D &offset_=Isometry3D::Identity())
bool g2o::ParameterCamera::write ( std::ostream &  os) const
virtual

write the data to a stream

Reimplemented from g2o::ParameterSE3Offset.

Reimplemented in g2o::ParameterStereoCamera.

Definition at line 76 of file parameter_camera.cpp.

References g2o::internal::toVectorQT().

76  {
78  for (int i=0; i<7; i++)
79  os << off[i] << " ";
80  os << _Kcam(0,0) << " ";
81  os << _Kcam(1,1) << " ";
82  os << _Kcam(0,2) << " ";
83  os << _Kcam(1,2) << " ";
84  return os.good();
85  }
Eigen::Matrix< double, 7, 1 > Vector7d
Definition: sim3.h:35
Vector7d toVectorQT(const Isometry3D &t)

Member Data Documentation

Matrix3D g2o::ParameterCamera::_invKcam
protected

Definition at line 54 of file parameter_camera.h.

Matrix3D g2o::ParameterCamera::_Kcam
protected

Definition at line 53 of file parameter_camera.h.

Matrix3D g2o::ParameterCamera::_Kcam_inverseOffsetR
protected

Definition at line 55 of file parameter_camera.h.

g2o::ParameterCamera::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 40 of file parameter_camera.h.


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