00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef CURVATUREDETECTOR_H_
00024 #define CURVATUREDETECTOR_H_
00025
00026 #include <feature/InterestPoint.h>
00027 #include <feature/Detector.h>
00028 #include <utils/Convolution.h>
00029 #include <utils/PeakFinder.h>
00030 #include <boost/graph/adjacency_matrix.hpp>
00031 #include <boost/graph/adjacency_list.hpp>
00032 #include <boost/graph/prim_minimum_spanning_tree.hpp>
00033 #include <boost/graph/johnson_all_pairs_shortest.hpp>
00034 #include <boost/graph/graph_utility.hpp>
00035 #include <vector>
00036
00037
00038 typedef boost::property<boost::vertex_distance_t, double> DistanceVertexProperty;
00039 typedef boost::property < boost::edge_weight_t, double > WeightEdgeProperty;
00040
00041 typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS, DistanceVertexProperty, WeightEdgeProperty> Graph;
00042 typedef boost::adjacency_matrix < boost::undirectedS, DistanceVertexProperty, WeightEdgeProperty > MatrixGraph;
00043 typedef std::pair<unsigned int, unsigned int> GraphEdge;
00044
00045
00046
00055 class CurvatureDetector: public Detector {
00056 public:
00066 CurvatureDetector(const PeakFinder* peak, unsigned int scales = 5, double sigma = 0.2, double step = 1.4, unsigned int dmst = 4);
00067
00068 virtual unsigned int detect(const LaserReading& reading, std::vector<InterestPoint*>& point) const;
00069
00082 virtual unsigned int detect(const LaserReading& reading, std::vector<InterestPoint*>& points,
00083 Graph& graph,
00084 std::vector< std::vector<Point2D> >& operatorA,
00085 std::vector< std::vector<double> >& signalDiff,
00086 std::vector< std::vector<unsigned int> >& indexes) const;
00087
00088 virtual unsigned int detect(const LaserReading& reading, std::vector<InterestPoint*>& point,
00089 std::vector< double >& signal,
00090 std::vector< std::vector<double> >& signalSmooth,
00091 std::vector< std::vector<double> >& signalDiff,
00092 std::vector< std::vector<unsigned int> >& indexes) const;
00093
00105 virtual void detect(const Graph& graph, const std::vector<Point2D>& graphPoints, std::vector< std::vector<Point2D> >& operatorA,
00106 std::vector< std::vector<double> >& signalDiff, std::vector< std::vector<unsigned int> >& indexes) const;
00107
00109 inline void setScaleNumber(unsigned int scales)
00110 {m_scaleNumber = scales; computeScaleBank();}
00111
00113 inline void setDmst(unsigned int dmst)
00114 {m_dmst = dmst;}
00115
00117 inline void setBaseSigma(double sigma)
00118 {m_baseSigma = sigma; computeScaleBank();}
00119
00121 inline void setSigmaStep(double step)
00122 {m_sigmaStep = step; computeScaleBank();}
00123
00125 inline void setPeakFinder(const PeakFinder* peak)
00126 {m_peakFinder = peak;}
00127
00129 inline void setUseMaxRange(bool use)
00130 {m_useMaxRange = use;}
00131
00133 inline unsigned int getScaleNumber() const
00134 {return m_scaleNumber;}
00135
00137 inline unsigned int getDmst() const
00138 {return m_dmst;}
00139
00141 inline const std::vector<double>& getScales() const
00142 {return m_scales;}
00143
00145 inline double getBaseSigma() const
00146 {return m_baseSigma;}
00147
00149 inline double getSigmaStep() const
00150 {return m_sigmaStep;}
00151
00153 inline const PeakFinder* getPeakFinder() const
00154 {return m_peakFinder;}
00155
00157 inline bool getUseMaxRange()
00158 {return m_useMaxRange;}
00159
00160 protected:
00162 virtual void computeGraph(const LaserReading& reading, std::vector<Point2D>& graphPoints, Graph& graph, std::vector<unsigned int>& maxRangeMapping) const;
00163
00165 virtual unsigned int computeInterestPoints(const LaserReading& reading, const std::vector< std::vector<Point2D> >& operatorA, std::vector<InterestPoint*>& point,
00166 const std::vector< std::vector<unsigned int> >& indexes, std::vector<unsigned int>& maxRangeMapping) const;
00167
00169 void computeScaleBank();
00170
00171 const PeakFinder* m_peakFinder;
00172 unsigned int m_scaleNumber;
00173 double m_baseSigma;
00174 double m_sigmaStep;
00175 bool m_useMaxRange;
00176 unsigned int m_dmst;
00177 std::vector<double> m_scales;
00178 };
00179
00180 #endif