From f41c1f3ff0c84bb193bc7f02b35dab9277ec473d Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Wed, 10 Feb 2016 19:15:19 +0000
Subject: [PATCH] Hough Line extraction debugged and checked with TNO rosbag.
It remains to modify entry type of laser points
---
src/line_detector.cpp | 16 ++++++++--------
1 file changed, 8 insertions(+), 8 deletions(-)
diff --git a/src/line_detector.cpp b/src/line_detector.cpp
index acc8164..650930c 100644
--- a/src/line_detector.cpp
+++ b/src/line_detector.cpp
@@ -218,8 +218,8 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
std::vector<std::vector<std::list<std::pair<double,double> > > > hough_grid;
std::list<std::pair<double,double> >::iterator pt_it; //iterator over the points of a given cell list
- //clear line list
- _line_list.clear();
+ //clear line list
+ _line_list.clear(); //TODO: TO BE DONE OUTSIDE!!
//resize hough_grid according range and theta steps and bounds
unsigned int hough_grid_rows = (unsigned int)ceil(M_PI/_alg_params.theta_step_);//[0,PI]
@@ -240,7 +240,7 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
for (unsigned int jth = 0; jth < hough_grid_rows; jth++) //loop over all theta values in the grid
{
//compute Real values of theta and range
- theta = jth*M_PI;
+ theta = jth*_alg_params.theta_step_;
range = _laser_cloud.at(laser_id)(0,ipt)*cos(theta) + _laser_cloud.at(laser_id)(1,ipt)*sin(theta); //r=xcos(th)+ysin(th)
//discretize range
@@ -269,6 +269,7 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
line.range_ = jj*_alg_params.range_step_; //range
//find xmax, xmin, ymax, ymin
+ xmax=-100; xmin=100; ymax=-100; ymin=100;
for (pt_it = hough_grid.at(ii).at(jj).begin(); pt_it != hough_grid.at(ii).at(jj).end(); pt_it++)
{
if (pt_it->first > xmax) xmax = pt_it->first;
@@ -280,13 +281,14 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
//set the limiting points of the line
if (ii < hough_grid_rows_half) //first and third quartile
{
-// line.point_first_ << ,,1;
-// line.point_last_ << ,,1;
+ line.point_first_ << xmin,ymax,1;
+ line.point_last_ << xmax,ymin,1;
}
else //second and fourth quartile
{
-
+ line.point_first_ << xmin,ymin,1;
+ line.point_last_ << xmax,ymax,1;
}
//push back the line to the list
@@ -352,8 +354,6 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
}
-
-
laserscanutils::ScalarT laserscanutils::angleBetweenLines(const laserscanutils::Line& line1, const laserscanutils::Line& line2)
{
// lines should be normalized
--
GitLab