diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a5d5601439b38e2b7637d3fa8710916eb72e1fda..75d4d5d029668b5c08c50d12bd61e2b70a2541e1 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -35,6 +35,7 @@ SET(HDRS
line_detector.h
line_finder.h
line_finder_hough.h
+ line_finder_iterative.h
line_finder_jump_fit.h
line_segment.h
point_set.h
@@ -57,6 +58,7 @@ SET(SRCS
line_detector.cpp
line_finder.cpp
line_finder_hough.cpp
+ line_finder_iterative.cpp
line_finder_jump_fit.cpp
line_segment.cpp
point_set.cpp
diff --git a/src/corner_finder_inscribed_angle.cpp b/src/corner_finder_inscribed_angle.cpp
index 92ca9fedde1015c299eb870757acd6c327a45bd7..08e8e9f3f09defbbb1353bc545432b2f72100af8 100644
--- a/src/corner_finder_inscribed_angle.cpp
+++ b/src/corner_finder_inscribed_angle.cpp
@@ -17,30 +17,34 @@ int CornerFinderInscribedAngle::findCorners( const Eigen::MatrixXs & _points, st
{
//params
unsigned int MIN_SIDE_POINTS = 5;
- ScalarT LINE_FIT_ERROR = 0.03; //maximum allowed mean point-line error to consider a line is straight enough
+ ScalarT LINE_FIT_ERROR = 0.1; //maximum allowed mean point-line error to consider a line is straight enough
ScalarT MIN_ANGLE = 45.*M_PI/180.; //minimum allowed absoulte angle between lines around the corner
//local variables
Eigen::VectorXs inscribed_angles;
Eigen::Vector3s line2o; //line from point i to _point(0)
Eigen::Vector3s line2e; //line from point i to _point(last)
- ScalarT aux, min_angle, angle, e1, e2;
+ ScalarT aux, min_angle, angle;
unsigned int ii_min;
LineSegment line1, line2;
CornerPoint new_corner;
- //TODO: first check: if segment is not enough populated -> return -1
- //TODO: second check: if segment has high eigen value ratio -> return -1
+ //first check: if segment is not enough populated -> return -1
+ if (_points.cols() < MIN_SIDE_POINTS*2 ) return -1;
+
+ //second check: if segment has high eigen value ratio -> return -1
+ //TODO: getEvalRatio()
//resize inscribed_angles
- inscribed_angles.resize(_points.cols()-2,1); //column vector
+// std::cout << "findCorners(): " << __LINE__ << std::endl;
+ inscribed_angles.resize(_points.cols()-2); //column vector
- //compute inscribed angles for each point (excepting extremes)
+ //compute inscribed angles for each point (exceptuating extremes)
for (unsigned int ipt = 1; ipt < _points.cols()-1; ipt++)
{
//find lines passing through point i and extremes of the set
line2o = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,0));
- line2e = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,_points.cols()));
+ line2e = _points.block<3,1>(0,ipt).cross(_points.block<3,1>(0,_points.cols()-1));
//normalize lines (http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html)
aux = sqrt(line2o(0)*line2o(0)+line2o(1)*line2o(1));
@@ -53,30 +57,28 @@ int CornerFinderInscribedAngle::findCorners( const Eigen::MatrixXs & _points, st
}
//find the minimum inscribed angle, and its index in the vector
+// std::cout << "findCorners(): " << __LINE__ << std::endl;
min_angle = inscribed_angles.minCoeff(&ii_min);
//check enough points in left and right
if ( ( ii_min > MIN_SIDE_POINTS ) && ( ii_min < (inscribed_angles.rows() - MIN_SIDE_POINTS) ) )
{
//fit lines at left and right of the point
+// std::cout << "findCorners(): " << __LINE__ << std::endl;
line_finder_.fitLine(_points.block(0,ii_min-MIN_SIDE_POINTS,3,MIN_SIDE_POINTS+1), line1);
line_finder_.fitLine(_points.block(0,ii_min,3,MIN_SIDE_POINTS+1), line2);
- //check line error
-// std::cout << "findCorners(): " << __LINE__ << std::endl;
- e1 = line1.fit_error_ / ( (ScalarT)MIN_SIDE_POINTS * 2. + 1.);
- e2 = line2.fit_error_ / ( (ScalarT)MIN_SIDE_POINTS * 2. + 1.);
-// std::cout << "findCorners(). e1: " << e1 << "; e2: " << e2 << std::endl;
-
- if ( ( e1 < LINE_FIT_ERROR ) && ( e2 < LINE_FIT_ERROR ) )
+ //check line error
+ if ( ( line1.fit_error_ < LINE_FIT_ERROR ) && ( line2.fit_error_ < LINE_FIT_ERROR ) )
{
//check angles between lines
- angle = line1.angleToLine(line2); //angle is in [0,2pi]
+ angle = fabs(line1.angleToLine(line2)); //angle is in [0,2pi]->TODO: Debug that !!
+// std::cout << "findCorners(). " << __LINE__ << ". angle: " << angle << std::endl;
+
if ( ( angle > MIN_ANGLE ) && ( angle < 2*M_PI-MIN_ANGLE ) )
{
//corner found !!
-// std::cout << "findCorners(): " << __LINE__ << std::endl;
-// std::cout << "findCorners(). angle " << angle << std::endl;
+ std::cout << "findCorners(). " << __LINE__ << ". angle: " << angle << std::endl;
new_corner.point_ << _points.block<3,1>(0,ii_min+1);//TODO crossing point between lines
new_corner.orientation_ = 0;//TODO
new_corner.aperture_ = angle;
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
index 3aebff9257434e14bac2715877520cd547e7dd2b..a9c83650c58025facfdcbe910d38a48919ef9f4c 100644
--- a/src/line_finder.cpp
+++ b/src/line_finder.cpp
@@ -22,12 +22,13 @@ void LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) c
//solve for line
_line.abc_ = AA.inverse().col(2);
- // normalize the line
+ // normalize the line (according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html)
_line.abc_ /= _line.abc_.head(2).norm();
// compute fitting error
_line.fit_error_ = (_points.transpose() * _line.abc_).squaredNorm() / _points.cols();
//_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
+ //_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / _points.cols();
}
void LineFinder::fitLineRansac(const Eigen::MatrixXs & _points, LineSegment & _line, ScalarT _error) const
diff --git a/src/line_finder_iterative.cpp b/src/line_finder_iterative.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf0129280a64bbe55ca836a61ea090b724aebe70
--- /dev/null
+++ b/src/line_finder_iterative.cpp
@@ -0,0 +1,77 @@
+#include "line_finder_iterative.h"
+
+namespace laserscanutils
+{
+
+LineFinderIterative::LineFinderIterative()
+{
+
+}
+
+LineFinderIterative::~LineFinderIterative()
+{
+
+}
+
+void LineFinderIterative::setIlfParams(const LineFinderIterativeParams & _params)
+{
+ ilf_params_.max_fit_error_ = _params.max_fit_error_;
+ ilf_params_.min_supports_ = _params.min_supports_;
+}
+
+unsigned int LineFinderIterative::findLines( const Eigen::MatrixXs & _points, std::list<laserscanutils::LineSegment> & _line_list)
+{
+ LineSegment line;//, line1, line2;
+ ScalarT max_dist, di;
+ unsigned int max_ii;
+
+ //fits a line between first and last points
+ line.abc_ = _points.block<3,1>(0,0).cross(_points.block<3,1>(0,_points.cols()-1));
+
+ //normalize the line
+ line.abc_ /= line.abc_.head(2).norm();
+
+ //for each point, check distance to line, and get the farthest point to line
+ max_dist = 0;
+ for (unsigned int ii=0; ii< _points.cols()-1; ii++)
+ {
+ di = fabs( line.abc_.dot(_points.block<3,1>(0,ii)) );
+ if ( di > max_dist )
+ {
+ max_dist = di;
+ max_ii = ii;
+ }
+ }
+
+ //check if farthest point at distance lower than threshold
+ if ( max_dist < ilf_params_.max_fit_error_ ) //straight line case
+ {
+ //Fit line, fill start and end points and push_back it
+ fitLine(_points, line);
+ line.point_first_ << _points.block<3,1>(0,0);
+ line.point_last_ << _points.block<3,1>(0,_points.cols()-1);
+ _line_list.push_back(line);
+ }
+ else //non straight line case -> carry on recursivity
+ {
+ //split _points into two subsets and call findLines again for each one, if enough points in the subset
+ if ( max_ii >= ilf_params_.min_supports_ )
+ {
+ this->findLines(_points.block(0,0,3,max_ii), _line_list);
+ }
+ if ( _points.cols()-max_ii >= ilf_params_.min_supports_ )
+ {
+ this->findLines(_points.block(0,max_ii,3,_points.cols()-max_ii), _line_list);
+ }
+ }
+
+ //return number of lines detected
+ return _line_list.size();
+}
+
+void LineFinderIterative::print() const
+{
+
+}
+
+}//end namespace
diff --git a/src/line_finder_iterative.h b/src/line_finder_iterative.h
new file mode 100644
index 0000000000000000000000000000000000000000..8e34e14bc4fac851e24e3fec88a60683e08be175
--- /dev/null
+++ b/src/line_finder_iterative.h
@@ -0,0 +1,81 @@
+#ifndef LINE_FINDER_ITERATIVE_H_
+#define LINE_FINDER_ITERATIVE_H_
+
+//laserscanutils
+#include "line_finder.h"
+
+namespace laserscanutils
+{
+/** \brief set of tunning parameters for the iterative line fit
+*
+* set of tunning parameters for the iterative line fit
+*
+*/
+struct LineFinderIterativeParams
+{
+ ScalarT max_fit_error_; //maximum allowed fit error to consider a straight line
+ unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
+};
+
+
+/** \brief Iterative line fit class
+*
+* Iterative line fit class
+* Implements iterative line fit
+*
+*/
+class LineFinderIterative : public LineFinder
+{
+ protected:
+
+ //Tunning params for iterative line fit approach
+ LineFinderIterativeParams ilf_params_;
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ LineFinderIterative();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~LineFinderIterative();
+
+ /** \brief Set Iterative Line Fit tunning params
+ *
+ * Set Iterative Line Fit tunning params.
+ *
+ **/
+ void setIlfParams(const LineFinderIterativeParams & _params);
+
+ /** \brief Find lines using Iterative line Fit. Result as a list of Line's
+ *
+ * Find lines from a set of points.
+ * Returns Lines as a std::list<laserscanutils::StraightSegment>
+ *
+ * \Requires:
+ * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is assumed
+ *
+ * \Provides:
+ * \param _line_list set of lines extracted from _points
+ * \return Number of lines extracted.
+ *
+ *
+ */
+ unsigned int findLines( const Eigen::MatrixXs & _points,
+ std::list<laserscanutils::LineSegment> & _line_list);
+
+ /** \brief Print things
+ *
+ * Print things about this class
+ *
+ **/
+ void print() const;
+};
+}//namespace
+#endif
\ No newline at end of file
diff --git a/src/line_segment.cpp b/src/line_segment.cpp
index c42fc07410818023547cb8e00f78c0e73c4f7950..f52a701bd76552800348cff295b58bf4d3c8d408 100644
--- a/src/line_segment.cpp
+++ b/src/line_segment.cpp
@@ -60,16 +60,26 @@ void LineSegment::pointProjectionToLine(const Eigen::Vector3s & _in_pt, Eigen::V
double LineSegment::angleToLine(const laserscanutils::LineSegment & _line) const
{
+ //See http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
+ //Assuming normalized lines
+ //ScalarT cos_a = abc_(0)*_line.abc_(0)+abc_(1)*_line.abc_(1);
+ //ScalarT angle = acos(cos_a);
+
+ //return value in [0,pi]
+ //return angle;
+
//compute each angle at 4-quadrant in [-pi,pi]
- ScalarT angle_this = atan2(abc_(1)/abc_(2), abc_(0)/abc_(2));
- ScalarT angle_line = atan2(_line.abc_(1)/_line.abc_(2), _line.abc_(0)/_line.abc_(2));
+ ScalarT angle_this = atan2(abc_(1), abc_(0));
+ ScalarT angle_line = atan2(_line.abc_(1), _line.abc_(0));
//Fit angles to [0,2pi] domain
if ( angle_this < 0 ) angle_this = 2*M_PI + angle_this;
if ( angle_line < 0 ) angle_line = 2*M_PI + angle_line;
- //return angle from this to _line, in [0,2pi]
- return (angle_line-angle_this);
+ //return angle from this to _line, in [-2pi,2pi]
+ ScalarT angle_diff = angle_line-angle_this;
+ if ( angle_diff < 0 ) return ( 2*M_PI + angle_diff );
+ else return angle_diff;
}
void LineSegment::merge(const LineSegment & _segment)
diff --git a/src/line_segment.h b/src/line_segment.h
index 9214900c8043813b20579579a97e60f1372e081f..013604b269456e6948b54c981061d4b13dc0bab8 100644
--- a/src/line_segment.h
+++ b/src/line_segment.h
@@ -22,14 +22,15 @@ class LineSegment
*
* Homogeneous parameterization of the line to which the segment lies on: (a,b,c)^T -> ax+by+c=0.
* Vector (a/c,b/c)^T is the normal vector, pointing to a free surface
+ * Normalized after operations according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
*
*/
- Eigen::Vector3s abc_;
+ Eigen::Vector3s abc_;
unsigned int idx_first_; //corresponding index to the scan ranges of the first point used
unsigned int idx_last_; //corresponding index to the scan ranges of the last point used
Eigen::Vector3s point_first_; //homogeneous coordinates of the starting 2D point
Eigen::Vector3s point_last_; //homogeneous coordinates of the ending 2D point
- ScalarT fit_error_; //sum of all distances from used points to line when fitting
+ ScalarT fit_error_; //mean error from points to line when fitting
unsigned int np_; // number of scan points supporting this line (through fitting, Hough voting, ...)
ScalarT range_; //range component in polar parameterization
ScalarT theta_; //theta component in polar parameterization.
diff --git a/src/scilab/homogeneous_lines.sce b/src/scilab/homogeneous_lines.sce
new file mode 100644
index 0000000000000000000000000000000000000000..1620067a96611ee3f0d1424ff7aae9c067ede515
--- /dev/null
+++ b/src/scilab/homogeneous_lines.sce
@@ -0,0 +1,57 @@
+
+//set points defining line 1 and 2
+p1o = [2;2;1];
+p1e = [-1;1;1];
+p2o = [-1;2;1];
+p2e = [2;-2;1];
+
+//compute lines
+l1 = cross(p1o,p1e);
+l2 = cross(p2o,p2e);
+
+//normalize (A) lines according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
+aux = sqrt( l1(1)*l1(1) + l1(2)*l1(2) );
+l1nA = l1./aux;
+aux = sqrt( l2(1)*l2(1) + l2(2)*l2(2) );
+l2nA = l2./aux;
+
+//normalize (B) lines according standard point normalization
+l1nB = l1./l1(3);
+l2nB = l2./l2(3);
+
+//plot lines
+fh=figure();
+fh2.background = color("white");
+
+ah = gca();
+ah.isoview = "on";
+ah.auto_margins = "off";
+ah.data_bounds = [-4,4,-4,4];
+
+plot([p1o(1) p1e(1)], [p1o(2) p1e(2)], '+b');
+xpoly([p1o(1) p1e(1)], [p1o(2) p1e(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",9);
+set(e,"thickness",3);
+
+plot([p2o(1) p2e(1)], [p2o(2) p2e(2)], '+r');
+xpoly([p2o(1) p2e(1)], [p2o(2) p2e(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",5);
+set(e,"thickness",3);
+
+//plot normal vectors
+p1c = (p1o+p1e)./2; //center point of l1
+xpoly([p1c(1) p1c(1)+l1nB(1)], [p1c(2) p1c(2)+l1nB(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",9);
+set(e,"thickness",3);
+
+p2c = (p2o+p2e)./2; //center point of l2
+xpoly([p2c(1) p2c(1)+l2nB(1)], [p2c(2) p2c(2)+l2nB(2)]);
+e=gce(); // get the current entity (the last created: here the polyline)
+set(e,"foreground",5);
+set(e,"thickness",3);
+
+
+