Skip to content
Snippets Groups Projects
Commit 72ec13a0 authored by jvallve's avatar jvallve
Browse files

added lines to corner struct and points to line struct and changed the... 

added lines to corner struct and points to line struct and changed the repeated corner, keeping the ones with more points
parent e08f2ddf
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/corner_detector.cpp
+ 39
− 31
View file @ 72ec13a0
......@@ -64,10 +64,13 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
fitLine(points.block(0,i_from, 3, ii-i_from+1), line);
//if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
if ( line.error < _params.range_std_dev_*_alg_params.k_sigmas )
if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
{
line.first = i_from;
line.last = ii;
line.first_ = i_from;
line.last_ = ii;
line.point_first_ = points.col(line.first_);
line.point_last_ = points.col(line.last_);
line.np_ = line.last_ - line.first_ + 1;
line_list.push_back(line);
}
}
......@@ -86,10 +89,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
update_iterators = true;
// check if last of line1 and first of line2 are within the threshold
if ( ( line_it2->first - line_it1->last ) <= _alg_params.max_beam_distance )
if ( ( line_it2->first_ - line_it1->last_ ) <= _alg_params.max_beam_distance )
{
//compute angle between lines 1 and 2
cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() );
cos_theta = (line_it1->vector_).dot(line_it2->vector_) / ( (line_it1->vector_).norm()*(line_it2->vector_).norm() );
theta = acos (cos_theta); //theta is in [0,PI]
if (theta > M_PI/2) theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
// std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
......@@ -102,14 +105,17 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
{
//compute a new line with all involved points
Line new_line;
fitLine(points.block(0,line_it1->first, 3, line_it2->last-line_it1->first+1), new_line);
fitLine(points.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
//check if error below a threshold to effectively concatenate
if ( new_line.error < _params.range_std_dev_*_alg_params.k_sigmas )
if ( new_line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
{
//update line1 as the concatenation of l1 and l2
new_line.first = line_it1->first;
new_line.last = line_it2->last;
new_line.first_ = line_it1->first_;
new_line.last_ = line_it2->last_;
new_line.point_first_ = points.col(new_line.first_);
new_line.point_last_ = points.col(new_line.last_);
new_line.np_ = new_line.last_ - new_line.first_ + 1;
*line_it1 = new_line;
//remove l2 from the list
......@@ -120,11 +126,11 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
// std::cout << "lines concatenated" << std::endl;
// std::cout << "line 1: " << std::endl << line_it1->first << std::endl <<
// line_it1->last << std::endl << line_it1->vector.transpose() << std::endl << line_it1->error << std::endl;
// line_it1->last << std::endl << line_it1->vector_.transpose() << std::endl << line_it1->error << std::endl;
// std::cout << "line 2: " << std::endl << line_it2->first << std::endl <<
// line_it2->last << std::endl << line_it2->vector.transpose() << std::endl << line_it2->error << std::endl;
// line_it2->last << std::endl << line_it2->vector_.transpose() << std::endl << line_it2->error << std::endl;
// std::cout << "line resultant: "<< std::endl << new_line.first << std::endl <<
// new_line.last << std::endl << new_line.vector.transpose() << std::endl << new_line.error << std::endl;
// new_line.last << std::endl << new_line.vector_.transpose() << std::endl << new_line.error << std::endl;
}
}
}
......@@ -137,7 +143,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
line_it2 ++;
}
}
//std::cout << "Lines after concatenation: " << line_list.size() << std::endl;
std::cout << "Lines after concatenation: " << line_list.size() << std::endl;
//STEP 3: find corners over the line list
line_it1 = line_list.begin();
......@@ -149,48 +155,50 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
update_iterators = true;
// check if last of line1 and first of line2 are within the threshold
if ( ( line_it2->first - line_it1->last ) <= _alg_params.max_beam_distance )
if ( ( line_it2->first_ - line_it1->last_ ) <= _alg_params.max_beam_distance )
{
//compute angle between lines 1 and 2
cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() );
cos_theta = (line_it1->vector_).dot(line_it2->vector_) / ( (line_it1->vector_).norm()*(line_it2->vector_).norm() );
theta = acos (cos_theta); //theta is in [0,PI]
if (theta > M_PI/2) theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
// std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
// "line 1: " << line_it1->first << "-" << line_it1->last << std::endl <<
// "line 2: " << line_it2->first << "-" << line_it2->last << std::endl <<
// " (*line_it1).dot(*line_it2): " << (line_it1->vector).dot(line_it2->vector) << std::endl <<
// " (*line_it1).norm()*(*line_it2).norm(): " << (line_it1->vector).norm()*(line_it2->vector).norm() << std::endl;
// " (*line_it1).dot(*line_it2): " << (line_it1->vector_).dot(line_it2->vector_) << std::endl <<
// " (*line_it1).norm()*(*line_it2).norm(): " << (line_it1->vector_).norm()*(line_it2->vector_).norm() << std::endl;
//Check angle threshold
if ( theta > _alg_params.theta_min ) //fabs(theta) not required since theta>0
{
// Find the corner ad the cross product between lines (intersection)
//corner = (line_it1->vector).cross(line_it2->vector);
corner.pt_ = (line_it1->vector).cross(line_it2->vector);
//corner = (line_it1->vector_).cross(line_it2->vector_);
corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
// normalize corner point to set last component to 1
corner.pt_ = corner.pt_ / corner.pt_(2);
// Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
if ( ( (points.col(line_it1->last) - corner.pt_).head(2).squaredNorm() < max_distance_sq )
if ( ( (points.col(line_it1->last_) - corner.pt_).head(2).squaredNorm() < max_distance_sq )
&&
( (points.col(line_it2->first) - corner.pt_).head(2).squaredNorm() < max_distance_sq ) )
( (points.col(line_it2->first_) - corner.pt_).head(2).squaredNorm() < max_distance_sq ) )
{
//vector from corner to first point of l1
Eigen::Vector2s v1 = points.col(line_it1->first).head(2) - corner.pt_.head(2);
Eigen::Vector2s v1 = points.col(line_it1->first_).head(2) - corner.pt_.head(2);
//compute corner orientation as the angle between v1 and local X, in [-pi,pi]
corner.orientation_ = atan2(v1(1),v1(0));
//Compute corner aperture with line_it1->first, corner and line_it2->last
corner.aperture_ = cornerAperture(points.col(line_it1->first), corner.pt_, points.col(line_it2->last));
corner.aperture_ = cornerAperture(points.col(line_it1->first_), corner.pt_, points.col(line_it2->last_));
//set other descriptors
corner.np_l1_ = line_it1->last - line_it1->first;//>0 assured due to scanning order
corner.np_l2_ = line_it2->last - line_it2->first;//>0 assured due to scanning order
corner.error_l1_ = line_it1->error;
corner.error_l2_ = line_it2->error;
corner.line_1_ = *line_it1;
corner.line_2_ = *line_it2;
// corner.np_l1_ = line_it1->last_ - line_it1->first_;//>0 assured due to scanning order
// corner.np_l2_ = line_it2->last_ - line_it2->first_;//>0 assured due to scanning order
// corner.error_l1_ = line_it1->error_;
// corner.error_l2_ = line_it2->error_;
//add the corner to the list
_corner_list.push_back(corner);
......@@ -214,7 +222,7 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
}
}
//std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
// Erase duplicated corners
if ( _corner_list.size() > 1 )
......@@ -226,10 +234,10 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
while ( corner_it2 != _corner_list.end() )
{
// Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
if ( ( (*corner_it1).pt_ - (*corner_it2).pt_ ).head(2).squaredNorm() < max_distance_sq )
if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
{
// compute the mean TODO: set the mean of other attributes also
(*corner_it1).pt_ = ( (*corner_it1).pt_ + (*corner_it2).pt_ ) / 2.;
// Keep the one with bigger product of number of points of each line
(*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
corner_it2 = _corner_list.erase(corner_it2);
}
else
......
src/entities.cpp
+ 12
− 9
View file @ 72ec13a0
......@@ -7,19 +7,22 @@
void laserscanutils::Line::print() const
{
std::cout << "Line Parameters : " << std::endl
<< " (a,b,c): " << vector.transpose() << std::endl
<< " error: " << error << std::endl
<< " first,last: " << first << " , " << last << std::endl;
<< " (a,b,c): " << vector_.transpose() << std::endl
<< " error: " << error_ << std::endl
<< " first,last: " << first_ << " , " << last_ << std::endl
<< " first point: " << point_first_.transpose() << std::endl
<< " last point: " << point_last_.transpose() << std::endl
<< " number of points: " << np_ << std::endl;
}
void laserscanutils::Corner::print() const
{
std::cout << "Corner Parameters : " << std::endl
<< " point: " << pt_.transpose() << std::endl
<< " corner point: " << pt_.transpose() << std::endl
<< " orientation: " << orientation_ << std::endl
<< " aperture: " << aperture_ << std::endl
<< " np_l1: " << np_l1_ << std::endl
<< " np_l2: " << np_l2_ << std::endl
<< " error_l1: " << error_l1_ << std::endl
<< " error_l2: " << error_l2_ << std::endl;
<< " aperture: " << aperture_ << std::endl;
std::cout << "Line 1: ";
line_1_.print();
std::cout << "Line 2: ";
line_2_.print();
}
src/entities.h
+ 9
− 8
View file @ 72ec13a0
......@@ -10,10 +10,13 @@ namespace laserscanutils
{
struct Line
{
Eigen::Vector3s vector; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0
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 last; //index of the range vector of the last point used
Eigen::Vector3s vector_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0
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 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_last_; //homogeneous parameterization of the line: (a,b,c)^T -> ax+by+c=0
unsigned int np_; // number of points of the line
//TODO: add an Eigen::Map to the supporting points ... but who ensures memory allocation of such points ???
//just a print method
......@@ -25,10 +28,8 @@ namespace laserscanutils
Eigen::Vector3s pt_; //homogeneous coordinates of the 2D point, [meters]
ScalarT orientation_; //orientation angle from the line1 to x axis, in [-pi,pi]
ScalarT aperture_; //angle aperture of the corner in [0,2pi]. Aperture angle defined passing thorugh the shadow area of the scan (not crossing rays)
unsigned int np_l1_; // number of points of the first line forming the corner
unsigned int np_l2_; // number of points of the second line forming the corner
ScalarT error_l1_; // error of the first line forming the corner
ScalarT error_l2_; // error of the second line forming the corner
Line line_1_; // line of the corner
Line line_2_; // line of the corner
//just a print method
void print() const;
......
src/line_fitting.cpp
+ 2
− 2
View file @ 72ec13a0
......@@ -8,8 +8,8 @@ void laserscanutils::fitLine(const Eigen::MatrixXs & _points, Line & _line)
AA.row(2) << 0,0,1;
//solve for line
_line.vector = AA.inverse().col(2);
_line.vector_ = AA.inverse().col(2);
// compute fitting error
_line.error = (_points.transpose() * _line.vector).array().abs().sum() / (_line.vector.head(2).norm()*_points.cols());
_line.error_ = (_points.transpose() * _line.vector_).array().abs().sum() / (_line.vector_.head(2).norm()*_points.cols());
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment