26 #ifndef __BIAS_CoordSystsemTransform3D_hh__
27 #define __BIAS_CoordSystsemTransform3D_hh__
32 #include <Base/Common/BIASpragmaStart.hh>
33 #include <Base/Debug/Debug.hh>
34 #include <Base/Geometry/Quaternion.hh>
35 #include <Base/Math/Matrix4x4.hh>
36 #include <Base/Geometry/HomgPoint3D.hh>
37 #include <Base/Geometry/HomgPoint3DCov.hh>
38 #include <Base/Math/Vector3.hh>
153 const double& scale = 1.0) {
154 Q_ = Q; C_ = C; scale_ = scale; UpdateMatrix_();
159 virtual inline void SetScale(
const double& scale) {
169 this->Mult(X_G, X_L);
179 invTransform_.Mult(X_L, X_G);
194 Cov_G = (*this) * Cov_G * (this->Transpose());
207 invTransform_.Mult(X, X_L);
208 Cov_L = invTransform_ * Cov_L * invTransform_.
Transpose();
235 for (
int i=0; i<3; i++){
236 res[i] = C_[i]; res[i+3] = Q_[i];
262 {
return invTransform_; }
264 friend std::ostream&
operator<<(std::ostream &os,
267 friend std::istream&
operator>>(std::istream &is,
281 bool Load(
const std::string& inputFile);
288 bool Save(
const std::string& outputFile);
328 Q_.Invert(); scale_ = 1.0/scale_;
329 C_ = -scale_ * Q_.MultVec(C_);
349 virtual void UpdateMatrix_();
374 os<<
"Q_= "<<p.
Q_<<
" C_= "<<p.
C_<<
" scale_= "<<p.
scale_;
390 #include <Base/Common/BIASpragmaEnd.hh>
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
class Vector4 contains a Vector of dim.
Matrix4x4 Transpose() 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 representing the covariance matrix of a homogenous point 3D
Implements a 3D rotation matrix.
is a 'fixed size' quadratic matrix of dim.
Matrix4x4 & operator=(const Matrix< T > &mat)
assignment operators calling corresponding operator from base class "TNT::Matrix" if appropriate ...
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.