From 87d4e7f05242e84a6bf745afcf8778ffd531578c Mon Sep 17 00:00:00 2001
From: andreucm <acoromin@iri.upc.edu>
Date: Mon, 22 Feb 2016 15:36:16 +0100
Subject: [PATCH] Working copy. Not yet tested
---
src/CMakeLists.txt | 2 +
src/corner_detector.cpp | 5 +-
src/laser_scan.cpp | 71 ++++++++++++++++++++++++++++
src/laser_scan.h | 100 ++++++++++++++++++++++++++++++++++++++++
src/line_finder.h | 4 +-
5 files changed, 179 insertions(+), 3 deletions(-)
create mode 100644 src/laser_scan.cpp
create mode 100644 src/laser_scan.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f33f546..15854d7 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/corner_detector.cpp b/src/corner_detector.cpp
index 6fdddda..b7848fc 100644
--- a/src/corner_detector.cpp
+++ b/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_);
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
new file mode 100644
index 0000000..a146372
--- /dev/null
+++ b/src/laser_scan.cpp
@@ -0,0 +1,71 @@
+#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
+
diff --git a/src/laser_scan.h b/src/laser_scan.h
new file mode 100644
index 0000000..618a1a7
--- /dev/null
+++ b/src/laser_scan.h
@@ -0,0 +1,100 @@
+#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
+
+
diff --git a/src/line_finder.h b/src/line_finder.h
index fcf93be..1310f9f 100644
--- a/src/line_finder.h
+++ b/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
{
--
GitLab