landmarkAbstract.hpp

Go to the documentation of this file.

#ifndef __LandmarkAbstract_H__
#define __LandmarkAbstract_H__

//#include "kernel/IdFactory.hpp"

#include "rtslam/rtSlam.hpp"

// include parents
#include "rtslam/mapAbstract.hpp"
#include "rtslam/mapManager.hpp"
#include "rtslam/mapObject.hpp"
#include "rtslam/descriptorAbstract.hpp"
#include "rtslam/visibilityMap.hpp"
//#include "rtslam/observationAbstract.hpp"
#include "rtslam/parents.hpp"

#include <boost/smart_ptr.hpp>


/*
 * STATUS: testing purpose
 * Use first-estimate-jacobian for euclidean landmarks
 */
#define FEJ_EKF 0


namespace jafar {
  namespace rtslam {


    // Forward declarations of children
    class ObservationAbstract;
//    class MapManagerAbstract;


    class LandmarkAbstract: public MapObject, public ChildOf<MapManagerAbstract> , public boost::enable_shared_from_this<
        LandmarkAbstract>, public ParentOf<ObservationAbstract> {

        friend std::ostream& operator <<(std::ostream & s, LandmarkAbstract const & obs);
        friend image::oimstream& operator <<(image::oimstream & s, LandmarkAbstract const & lmk);
        
        // define the function linkToParentMapManager().
           ENABLE_LINK_TO_PARENT(MapManagerAbstract,MapManager,LandmarkAbstract)
        ;
        // define the functions mapManagerPtr() and mapManager().
      ENABLE_ACCESS_TO_PARENT(MapManagerAbstract,mapManager)
        ;
        // define the type ObservationList, and the function observationList().
      ENABLE_ACCESS_TO_CHILDREN(ObservationAbstract,Observation,observation)
        ;

      public:
      LandmarkAbstract(const map_ptr_t & _mapPtr, const size_t _size, bool freeze_first = false);
      LandmarkAbstract(const simulation_t dummy, const map_ptr_t & _mapPtr, const size_t _size, bool freeze_first = false);
      LandmarkAbstract(const map_ptr_t & _mapPtr, const landmark_ptr_t & _prevLmk, const size_t _size,jblas::ind_array & _icomp, bool freeze_first = false);

        virtual ~LandmarkAbstract();


        static IdFactory landmarkIds;
        void setId(){id(landmarkIds.getId());}

        enum geometry_t {
            POINT,
            LINE,
            PLANE,
            ELLIPSE
        } ;

        enum type_enum{
                  PNT_EUC, PNT_AH, LINE_AHPL
        };
        type_enum type;
        bool converged;
        bool visible;
        bool updatable;
        enum killed_enum { kNo = 0, kDuplicate, kCellOverflow };
        killed_enum killed;

      protected:
        geometry_t geomType;
      public:
        std::string categoryName() const {return "LANDMARK";}
        geometry_t getGeomType() const {return geomType;}
        virtual std::string typeName() const {return "Abstract";}

        descriptor_ptr_t descriptorPtr; 
        VisibilityMap visibilityMap;
        Gaussian first_state;

        jblas::mat LNEW_lmk; 

        //Reparametrize old Landmarks into new ones
        void reparametrize(const landmark_ptr_t & lmkDestPtr);
        void reparametrize(int size, vec &xNew, sym_mat &pNew);
        jblas::vec reparametrized() const { return reparametrize_func(state.x()); }
        virtual vec reparametrize_func(const vec & lmk) const = 0;
        virtual void reparametrize_func(const vec & lmk, vec & lnew, mat & LNEW_lmk) const = 0;

        virtual jblas::ind_array nobs() = 0;

        virtual size_t mySize() = 0;

        virtual size_t reparamSize() = 0;

        virtual jblas::vec3 center() = 0;

        virtual double uncertainty() = 0;

        // Create a landmark descriptor
        virtual void setDescriptor(const descriptor_ptr_t & descPtr)
        {
          descriptorPtr = descPtr;
        }

        enum DecisionMethod {
            ANY,
            ALL,
            MAJORITY
        };

        virtual bool needToDie() { return false; }

        void destroyDisplay();
        void suicide();
#if 0

        virtual bool needToReparametrize(DecisionMethod repMethod = ALL);
#endif
        virtual void transferInfoLmk(landmark_ptr_t & lmkSourcePtr);


        void fillEvents();

        unsigned countObserved();

    };

  }
}

#endif // #ifndef __LandmarkAbstract_H__