Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
laser_scan_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
labrobotica
algorithms
laser_scan_utils
Commits
9681e4e4
Commit
9681e4e4
authored
9 years ago
by
andreucm
Browse files
Options
Downloads
Patches
Plain Diff
Hough line detection improved. Scan points now allowed to vote for a sinlge line
parent
f9ee2867
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
src/laser_scan_utils.h
+1
-0
1 addition, 0 deletions
src/laser_scan_utils.h
src/line_finder_hough.cpp
+182
-72
182 additions, 72 deletions
src/line_finder_hough.cpp
src/line_finder_hough.h
+1
-0
1 addition, 0 deletions
src/line_finder_hough.h
with
184 additions
and
72 deletions
src/laser_scan_utils.h
+
1
−
0
View file @
9681e4e4
...
@@ -5,6 +5,7 @@
...
@@ -5,6 +5,7 @@
//includes from Eigen lib
//includes from Eigen lib
#include
<eigen3/Eigen/Dense>
#include
<eigen3/Eigen/Dense>
#include
<eigen3/Eigen/Geometry>
#include
<eigen3/Eigen/Geometry>
#include
<eigen3/Eigen/Sparse>
//namespace laserscanutils;
//namespace laserscanutils;
...
...
This diff is collapsed.
Click to expand it.
src/line_finder_hough.cpp
+
182
−
72
View file @
9681e4e4
...
@@ -33,27 +33,37 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
...
@@ -33,27 +33,37 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
unsigned
int
LineFinderHough
::
findLines
(
const
Eigen
::
MatrixXs
&
_points
,
unsigned
int
LineFinderHough
::
findLines
(
const
Eigen
::
MatrixXs
&
_points
,
std
::
list
<
laserscanutils
::
LineSegment
>
&
_line_list
)
std
::
list
<
laserscanutils
::
LineSegment
>
&
_line_list
)
{
{
ScalarT
theta
,
range
;
ScalarT
theta
,
range
,
max_value
;
unsigned
int
ii_max_value
,
jj_max_value
;
int
kr
;
int
kr
;
LineSegment
line
;
LineSegment
line
;
ScalarT
xmax
,
xmin
,
ymax
,
ymin
;
ScalarT
xmax
,
xmin
,
ymax
,
ymin
;
Eigen
::
Vector3s
q_pt
,
p_pt
;
//auxiliary points
Eigen
::
Vector3s
q_pt
,
p_pt
;
//auxiliary points
Eigen
::
MatrixXs
pts
;
//auxiliar array of points
Eigen
::
MatrixXs
pts
;
//auxiliar array of points
std
::
list
<
Eigen
::
Vector3s
>::
iterator
pts_it
;
std
::
list
<
Eigen
::
Vector3s
>::
iterator
pts_it
;
Eigen
::
SparseMatrix
<
unsigned
int
>
sum_grid
(
hough_grid_rows
,
hough_grid_cols
);
std
::
list
<
Eigen
::
SparseMatrix
<
unsigned
int
>
>::
iterator
subgrid_it
;
Eigen
::
MatrixXs
support_points
;
//clear the Houhg Grid
//clear the Houhg Grid
for
(
unsigned
int
ii
=
0
;
ii
<
hough_grid_
.
size
();
ii
++
)
// for (unsigned int ii = 0; ii < hough_grid_.size(); ii++)
{
// {
for
(
unsigned
int
jj
=
0
;
jj
<
hough_grid_
.
at
(
ii
).
size
();
jj
++
)
// for (unsigned int jj = 0; jj < hough_grid_.at(ii).size(); jj++)
{
// {
hough_grid_
.
at
(
ii
).
at
(
jj
).
clear
();
// hough_grid_.at(ii).at(jj).clear();
}
// }
}
// }
hough_grid_x_
.
clear
();
//STEP 1. For each scan point, accumulate hits in the Hough Grid
//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
for
(
unsigned
int
ipt
=
0
;
ipt
<
_points
.
cols
();
ipt
++
)
//loop over all points
{
{
for
(
unsigned
int
jth
=
0
;
jth
<
hough_grid_rows
;
jth
++
)
//loop over all theta values in the grid
//push_back a new Sparse Matrix to hough_grid_. Each SparseMatrix represents the grid for one point.
hough_grid_x_
.
push_back
(
Eigen
::
SparseMatrix
<
unsigned
int
>
(
hough_grid_rows
,
hough_grid_cols
));
//loop over all theta values in the grid
for
(
unsigned
int
jth
=
0
;
jth
<
hough_grid_rows
;
jth
++
)
{
{
//compute Real values of theta and range
//compute Real values of theta and range
theta
=
jth
*
hough_params_
.
theta_step_
;
theta
=
jth
*
hough_params_
.
theta_step_
;
...
@@ -66,80 +76,180 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
...
@@ -66,80 +76,180 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
if
(
(
kr
>=
0
)
&&
(
kr
<
hough_grid_cols
)
)
if
(
(
kr
>=
0
)
&&
(
kr
<
hough_grid_cols
)
)
{
{
//Add support to cell(jth,kr), by pushing back the point homogeneous coordinates
//Add support to cell(jth,kr), by pushing back the point homogeneous coordinates
hough_grid_
.
at
(
jth
).
at
(
kr
).
push_back
(
Eigen
::
Vector3s
(
_points
(
0
,
ipt
),
_points
(
1
,
ipt
),
1
));
//hough_grid_.at(jth).at(kr).push_back( Eigen::Vector3s(_points(0,ipt),_points(1,ipt),1 ));
//Anotate the support of point ipt to cell (jth,kr)
hough_grid_x_
.
back
().
coeffRef
(
jth
,
kr
)
=
1
;
}
}
}
}
}
}
////STEP 2 .Find cells having a peak of at least min_supports_ points supporting them
////STEP 2 .Find cells having a peak of at least min_supports_ points supporting them
for
(
unsigned
int
ii
=
1
;
ii
<
hough_grid_rows
-
1
;
ii
++
)
while
(
hough_grid_x_
.
size
()
>
hough_params_
.
min_supports_
)
{
{
for
(
unsigned
int
jj
=
1
;
jj
<
hough_grid_cols
-
1
;
jj
++
)
//sum all subgrids
sum_grid
.
setZero
();
for
(
subgrid_it
=
hough_grid_x_
.
begin
();
subgrid_it
!=
hough_grid_x_
.
end
();
subgrid_it
++
)
{
sum_grid
=
sum_grid
+
(
*
subgrid_it
);
}
//Find the max_value at sum_grid -> r,th TODO: improve it by exploiting sparsity (not running over all matrix!)
max_value
=
0
;
for
(
unsigned
int
ii
=
0
;
ii
<
hough_grid_rows
;
ii
++
)
{
{
//check min supports
for
(
unsigned
int
jj
=
0
;
jj
<
hough_grid_cols
;
jj
++
)
unsigned
int
cell_supports
=
hough_grid_
.
at
(
ii
).
at
(
jj
).
size
();
if
(
cell_supports
>=
hough_params_
.
min_supports_
)
{
{
//check if cell ii,jj is a peak ( 8 neighboring cells should be below in number of supports)
if
(
sum_grid
.
coeffRef
(
ii
,
jj
)
>
max_value
)
if
(
(
cell_supports
>=
hough_grid_
.
at
(
ii
-
1
).
at
(
jj
-
1
).
size
()
)
&&
{
(
cell_supports
>=
hough_grid_
.
at
(
ii
-
1
).
at
(
jj
).
size
()
)
&&
max_value
=
sum_grid
.
coeffRef
(
ii
,
jj
);
(
cell_supports
>=
hough_grid_
.
at
(
ii
-
1
).
at
(
jj
+
1
).
size
()
)
&&
ii_max_value
=
ii
;
(
cell_supports
>=
hough_grid_
.
at
(
ii
).
at
(
jj
-
1
).
size
()
)
&&
jj_max_value
=
jj
;
(
cell_supports
>=
hough_grid_
.
at
(
ii
).
at
(
jj
+
1
).
size
()
)
&&
}
(
cell_supports
>=
hough_grid_
.
at
(
ii
+
1
).
at
(
jj
-
1
).
size
()
)
&&
}
(
cell_supports
>=
hough_grid_
.
at
(
ii
+
1
).
at
(
jj
).
size
()
)
&&
}
(
cell_supports
>=
hough_grid_
.
at
(
ii
+
1
).
at
(
jj
+
1
).
size
()
)
)
{
//max_value is the number of supporters for the most supported cell. Ceck if it has enough supporters
//find best fitting line with the supporting points
if
(
max_value
<
hough_params_
.
min_supports_
)
pts
.
resize
(
3
,
cell_supports
);
//each point in a column
{
unsigned
int
kk
;
break
;
//Exit while(), since the max_value does not get the min_supports_
for
(
kk
=
0
,
pts_it
=
hough_grid_
.
at
(
ii
).
at
(
jj
).
begin
();
pts_it
!=
hough_grid_
.
at
(
ii
).
at
(
jj
).
end
();
pts_it
++
,
kk
++
)
}
{
else
pts
.
block
<
3
,
1
>
(
0
,
kk
)
<<
*
pts_it
;
//copy point
{
}
//resize supporter set
this
->
fitLine
(
pts
,
line
);
support_points
.
resize
(
3
,
max_value
);
//set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
//select points supporting the cell of max value
line
.
np_
=
hough_grid_
.
at
(
ii
).
at
(
jj
).
size
();
//supporters
unsigned
int
ipt
=
0
;
//index over _points
line
.
theta_
=
ii
*
hough_params_
.
theta_step_
;
//theta
unsigned
int
jpt
=
0
;
//index over support_points
line
.
range_
=
jj
*
hough_params_
.
range_step_
;
//range
for
(
ipt
=
0
,
subgrid_it
=
hough_grid_x_
.
begin
();
ipt
<
_points
.
cols
(),
subgrid_it
!=
hough_grid_x_
.
end
();
ipt
++
)
{
//set starting and ending points of the line
if
(
subgrid_it
->
coeff
(
ii_max_value
,
jj_max_value
)
!=
0
)
//point ipt hit this cell
//TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
{
xmax
=-
100
;
xmin
=
100
;
ymax
=-
100
;
ymin
=
100
;
//copy ipt point to support_points
for
(
pts_it
=
hough_grid_
.
at
(
ii
).
at
(
jj
).
begin
();
pts_it
!=
hough_grid_
.
at
(
ii
).
at
(
jj
).
end
();
pts_it
++
)
support_points
.
block
<
3
,
1
>
(
0
,
jpt
)
<<
_points
.
block
<
3
,
1
>
(
0
,
ipt
);
{
jpt
++
;
//find xmax, xmin, ymax, ymin
if
(
pts_it
->
x
()
>
xmax
)
xmax
=
pts_it
->
x
();
if
(
pts_it
->
y
()
>
ymax
)
ymax
=
pts_it
->
y
();
if
(
pts_it
->
x
()
<
xmin
)
xmin
=
pts_it
->
x
();
if
(
pts_it
->
y
()
<
ymin
)
ymin
=
pts_it
->
y
();
}
if
(
ii
<
hough_grid_rows_half
)
//theta in [0,PI/2]
{
q_pt
<<
xmin
,
ymax
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_first_
<<
p_pt
;
q_pt
<<
xmax
,
ymin
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_last_
<<
p_pt
;
}
else
//theta in [PI/2,PI]
{
q_pt
<<
xmin
,
ymin
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_first_
<<
p_pt
;
q_pt
<<
xmax
,
ymax
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_last_
<<
p_pt
;
}
//
push back the line to the list
//
remove this subgrid
_line_list
.
push_back
(
line
);
subgrid_it
=
hough_grid_x_
.
erase
(
subgrid_it
);
}
}
else
//point ipt didn't hit the cell, just carry on
{
subgrid_it
++
;
}
}
//fit a line with the supporter points (support_points)
this
->
fitLine
(
support_points
,
line
);
//set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
line
.
np_
=
max_value
;
//supporters
line
.
theta_
=
ii_max_value
*
hough_params_
.
theta_step_
;
//theta
line
.
range_
=
jj_max_value
*
hough_params_
.
range_step_
;
//range
//set starting and ending points of the line
//TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
xmax
=-
100
;
xmin
=
100
;
ymax
=-
100
;
ymin
=
100
;
for
(
jpt
=
0
;
jpt
<
support_points
.
cols
();
jpt
++
)
{
//find xmax, xmin, ymax, ymin
if
(
support_points
(
0
,
jpt
)
>
xmax
)
xmax
=
support_points
(
0
,
jpt
);
if
(
support_points
(
1
,
jpt
)
>
ymax
)
ymax
=
support_points
(
1
,
jpt
);
if
(
support_points
(
0
,
jpt
)
<
xmin
)
xmin
=
support_points
(
0
,
jpt
);
if
(
support_points
(
1
,
jpt
)
<
ymin
)
ymin
=
support_points
(
1
,
jpt
);
}
}
if
(
ii_max_value
<
hough_grid_rows_half
)
//theta in [0,PI/2]
{
q_pt
<<
xmin
,
ymax
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_first_
<<
p_pt
;
q_pt
<<
xmax
,
ymin
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_last_
<<
p_pt
;
}
else
//theta in [PI/2,PI]
{
q_pt
<<
xmin
,
ymin
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_first_
<<
p_pt
;
q_pt
<<
xmax
,
ymax
,
1
;
pointProjectionToLine
(
line
.
abc_
,
q_pt
,
p_pt
);
line
.
point_last_
<<
p_pt
;
}
//push back the line to the list
_line_list
.
push_back
(
line
);
}
}
}
}
// for (unsigned int ii = 1; ii < hough_grid_rows-1; ii++)
// {
// for (unsigned int jj = 1; jj < hough_grid_cols-1; jj++)
// {
// //check min supports
// unsigned int cell_supports = hough_grid_.at(ii).at(jj).size();
// if( cell_supports >= hough_params_.min_supports_ )
// {
// //check if cell ii,jj is a peak ( 8 neighboring cells should be below in number of supports)
// if ( ( cell_supports >= hough_grid_.at(ii-1).at(jj-1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii-1).at(jj).size() ) &&
// ( cell_supports >= hough_grid_.at(ii-1).at(jj+1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii).at(jj-1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii).at(jj+1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii+1).at(jj-1).size() ) &&
// ( cell_supports >= hough_grid_.at(ii+1).at(jj).size() ) &&
// ( cell_supports >= hough_grid_.at(ii+1).at(jj+1).size() ) )
// {
// //find best fitting line with the supporting points
// pts.resize(3,cell_supports); //each point in a column
// unsigned int kk;
// for(kk=0, pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++, kk++)
// {
// pts.block<3,1>(0,kk) << *pts_it; //copy point
// }
// this->fitLine(pts, line);
//
// //set the line hough params TODO: Compute them from fitLine result, not from the Grid !!
// line.np_ = hough_grid_.at(ii).at(jj).size(); //supporters
// line.theta_ = ii*hough_params_.theta_step_; //theta
// line.range_ = jj*hough_params_.range_step_; //range
//
// //set starting and ending points of the line
// //TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
// xmax=-100; xmin=100; ymax=-100; ymin=100;
// for (pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++)
// {
// //find xmax, xmin, ymax, ymin
// if (pts_it->x() > xmax) xmax = pts_it->x();
// if (pts_it->y() > ymax) ymax = pts_it->y();
// if (pts_it->x() < xmin) xmin = pts_it->x();
// if (pts_it->y() < ymin) ymin = pts_it->y();
// }
// if (ii < hough_grid_rows_half) //theta in [0,PI/2]
// {
// q_pt << xmin,ymax,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_first_ << p_pt;
// q_pt << xmax,ymin,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_last_ << p_pt;
// }
// else //theta in [PI/2,PI]
// {
// q_pt << xmin,ymin,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_first_ << p_pt;
// q_pt << xmax,ymax,1;
// pointProjectionToLine(line.abc_, q_pt, p_pt);
// line.point_last_ << p_pt;
// }
//
// //push back the line to the list
// _line_list.push_back(line);
// }
// }
// }
// }
//return the number of lines detected
//return the number of lines detected
return
_line_list
.
size
();
return
_line_list
.
size
();
...
...
This diff is collapsed.
Click to expand it.
src/line_finder_hough.h
+
1
−
0
View file @
9681e4e4
...
@@ -35,6 +35,7 @@ class LineFinderHough : public LineFinder
...
@@ -35,6 +35,7 @@ class LineFinderHough : public LineFinder
//Hough grid in theta-range space. Each cell holds a list of the point coordinates supporting the cell
//Hough grid in theta-range space. Each cell holds a list of the point coordinates supporting the cell
std
::
vector
<
std
::
vector
<
std
::
list
<
Eigen
::
Vector3s
>
>
>
hough_grid_
;
std
::
vector
<
std
::
vector
<
std
::
list
<
Eigen
::
Vector3s
>
>
>
hough_grid_
;
std
::
list
<
Eigen
::
SparseMatrix
<
unsigned
int
>
>
hough_grid_x_
;
//hough grid is a list of grids. Each one corresponds to the votes of a single point
//dereived Hough params
//dereived Hough params
unsigned
int
hough_grid_rows
;
unsigned
int
hough_grid_rows
;
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment