1 #include "PoseParametrization.hh"
3 #include "PoseParametrizationCovariance.hh"
5 #include <Base/Debug/Exception.hh>
6 #include <Base/Common/CompareFloatingPoint.hh>
7 #include <Base/Math/Matrix4x4.hh>
8 #include <Base/Geometry/HomgPoint3D.hh>
27 Set(CQvec, covariance);
37 Set(position, orientation, covariance);
46 CQ2Pose_(position, orientation, CQvec);
47 Set(CQvec, covariance);
55 CQ2Pose_(position, orientation, CQvec);
64 if (CQvec.
size() != 7){
65 BEXCEPTION(
"invalid vector size, must be 7");
76 BEXCEPTION(
"dimension of argument covariance matrix does not match!\n");
127 std::ifstream ifs(file.c_str());
141 Save(
const std::string& file)
const
143 std::ofstream ofs(file.c_str());
163 for (
int r=0; r<3; r++){
165 for (
int c=0; c<3; c++){
166 Transf[r][c] = R[r][c];
174 BEXCEPTION(
"position is at infinity: "<<dst);
185 Set(new_pos, o, cov);
196 for (
int r=0; r<3; r++){
198 for (
int c=0; c<3; c++){
199 Transf[r][c] = s*R[r][c];
207 BEXCEPTION(
"position is at infinity: "<<dst);
218 Set(new_pos, o, cov);
238 is>>temp>>position>>temp>>orientation>>temp>>cov;
239 p.
Set(position, orientation, cov);
bool Load(const std::string &file)
void GetEuclidean(Vector3< HOMGPOINT3D_TYPE > &dest) const
calculate affine coordinates of this and write them to dest affine coordinates are projective coordin...
void Pose2CQ_(const Vector< PP_TYPE > &pose, Vector3< PP_TYPE > &position, Quaternion< PP_TYPE > &orientation) const
Subscript num_cols() const
void Transform(const Vector3< double > &T, const Quaternion< double > &q)
Transforms the pose and associated covariance according to the point transformation parametrized as t...
void SetCovarianceMatrix(const Matrix< PP_TYPE > &Cov)
Sets covariance matrix from Cov.
bool Save(const std::string &file) const
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
Slim class bundeling pose parametrization and associated covariance matrix.
Vector< PP_TYPE > Pose_
first 3 entries relate to the position
void SetZero()
equivalent to matrix call
void SetZero()
Sets all values to zero.
const Matrix< PP_TYPE > & GetCovarianceMatrix() const
returns the covariance matrix associated with the CQ vector
void SetCQ(const Vector< PP_TYPE > &CQvec)
Set this from pose parametrization.
void SetPosition(const Vector3< PP_TYPE > &C)
set position part of vector and zero covariance matrix
bool IsAtInfinity() const
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
class Vector3 contains a Vector of fixed dim.
Vector3< PP_TYPE > GetPosition() const
returns the position (first 3 entries) part of the pose parametrization vector
void Set(const Vector3< PP_TYPE > &position, const Quaternion< PP_TYPE > &orientation, const Matrix< PP_TYPE > &covariance=Matrix< double >(7, 7, MatrixZero))
Set this from position, orientation and covariance.
void SetOrientation(const Quaternion< PP_TYPE > &q)
set orientation part of vector and zero covariance matrix
Matrix< PP_TYPE > Cov_
first columns relate to position
Implements a 3D rotation matrix.
class encapsulating the transfromations between different pose parametrizations
Subscript num_rows() const
is a 'fixed size' quadratic matrix of dim.
void Transform(const Matrix4x4< double > &transf)
Given a 6D pose (position and euler angles) and its associated covariance, the function transforms th...
class for rotation with axis and angle
Quaternion< PP_TYPE > GetOrientation() const
returns the quaternion (last 4 entries) part of the pose parametrization vector
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.