Berkeley SfM
Public Member Functions | Private Attributes | List of all members
bsfm::Pose Class Reference

#include <pose.h>

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW Pose ()
 
 Pose (const Matrix3d &R, const Vector3d &t)
 
 Pose (const Matrix34d &Rt)
 
 Pose (const Pose &other)
 
 Pose (const Matrix4d &other)
 
 ~Pose ()
 
double & operator() (int i, int j)
 
const double & operator() (int i, int j) const
 
Matrix4d & Get ()
 
const Matrix4d & Get () const
 
Matrix3d Rotation () const
 
Vector3d Translation () const
 
void Set (const Matrix4d &transformation)
 
void SetRotation (const Matrix3d &rotation)
 
void SetTranslation (const Vector3d &translation)
 
Pose operator* (const Pose &other) const
 
Vector4d Project (const Vector4d &)
 
Vector2d ProjectTo2D (const Vector3d &)
 
bool IsApprox (const Pose &) const
 
void Compose (const Pose &other)
 
Pose Inverse () const
 
Matrix34d Dehomogenize ()
 
Vector3d AxisAngle ()
 
Matrix4d FromAxisAngle (const Vector3d &aa)
 
void SetX (double x)
 
void SetY (double y)
 
void SetZ (double z)
 
const double & X () const
 
const double & Y () const
 
const double & Z () const
 
double & MutableX ()
 
double & MutableY ()
 
double & MutableZ ()
 
void TranslateX (double dx)
 
void TranslateY (double dy)
 
void TranslateZ (double dz)
 
Pose Delta (const Pose &rhs) const
 
void Print (const std::string &prefix=std::string()) const
 

Private Attributes

Matrix4d Rt_
 

Detailed Description

Definition at line 56 of file pose.h.

Constructor & Destructor Documentation

bsfm::Pose::Pose ( )

Definition at line 45 of file pose.cpp.

