00001 #ifndef _FACETS_TRACKER_HPP_
00002 #define _FACETS_TRACKER_HPP_
00003
00004 #include <camera/cameraPinhole.hpp>
00005 #include <image/Image.hpp>
00006
00007 #include "facetsmap/types.hpp"
00008 #include <gfm/HarrisDetector.hpp>
00009
00010 namespace jafar {
00011 namespace gfm {
00012 class Detector;
00013 }
00014 namespace facetsmap {
00015 class Facet;
00016 class FacetsParams;
00017 class FacetsDetector;
00022 struct FacetsTrack {
00023 Facet * facet;
00024 double left_u, left_v, right_u, right_v;
00025 double scoreTexture,score;
00026 bool operator==(const FacetsTrack& fT) const {
00027 return (fT.facet == facet) and (fT.left_u == left_u) and (fT.left_v == left_v) and (fT.right_u == right_u) and (fT.right_u == right_u) and (fT.scoreTexture == scoreTexture) and (fT.score == score);
00028 }
00029 };
00034 struct FacetsTrackingResult {
00035 std::vector<FacetsTrack> tracks;
00036 jafar::gfm::DetectionResult leftpoints;
00037 jafar::gfm::DetectionResult rightpoints;
00038 vFacets facets;
00039 vFacets trackedfacets;
00040 vFacets untrackedfacets;
00041 vFacets untrackablefacets;
00042 };
00043 class FacetsTracker {
00044 public:
00045 FacetsTracker(const FacetsParams* params, const camera::StereoBench& stereoBench, FacetsDetector* fd = 0, gfm::Detector* ipDetect = 0);
00046 vFacets initTracking(const jafar::image::Image& imgl, const jafar::image::Image& imgr);
00047 FacetsTrackingResult track(const boost::numeric::ublas::bounded_matrix<double,4,4>& robotmovement, const boost::numeric::ublas::bounded_matrix<double,4,4>& leftToRight, const jafar::image::Image& imgl, const jafar::image::Image& imgr);
00048 FacetsTrackingResult track(const boost::numeric::ublas::bounded_matrix<double,4,4>& robotmovement, const boost::numeric::ublas::bounded_matrix<double,4,4>& leftToRight, const vFacets& previous, const jafar::image::Image& imgl, const jafar::image::Image& imgr);
00049 public:
00050 inline void setPosError(double pE) { m_posError = pE; m_posError2 = pE*pE; }
00051 inline double posError() const { return m_posError; }
00052 inline double posError2() const { return m_posError2; }
00053 inline void setStereoPosError(double pE) { m_posStereoError = pE; }
00054 inline double stereoPosError() const { return m_posStereoError; }
00055 private:
00056 const FacetsParams* m_params;
00057 double m_posError, m_posError2, m_posStereoError;
00058 FacetsDetector* m_facetsDetector;
00059 gfm::Detector* m_interestPointDetector;
00060 camera::StereoBench m_stereoBench;
00061 vFacets m_lastFacets;
00062 };
00063 }
00064 }
00065
00066 #endif
