diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 634b323b1b76447de0f0f67e276c722c66882078..3f88f9f8f32d218236bf70481d8638719975a0b3 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -13,18 +13,19 @@ LaserScan::~LaserScan()
}
-void LaserScan::setScanParams(const ScanParams & _params)
+void LaserScan::setLaserScanParams(const LaserScanParams & _params)
{
params_ = _params;
}
-void LaserScan::ranges2xy()
+void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
{
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.
+ ScalarT kr = 2; //TODO: as a parameters somewhere.
+ Eigen::Vector4s point_laser, point_ref;
//resize to all points case
points_.resize(3,ranges_.size());
@@ -41,13 +42,23 @@ void LaserScan::ranges2xy()
( !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;
+ //transform the laser hit from polar to 3D euclidean homogeneous
+ point_laser << ranges_[ii]*cos(azimuth), ranges_[ii]*sin(azimuth),0,1;
+
+ //apply device mounting point calibration (p_r = T * p_l)
+ point_ref = _device_T*point_laser;
+
+ //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
if ( fabs(ranges_[ii]-prev_range) > 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);
}
@@ -56,11 +67,13 @@ void LaserScan::ranges2xy()
ii_ok ++;
//keep current range as previous for the next iteration
- prev_range = ranges_[ii];
-
- //increment azimuth with angle step
- azimuth += params_.angle_step_;
+ prev_range = ranges_[ii];
}
+ else
+ 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()
diff --git a/src/laser_scan.h b/src/laser_scan.h
index 7f36d90655c4456ef803f51bd601bb6b42281283..e0a0f17114ed8eb45db981ac9e3ba72f8674b9c3 100644
--- a/src/laser_scan.h
+++ b/src/laser_scan.h
@@ -18,7 +18,7 @@ namespace laserscanutils
* Laser scan parameters
*
**/
-struct ScanParams
+struct LaserScanParams
{
//members
double angle_min_; //radians
@@ -41,7 +41,7 @@ struct ScanParams
class LaserScan
{
protected:
- ScanParams params_;
+ LaserScanParams params_;
public:
//Ordered raw range data
@@ -73,7 +73,7 @@ class LaserScan
* Set scan params.
*
**/
- void setScanParams(const laserscanutils::ScanParams & _params);
+ void setLaserScanParams(const laserscanutils::LaserScanParams & _params);
/** \brief Transforms from ranges (polar) to euclidean (xy)
*
@@ -81,10 +81,14 @@ class LaserScan
* Invalid values (inf's and nan's) and out of range measurements are not transformed.
* Valid measurements are set to points_ in homogeneous coordinates.
*
+ * _device_T is the homogeneous transformation, moving points from laser device frame to the reference one:
+ * p_ref = _device_T*p_laser;
+ * If _device_T is provided, points are transformed to a new reference frame.
+ *
* Set also the jumps_ vector, which after the call holds the indexes to points_ where a scan segment starts
*
**/
- void ranges2xy();
+ void ranges2xy(Eigen::Matrix4s _device_T = Eigen::Matrix4s::Identity());
/** \brief Find segments based on jumps of consecutive scan points
*
diff --git a/src/line_finder.h b/src/line_finder.h
index 1310f9f73a49de62dafc813ddaa221269e65405e..6d888d6bcaaf558dcd4774b576890365417f747d 100644
--- a/src/line_finder.h
+++ b/src/line_finder.h
@@ -69,8 +69,8 @@ class LineFinder
* \return Number of lines extracted.
*
*/
- virtual unsigned int findLines( const Eigen::MatrixXd & _points,
- std::list<LineSegment> & _line_list) const = 0;
+ virtual unsigned int findLines( const Eigen::MatrixXs & _points,
+ std::list<laserscanutils::LineSegment> & _line_list) = 0;
/** \brief Print things
*
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
index 10a6e0afd794f47449ad33cc9f5b80920f6e254d..de178a474269ee14c7d31ae4c2d3855fd8e13014 100644
--- a/src/line_finder_hough.cpp
+++ b/src/line_finder_hough.cpp
@@ -30,7 +30,7 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
}
}
-unsigned int LineFinderHough::findLines( const Eigen::MatrixXd & _points,
+unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list)
{
double theta, range;
@@ -41,6 +41,15 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXd & _points,
Eigen::MatrixXs pts; //auxiliar array of points
std::list<Eigen::Vector3s>::iterator pts_it;
+ //clear the Houhg Grid
+ for (unsigned int ii = 0; ii < hough_grid_.size(); ii++)
+ {
+ for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++)
+ {
+ hough_grid_.at(ii).at(jj).clear();
+ }
+ }
+
//STEP 1. For each scan point, accumulate hits in the Hough Grid
for (unsigned int ipt = 0; ipt < _points.cols(); ipt++) //loop over all points
{
diff --git a/src/line_finder_hough.h b/src/line_finder_hough.h
index 2e993341da324a316b7ccf56e9fbbf6856c803b0..e2a2c4c9d86219bfefa6c91f0ec53a0f40fa64dd 100644
--- a/src/line_finder_hough.h
+++ b/src/line_finder_hough.h
@@ -78,7 +78,7 @@ class LineFinderHough : public LineFinder
*
*
*/
- unsigned int findLines( const Eigen::MatrixXd & _points,
+ unsigned int findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list);
/** \brief Print things
diff --git a/src/line_finder_jump_fit.cpp b/src/line_finder_jump_fit.cpp
index 28f5e5888e049e7607da238d7769e2850f2feaf8..393129d1acd471e5d9973ecbea44e573ef06d401 100644
--- a/src/line_finder_jump_fit.cpp
+++ b/src/line_finder_jump_fit.cpp
@@ -18,7 +18,7 @@ void LineFinderJumpFit::setJumpFitParams(const LineFinderJumpFitParams & _params
this->jump_fit_params_ = _params;
}
-unsigned int LineFinderJumpFit::findLines( const Eigen::MatrixXd & _points,
+unsigned int LineFinderJumpFit::findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list)
{
//TODO
diff --git a/src/line_finder_jump_fit.h b/src/line_finder_jump_fit.h
index 8f6b2fbccd8fbd39c2d3b8c4e25a8e808ae2c921..e6040cb9f3ec03e102d8c03e86e391d5154f4251 100644
--- a/src/line_finder_jump_fit.h
+++ b/src/line_finder_jump_fit.h
@@ -74,7 +74,7 @@ class LineFinderJumpFit : public LineFinder
*
*
*/
- unsigned int findLines( const Eigen::MatrixXd & _points,
+ unsigned int findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list);
/** \brief Print things
diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp
index 49533474f3dd72b4225408c80d9775deca418f97..4d60dc190f77a4930de7426ddac6651aee9d1944 100644
--- a/src/scan_segment.cpp
+++ b/src/scan_segment.cpp
@@ -22,6 +22,32 @@ void ScanSegment::merge(const ScanSegment & _segment)
//TODO
}
+unsigned int ScanSegment::numPoints() const
+{
+ return points_.cols();
+}
+
+const Eigen::Vector3s & ScanSegment::getCentroid() const
+{
+ return centroid_;
+}
+
+const Eigen::Vector3s & ScanSegment::getBBox(unsigned int _corner_idx) const
+{
+ if (_corner_idx < 4)
+ return bbox_corners_[_corner_idx];
+ else
+ return bbox_corners_[0];
+}
+
+ScalarT ScanSegment::getEvalRatio() const
+{
+ if ( eval_1_ != 0 )
+ return eval_2_/eval_1_;
+ else
+ return 1e10;
+}
+
void ScanSegment::computeCentroid()
{
ScalarT mx=0, my=0;
diff --git a/src/scan_segment.h b/src/scan_segment.h
index fe6acdd7e0fbe5c17041363a1460b0bbbd051a40..6dff963bd81e5837f20a1787f97b5bfd84fc5dfe 100644
--- a/src/scan_segment.h
+++ b/src/scan_segment.h
@@ -45,6 +45,34 @@ class ScanSegment
//merges this ScanSegment with the argument.
void merge(const ScanSegment & _segment);
+ /** \brief Return the number of points supporting this segment
+ *
+ * Return the number of points supporting this segment
+ *
+ **/
+ unsigned int numPoints() const;
+
+ /** \brief Return const ref to centroid
+ *
+ * Return const ref to centroid
+ *
+ **/
+ const Eigen::Vector3s & getCentroid() const;
+
+ /** \brief Return const ref to one corner of the bounding box
+ *
+ * Return const ref to one corner of the bounding box
+ *
+ **/
+ const Eigen::Vector3s & getBBox(unsigned int _corner_idx) const;
+
+ /** \brief Return the ratio eval_2_/eval_1_
+ *
+ * Return the ratio eval_2_/eval_1_
+ *
+ **/
+ ScalarT getEvalRatio() const;
+
/** \brief Compute centroid coordinates
*
* Compute centroid coordinate.
@@ -58,7 +86,7 @@ class ScanSegment
* Clearance is an extra distance added to the bb limits. Typically half od the grid cell size
*
**/
- void computeBoundingBox(const ScalarT & _clearance = 0.01);
+ void computeBoundingBox(const ScalarT & _clearance = 0.1);
/** \brief Compute both centroids and bounding box
*