diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 63b08cdc7a135d4bcebe4a359972cb763b6c713d..f95a3926851dc2335997e94cf9c46e5b499fe4ff 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 0000000000000000000000000000000000000000..73ac303629b3bfab70733add8836abdbb1d1e2ac
--- /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 0000000000000000000000000000000000000000..c9fd969746807776cfea29322f0ac43d825f7218
--- /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 0000000000000000000000000000000000000000..7b996896f168a61afb89ef77cc088a7328f90048
--- /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 0000000000000000000000000000000000000000..182f1cc339d4f2babc20c161a7c3ab261d67e24e
--- /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 0000000000000000000000000000000000000000..9cdf404a76073e5002ca19a72ff46a7234b3765c
--- /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 0000000000000000000000000000000000000000..98a0011db27e44cc369bf785cb48ba4a3db64cdd
--- /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 2f43810c672334129bdf790a21bd13a2322afb48..a627990085f6872d0d40bc0e569f77f27a9d5951 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
 };