29 #include <bias_config.h>
30 #include <Base/Geometry/Quaternion.hh>
31 #include <Base/Geometry/HomgPoint3D.hh>
32 #include <Base/Geometry/HomgPoint3DCov.hh>
33 #include <Base/Math/Matrix4x4.hh>
34 #include <Base/Math/Matrix3x4.hh>
35 #include <Base/Geometry/RMatrixBase.hh>
36 #include <Geometry/CoordinateTransform3D.hh>
37 #include <Base/Geometry/PoseParametrization.hh>
39 # include <Base/Common/XMLBase.hh>
42 #define POSE_TYPE double
162 bool Load(
const std::string& inputFile);
169 bool Save(
const std::string& outputFile);
171 #ifdef BIAS_HAVE_XML2
173 virtual int XMLGetClassName(std::string& TopLevelTag,
174 double& Version)
const;
177 virtual int XMLOut(
const xmlNodePtr Node,
BIAS::XMLIO& XMLObject)
const;
180 virtual int XMLIn(
const xmlNodePtr Node,
BIAS::XMLIO& XMLObject);
218 bool retVal = CheckLookAt_(center, up);
233 return LookAt(GetC(), center, up);
245 return LookAt(eye, center, up);
254 return LookAtGL(GetC(), center, up);
264 virtual void SetScale(
const double& scale) { BIASABORT; }
269 if(center.
NormL2() < std::numeric_limits<double>::epsilon()){
270 BIASERR(
"Vectors eye and center are the same!")
273 if(fabs(up.
NormL2() - 1.0) > std::numeric_limits<double>::epsilon()){
274 BIASERR(
"Vector up is not normalized!")
289 os<<
"Q_= "<<p.
Q_<<
" C_= "<<p.
C_<<
" scale_= "<<p.
scale_<<
" Cov_="<<p.
Cov_;
291 for(
int i=0; i < 4; i++){
292 for(
int j=0; j < 4; j++){
307 BIASERR(
"Loaded transformation is not a pose - resetting scale!\n");
const bool LookAtGL(const Vector3< double > ¢er, const Vector3< double > &up)
Looks at given point, similar to gluLookAt.
bool CheckLookAt_(Vector3< double > ¢er, const Vector3< double > &up)
Checks constraints for parameter of LookAt().
Slim class bundeling pose parametrization and associated covariance matrix.
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
void SetR(const RMatrixBase &R)
Get rotation (resp.
int SetFromOriUp(BIAS::Vector3< ROTATION_MATRIX_TYPE > ori, BIAS::Vector3< ROTATION_MATRIX_TYPE > up)
Set rotation matrix from orientation and up vector.
Represents 3d pose transformations, parametrized as Euclidean translation and unit quaternion orienta...
const bool LookAt(const Vector3< double > ¢er, const Vector3< double > &up)
Create pose of camera looking at given point, similar to gluLookAt().
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
is a 'fixed size' quadratic matrix of dim.
Base class with interface for xml output.
std::ostream & operator<<(std::ostream &os, const Array2D< T > &arg)
Matrix< POSE_TYPE > Cov_
7x7 covariance matrix for the 7-vector representation of this pose transformation (first C...
class Vector3 contains a Vector of fixed dim.
void SubIP(const T &scalar)
Substraction (in place) of an scalar.
virtual void SetScale(const double &scale)
Scale can not be set for poses but is fixed as 1.
const bool LookAtGL(const Vector3< double > &eye, Vector3< double > center, const Vector3< double > &up)
Create pose of camera looking at given point, similar to gluLookAt().
Implements a 3D rotation matrix.
is a 'fixed size' quadratic matrix of dim.
const bool LookAt(const Vector3< double > &eye, Vector3< double > center, const Vector3< double > &up)
Create pose of camera looking at given point, similar to gluLookAt().
class for rotation with axis and angle
const Matrix< POSE_TYPE > & GetCovariance() const
Return the 7x7 covariance matrix Cov([C Q]).
const Pose & operator=(const Pose &p)
Copy from another pose transformation instance.
class BIASGeometry_EXPORT Pose
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.