Working copy. Not yet tested

src/CMakeLists.txt

@@ -26,6 +26,7 @@ SET(HDRS_BASE
 SET(HDRS
     corner_detector.h
     entities.h
+    laser_scan.h
     line_detector.h
     line_finder.h
     line_finder_hough.h
@@ -40,6 +41,7 @@ SET(HDRS
 SET(SRCS
     corner_detector.cpp
     entities.cpp
+    laser_scan.cpp
     line_detector.cpp
     line_finder.cpp
     line_finder_hough.cpp

src/corner_detector.cpp

@@ -54,9 +54,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
 
                         //vector from corner to first point of l1
                         Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
+                        Eigen::Vector2s v2 = line_it2->point_last_.head(2) - corner.pt_.head(2);
 
-                        //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
-                        corner.orientation_ = atan2(v1(1),v1(0));
+                        //compute corner orientation as the angle of the bisector w.r.t local frame
+                        corner.orientation_ = ( atan2(v1(1),v1(0)) + atan2(v2(1),v2(0)) ) / 2;
 
                         //Compute corner aperture with line_it1->first, corner and line_it2->last
                         corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);

src/laser_scan.cpp

+#include "laser_scan.h"
+
+namespace laserscanutils
+{
+    
+LaserScan::LaserScan()
+{
+
+}
+
+LaserScan::~LaserScan()
+{
+
+}
+
+void LaserScan::setScanParams(const ScanParams & _params)
+{
+    params_ = _params; 
+}
+
+void LaserScan::ranges2xy()
+{
+    ScalarT azimuth = params_.angle_min_;
+    ScalarT prev_range = 0;
+    unsigned int ii = 0;
+    unsigned int ii_ok = 0;
+    ScalarT kr = 1.5; //TODO: as a parameters somewhere. Times 
+
+    //resize to all points case
+    points_.resize(3,ranges_.size());
+
+    //clear jumps_ vector
+    jumps_.clear();
+
+    //for each range, check correctness of value and translate from polar to xy coordinates
+    for ( ii = 0; ii < ranges_.size(); ii++ )
+    {
+        //check raw range integrity
+        if ( ( ranges_[ii] > params_.range_min_ ) && 
+             ( ranges_[ii] < params_.range_max_ ) && 
+             ( !std::isnan(ranges_[ii]) ) && 
+             ( !std::isinf(ranges_[ii]) ) )
+        {
+            //transform from polar to euclidean
+            points_(0,ii_ok) = ranges_[ii] * cos(azimuth);
+            points_(1,ii_ok) = ranges_[ii] * sin(azimuth);
+            points_(2,ii_ok) = 1;
+            
+            //check jump. Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
+            if ( fabs(ranges_[ii]-prev_range) > kr*ranges_[ii]*params_.angle_step_) //jump condition (kr*r*sin(a) ~ kr*r*a)
+            {
+                jumps_.push_back(ii_ok);
+            }
+           
+            //increment ok counter
+            ii_ok ++; 
+            
+            //keep current range as previous for the next iteration
+            prev_range = ranges_[ii];
+            
+            //increment azimuth with angle step
+            azimuth += params_.angle_step_;
+        }
+    }
+
+    //resize the output matrix to the number of correct points, while keeping values
+    points_.conservativeResize(3, ii_ok);
+}
+
+}//namespace
+

src/laser_scan.h

+#ifndef LASER_SCAN_H_
+#define LASER_SCAN_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+
+//std
+#include <vector>
+#include <list>
+#include <iostream>
+
+namespace laserscanutils
+{
+
+/** \brief Laser scan parameters
+ * 
+ * Laser scan parameters
+ * 
+ **/
+struct ScanParams
+{
+    //members
+    double angle_min_; //radians
+    double angle_max_; //radians
+    double angle_step_; //radians
+    double scan_time_; //time from the measuring the first ray up to measuring the last one, seconds
+    double range_min_; //meters 
+    double range_max_; //meters
+    double range_std_dev_; //standard deviation measurement noise in range, meters 
+    
+    //just a print method
+    void print() const;
+};
+    
+/** \brief Class for raw laser scan data and basic methods
+* 
+* Class for raw laser scan data and basic methods
+* 
+*/
+class LaserScan
+{
+    protected:
+        ScanParams params_;
+        
+    public: 
+        //Ordered raw range data
+        std::vector<float> ranges_; 
+        
+        //ordered 2D points, each one expressed in homogeneous coordinates (x,y,1)^T. NaN and inf's are filtered out. 
+        Eigen::MatrixXs points_;
+        
+        //list of indexes where a scan jump is found. Indexes incicate start of a scan segment
+        std::list<unsigned int> jumps_; 
+            
+    public: 
+        /** \brief Constructor
+         * 
+         * Constructor
+         * 
+         **/
+        LaserScan(); 
+        
+        /** \brief Destructor
+         * 
+         * Destructor
+         * 
+         **/
+        ~LaserScan(); 
+        
+        /** \brief Set scan params
+         * 
+         * Set scan params. 
+         * 
+         **/
+        void setScanParams(const ScanParams & _params);
+        
+        /** \brief Transforms from ranges (polar) to euclidean (xy)
+        *
+        * Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
+        * Invalid values (inf's and nan's) and out of range measurements are not transformed.
+        * Valid measurements are set to points_ in homogeneous coordinates.
+        * 
+        * Set also the jumps_ vector, which after the call holds the indexes to points_ where a scan segment starts
+        *
+        **/
+        void ranges2xy();        
+
+        /** \brief Detect jumps on range data
+        *
+        * Detect jumps on range data. Sets jumps_ vector
+        *
+        **/
+//         void detectJumps();        
+      
+};
+
+}//namespace
+#endif
+
+

src/line_finder.h

@@ -4,7 +4,9 @@
 //laserscanutils
 #include "laser_scan_utils.h"
 #include "line_segment.h"
-#include "scan_basics.h"
+
+//std
+#include <list>
 
 namespace laserscanutils
 {