imageSegmentManager.hpp

/* $$ */

#ifndef SLAM_IMAGE_SEGMENT_MANAGER_HPP
#define SLAM_IMAGE_SEGMENT_MANAGER_HPP

#include <iostream>
#include <vector>
#include <map>

#include "jmath/jblas.hpp"

#include "slam/bearingOnlySlam.hpp"

#include "lines/lineSegmentSet.hpp"
#include "lines/lineSegment.hpp"
#include "lines/matchingSet.hpp"
#include "slam/observation.hpp"

namespace jafar {
  namespace slamlines {

    class ImageSegment {

    public:

      static const unsigned int NO_ID = 0;

      unsigned int id;

      jblas::vec2 ext1;
      jblas::vec2 ext2;

      jblas::sym_mat ext1Cov;
      jblas::sym_mat ext2Cov;


      ImageSegment(jblas::vec2 ext1_, jblas::vec2 ext2_) :
  id(NO_ID) ,
  ext1(ext1_),
  ext2(ext2_)
      {}

      ImageSegment(jblas::vec2 ext1_, jblas::vec2 ext2_, jblas::sym_mat ext1Cov_, jblas::sym_mat ext2Cov_,unsigned int id_) :
  id(id_) ,
  ext1(ext1_),
  ext2(ext2_),
  ext1Cov(ext1Cov_),
  ext2Cov(ext2Cov_)
       {}
    };

    std::ostream& operator <<(std::ostream& s, ImageSegment const& seg);

    typedef std::pair<std::size_t, std::size_t> Match;

    typedef std::vector<Match> VecMatches;

    typedef std::vector<ImageSegment> VecImageSegments;

    void readImageSegments(std::string const& filename, VecImageSegments& vecImageSegments);

    void readImageSegmentsId(std::string const& filename, VecImageSegments& vecImageSegments);

    void readImageSegmentsMatches(std::string const& filename, VecMatches& matches);

    void lineSegmentSetToImageSegmentVec(jafar::lines::LineSegmentSet& lineSegmentSet, VecImageSegments& vecImageSegments);

    void matchingSetToImageSegmentsMatches(jafar::lines::MatchingSet& matchingSet, VecMatches& matches);

    unsigned int propagateId(VecMatches const& vecMatches, 
           VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch);


  
    class ImageSegmentManager {

    protected:

      unsigned int p_idFactory;
      unsigned int getNewFeatureId() {return ++p_idFactory;}
      //unsigned int setNewFeatureId(unsigned int& p_idFactory, unsigned int global_id) {p_idFactory = global_id;}

      slam::BearingOnlySlam& p_slam;

      double p_minSegmentSize;

      jblas::vec2 p_zone1;
      jblas::vec2 p_zone2;

      bool isInZone(jblas::vec2 const& pix) const {
  return pix(0) > p_zone1(0) && pix(1) > p_zone1(1) && pix(0) < p_zone2(0) && pix(1) < p_zone2(1);
      }

      void featureSelection(VecImageSegments& vecImageSegments);

      void slamProcessObservations(unsigned int currentFrameIndex, VecImageSegments& vecImageSegments) const;

    public:
      slam::Observation::ObservationType type;

          
      ImageSegmentManager(slam::BearingOnlySlam& slam, slam::Observation::ObservationType type_=slam::Observation::SEGMENT_IMAGE) : p_idFactory(0), p_slam(slam), type(type_) {}

      void setup(double minSegmentSize, double u1, double v1, double u2, double v2) {
  p_minSegmentSize = minSegmentSize;
  p_zone1(0) = u1;
  p_zone1(1) = v1;
  p_zone2(0) = u2;
  p_zone2(1) = v2;
      }

      void initFrame(unsigned int frameIndex_, VecImageSegments& vecImageSegments);
      void processFrame(unsigned int frameIndex, VecImageSegments& vecImageSegments);
     /* void ImageSegmentManager::getSegmentsPrediction(lines::LineSegmentSet& lineSegmentSet, VecImageSegments& vecImageSegments);*/
      unsigned int compareId(VecMatches const& matches, 
     VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch, jafar::lines::LineSegmentSet& lineSegmentSet);

      unsigned int compareIdInv(VecMatches const& matches, 
     VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch, jafar::lines::LineSegmentSet& lineSegmentSet);
      unsigned int compareIdInvAll(VecMatches const& matches, VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch, jafar::lines::LineSegmentSet& lineSegmentSet);
    }; // class ImageSegmentManager

    
  
// class ImageHomogenCoordSegmentManager
class ImageHomogenCoordSegmentManager {

  protected:

    unsigned int p_idFactory;
    unsigned int getNewFeatureId() {return ++p_idFactory;}
//unsigned int setNewFeatureId(unsigned int& p_idFactory, unsigned int global_id) {p_idFactory = global_id;}
    slam::SlamEkf& slam;

    double p_minSegmentSize;

    jblas::vec2 p_zone1;
    jblas::vec2 p_zone2;

    bool isInZone(jblas::vec2 const& pix) const {
      return pix(0) > p_zone1(0) && pix(1) > p_zone1(1) && pix(0) < p_zone2(0) && pix(1) < p_zone2(1);
    }

    void featureSelection(VecImageSegments& vecImageSegments);

    void slamProcessObservations(unsigned int currentFrameIndex, VecImageSegments& vecImageSegments) const;

  public:
          
                slam::Observation::ObservationType type;

                ImageHomogenCoordSegmentManager(slam::SlamEkf& slam_, slam::Observation::ObservationType type_=slam::Observation::SEGMENT_IMAGE) : p_idFactory(0), slam(slam_), type(type_) {}

    void setup(double minSegmentSize, double u1, double v1, double u2, double v2) {
      p_minSegmentSize = minSegmentSize;
      p_zone1(0) = u1;
      p_zone1(1) = v1;
      p_zone2(0) = u2;
      p_zone2(1) = v2;
    }

    void initFrame(unsigned int frameIndex_, VecImageSegments& vecImageSegments);
    
    void processFrame(unsigned int frameIndex, VecImageSegments& vecImageSegments);
    /* void ImageSegmentManager::getSegmentsPrediction(lines::LineSegmentSet& lineSegmentSet, VecImageSegments& vecImageSegments);*/
    
    
    unsigned int compareId(VecMatches const& matches, 
          VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch, jafar::lines::LineSegmentSet& lineSegmentSet);

    unsigned int compareIdInv(VecMatches const& matches, 
          VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch, jafar::lines::LineSegmentSet& lineSegmentSet);
    
    unsigned int compareIdInvAll(VecMatches const& matches, VecImageSegments const& vecImageSegmentsRef, VecImageSegments& vecImageSegmentsMatch, jafar::lines::LineSegmentSet& lineSegmentSet);
}; // class ImageHomogenCoordSegmentManager


  } // namespace slam
} // namespace jafar

#endif // SLAM_IMAGE_SEGMENT_MANAGER_HPP