dataManagerOnePointRansac.hpp

Go to the documentation of this file.

#ifndef DATAMANAGERONEPOINTRANSAC_HPP_
#define DATAMANAGERONEPOINTRANSAC_HPP_

#include <vector>
#include <list>
#include "boost/shared_ptr.hpp"

#include "rtslam/dataManagerAbstract.hpp"
#include "rtslam/quatTools.hpp"

namespace jafar {
  namespace rtslam {

    class LoggableMatchings: public kernel::Loggable
    {
     public:
      struct Matching
      {
        unsigned id; double u, v;
        Matching(unsigned id, double u, double v): id(id), u(u), v(v) {}
      };

     private:
      std::string filename;
      std::vector<Matching> matchings;

     public:
      LoggableMatchings(std::string filename): filename(filename) {}
      void addMatching(unsigned id, double u, double v) { matchings.push_back(Matching(id,u,v)); }
      virtual void log()
      {
        std::ofstream f(filename.c_str());
        for(std::vector<Matching>::iterator it = matchings.begin(); it != matchings.end(); ++it)
          f << it->id << "\t" << it->u << "\t" << it->v << std::endl;
        f.close();
      }
    };


    typedef std::vector<observation_ptr_t> ObsList;
    struct RansacSet {
        observation_ptr_t obsBasePtr;
        std::map<unsigned, Measurement> inlierMeasurements;
        ObsList inlierObs;
        ObsList pendingObs;
        size_t size() {
          return inlierObs.size();
        }

        void add_inlier(observation_ptr_t obsPtr)
        {
          inlierObs.push_back(obsPtr);
          inlierMeasurements.erase(obsPtr->id());
          inlierMeasurements.insert(std::make_pair(obsPtr->id(), obsPtr->measurement));
        }

    };
    typedef boost::shared_ptr<RansacSet> ransac_set_ptr_t;
    typedef std::list<ransac_set_ptr_t> RansacSetList;


    // TODO extend to n-point ransac ?
    template<class RawSpec,class SensorSpec, class FeatureSpec, class RoiSpec, class FeatureManagerSpec, class DetectorSpec, class MatcherSpec>
    class DataManagerOnePointRansac: public DataManagerAbstract, public SpecificChildOf<SensorSpec> {

      public:
        // Define the function linkToParentSensorSpec.
        ENABLE_LINK_TO_SPECIFIC_PARENT(SensorExteroAbstract, SensorSpec, SensorSpec, DataManagerAbstract);
        // Define the functions sensorSpec() and sensorSpecPtr().
        ENABLE_ACCESS_TO_SPECIFIC_PARENT(SensorSpec, sensorSpec);

      public: // public interface
        DataManagerOnePointRansac(const boost::shared_ptr<DetectorSpec> & _detector, const boost::shared_ptr<MatcherSpec> & _matcher, const boost::shared_ptr<FeatureManagerSpec> _featMan, int n_updates_total, int n_updates_ransac, int n_tries, int n_init, int n_recomp_gains, bool multi_hyps = false, kernel::LoggerTask *loggerTask = NULL):
          detector(_detector), matcher(_matcher), featMan(_featMan), loggerTask(loggerTask), multi_hyps_(multi_hyps)
        {
          algorithmParams.n_updates_total = n_updates_total;
          algorithmParams.n_updates_ransac = n_updates_ransac;
          algorithmParams.n_tries = n_tries;
          algorithmParams.n_init = n_init;
          algorithmParams.n_recomp_gains = n_recomp_gains;
          prev_average_ransac_update_time = -1.;
          prev_average_as_update_time = -1.;
          clearCounters();
        }
        virtual ~DataManagerOnePointRansac() {
        }
        void processKnown(raw_ptr_t data, double date_limit = -1.);
        void detectNew(raw_ptr_t data);
//        void process(boost::shared_ptr<RawAbstract> data);
        void dump(raw_ptr_t data);

      protected: // main data members
        boost::shared_ptr<DetectorSpec> detector;
        boost::shared_ptr<MatcherSpec> matcher;
        boost::shared_ptr<FeatureManagerSpec> featMan;
        // the list of observations sorted by information gain
        typedef map<double, ObsList::iterator> ObservationListSorted;
        ObservationListSorted obsListSorted;
        // the list of visible observations to handle
        ObsList obsVisibleList;
        unsigned remainingObsCount;
        ObsList obsBaseList;
        ObsList obsFailedList;
        RansacSetList ransacSetList;
        kernel::LoggerTask *loggerTask;
        jblas::vec ransac_exp;
        bool multi_hyps_;

        double prev_average_ransac_update_time;
        double prev_average_as_update_time;

        struct Counters { 
            int visible;
            int measured;
            int matched;
            int updated;
            int created;
            int killed;
        } counters;
        void clearCounters() {
          int size = sizeof(Counters)/sizeof(int);
          for (int i = 0; i < size; ++i)
            ((int*)&counters)[i] = 0;
        }

      protected: // parameters
        struct alg_params_t {
            unsigned n_updates_total;  
            unsigned n_updates_ransac; 
            unsigned n_tries;   
            unsigned n_init;    
            unsigned n_recomp_gains; 
        } algorithmParams;

      public: // getters ans setters
/*        boost::shared_ptr<FeatureManagerSpec> featureManager(void) {
          return featMan;
        }*/
//        void setAlgorithmParams(int n_updates, int n_tries) {
//          algorithmParams_.n_updates = n_updates;
//          algorithmParams_.n_tries = n_tries;
//        }
//        alg_params_t algorithmParams() {
//          return algorithmParams_;
//        }

      protected: // helper functions
        void projectAndCollectVisibleObs();
        void updateVisibleObs();
        void getOneMatchedBaseObs(observation_ptr_t & obsBasePtr, boost::shared_ptr<RawSpec> rawData);
        observation_ptr_t selectOneRandomObs();
        vec updateMean(const observation_ptr_t & obsPtr);
        void projectFromMean(vec & exp, const observation_ptr_t & obsPtr, const vec & x);
        bool isLowInnovationInlier(const observation_ptr_t & obsPtr, const vec & exp, double lowInnTh);
        bool matchWithExpectedInnovation(boost::shared_ptr<RawSpec> rawData,  observation_ptr_t obsPtr, bool lowInnov = false);

        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
    };

  }
}


#include "rtslam/dataManagerOnePointRansac.impl.hpp"

#endif /* DATAMANAGERONEPOINTRANSAC_HPP_ */