From f9ee2867bf9c54ee27ef9ab9a19d17ba3d943f9a Mon Sep 17 00:00:00 2001
From: andreucm <andreu@beta-robots.com>
Date: Fri, 26 Feb 2016 11:01:31 +0100
Subject: [PATCH] fitLineRansac pseudocode added
---
README.txt | 2 +-
src/line_finder.cpp | 39 +++++++++++++++++++++++++++++++++++++++
src/line_finder.h | 16 ++++++++++++++++
3 files changed, 56 insertions(+), 1 deletion(-)
diff --git a/README.txt b/README.txt
index a7c96da..a3d6ae3 100644
--- a/README.txt
+++ b/README.txt
@@ -1 +1 @@
-A Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri.upc.edu)
+A C++ Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri.upc.edu)
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
index da4fe52..273171c 100644
--- a/src/line_finder.cpp
+++ b/src/line_finder.cpp
@@ -30,6 +30,45 @@ void LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) c
//_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
}
+void LineFinder::fitLineRansac(const Eigen::MatrixXs & _points, LineSegment & _line, ScalarT _error) const
+{
+ std::vector<bool> used_points(_points.cols(),false); //mask, true means used
+ std::vector<unsigned int> available_points(_points.cols());
+ unsigned int support_counter, best_support_counter;
+ LineSegment line;
+
+ //init best_support_counter
+ best_support_counter = 0;
+
+ // while (attempts < max_attempts)
+ // {
+ //init available_points vector with all indexes
+ for ( unsigned int ii=0; ii<available_points.size(); ii++ )
+ {
+ available_points[ii] = ii;
+ }
+ // pick randomly two indexes from available_points
+ // Set the selecte points to MatrixXs P
+ // fitLine(P,line)
+ // support_counter = 0;
+ // while (available_points.size() > 0 )
+ // {
+ // choose a point q randomly from the index vector available_points
+ // remove that index from available_points
+ // Compute dist between q and line
+ // if (dist < max_error) support_counter ++;
+ // }
+ //
+ // if ( support_counter > best_support_counter )
+ // {
+ // _line = line;
+ // best_support_counter = support_counter;
+ // }
+ // attempts ++;
+ // }
+
+}
+
void LineFinder::pointProjectionToLine(const Eigen::Vector3s & _line, const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const
{
ScalarT a,b,c,qx,qy,dd;
diff --git a/src/line_finder.h b/src/line_finder.h
index 923d9cd..d375cb7 100644
--- a/src/line_finder.h
+++ b/src/line_finder.h
@@ -47,6 +47,22 @@ class LineFinder
**/
void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) const;
+ /** \brief Find the best fittig line given a set of points, applying RANSAC
+ *
+ * Find the best fittig line given a set of points, applying RANSAC
+ *
+ * \Requires:
+ * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
+ * \param _error: maximum fitting error to accept a point in a line
+ *
+ * \Provides:
+ * \param _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense
+ *
+ * RANSAC is applied as outlier rejection method.
+ *
+ **/
+ void fitLineRansac(const Eigen::MatrixXs & _points, LineSegment & _line, ScalarT _error = 0.1) const;
+
/** \brief Computes projection of point to line
*
* Computes the projection of _in_pt to _line. Result at _out_pt
--
GitLab