Go to the documentation of this file.00001
00022 #ifndef LOCATEDOBJECT_H
00023 #define LOCATEDOBJECT_H
00024
00025 #ifndef LO_TYPE_PERCEIVED
00026 #define LO_TYPE_PERCEIVED 0
00027 #endif
00028 #include <vector>
00029
00030
00031
00032
00033 #include "lo/NewMatExhaustive.h"
00034
00035
00036 #define RELOCATE_OBJECT 1
00037 #define NO_MOVE 0
00038
00039 namespace jlo
00040 {
00041
00042
00047 class LazyLocatedObjectLoader;
00048
00049
00055 class LocatedObject
00056 {
00057 public:
00058 LocatedObject(LazyLocatedObjectLoader* loader);
00059 LocatedObject(LazyLocatedObjectLoader* loader, unsigned long id, unsigned long parent_id, const Matrix &matrix , const Matrix &covariance);
00060 LocatedObject(LazyLocatedObjectLoader* loader, unsigned long id, unsigned long parent_id,
00061 double x = 0.0, double y = 0.0, double z = 0.0,
00062 double roll = 0.0, double pitch = 0.0, double yaw = 0.0,
00063 double sigmaX = 0.0, double sigmaY = 0.0, double sigmaZ = 0.0,
00064 double sigmaRoll = 0.0, double sigmaPitch = 0.0, double sigmaYaw = 0.0);
00065
00073 unsigned long m_uniqueID;
00074
00075
00083 unsigned long m_parentID;
00084
00085
00091 LazyLocatedObjectLoader* m_lazyObjLoader;
00092
00093
00094
00103 virtual ReturnMatrix GetMatrix (LocatedObject &obj_to_stop );
00104 virtual ReturnMatrix GetMatrix (const unsigned long &id_to_stop = 0 );
00105
00114 virtual ReturnMatrix GetInvMatrix (LocatedObject &obj_to_stop );
00115 virtual ReturnMatrix GetInvMatrix (const unsigned long &obj_to_stop = 0 );
00116
00117
00118
00127 ReturnMatrix GetCovarianceMatrix (LocatedObject &obj_to_stop ) ;
00128 ReturnMatrix GetCovarianceMatrix (const unsigned long &obj_to_stop = 0) ;
00129
00138 double CompareLo(LocatedObject &obj_to_stop ) ;
00139
00140
00149 ReturnMatrix GetOrigin (LocatedObject &obj_to_stop ) ;
00150 ReturnMatrix GetOrigin (const unsigned long &obj_to_stop = 0) ;
00151
00160 ReturnMatrix GetRotation (LocatedObject &obj_to_stop ) ;
00161 ReturnMatrix GetRotation (const unsigned long &obj_to_stop = 0) ;
00162
00173 void TransformPointLocally(const double& x_in, const double& y_in, const double& z_in, double& x_out, double& y_out, double& z_out, const double& scale = 1);
00174
00175
00181 bool CheckWorldCoordinates (const unsigned long &levelsTogo) const;
00182
00187 virtual unsigned long GetLOType(){return LO_TYPE_PERCEIVED;}
00188
00189 protected:
00190
00191
00192
00202 Matrix m_relativePosition;
00203
00204
00210 Matrix m_invRelativePosition;
00211
00212
00219 Matrix m_covariance;
00220
00221
00227 virtual ReturnMatrix Get ( ) ;
00228 virtual ReturnMatrix GetCovariance();
00229
00230
00236 virtual ReturnMatrix GetInv ( ) ;
00237
00238
00239
00247 virtual unsigned long Set (const Matrix &matrix, const Matrix &covMatrix ) ;
00248
00256 virtual unsigned long Move (const Matrix &matrix, const Matrix &covMatrix) ;
00257 protected:
00258
00267 unsigned long FindCommonFather (const LocatedObject* obj) const;
00268
00269
00270
00275 ReturnMatrix CalcNewCov(const DiagonalMatrix& eigenValues, const Matrix& eigenVectors, const Matrix& hom) const;
00276 ReturnMatrix TransformSixVec(const ColumnVector& point, const Matrix& hom) const;
00277
00285 ReturnMatrix UnscentedTrans(const Matrix& cov, const Matrix& mean, const Matrix& hom) const;
00286
00287
00288 };
00289
00290 class LazyLocatedObjectLoader
00291 {
00292 public:
00293 LazyLocatedObjectLoader(){}
00294 virtual LocatedObject* GetParent(const LocatedObject& child)= 0;
00295 };
00296
00297 }
00298 #endif // LOCATEDOBJECT_H