D:/Travail/These/Determination caracteristiques camera/GSoC/SfM/src/TracksOfPoints.h

#ifndef _GSOC_SFM_TRACKS_OF_POINTS_H
#define _GSOC_SFM_TRACKS_OF_POINTS_H 1

#include "macro.h" //SFM_EXPORTS

#include <numeric>
#include "opencv2/calib3d/calib3d.hpp"

namespace OpencvSfM{
  class SFM_EXPORTS PointsToTrack;
  class SFM_EXPORTS PointOfView;

  class SFM_EXPORTS TrackOfPoints
  {
    friend class SequenceAnalyzer;

  protected:
    cv::Ptr<cv::Vec3d> point3D;
    std::vector<unsigned int> images_indexes_;
    std::vector<unsigned int> point_indexes_;
    unsigned int color;

    std::vector<bool> good_values;
    int track_consistance;
  public:
    template<typename Type, int size>
    operator cv::Vec<Type,size>&( ) {
      return *point3D;
    }
    TrackOfPoints( ):track_consistance( 0 ),color(0){};
    ~TrackOfPoints();
    bool addMatch( const int image_src, const int point_idx );

    inline bool containImage( const int image_wanted ) const
    {
      std::vector<unsigned int>::const_iterator idx = std::find(
        images_indexes_.begin( ),images_indexes_.end( ),
        image_wanted );
      return (idx != images_indexes_.end( ) &&
        good_values[idx - images_indexes_.begin( ) ]==true );
    }
    bool containPoint( const int image_src, const int point_idx1 ) const;
    inline unsigned int getNbTrack( ) const
    {
      return track_consistance<0 ? 0 : std::accumulate(
    good_values.begin( ), good_values.end( ), 0 );
    };
    cv::DMatch toDMatch( const int img1,const int img2 ) const;
    void getMatch( const unsigned int index,
      int &idImage, int &idPoint ) const;
    inline int getPointIndex( const unsigned int image ) const
    {
      std::vector<unsigned int>::const_iterator result =
        std::find( images_indexes_.begin( ),images_indexes_.end( ),image );
      if( result == images_indexes_.end( ) )
        return -1;
      return point_indexes_[ result - images_indexes_.begin( ) ];
    };
    inline int getImageIndex( const unsigned int idx ) const
    {
      return images_indexes_[ idx ];
    };
    
    double triangulateLinear( std::vector<PointOfView>& cameras,
      const std::vector< cv::Ptr<PointsToTrack> > &points_to_track,
      cv::Vec3d& points3D,
      const std::vector<bool> &masks = std::vector<bool>( ) );
    double triangulateRobust( std::vector<PointOfView>& cameras,
      const std::vector< cv::Ptr< PointsToTrack > > &points_to_track,
      cv::Vec3d& points3D,
      double reproj_error = 4,
      const std::vector<bool> &masks = std::vector<bool>( ) );
    
    void removeOutliers( std::vector<PointOfView>& cameras,
      const std::vector< cv::Ptr< PointsToTrack > > &points_to_track,
      double reproj_error = 4,
      std::vector<bool> *masks = NULL );

    static void fusionDuplicates( std::vector<TrackOfPoints>& tracks );
    
    inline void set3DPosition( cv::Vec3d newPoint )
    {
      if( !point3D )
        point3D = new cv::Vec3d( newPoint );
      else
        *point3D = newPoint;
    }
    inline cv::Ptr<cv::Vec3d> get3DPosition(){ return point3D; };
    
    inline unsigned int getColor() const {return color;};
    inline void setColor(unsigned int c) {color = c;};
    
    static void keepTrackHavingImage( unsigned int idx_image,
      std::vector<TrackOfPoints>& tracks );
    static void keepTrackWithImages( const
      std::vector<int>& imgList,
      std::vector<TrackOfPoints>& tracks );

    static void mixTracks( const std::vector<TrackOfPoints>& list_tracks,
      std::vector<TrackOfPoints>* mixed_tracks);

  protected:
    double errorEstimate( std::vector< PointOfView >& cameras,
      const std::vector< cv::Ptr< PointsToTrack > > &points_to_track,
      cv::Vec3d& points3D,
      const std::vector<bool> &masks = std::vector<bool>( ) ) const;
  };


  typedef struct
  {
    int imgSrc;
    int imgDest;
  } ImageLink;
  class SFM_EXPORTS ImagesGraphConnection
  {
  protected:
    cv::SparseMat images_graph_;

    inline void orderedIdx( int i1,int i2,int idx[ 2 ] )
    {
      if( i1<i2 )
      {
        idx[ 0 ] = i1;
        idx[ 1 ] = i2;
      }
      else
      {
        idx[ 0 ] = i2;
        idx[ 1 ] = i1;
      }
    }
  public:
    ImagesGraphConnection( ){};

    inline bool isGraphCreated( int nbImages )
    {
      return images_graph_.size( )!=0 && images_graph_.nzcount( )!=0 &&
        images_graph_.size( )[ 0 ] == nbImages &&
        images_graph_.size( )[ 1 ] == nbImages;
    }

    inline void initStructure( int nb_images )
    {
      const int dims = 2;
      int size[ ] = {nb_images,nb_images};
      images_graph_.create( dims,size,CV_32S );
    }
    inline void addLink( int first_image,int second_image )
    {
      int idx[ 2 ];
      orderedIdx( first_image,second_image,idx );
      images_graph_.ref<int>( idx ) += 1;
    }
    inline int getNumbersOfLinks( int first_image,int second_image )
    {
      int idx[ 2 ];
      orderedIdx( first_image,second_image,idx );
      const int *value=images_graph_.find<int>( idx );
      if( value!=NULL )
        return *value;
      return 0;
    }
    int getHighestLink( int &first_image,int &second_image,
      int max_number=1e9 );
    void getOrderedLinks( std::vector<ImageLink>& outList,
      int min_number=0, int max_number=1e9 );
    void getImagesRelatedTo( int first_image, std::vector<ImageLink>& outList,
      int min_number=0, int max_number=1e9 );
  };
}
#endif