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
65005a8e
Commit
65005a8e
authored
9 years ago
by
acoromin
Browse files
Options
Downloads
Patches
Plain Diff
Working version. Filling start and end points to Hough lines. Not yet tested.
parent
517e1529
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/entities.h
+2
-2
2 additions, 2 deletions
src/entities.h
src/line_detector.cpp
+56
-34
56 additions, 34 deletions
src/line_detector.cpp
with
58 additions
and
36 deletions
src/entities.h
+
2
−
2
View file @
65005a8e
...
@@ -17,8 +17,8 @@ namespace laserscanutils
...
@@ -17,8 +17,8 @@ namespace laserscanutils
ScalarT
error_
;
//sum of all distances from used points to line
ScalarT
error_
;
//sum of all distances from used points to line
unsigned
int
first_
;
//index of the range vector of the first point used
unsigned
int
first_
;
//index of the range vector of the first point used
unsigned
int
last_
;
//index of the range vector of the last point used
unsigned
int
last_
;
//index of the range vector of the last point used
Eigen
::
Vector3s
point_first_
;
//homogeneous
parameterization of the line: (a,b,c)^T -> ax+by+c=0
Eigen
::
Vector3s
point_first_
;
//homogeneous
coordinates of the starting 2D point
Eigen
::
Vector3s
point_last_
;
//homogeneous
parameterization of the line: (a,b,c)^T -> ax+by+c=0
Eigen
::
Vector3s
point_last_
;
//homogeneous
coordinates of the ending 2D point
unsigned
int
np_
;
// number of points of the line
unsigned
int
np_
;
// number of points of the line
double
range_
;
//range component in polar coordinates
double
range_
;
//range component in polar coordinates
double
theta_
;
//theta component in polar coordinates
double
theta_
;
//theta component in polar coordinates
...
...
This diff is collapsed.
Click to expand it.
src/line_detector.cpp
+
56
−
34
View file @
65005a8e
...
@@ -209,19 +209,22 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
...
@@ -209,19 +209,22 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
std
::
list
<
laserscanutils
::
Line
>
&
_line_list
)
std
::
list
<
laserscanutils
::
Line
>
&
_line_list
)
{
{
double
theta
,
range
;
double
theta
,
range
;
unsigned
int
kr
;
int
kr
;
Line
line
;
Line
line
;
//A 2D array of lists. Each cell is a (r,theta) discretization in the line parameter space.
//A 2D array of lists. Each cell is a (r,theta) discretization in the line parameter space.
//Each list keeps the laser point coordinates that support that cell
//Each list keeps the laser point coordinates that support that cell
std
::
vector
<
std
::
vector
<
std
::
list
<
std
::
pair
<
double
,
double
>
>
>
>
hough_grid
;
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
//clear line list
_line_list
.
clear
();
_line_list
.
clear
();
//resize hough_grid according range and theta steps and bounds
//resize hough_grid according range and theta steps and bounds
unsigned
int
hough_grid_rows
=
(
unsigned
int
)
ceil
((
M_PI
/
2.
)
/
_alg_params
.
theta_step_
);
unsigned
int
hough_grid_rows
=
(
unsigned
int
)
ceil
(
M_PI
/
_alg_params
.
theta_step_
);
//[0,PI]
unsigned
int
hough_grid_cols
=
(
unsigned
int
)
ceil
(
_alg_params
.
range_max_
/
_alg_params
.
range_step_
);
unsigned
int
hough_grid_rows_half
=
(
unsigned
int
)
ceil
(
0.5
*
M_PI
/
_alg_params
.
theta_step_
);
//middle row index
unsigned
int
hough_grid_cols
=
(
unsigned
int
)
ceil
(
2
*
_alg_params
.
range_max_
/
_alg_params
.
range_step_
);
//[-rmax,+rmax]
unsigned
int
hough_grid_cols_half
=
(
unsigned
int
)
ceil
(
_alg_params
.
range_max_
/
_alg_params
.
range_step_
);
//middle col index
hough_grid
.
resize
(
hough_grid_rows
);
hough_grid
.
resize
(
hough_grid_rows
);
for
(
unsigned
int
ii
=
0
;
ii
<
hough_grid_rows
;
ii
++
)
for
(
unsigned
int
ii
=
0
;
ii
<
hough_grid_rows
;
ii
++
)
{
{
...
@@ -236,13 +239,13 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
...
@@ -236,13 +239,13 @@ 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
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
//compute Real values of theta and range
theta
=
jth
*
(
M_PI
/
2.
)
;
theta
=
jth
*
M_PI
;
range
=
_laser_cloud
.
at
(
laser_id
)(
0
,
ipt
)
*
cos
(
theta
)
+
_laser_cloud
.
at
(
laser_id
)(
1
,
ipt
)
*
sin
(
theta
);
//r=xcos(th)+ysin(th)
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
//discretize range
kr
=
(
unsigned
int
)
floor
(
range
/
_alg_params
.
range_step_
);
kr
=
(
int
)
floor
(
range
/
_alg_params
.
range_step_
)
+
(
int
)
hough_grid_cols_half
;
//check validity of the dicretized values
: TODO: take into account negative ranges
//check validity of the dicretized values
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 coordinates
//Add support to cell(jth,kr), by pushing back the point coordinates
...
@@ -252,46 +255,65 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
...
@@ -252,46 +255,65 @@ unsigned int laserscanutils::extractLinesHough( const std::vector<Eigen::MatrixX
}
}
}
}
// //Check cells having a list with >= min_supports_ members
// for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
// {
// for (unsigned int jj = 0; jj < hough_grid_cols; jj++)
// {
// if( hough_grid.at(ii).at(jj).size() >= _alg_params.min_supports_ )
// {
// //set the line params
// line.np_ = hough_grid.at(ii).at(jj).size();
// line.theta_ = ii*_alg_params.theta_step_;
// line.range_ = jj*_alg_params.range_step_;
// //line.point_first_ << ;
// //line.point_last_ << ;
//
// //push back the line to the list
// _line_list.push_back(line);
// }
// }
// }
//Check cells having a list with >= min_supports_ members
//Check cells having a list with >= min_supports_ members
std
::
list
<
std
::
pair
<
unsigned
int
,
unsigned
int
>
>
best_cells
;
//list of the indexes corresponding to the cells above the threshold
for
(
unsigned
int
ii
=
0
;
ii
<
hough_grid_rows
;
ii
++
)
for
(
unsigned
int
ii
=
0
;
ii
<
hough_grid_rows
;
ii
++
)
{
{
for
(
unsigned
int
jj
=
0
;
jj
<
hough_grid_cols
;
jj
++
)
for
(
unsigned
int
jj
=
0
;
jj
<
hough_grid_cols
;
jj
++
)
{
{
if
(
hough_grid
.
at
(
ii
).
at
(
jj
).
size
()
>=
_alg_params
.
min_supports_
)
if
(
hough_grid
.
at
(
ii
).
at
(
jj
).
size
()
>=
_alg_params
.
min_supports_
)
{
{
//push ii,jj pair as candidate
//set the line hough params
best_cells
.
push_back
(
std
::
pair
<
unsigned
int
,
unsigned
int
>
(
ii
,
jj
)
);
line
.
np_
=
hough_grid
.
at
(
ii
).
at
(
jj
).
size
();
//supporters
line
.
theta_
=
ii
*
_alg_params
.
theta_step_
;
//theta
line
.
range_
=
jj
*
_alg_params
.
range_step_
;
//range
//find xmax, xmin, ymax, ymin
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
;
if
(
pt_it
->
second
>
ymax
)
ymax
=
pt_it
->
second
;
if
(
pt_it
->
first
<
xmin
)
xmin
=
pt_it
->
first
;
if
(
pt_it
->
second
<
ymin
)
ymin
=
pt_it
->
second
;
}
//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
;
}
else
//second and fourth quartile
{
}
//push back the line to the list
_line_list
.
push_back
(
line
);
}
}
}
}
}
}
//Check cells having a list with >= min_supports_ members
// std::list<std::pair<unsigned int,unsigned int> > best_cells; //list of the indexes corresponding to the cells above the threshold
// for (unsigned int ii = 0; ii < hough_grid_rows; ii++)
// {
// for (unsigned int jj = 0; jj < hough_grid_cols; jj++)
// {
// if( hough_grid.at(ii).at(jj).size() >= _alg_params.min_supports_ )
// {
// //push ii,jj pair as candidate
// best_cells.push_back( std::pair<unsigned int,unsigned int>(ii,jj) );
// }
// }
// }
//clustering over candidates
//clustering over candidates
std
::
list
<
std
::
pair
<
unsigned
int
,
unsigned
int
>
>::
iterator
it_best_cells
;
//
std::list<std::pair<unsigned int,unsigned int> >::iterator it_best_cells;
for
(
it_best_cells
=
best_cells
.
begin
();
it_best_cells
!=
best_cells
.
end
();
it_best_cells
++
)
//
for (it_best_cells = best_cells.begin(); it_best_cells != best_cells.end(); it_best_cells++)
{
//
{
//
}
//
}
//get the 10 most supported lines
//get the 10 most supported lines
// std::list<unsigned int> peak_values; //list of the ten highest peak values in the hough_grid. Last the highest.
// std::list<unsigned int> peak_values; //list of the ten highest peak values in the hough_grid. Last the highest.
...
...
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