26 #ifndef __PoseParametrization_hh__
27 #define __PoseParametrization_hh__
29 #include <Base/Math/Vector.hh>
30 #include <Base/Math/Vector3.hh>
31 #include <Base/Math/Matrix.hh>
32 #include <Base/Math/Matrix4x4.hh>
33 #include <Base/Geometry/Quaternion.hh>
39 #define PP_TYPE double
77 { SetCQ(CQvec); SetCovarianceMatrix(Cov); }
106 bool Load(
const std::string& file);
108 bool Save(
const std::string& file)
const;
132 position[0] = pose[0];
133 position[1] = pose[1];
134 position[2] = pose[2];
135 orientation[0] = pose[3];
136 orientation[1] = pose[4];
137 orientation[2] = pose[5];
138 orientation[3] = pose[6];
144 pose[0] = position[0];
145 pose[1] = position[1];
146 pose[2] = position[2];
147 pose[3] = orientation[0];
148 pose[4] = orientation[1];
149 pose[5] = orientation[2];
150 pose[6] = orientation[3];
155 BIASGeometryBase_EXPORT std::ostream&
156 operator<<(std::ostream &os,
const PoseParametrization& p);
158 BIASGeometryBase_EXPORT std::istream&
159 operator>>(std::istream &is, PoseParametrization& p);
void Pose2CQ_(const Vector< PP_TYPE > &pose, Vector3< PP_TYPE > &position, Quaternion< PP_TYPE > &orientation) const
void Set(const Vector< PP_TYPE > &CQvec, const Matrix< PP_TYPE > &Cov)
Slim class bundeling pose parametrization and associated covariance matrix.
Vector< PP_TYPE > Pose_
first 3 entries relate to the position
const Matrix< PP_TYPE > & GetCovarianceMatrix() const
returns the covariance matrix associated with the CQ vector
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
class Vector3 contains a Vector of fixed dim.
const Vector< PP_TYPE > & GetCQ() const
returns a vector of dimension 7 where first 3 entries are the position and the last 4 entries are the...
Matrix< PP_TYPE > Cov_
first columns relate to position
class for rotation with axis and angle
void CQ2Pose_(const Vector3< PP_TYPE > &position, const Quaternion< PP_TYPE > &orientation, Vector< PP_TYPE > &pose) const
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.