From b509f350aa939ade40b1463d4bb6a833e86ead19 Mon Sep 17 00:00:00 2001
From: andreucm <acoromin@iri.upc.edu>
Date: Mon, 21 Mar 2016 15:51:56 +0100
Subject: [PATCH] Added new members to LaserScan class: ranges_raw_,
jumps_mask_ and is_raw_processed_
---
src/corner_finder_range_diff.cpp | 21 ++++++-----
src/laser_scan.cpp | 63 ++++++++++++++++++++------------
src/laser_scan.h | 22 ++++++++---
3 files changed, 68 insertions(+), 38 deletions(-)
diff --git a/src/corner_finder_range_diff.cpp b/src/corner_finder_range_diff.cpp
index ce64938..650c5bb 100644
--- a/src/corner_finder_range_diff.cpp
+++ b/src/corner_finder_range_diff.cpp
@@ -13,20 +13,20 @@ CornerFinderRangeDiff::~CornerFinderRangeDiff()
}
-// unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points,
-// std::list<laserscanutils::CornerPoint> & _corner_list)
-unsigned int CornerFinderRangeDiff::findCorners( const laserscanutils::LaserScan & _scan,
+unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points,
std::list<laserscanutils::CornerPoint> & _corner_list)
+// unsigned int CornerFinderRangeDiff::findCorners( const laserscanutils::LaserScan & _scan,
+// std::list<laserscanutils::CornerPoint> & _corner_list)
{
ScalarT range_diff;
- for (unsigned int ii = 1; ii<_scan.ranges.size(); ii++)
- {
- if (no jump)
- {
- range_diff = _scan.ranges[ii] - _scan.ranges[ii-1]
- }
- }
+// for (unsigned int ii = 1; ii<_scan.ranges.size(); ii++)
+// {
+// if (no jump)
+// {
+// range_diff = _scan.ranges[ii] - _scan.ranges[ii-1]
+// }
+// }
/*
range_diff_i = range_i - range_(i-1)
@@ -45,6 +45,7 @@ unsigned int CornerFinderRangeDiff::findCorners( const laserscanutils::LaserScan
// _corner_list.push_back();
}
}
+*/
}
void CornerFinderRangeDiff::print() const
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 4bedf4d..acaa498 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -3,7 +3,8 @@
namespace laserscanutils
{
-LaserScan::LaserScan()
+LaserScan::LaserScan() :
+ is_raw_processed_(false)
{
}
@@ -18,6 +19,11 @@ void LaserScan::setLaserScanParams(const LaserScanParams & _params)
params_ = _params;
}
+bool LaserScan::isRawProcessed() const
+{
+ return is_raw_processed_;
+}
+
void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
{
ScalarT azimuth = params_.angle_min_;
@@ -27,21 +33,25 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
ScalarT kr = 10; //TODO: as a parameters somewhere.
Eigen::Vector4s point_laser, point_ref;
- //resize to all points case
- points_.resize(3,ranges_.size());
+ //resize onvolved vectors for all points case
+ ranges_.resize(ranges_raw_.size());
+ points_.resize(3,ranges_raw_.size());
+ //jumps_indexes_.resize(ranges_raw_.size());
+ jumps_mask_.resize(ranges_raw_.size());
//clear jumps_ vector
- jumps_.clear();
+ jumps_indexes_.clear();
//for each range, check correctness of value and translate from polar to xy coordinates
- for ( ii = 0; ii < ranges_.size(); ii++ )
+ for ( ii = 0; ii < ranges_raw_.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]) ) )
+ if ( ( ranges_raw_[ii] > params_.range_min_ ) && ( ranges_raw_[ii] < params_.range_max_ ) &&
+ ( !std::isnan(ranges_raw_[ii]) ) && ( !std::isinf(ranges_raw_[ii]) ) )
{
+ //set as valid range
+ ranges_[ii] = ranges_raw_[ii];
+
//transform the laser hit from polar to 3D euclidean homogeneous
point_laser << ranges_[ii]*cos(azimuth), ranges_[ii]*sin(azimuth),0,1;
@@ -51,36 +61,43 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
//set to points_ as a 2D homogeneous
points_.block<3,1>(0,ii_ok) << point_ref(0),point_ref(1),1;
-// //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
+ //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) > fabs(kr*ranges_[ii]*params_.angle_step_)) //jump condition (kr*r*sin(a) ~ kr*r*a)
- //if ( fabs(ranges_[ii]-prev_range) > 0.5) //jump condition >0.5m
{
- jumps_.push_back(ii_ok);
+ jumps_indexes_.push_back(ii_ok); //indexes over points_
+ jumps_mask_[ii] = true; //masks over ranges_
+ }
+ else
+ {
+ jumps_mask_[ii] = false;
}
//increment ok counter
ii_ok ++;
//keep current range as previous for the next iteration
- prev_range = ranges_[ii];
+ prev_range = ranges_raw_[ii];
}
- else
+ else //invalid raw value
+ {
+ ranges_[ii] = -1.;
+ jumps_mask_[ii] = 1;
prev_range = 0;
+ }
//increment azimuth with angle step
azimuth += params_.angle_step_;
}
- //push back the last index to jumps_, to properly close the jumps_ vector. This will be used by findSegments()
- jumps_.push_back(ii_ok);
+ //push back the last index to jumps_indexes_, to properly close the list. This will be used by findSegments()
+ jumps_indexes_.push_back(ii_ok);
//resize the output matrix to the number of correct points, while keeping values
points_.conservativeResize(3, ii_ok);
+
+ //raise the flag
+ is_raw_processed_ = true;
}
void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_list)
@@ -89,10 +106,10 @@ void LaserScan::findSegments(std::list<laserscanutils::ScanSegment> & _segment_l
unsigned int num_points;
//set jumps_last to the last valid element of jumps
- jumps_last = std::prev(jumps_.end());
+ jumps_last = std::prev(jumps_indexes_.end());
//run over all jumps (except the last, which indicates the closing index)
- for (jumps_it = jumps_.begin(); jumps_it != jumps_last; jumps_it ++)
+ for (jumps_it = jumps_indexes_.begin(); jumps_it != jumps_last; jumps_it ++)
{
//new segment in the list
_segment_list.push_back(ScanSegment());
diff --git a/src/laser_scan.h b/src/laser_scan.h
index d63af25..aab35b3 100644
--- a/src/laser_scan.h
+++ b/src/laser_scan.h
@@ -41,23 +41,28 @@ struct LaserScanParams
class LaserScan
{
protected:
+ //laser scan parameters
LaserScanParams params_;
+ //flag indicating wheter this scan has been raw-processed or not
+ bool is_raw_processed_;
+
public:
//Ordered raw range data
std::vector<float> ranges_raw_;
- //Ordered range data, without NaN's, Inf's
+ //Ordered range data, without NaN's, Inf's. Bad values ar indicated with -1
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 indicate the second point of the jump (start of a scan segment)
- std::list<unsigned int> jumps_indexes_;
-
//For each element in ranges_, r_i, indicates if there is a jump (true) between that element and the previouos.
std::vector<bool> jumps_mask_;
+
+ //list of indexes over points_ where a scan jump is found. Indexes indicate the second point of the jump (start of a scan segment)
+ std::list<unsigned int> jumps_indexes_;
+
public:
/** \brief Constructor
@@ -81,7 +86,14 @@ class LaserScan
**/
void setLaserScanParams(const laserscanutils::LaserScanParams & _params);
- /** \brief Transforms from ranges (polar) to euclidean (xy)
+ /** \brief Returns the value of is_raw_processed_ flag
+ *
+ * Returns the value of is_raw_processed_ flag
+ *
+ **/
+ bool isRawProcessed() const;
+
+ /** \brief Sets points_ by transforming from ranges (polar) to euclidean (xy), and sets jumps_x_ vectors
*
* 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.
--
GitLab