From 72286d2e097393e643dfbc4e593f6c0106f56782 Mon Sep 17 00:00:00 2001
From: andreucm <andreu@beta-robots.com>
Date: Thu, 17 Mar 2016 09:33:20 +0100
Subject: [PATCH] Added main classes for corner detection. Not yet implemented.
---
src/CMakeLists.txt | 7 +++
src/corner_finder.cpp | 17 +++++++
src/corner_finder.h | 61 ++++++++++++++++++++++++
src/corner_finder_range_diff.cpp | 27 +++++++++++
src/corner_finder_range_diff.h | 82 ++++++++++++++++++++++++++++++++
src/corner_point.cpp | 26 ++++++++++
src/corner_point.h | 39 +++++++++++++++
src/line_finder_hough.h | 6 +--
8 files changed, 262 insertions(+), 3 deletions(-)
create mode 100644 src/corner_finder.cpp
create mode 100644 src/corner_finder.h
create mode 100644 src/corner_finder_range_diff.cpp
create mode 100644 src/corner_finder_range_diff.h
create mode 100644 src/corner_point.cpp
create mode 100644 src/corner_point.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 63b08cd..f95a392 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,6 +25,9 @@ SET(HDRS_BASE
SET(HDRS
corner_detector.h
+ corner_finder.h
+ corner_finder_range_diff.h
+ corner_point.h
entities.h
grid_2d.h
grid_cluster.h
@@ -42,7 +45,11 @@ SET(HDRS
#sources
SET(SRCS
+ corner_point.cpp
corner_detector.cpp
+ corner_finder.cpp
+ corner_finder_range_diff.cpp
+ corner_point.cpp
entities.cpp
grid_2d.cpp
grid_cluster.cpp
diff --git a/src/corner_finder.cpp b/src/corner_finder.cpp
new file mode 100644
index 0000000..73ac303
--- /dev/null
+++ b/src/corner_finder.cpp
@@ -0,0 +1,17 @@
+#include "corner_finder.h"
+
+namespace laserscanutils
+{
+
+CornerFinder::CornerFinder()
+{
+
+}
+
+CornerFinder::~CornerFinder()
+{
+
+}
+
+}//namespace
+
diff --git a/src/corner_finder.h b/src/corner_finder.h
new file mode 100644
index 0000000..c9fd969
--- /dev/null
+++ b/src/corner_finder.h
@@ -0,0 +1,61 @@
+#ifndef CORNER_FINDER_H_
+#define CORNER_FINDER_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+#include "corner_point.h"
+
+//std
+#include <list>
+
+namespace laserscanutils
+{
+/** \brief Base Class for methods to extract corners from a scan
+*
+* Base Class for methods to extract corners from a scan
+*
+*/
+class CornerFinder
+{
+ protected:
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ CornerFinder();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~CornerFinder();
+
+ /** \brief Find corners. Pure virtual. To be implemented by each inherited class
+ *
+ * Find corners from a set of scans.
+ * Returns corners as a std::list<CornerPoint>
+ *
+ * \Requires:
+ * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
+ *
+ * \Provides:
+ * \param _corner_list set of corners extracted from _points
+ * \return Number of corners extracted.
+ *
+ */
+ virtual unsigned int findCorners( const Eigen::MatrixXs & _points,
+ std::list<laserscanutils::CornerPoint> & _corner_list) = 0;
+
+ /** \brief Print things
+ *
+ * Print things about this class
+ *
+ **/
+ virtual void print() const = 0;
+};
+}//namespace
+#endif
diff --git a/src/corner_finder_range_diff.cpp b/src/corner_finder_range_diff.cpp
new file mode 100644
index 0000000..7b99689
--- /dev/null
+++ b/src/corner_finder_range_diff.cpp
@@ -0,0 +1,27 @@
+#include "corner_finder_range_diff.h"
+
+namespace laserscanutils
+{
+
+CornerFinderRangeDiff::CornerFinderRangeDiff() : CornerFinder()
+{
+
+}
+
+CornerFinderRangeDiff::~CornerFinderRangeDiff()
+{
+
+}
+
+unsigned int CornerFinderRangeDiff::findCorners( const Eigen::MatrixXs & _points,
+ std::list<laserscanutils::CornerPoint> & _corner_list)
+{
+
+}
+
+void CornerFinderRangeDiff::print() const
+{
+
+}
+
+}//namespace
diff --git a/src/corner_finder_range_diff.h b/src/corner_finder_range_diff.h
new file mode 100644
index 0000000..182f1cc
--- /dev/null
+++ b/src/corner_finder_range_diff.h
@@ -0,0 +1,82 @@
+#ifndef CORNER_FINDER_RANGE_DIFF_H_
+#define CORNER_FINDER_RANGE_DIFF_H_
+
+//laserscanutils
+#include "corner_finder.h"
+#include "line_finder_hough.h"
+
+namespace laserscanutils
+{
+
+/** \brief set of tunning parameters for the Hough transform line detection
+*
+* set of tunning parameters for the Hough transform line detection
+*
+*/
+struct CornerFinderRangeDiffParams
+{
+ ScalarT zero_range_diff_; //threshold to consider range diff between consecutive scan hits is zero
+ ScalarT corner_window_size_; //number of scan points, separately at left and right of the corner, to consider to fit a line
+ ScalarT line_fit_error_; //Maximum allowed error to consider thar both left and right subset fits in a line
+ ScalarT max_aperture_; //Maximum aperture angle to consider a candidate point as a corner. See corner.h for a precise definition of corner aperture
+ void print() const; //just a print method
+};
+
+/** \brief Base Class for methods to extract corners from a scan
+*
+* Base Class for methods to extract corners from a scan
+*
+*/
+class CornerFinderRangeDiff : public CornerFinder
+{
+ protected:
+ CornerFinderRangeDiffParams params_;
+ LineFinderHough line_finder_; //required just fitLine method, but cannot use LineFinder class becaus it is virtual. TODO: May devirtualize LineFinder
+
+ public:
+ /** \brief Constructor
+ *
+ * Constructor
+ *
+ **/
+ CornerFinderRangeDiff();
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ ~CornerFinderRangeDiff();
+
+ /** \brief Set tunning params
+ *
+ * Set tunning params.
+ *
+ **/
+ void setParams(const CornerFinderRangeDiffParams & _params);
+
+ /** \brief Find corners.
+ *
+ * Find corners from a set of scans.
+ * Returns corners as a std::list<CornerPoint>
+ *
+ * \Requires:
+ * \param _points: 3xN matrix, set of points. Each column is a 2D point in homogeneous (x,y,1). Ordering is not required.
+ *
+ * \Provides:
+ * \param _corner_list set of corners extracted from _points
+ * \return Number of corners extracted.
+ *
+ */
+ unsigned int findCorners( const Eigen::MatrixXs & _points,
+ std::list<laserscanutils::CornerPoint> & _corner_list);
+
+ /** \brief Print things
+ *
+ * Print things about this class
+ *
+ **/
+ void print() const;
+};
+}//namespace
+#endif
diff --git a/src/corner_point.cpp b/src/corner_point.cpp
new file mode 100644
index 0000000..9cdf404
--- /dev/null
+++ b/src/corner_point.cpp
@@ -0,0 +1,26 @@
+#include "corner_point.h"
+
+//open namespace
+namespace laserscanutils
+{
+
+CornerPoint::CornerPoint()
+{
+
+}
+
+CornerPoint::~CornerPoint()
+{
+
+}
+
+void CornerPoint::print() const
+{
+ std::cout << "CornerPoint Parameters : " << std::endl
+ << "\tpoint: " << point_.transpose() << std::endl
+ << "\torientation: " << orientation_ << std::endl
+ << "\taperture: " << aperture_ << std::endl;
+}
+
+}//namespace
+
diff --git a/src/corner_point.h b/src/corner_point.h
new file mode 100644
index 0000000..98a0011
--- /dev/null
+++ b/src/corner_point.h
@@ -0,0 +1,39 @@
+#ifndef CORNER_POINT_H_
+#define CORNER_POINT_H_
+
+//laserscanutils
+#include "laser_scan_utils.h"
+
+//std
+#include <list>
+#include <iostream>
+
+//open namespace
+namespace laserscanutils
+{
+/** \brief Corner point
+*
+* Corner point
+*
+*/
+class CornerPoint
+{
+ public:
+ Eigen::Vector3s point_; //2D corner coordinates in homogeneous form
+ 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)
+// Line line_1_; // line of the corner
+// Line line_2_; // line of the corner
+
+ public:
+ //constructor
+ CornerPoint();
+
+ //Destructor
+ ~CornerPoint();
+
+ //print
+ void print() const;
+};
+}//namespace
+#endif
diff --git a/src/line_finder_hough.h b/src/line_finder_hough.h
index 2f43810..a627990 100644
--- a/src/line_finder_hough.h
+++ b/src/line_finder_hough.h
@@ -13,9 +13,9 @@ namespace laserscanutils
*/
struct LineFinderHoughParams
{
- double range_max_; //maximum allowed range for lines
- double range_step_; //range step in the voting grid
- double theta_step_; //theta step in the voting grid
+ ScalarT range_max_; //maximum allowed range for lines
+ ScalarT range_step_; //range step in the voting grid
+ ScalarT theta_step_; //theta step in the voting grid
unsigned int min_supports_; //Min supports at the hough grid to consider a cell as a line
void print() const; //just a print method
};
--
GitLab