Orca Robotics

00001 #ifndef HYDROLFEXTRACT_ANNOTATEDLINES_H
00002 #define HYDROLFEXTRACT_ANNOTATEDLINES_H
00003 
00004 #include <hydrolaserfeatureextract/lines.h>
00005 
00006 namespace hydrolfextract {
00007 
00008 enum EndpointVisibility {
00009     NotVisible,
00010     VisibleInternalLineJunction,
00011     VisibleExternalLineJunction,
00012     VisibleFreeStandingLine,
00013 };
00014 std::string toString( EndpointVisibility e );
00015 inline std::ostream &operator<<( std::ostream &s, const EndpointVisibility &e )
00016 { return s << toString(e); }
00017 inline bool isVisible( EndpointVisibility e )
00018 { return e != NotVisible; }
00019 
00020 class AnnotatedCluster {
00021 public:
00022 
00023     AnnotatedCluster( const Cluster &cluster );
00024 
00025     const Cluster &cluster() const { return cluster_; }
00026 
00027     const std::vector<Point> &points() const { return cluster_.points(); }
00028     uint numLineSegments() const { return cluster_.numLineSegments(); }
00029     const std::vector<LineSegment> &lines() const { return cluster_.lines(); }
00030 
00031     const Point &firstPointInLine( int lineI ) const { return cluster_.firstPointInLine(lineI); }
00032     const Point &lastPointInLine( int lineI ) const { return cluster_.lastPointInLine(lineI); }
00033 
00034     double lineLength( int lineI ) const { return cluster_.lineLength(lineI); }
00035 
00036     EndpointVisibility startVisibility( int lineI ) const { return notes_[lineI].startVisibility; }
00037     EndpointVisibility endVisibility( int lineI ) const { return notes_[lineI].endVisibility; }
00038     double maxLineFitError( int lineI ) const { return notes_[lineI].maxLineFitError; }
00039     double lineFitRhoSd( int lineI ) const { return notes_[lineI].lineFitRhoSd; }
00040     double lineFitAlphaSd( int lineI ) const { return notes_[lineI].lineFitAlphaSd; }
00041 
00042     void setStartVisibility( int lineI, EndpointVisibility v ) { notes_[lineI].startVisibility = v; }
00043     void setEndVisibility( int lineI, EndpointVisibility v ) { notes_[lineI].endVisibility = v; }
00044     void setMaxLineFitError( int lineI, double maxLineFitError ) { notes_[lineI].maxLineFitError = maxLineFitError; }
00045     void setLineFitUncertainty( int lineI, double rhoSd, double alphaSd )
00046         { notes_[lineI].lineFitRhoSd = rhoSd; notes_[lineI].lineFitAlphaSd = alphaSd; }
00047 
00048 private:
00049 
00050     struct LineNotes {
00051         // The most distance by which any single point deviates from the best-fit line
00052         double             maxLineFitError;
00053 
00054         // The uncertainty in the line estimate due to line-fitting issues
00055         double lineFitRhoSd;
00056         double lineFitAlphaSd;
00057 
00058         EndpointVisibility startVisibility;
00059         EndpointVisibility endVisibility;
00060     };
00061 
00062     Cluster cluster_;
00063     // One per line segment
00064     std::vector<LineNotes> notes_;
00065 };
00066 std::string toString( const AnnotatedCluster &a );
00067 inline std::ostream &operator<<( std::ostream &s, const AnnotatedCluster &a )
00068 { return s << toString(a); }
00069 
00070 class AnnotatedLines {
00071 public:
00072     AnnotatedLines( const Lines &lines );
00073 
00074     uint numClusters() const { return annotatedClusters_.size(); }
00075     const std::vector<AnnotatedCluster> &clusters() const { return annotatedClusters_; }
00076     std::vector<AnnotatedCluster> &clusters() { return annotatedClusters_; }
00077     
00078 private:
00079 
00080     std::vector<AnnotatedCluster> annotatedClusters_;
00081 };
00082 std::string toString( const AnnotatedLines &a );
00083 inline std::ostream &operator<<( std::ostream &s, const AnnotatedLines &a )
00084 { return s << toString(a); }
00085 
00086 AnnotatedLines
00087 getAnnotatedLines( const Lines &lines,
00088                    const std::vector<float> &ranges,
00089                    double minLineLengthForVisibleEndpoints );
00090 
00091 }
00092 
00093 #endif
annotatedlines.h
