Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2008 Alex Brooks
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef HYDROLFEXTRACT_LINESEGMENTS_H
00011 #define HYDROLFEXTRACT_LINESEGMENTS_H
00012 
00013 #include <vector>
00014 #include <hydrogeom2d/geom2d.h>
00015 #include <hydrolaserfeatureextract/scanutil.h>
00016 
00017 namespace hydrolfextract {
00018 
00019 //
00020 // A Point is a single return from the scan.
00021 //
00022 class Point {
00023 public:
00024     Point( double x, double y, int returnI )
00025         : p_(x,y),
00026           returnI_(returnI)
00027         {}
00028 
00029     double x() const { return p_.x(); }
00030     double y() const { return p_.y(); }
00031 
00032     double &x() { return p_.x(); }
00033     double &y() { return p_.x(); }
00034 
00035     const geom2d::Point &point() const { return p_; }
00036 
00037     // Convert to polar
00038     double range()   const { return p_.range(); }
00039     double bearing() const { return p_.bearing(); }
00040 
00041     int returnI() const { return returnI_; }
00042 
00043 private:
00044 
00045     geom2d::Point p_;
00046 
00047     // The index of the return this point represents
00048     int returnI_;
00049 };
00050 
00051 enum StartOrEnd {
00052     Start,
00053     End,
00054 };
00055 
00056 // A line segment is a set of points such that no point is more than a certain distance
00057 // from the line.
00058 //
00059 class LineSegment {
00060 public:
00061 
00062     LineSegment( const std::vector<Point> &points,
00063                  int                       firstPointI,
00064                  int                       lastPointI );
00065 
00066     int firstPointI() const { return firstPointI_; }
00067     int lastPointI() const { return lastPointI_; }
00068 
00069     const geom2d::Line &bestFitLine() const { return bestFitLine_; }
00070 
00071     int numPoints() const { return lastPointI_-firstPointI_+1; }
00072 
00073 private:
00074 
00075     int          firstPointI_;
00076     int          lastPointI_;
00077     geom2d::Line bestFitLine_;
00078 };
00079 std::string toString( const LineSegment &lineSegment );
00080 inline std::ostream &operator<<( std::ostream &s, const LineSegment &l )
00081 { return s << toString(l); }
00082 
00083 // A cluster is a set of points such that adjacent points are no more than 
00084 // a certain distance apart.
00085 // It is further sub-divided into line segments.
00086 class Cluster {
00087 public:
00088 
00089     // Create a cluster from firstRangeI to lastRangeI inclusive.
00090     Cluster( const std::vector<Point>       &points,
00091              const std::vector<LineSegment> &lineSegments )
00092         : points_(points),
00093           lines_(lineSegments)
00094         {}
00095 
00096     const std::vector<Point> &points() const { return points_; }
00097     const std::vector<LineSegment> &lines() const { return lines_; }
00098     uint numLineSegments() const { return lines_.size(); }
00099 
00100     const Point &firstPointInLine( int lineI ) const { return points_[lines_[lineI].firstPointI()]; }
00101     const Point &lastPointInLine( int lineI ) const { return points_[lines_[lineI].lastPointI()]; }
00102     const Point &nthPointInLine( int lineI, int n ) const { return points_[lines_[lineI].firstPointI()+n]; }
00103 
00104     const Point &lineExtremity( int lineI, StartOrEnd startOrEnd ) const
00105         {
00106             if ( startOrEnd == Start ) return firstPointInLine(lineI);
00107             else return lastPointInLine(lineI);
00108         }    
00109 
00110     double lineLength( int lineI ) const;
00111 
00112 private:
00113 
00114     std::vector<Point>       points_;
00115     std::vector<LineSegment> lines_;
00116 };
00117 std::string toString( const Cluster &c );
00118 inline std::ostream &operator<<( std::ostream &s, const Cluster &c )
00119 { return s << toString(c); }
00120 
00121 class Lines {
00122 public:
00123 
00124     uint numClusters() const { return clusters_.size(); }
00125 
00126     void addCluster( const Cluster &cluster ) { clusters_.push_back( cluster ); }
00127     const std::vector<Cluster> &clusters() const { return clusters_; }
00128 
00129 private:
00130     std::vector<Cluster> clusters_;
00131 };
00132 std::string toString( const Lines &lines );
00133 inline std::ostream &operator<<( std::ostream &s, const Lines &lines )
00134 { return s << toString(lines); }
00135 
00136 Lines
00137 extractLines( const std::vector<float> &ranges,
00138               const ScannerConfig      &scannerConfig,
00139               int                       minPointsInLine,
00140               double                    maxDeltaRangeInCluster,
00141               double                    maxDeltaInLine );
00142 
00143 }
00144 
00145 #endif
lines.h