45  {
46  Rt_ = Matrix4d::Identity();
47 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
bsfm::Pose::Pose ( const Matrix3d &  R,
const Vector3d &  t 
)

Definition at line 50 of file pose.cpp.

50  {
51  Rt_ = Matrix4d::Identity();
52  Rt_.block(0, 0, 3, 3) = R;
53  Rt_.col(3).head(3) = t;
54 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
bsfm::Pose::Pose ( const Matrix34d &  Rt)

Definition at line 57 of file pose.cpp.

57  {
58  Rt_ = Matrix4d::Identity();
59  Rt_.block(0, 0, 3, 4) = Rt;
60 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
bsfm::Pose::Pose ( const Pose &  other)

Definition at line 68 of file pose.cpp.

68  {
69  Rt_ << other.Rt_;
70 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
bsfm::Pose::Pose ( const Matrix4d &  other)

Definition at line 63 of file pose.cpp.

63  {
64  Rt_ << Rt;
65 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
bsfm::Pose::~Pose ( )
inline

Definition at line 76 of file pose.h.

76 { };

Member Function Documentation

Vector3d bsfm::Pose::AxisAngle ( )

Definition at line 162 of file pose.cpp.

162  {
163  // From https://en.wikipedia.org/wiki/Axis-angle-representation.
164  const double angle = acos(0.5 * (Rt_.trace() - 2.0));
165  Vector3d axis = Vector3d(Rt_(2, 1) - Rt_(1, 2),
166  Rt_(0, 2) - Rt_(2, 0),
167  Rt_(1, 0) - Rt_(0, 1)) * 0.5 / sin(angle);
168 
169  axis /= axis.norm();
170  return axis * angle;
171 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::Compose ( const Pose &  other)

Definition at line 145 of file pose.cpp.

145  {
146  Rt_ *= other.Rt_;
147 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Matrix34d bsfm::Pose::Dehomogenize ( )

Definition at line 157 of file pose.cpp.

157  {
158  return Rt_.block(0, 0, 3, 4);
159 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Pose bsfm::Pose::Delta ( const Pose &  rhs) const

Definition at line 256 of file pose.cpp.

256  {
257  return this->Inverse() * rhs;
258 }
bsfm::Pose::Inverse
Pose Inverse() const
Definition: pose.cpp:150
Matrix4d bsfm::Pose::FromAxisAngle ( const Vector3d &  aa)

Definition at line 174 of file pose.cpp.

174  {
175  // From https://en.wikipedia.org/wiki/Rotation_matrix.
176  double angle = aa.norm();
177  Vector3d axis = aa / angle;
178 
179  Matrix3d cross;
180  cross <<
181  0.0, -axis(2), axis(1),
182  axis(2), 0.0, -axis(0),
183  -axis(1), axis(0), 0.0;
184 
185  Matrix3d tensor;
186  tensor <<
187  axis(0)*axis(0), axis(0)*axis(1), axis(0)*axis(2),
188  axis(0)*axis(1), axis(1)*axis(1), axis(1)*axis(2),
189  axis(0)*axis(2), axis(1)*axis(2), axis(2)*axis(2);
190 
191  Matrix3d R =
192  cos(angle) * Matrix3d::Identity() +
193  sin(angle) * cross + (1-cos(angle)) * tensor;
194  Rt_.block(0, 0, 3, 3) = R;
195 
196  return Rt_;
197 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Matrix4d & bsfm::Pose::Get ( )

Definition at line 82 of file pose.cpp.

82  {
83  return Rt_;
84 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
const Matrix4d & bsfm::Pose::Get ( ) const

Definition at line 86 of file pose.cpp.

86  {
87  return Rt_;
88 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Pose bsfm::Pose::Inverse ( ) const

Definition at line 150 of file pose.cpp.

150  {
151  Matrix4d Rt_inverse = Rt_.inverse();
152  Pose out(Rt_inverse);
153  return out;
154 }
bsfm::Pose::Pose
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Pose()
Definition: pose.cpp:45
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
bool bsfm::Pose::IsApprox ( const Pose &  other) const

Definition at line 139 of file pose.cpp.

139  {
140  return Rt_.isApprox(other.Rt_);
141 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
double & bsfm::Pose::MutableX ( )

Definition at line 223 of file pose.cpp.

223  {
224  return Rt_(0, 3);
225 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
double & bsfm::Pose::MutableY ( )

Definition at line 227 of file pose.cpp.

227  {
228  return Rt_(1, 3);
229 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
double & bsfm::Pose::MutableZ ( )

Definition at line 231 of file pose.cpp.

231  {
232  return Rt_(2, 3);
233 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
double & bsfm::Pose::operator() ( int  i,
int  j 
)

Definition at line 73 of file pose.cpp.

73  {
74  return Rt_(i, j);
75 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
const double & bsfm::Pose::operator() ( int  i,
int  j 
) const

Definition at line 77 of file pose.cpp.

77  {
78  return Rt_(i, j);
79 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Pose bsfm::Pose::operator* ( const Pose &  other) const

Definition at line 115 of file pose.cpp.

115  {
116  Matrix4d Rt_out = Rt_ * other.Rt_;
117  return Pose(Rt_out);
118 }
bsfm::Pose::Pose
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Pose()
Definition: pose.cpp:45
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::Print ( const std::string &  prefix = std::string()) const

Definition at line 248 of file pose.cpp.

248  {
249  if (!prefix.empty()) {
250  std::cout << prefix << std::endl;
251  }
252  std::cout << Rt_ << std::endl;
253 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Vector4d bsfm::Pose::Project ( const Vector4d &  pt3d)

Definition at line 121 of file pose.cpp.

121  {
122  Vector4d proj = Rt_ * pt3d;
123  return proj;
124 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Vector2d bsfm::Pose::ProjectTo2D ( const Vector3d &  pt3d)

Definition at line 127 of file pose.cpp.

127  {
128  Vector4d pt3d_h = Vector4d::Constant(1.0);
129  pt3d_h.head(3) = pt3d;
130 
131  const Vector4d proj_h = Rt_ * pt3d_h;
132  Vector2d proj = proj_h.head(2);
133  proj /= proj_h(2);
134 
135  return proj;
136 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Matrix3d bsfm::Pose::Rotation ( ) const

Definition at line 91 of file pose.cpp.

91  {
92  return Rt_.block(0, 0, 3, 3);
93 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::Set ( const Matrix4d &  transformation)

Definition at line 101 of file pose.cpp.

101  {
102  Rt_ = transformation;
103 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::SetRotation ( const Matrix3d &  rotation)

Definition at line 106 of file pose.cpp.

106  {
107  Rt_.block(0, 0, 3, 3) = rotation;
108 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::SetTranslation ( const Vector3d &  translation)

Definition at line 110 of file pose.cpp.

110  {
111  Rt_.block(0, 3, 3, 1) = translation;
112 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::SetX ( double  x)

Definition at line 199 of file pose.cpp.

199  {
200  Rt_(0, 3) = x;
201 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::SetY ( double  y)

Definition at line 203 of file pose.cpp.

203  {
204  Rt_(1, 3) = y;
205 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::SetZ ( double  z)

Definition at line 207 of file pose.cpp.

207  {
208  Rt_(2, 3) = z;
209 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::TranslateX ( double  dx)

Definition at line 235 of file pose.cpp.

235  {
236  Rt_(0, 3) += dx;
237 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::TranslateY ( double  dy)

Definition at line 239 of file pose.cpp.

239  {
240  Rt_(1, 3) += dy;
241 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
void bsfm::Pose::TranslateZ ( double  dz)

Definition at line 243 of file pose.cpp.

243  {
244  Rt_(2, 3) += dz;
245 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
Vector3d bsfm::Pose::Translation ( ) const

Definition at line 96 of file pose.cpp.

96  {
97  return Rt_.block(0, 3, 3, 1);
98 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
const double & bsfm::Pose::X ( ) const

Definition at line 211 of file pose.cpp.

211  {
212  return Rt_(0, 3);
213 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
const double & bsfm::Pose::Y ( ) const

Definition at line 215 of file pose.cpp.

215  {
216  return Rt_(1, 3);
217 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152
const double & bsfm::Pose::Z ( ) const

Definition at line 219 of file pose.cpp.

219  {
220  return Rt_(2, 3);
221 }
bsfm::Pose::Rt_
Matrix4d Rt_
Definition: pose.h:152

Member Data Documentation

Matrix4d bsfm::Pose::Rt_
private

Definition at line 152 of file pose.h.

The documentation for this class was generated from the following files:
Generated by   doxygen 1.8.10