Working & Tested version of corner detection and initial structs

CMakeLists.txt

@@ -9,7 +9,7 @@ if(COMMAND cmake_policy)
 endif(COMMAND cmake_policy)
 
 # The project name and the type of project
-PROJECT(wolf)
+PROJECT(laser_scan_utils)
 
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)

Findlaser_scan_utils.cmake

 #edit the following line to add the librarie's header files
 FIND_PATH(
-    wolf_INCLUDE_DIR
-    NAMES wolf.h
+    laser_scan_utils_INCLUDE_DIR
+    NAMES scan_params.h corners.h
     PATHS /usr/local/include/iri-algorithms
 )
 
 FIND_LIBRARY(
-    wolf_LIBRARY
-    NAMES wolf
+    laser_scan_utils_LIBRARY
+    NAMES laser_scan_utils
     PATHS /usr/lib /usr/local/lib /usr/local/lib/iri-algorithms) 
 
-IF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
-   SET(wolf_FOUND TRUE)
-ENDIF (wolf_INCLUDE_DIR AND wolf_LIBRARY)
+IF (laser_scan_utils_INCLUDE_DIR AND laser_scan_utils_LIBRARY)
+   SET(laser_scan_utils_FOUND TRUE)
+ENDIF (laser_scan_utils_INCLUDE_DIR AND laser_scan_utils_LIBRARY)
 
-IF (wolf_FOUND)
-   IF (NOT wolf_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf: ${wolf_LIBRARY}")
-   ENDIF (NOT wolf_FIND_QUIETLY)
-ELSE (wolf_FOUND)
-   IF (wolf_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf")
-   ENDIF (wolf_FIND_REQUIRED)
-ENDIF (wolf_FOUND)
+IF (laser_scan_utils_FOUND)
+   IF (NOT laser_scan_utils_FIND_QUIETLY)
+      MESSAGE(STATUS "Found laser_scan_utils: ${laser_scan_utils_LIBRARY}")
+   ENDIF (NOT laser_scan_utils_FIND_QUIETLY)
+ELSE (laser_scan_utils_FOUND)
+   IF (laser_scan_utils_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find laser_scan_utils")
+   ENDIF (laser_scan_utils_FIND_REQUIRED)
+ENDIF (laser_scan_utils_FOUND)
 

README.txt

-A Toolbox of 2D laser scan utilities, made at IRI (www.iri.upc.edu)
+A Toolbox with utilities for 2D laser scan processing, made at IRI (www.iri.upc.edu)

src/CMakeLists.txt

 
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
+MESSAGE("Starting laser_scan_utils CMakeLists ...")
 CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 
 #find dependencies. 
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
-    MESSAGE("Ceres Library FOUND: ceres_wrapper will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
+FIND_PACKAGE(faramotics QUIET) #faramotics is only required for some tests
 IF(faramotics_FOUND)
 	FIND_PACKAGE(GLUT REQUIRED)
 	FIND_PACKAGE(pose_state_time REQUIRED)
-	FIND_LIBRARY(Assimp_LIBRARIES assimp HINTS /usr/local/lib NO_CMAKE_PATH NO_CMAKE_ENVIRONMENT_PATH)
-    MESSAGE("Faramotics Library FOUND: test_ceres_laser will be built.")
+    MESSAGE("Faramotics Library FOUND: Tests requiring it will be built.")
 ENDIF(faramotics_FOUND)
 
 #include directories
@@ -22,87 +15,25 @@ INCLUDE_DIRECTORIES(.)
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
+IF(faramotics_FOUND)
+    INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
+ENDIF(faramotics_FOUND)
 
 #headers
 SET(HDRS
-    capture_base.h
-    capture_relative.h
-    capture_gps_fix.h
-    capture_laser_2D.h
-    capture_odom_2D.h
-    constraint_base.h
-    constraint_sparse.h
-    constraint_gps_2D.h
-    constraint_odom_2D_theta.h
-    constraint_odom_2D_complex_angle.h
-    feature_base.h
-    feature_corner_2D.h
-    feature_gps_fix.h
-    feature_odom_2D.h
-    frame_base.h
-    landmark_base.h
-    landmark_corner_2D.h
-    map_base.h
-    node_base.h
-    node_terminus.h
-    node_linked.h
-    sensor_base.h
-    sensor_laser_2D.h
-    sensor_odom_2D.h
-    sensor_gps_fix.h
-    state_base.h
-    state_point.h
-    state_complex_angle.h
-    time_stamp.h
-    trajectory_base.h
-    wolf.h
-    wolf_problem.h)
-    
+    corners.h
+    geometry.h
+    scan_params.h
+    types.h)
+
 #sources
 SET(SRCS
-    capture_base.cpp
-    capture_relative.cpp
-    capture_gps_fix.cpp
-    capture_laser_2D.cpp
-    capture_odom_2D.cpp
-    constraint_base.cpp
-    feature_base.cpp
-    feature_corner_2D.cpp
-    feature_gps_fix.cpp
-    feature_odom_2D.cpp
-    frame_base.cpp
-    landmark_base.cpp
-    landmark_corner_2D.cpp
-    map_base.cpp
-    node_base.cpp
-    node_terminus.cpp
-    sensor_base.cpp
-    sensor_laser_2D.cpp
-    sensor_odom_2D.cpp
-    sensor_gps_fix.cpp
-    state_base.cpp
-    state_complex_angle.cpp
-    time_stamp.cpp
-    trajectory_base.cpp
-    wolf_problem.cpp)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
-    SET(SRCS ${SRCS} ceres_wrapper/complex_angle_parametrization.cpp)
-    SET(HDRS ${HDRS} ceres_wrapper/complex_angle_parametrization.h)
-    SET(SRCS ${SRCS} ceres_wrapper/ceres_manager.cpp)
-    SET(HDRS ${HDRS} ceres_wrapper/ceres_manager.h)
-ENDIF(Ceres_FOUND)
-
+    corners.cpp
+    geometry.cpp)
+    
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
 
-#Link the created library with ceres
-IF (Ceres_FOUND)
-    TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-
 #install library
 INSTALL(TARGETS ${PROJECT_NAME}
       RUNTIME DESTINATION bin
@@ -113,7 +44,7 @@ INSTALL(TARGETS ${PROJECT_NAME}
 INSTALL(FILES ${HDRS} 
       DESTINATION include/iri-algorithms)
 
-INSTALL(FILES ../Findwolf.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+INSTALL(FILES ../Findlaser_scan_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
 #Build examples & tests
 MESSAGE("Building examples and tests.")

src/corners.cpp

+
+#include "corners.h"
+
+unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list)
+{
+    //local variables
+    Eigen::MatrixXs points(3,_ranges.size());
+    double azimuth, cos_theta, theta;
+    Eigen::Vector3s corner;
+    Line line;
+    std::list<Line> line_list;
+    std::list<Line>::iterator line_it1, line_it2;
+    std::queue<unsigned int> jumps;
+
+    //init random generator
+    std::default_random_engine generator(1);
+    std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
+    
+    //convert range polar data to cartesian points.
+    for (unsigned int ii = 0; ii<_ranges.size(); ii++)
+    {
+        azimuth = _params.angle_max_ - _params.angle_step_ * ii;
+        //std::cout << "  azimuth: " << azimuth << std::endl;
+        points.col(ii) << _ranges(ii)*cos(azimuth), _ranges(ii)*sin(azimuth), 1; //points.row0-> x coordinate, points.row1->y coordinate
+
+        if (ii > 0 && fabs(_ranges(ii)-_ranges(ii-1)) > _alg_params.max_distance)
+        {
+            // store jumps
+            jumps.push(ii);
+            // consider jumps as a corners
+            if (_ranges(ii) < _ranges(ii-1))
+                _corner_list.push_back(Eigen::Vector4s(points(0,ii),points(1,ii),0,0)); // TODO: compute orientation
+            else
+                _corner_list.push_back(Eigen::Vector4s(points(0,ii-1),points(1,ii-1),0,0));// TODO: compute orientation
+        }
+    }
+
+    //find line segments running over the scan
+    for (unsigned int ii = _alg_params.segment_window_size-1; ii<_ranges.size(); ii=ii+rand_window_overlapping(generator) )
+    {
+        unsigned int i_from = ii - _alg_params.segment_window_size + 1;
+
+        // update the jump to be checked
+        while (!jumps.empty() && i_from > jumps.front())
+            jumps.pop();
+
+        // check if there is a jump inside the window (not fitting if it is the case)
+        if (jumps.front() > i_from && jumps.front() <= ii)
+            continue;
+
+        //Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
+        //fitLine(i_from, ii, points, line);
+        fitLine(points.block(0,i_from, 3, ii-i_from+1), line);
+
+        //if error below stdev, add line to result set
+        if ( line.error < _params.range_std_dev_*_alg_params.k_sigmas )
+        {
+            line.first = i_from; 
+            line.last = ii;
+            line_list.push_back(line);
+        }
+    }
+
+    //std::cout << "Lines fitted: " << line_list.size() << std::endl;
+
+    // concatenating and corners only if more than 1 line
+    if ( line_list.size() > 1 )
+    {
+        // concatenate lines
+        line_it1 = line_list.begin();
+        line_it2 = line_it1;
+        line_it2 ++;
+        while ( line_it1 != line_list.end() )
+        {
+            // last of current line and first of next line too far
+            if (line_it2->first > line_it1->last + _alg_params.max_beam_distance)
+            {
+                //std::cout << "lines too far:\nlast of current: " << line_it1->last << " first of next: " << line_it2->first << std::endl;
+                line_it1 ++;
+                line_it2 = line_it1;
+                line_it2 ++;
+            }
+            else
+            {
+                //compute angle between lines 1 and 2
+                cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() );
+                theta = acos (cos_theta);
+
+                //TODO: fabs? acos returns [0 PI]
+                if (theta > M_PI/2)
+                    theta -= M_PI;
+    //            std::cout << std::endl << "cos_theta: " << cos_theta << std::endl <<
+    //                                      "theta: " << theta << std::endl <<
+    //                                      "*index_it1: " << *index_it1 << std::endl <<
+    //                                      "*index_it2: " << *index_it2 << std::endl;
+    //                                       "   (*line_it1).dot(*line_it2): " << (*line_it1).dot(*line_it2) << std::endl <<
+    //                                       "   (*line_it1).norm()*(*line_it2).norm(): " << (*line_it1).norm()*(*line_it2).norm() << std::endl;
+
+
+                //Check angle threshold and consecutiveness of lines in the scan
+                if ( fabs(theta) < _alg_params.theta_max_parallel )
+                {
+                    Line new_line;
+                    //fitLine(line_it1->first, line_it2->last, points, new_line);
+                    fitLine(points.block(0,line_it1->first, 3, line_it2->last-line_it1->first+1), new_line);
+                    if ( new_line.error < _params.range_std_dev_*_alg_params.k_sigmas )
+                    {
+                        new_line.first = line_it1->first;
+                        new_line.last = line_it2->last;
+                        *line_it1 = new_line;
+                        line_it2 = line_list.erase(line_it2);
+
+//                      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;
+//                      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;
+//                      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;
+                    }
+                    else
+                    {
+                        //update iterators
+                        line_it1 ++;
+                        line_it2 = line_it1;
+                        line_it2 ++;
+                    }
+                }
+                else
+                {
+                    //update iterators
+                    line_it1 ++;
+                    line_it2 = line_it1;
+                    line_it2 ++;
+                }
+            }
+        }
+
+        //std::cout << "Lines after concatenation: " << line_list.size() << std::endl;
+
+        // find corners over the line list
+        line_it1 = line_list.begin();
+        line_it2 = line_it1;
+        line_it2 ++;
+        while ( line_it1 != line_list.end() )
+        {   
+            // last of current line and first of next line too far
+            if (line_it2->first > line_it1->last + _alg_params.max_beam_distance)
+            {
+                //std::cout << "lines too far:\nlast of current: " << line_it1->last << " first of next: " << line_it2->first << std::endl;
+                line_it1 ++;
+                line_it2 = line_it1;
+                line_it2 ++;
+            }
+            else
+            {
+                //compute angle between lines 1 and 2
+                cos_theta = (line_it1->vector).dot(line_it2->vector) / ( (line_it1->vector).norm()*(line_it2->vector).norm() );
+                theta = acos (cos_theta);
+
+                //TODO: fabs? acos returns [0 PI]
+                if (theta > M_PI/2)
+                    theta -= M_PI;
+
+//              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;
+
+
+                //Check angle threshold and consecutiveness of lines in the scan
+                if ( fabs(theta) > _alg_params.theta_min ) //TODO: fabs? acos returns [0 PI]
+                {
+                    corner = (line_it1->vector).cross(line_it2->vector); //cross product between lines is the intersection
+                    corner = corner / corner(2); //normalize point to set last component to 1
+
+                    // Check if the corner is close to both lines ends
+                    if ( ( (points.col(line_it1->last) - corner).head(2).norm() < _alg_params.max_distance )
+                        && 
+                         ( (points.col(line_it2->first) - corner).head(2).norm() < _alg_params.max_distance ) )
+                    {
+                        Eigen::Vector3s v1 = (points.col(line_it1->first) - corner);
+                        Eigen::Vector3s v2 = (points.col(line_it2->last) - corner);
+                        v1 /= v1.norm();
+                        v2 /= v2.norm();
+
+                        //corner orientation
+                        if (v1(0) * v2(1) > v1(1) * v2(0))
+                            corner(2) = asin (v1(1));
+                        else
+                            corner(2) = asin (v2(1));
+
+//                      std::cout << "Detected corner!" << std::endl <<
+//                                   "\tcorner: " << corner.transpose() << std::endl <<
+//                                   "\tline 1 point: " << points.col(line_it1->first).transpose() << std::endl <<
+//                                   "\tline 2 point: " << points.col(line_it2->last).transpose() << std::endl <<
+//                                   "\tv1: " << v1.transpose() << std::endl <<
+//                                   "\tv2: " << v2.transpose() << std::endl;
+
+                        _corner_list.push_back(Eigen::Vector4s(corner(0),corner(1),corner(2),theta));
+                    }
+                }
+
+                //update iterators
+                if (line_it2 != line_list.end() )
+                    line_it2 ++;
+                else
+                {
+                    line_it1 ++;
+                    line_it2 = line_it1;
+                    line_it2 ++;
+                }
+            }
+        }
+    }
+
+    //std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
+
+    // Erase duplicated corners
+    if ( _corner_list.size() > 1 )
+    {
+        // concatenate lines
+        std::list<Eigen::Vector4s>::iterator corner_it1 = _corner_list.begin();
+        std::list<Eigen::Vector4s>::iterator corner_it2 = corner_it1;
+        corner_it2 ++;
+        while ( corner_it1 != _corner_list.end() )
+        {
+            // Check if two corners are close enough
+            if ( (*corner_it1 - *corner_it2).head(2).norm() < _alg_params.max_distance )
+            {
+                // TODO: keep the one with larger lines
+                // compute the mean
+                *corner_it1 = (*corner_it1 + *corner_it2) / 2;
+                corner_it2 = _corner_list.erase(corner_it2);
+            }
+            else
+            {
+                corner_it1 ++;
+                corner_it2 = corner_it1;
+                corner_it2 ++;
+            }
+        }
+    }
+
+    //std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
+
+    return _corner_list.size();
+}

src/corners.h

+
+#ifndef CORNERS_H_
+#define CORNERS_H_
+
+//std
+#include <list>
+#include <queue>
+#include <random>
+
+// Eigen ingludes
+#include <eigen3/Eigen/Geometry>
+
+//laserscanutils
+#include "types_laser_scan_utils.h"
+#include "scan_params.h"
+#include "geometry.h"
+
+namespace laserscanutils
+{
+    /** \brief Set of tunning parameters
+     * 
+     * Set of tunning parameters for corner extraction
+     * 
+     */
+    struct ExtractCornerParams
+    {
+        unsigned int segment_window_size; //number of points to fit lines in the first pass
+        ScalarT theta_min; //minimum theta between consecutive segments to detect corner. PI/6=0.52
+        ScalarT theta_max_parallel; //maximum theta between consecutive segments to fuse them in a single line.
+        ScalarT k_sigmas;//How many std_dev are tolerated to count that a point is supporting a line
+        unsigned int max_beam_distance;//max number of beams of distance between lines to consider corner or concatenation
+        ScalarT max_distance;//max distance between line ends to consider corner or concatenation
+    };
+    
+    /** \brief Extract corners from a given scan
+     * 
+     * Extract corners from a given scan. 
+     * Returns corners as a list of Vector-4, where each corner is parametrized as: 
+     *  - corner(0): x coordinate
+     *  - corner(1): y coordinate
+     *  - corner(2): ...
+     *  - corner(3): ...
+     * 
+     * TODO: use the struct Corner defined  at geometry.h to encode a corner
+     * 
+     */
+    unsigned int extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list);
+}
+#endif
+

src/examples/CMakeLists.txt

-# NodeLinked class test
-ADD_EXECUTABLE(test_node_linked test_node_linked.cpp)
-TARGET_LINK_LIBRARIES(test_node_linked ${PROJECT_NAME})
-
-# numeric test
-ADD_EXECUTABLE(test_ceres_wrapper_numeric test_ceres_wrapper_numeric.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_numeric ${PROJECT_NAME})
-
-# jet test
-ADD_EXECUTABLE(test_ceres_wrapper_jet test_ceres_wrapper_jet.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet ${PROJECT_NAME})
-
-# jet individual blocks test
-ADD_EXECUTABLE(test_ceres_wrapper_jet_ind_block test_ceres_wrapper_jet_ind_block.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_ind_block ${PROJECT_NAME})
-
-# jet test with 2 sensors
-ADD_EXECUTABLE(test_ceres_wrapper_jet_2_sensors test_ceres_wrapper_jet_2_sensors.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_2_sensors ${PROJECT_NAME})
-
-# jet test manager
-ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
-
-# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-ADD_EXECUTABLE(test_ceres_odom_batch test_ceres_odom_batch.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_odom_batch ${PROJECT_NAME})
-
-# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
-ADD_EXECUTABLE(test_ceres_odom_iterative test_ceres_odom_iterative.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
-
-# Building and populating the wolf tree 
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
-
-# test ceres manager, wolf manager and wolf tree
-ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME})
-
-ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
-TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
-
+# corner extraction test
 IF(faramotics_FOUND)
-	ADD_EXECUTABLE(test_ceres_laser test_ceres_laser.cpp)
-    TARGET_LINK_LIBRARIES(test_ceres_laser ${GLUT_glut_LIBRARY} ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} ${PROJECT_NAME})
+    ADD_EXECUTABLE(test_extract_corners test_extract_corners.cpp)
+    TARGET_LINK_LIBRARIES(test_extract_corners ${GLUT_glut_LIBRARY} ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} ${PROJECT_NAME})
 ENDIF(faramotics_FOUND)
\ No newline at end of file

src/examples/test_extract_corners.cpp

+//std includes
+#include <iostream>
+#include <random>
+
+//GLUT
+#include <GL/glut.h>
+
+//faramotics includes
+#include "faramotics/dynamicSceneRender.h"
+#include "faramotics/rangeScan2D.h"
+#include "btr-headers/pose3d.h"
+
+//laserscanutils
+#include "types_laser_scan_utils.h"
+#include "corners.h"
+
+//namespaces
+using namespace std;
+
+//function to travel around each model
+void motionCampus(unsigned int ii, Cpose3d & pose)
+{
+    if (ii<=40)
+    {
+        //pose.rt.setEuler( pose.rt.head()-2*M_PI/180., pose.rt.pitch(), pose.rt.roll() );
+        pose.pt(0) = pose.pt(0) + 0.1;
+    }
+    if ( (ii>40) && (ii<=80) )
+    {
+        pose.pt(0) = pose.pt(0) + 0.1;
+        pose.rt.setEuler(pose.rt.head(), pose.rt.pitch(), pose.rt.roll() + 0.01);
+    }
+    if ( (ii>80) && (ii<=120) )
+    {
+        pose.pt(0) = pose.pt(0) + 0.1;
+        pose.rt.setEuler(pose.rt.head(), pose.rt.pitch(), pose.rt.roll() - 0.015);
+    }
+    if ( (ii>120) && (ii<=170) )
+    {
+        pose.rt.setEuler( pose.rt.head()+1.8*M_PI/180., pose.rt.pitch(), pose.rt.roll() );
+        pose.moveForward(0.2);
+    }
+    if ( (ii>170) && (ii<=220) )
+    {
+        pose.rt.setEuler( pose.rt.head()-1.8*M_PI/180., pose.rt.pitch(), pose.rt.roll()-0.05*M_PI/180. );
+    }
+    if ( (ii>220) && (ii<=310) ) 
+    {
+        pose.pt(0) = pose.pt(0) + 0.1;
+    }
+    if ( (ii>310) && (ii<=487) ) 
+    {
+        pose.rt.setEuler( pose.rt.head()-1.*M_PI/180., pose.rt.pitch(), pose.rt.roll()+0.1*M_PI/180. );
+        pose.moveForward(0.1);
+    }
+    if ( (ii>487) && (ii<=582) ) 
+    {
+        pose.moveForward(0.2);
+    }
+    if ( (ii>582) && (ii<=700) ) 
+    {
+        pose.pt(2) = pose.pt(2) + 0.001;
+        pose.rt.setEuler( pose.rt.head()-1.*M_PI/180., pose.rt.pitch(), pose.rt.roll());
+        pose.moveForward(0.1);
+    }    
+}
+
+int main(int argc, char** argv)
+{
+	std::cout << "\n ========= Corner extraction test ===========\n";
+
+	// INITIALIZATION ============================================================================================
+
+	// Faramotics stuff
+	CdynamicSceneRender *myRender;
+	CrangeScan2D *myScanner;
+	Cpose3d viewPoint;
+	Cpose3d devicePose(2,8,0.2,0,0,0);
+	vector<float> myScan;
+	string modelFileName("/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj");
+    
+    //laserscanutils
+    laserscanutils::ScanParams scan_params;
+    scan_params.angle_min_ = -M_PI/2;
+    scan_params.angle_max_ = M_PI/2;
+    scan_params.angle_step_ = M_PI/720;
+    scan_params.scan_time_ = 0.01;//not relevant
+    scan_params.range_min_ = 0.1;
+    scan_params.range_max_ = 30;
+    scan_params.range_std_dev_ = 0.01;
+
+    laserscanutils::ExtractCornerParams alg_params;
+    alg_params.segment_window_size = 8;
+    alg_params.theta_min = 0.4;
+    alg_params.theta_max_parallel = 0.1;
+    alg_params.k_sigmas = 3;
+    alg_params.max_beam_distance = 5;
+    alg_params.max_distance = 0.5;
+    
+    //init random generators
+    std::default_random_engine generator(1);
+    std::normal_distribution<laserscanutils::ScalarT> laser_range_noise(0.001, scan_params.range_std_dev_); 
+
+	//glut initialization
+	glutInit(&argc, argv);
+
+    //create a viewer for the 3D model and scan points
+	myRender = new CdynamicSceneRender(1200,700,90*M_PI/180,90*700.0*M_PI/(1200.0*180.0),0.2,100);
+	myRender->loadAssimpModel(modelFileName,true); //with wireframe
+    
+	//create scanner and load 3D model
+	myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);//HOKUYO_UTM30LX_180DEG
+	myScanner->loadAssimpModel(modelFileName);
+
+	// START TRAJECTORY ============================================================================================
+	for (uint step=1; step < 1000; step++)
+	{
+		// moves the device position
+		motionCampus(step, devicePose);
+
+		//compute scan
+		myScan.clear();
+		myScanner->computeScan(devicePose,myScan);
+		vector<double> myScanDoubles(myScan.begin(), myScan.end());
+		Eigen::Map<Eigen::VectorXs> scan_reading(myScanDoubles.data(), 720);
+
+		//draws the device frame, scan hits and depth image
+		myRender->drawPoseAxis(devicePose);
+		myRender->drawScan(devicePose,myScan,180.*M_PI/180.,90.*M_PI/180.); //draw scan
+
+		// extract corners
+		std::list<Eigen::Vector4s> corner_list;
+		std::vector<double> corner_vector;
+		laserscanutils::extractCorners(scan_params, alg_params, scan_reading, corner_list);
+        
+        //draw corners
+		for (std::list<Eigen::Vector4s>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
+		{
+			corner_vector.push_back(corner_it->x());
+			corner_vector.push_back(corner_it->y());
+		}
+		myRender->drawCorners(devicePose,corner_vector);
+
+		//Set view point and render the scene. Locate visualization view point, somewhere behind the device
+		viewPoint.setPose(devicePose);
+		viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
+		viewPoint.moveForward(-5);
+		myRender->setViewPoint(viewPoint);
+		myRender->render();
+	}
+
+	//delete things
+	delete myRender;
+    delete myScanner;
+
+	//exit
+	return 0;
+}

src/geometry.cpp

+
+#include "geometry.h"
+
+//void laserscanutils::fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs & _points, Line & _line)
+void laserscanutils::fitLine(const Eigen::MatrixXs & _points, Line & _line)
+{
+//     _line.first = _idx_from;
+//     _line.last = _idx_to;
+
+    //Found the best fitting line over points within the window. Build the system: A*line=[0 0 1]'. Matrix A = a_ij
+    //Eigen::Matrix3s AA = _points.block(0, _idx_from, 3, _idx_to-_idx_from+1) * _points.block(0, _idx_from, 3, _idx_to-_idx_from+1).transpose();
+    Eigen::Matrix3s AA = _points * _points.transpose();
+    AA.row(2) << 0,0,1;
+
+    //solve for line
+    _line.vector = AA.inverse().col(2);
+
+    // compute fitting error TODO or TOREVIEW ....
+//  _line.error = 0;
+//  for(unsigned int jj = _idx_from; jj<=_idx_to; jj++)
+//  _line.error += fabs( _line.vector(0)*_points(0,jj) + _line.vector(1)*_points(1,jj) + _line.vector(2)) / sqrt( _line.vector(0)*_line.vector(0) + _line.vector(1)*_line.vector(1) );
+//  _line.error /= (_idx_to-_idx_from+1);
+
+
+    //_line.error = (_points.block(0, _idx_from, 3, _idx_to-_idx_from+1).transpose() * _line.vector).array().abs().sum()/(_line.vector.head(2).norm()*(_idx_to-_idx_from+1));
+    _line.error = (_points.transpose() * _line.vector).array().abs().sum() / (_line.vector.head(2).norm()*_points.cols());
+}

src/geometry.h

+
+#ifndef GEOMETRY_H_
+#define GEOMETRY_H_
+
+//laserscanutils
+#include "types_laser_scan_utils.h"
+
+namespace laserscanutils
+{
+    struct Line
+    {
+        Eigen::Vector3s vector;
+        ScalarT error;
+        unsigned int first, last;
+        //TODO: add an Eigen::Map to the supporting points ... but who ensures memory allocation of such points ???
+    };
+    
+    struct Corner 
+    {
+        double x_; //x coordinate , meters
+        double y_; //y coordinate , meters
+        double orientation_; //counterclockwise from the X axis
+        double aperture_; //minor aperture of the corner
+    };
+    
+    /** \brief Find the best fittig line given a set of points
+     * 
+     * Find the best fittig line given a set of points
+     * Input points at each column of _points matrix
+     * 
+     **/
+    void fitLine(const Eigen::MatrixXs & _points, Line & _line);
+    
+}
+#endif

src/scan_params.h

+
+#ifndef SCAN_PARAMS_H_
+#define SCAN_PARAMS_H_
+
+//laserscanutils
+#include "types_laser_scan_utils.h"
+
+namespace laserscanutils
+{
+    struct ScanParams
+    {
+        double angle_min_; //radians
+        double angle_max_; //radians
+        double angle_step_; //radians
+        double scan_time_; //time from the measuring the first ray up to measuring the last one, seconds
+        double range_min_; //meters 
+        double range_max_; //meters
+        double range_std_dev_; //standard deviation measurement noise in range, meters 
+    };
+}
+#endif

src/types.h

+
+#ifndef TYPES_H_
+#define TYPES_H_
+
+//includes from Eigen lib
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+
+//namespace laserscanutils;
+
+//type scalar, in case working on 32-bit architectures , or benchmarking
+namespace laserscanutils
+{
+    //typedef float ScalarT;         // Use this for float, 32 bit precision
+    typedef double ScalarT;        // Use this for double, 64 bit precision
+    //typedef long double ScalarT;   // Use this for long double, 128 bit precision
+}
+
+// Eigen namespace extension using the above defined scalar
+namespace Eigen  
+{
+    // 1. Vectors and Matrices
+    typedef Matrix<laserscanutils::ScalarT, 2, 2> Matrix2s;                ///< 2x2 matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 3, 3> Matrix3s;                ///< 3x3 matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 4, 4> Matrix4s;                ///< 4x4 matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, Dynamic, Dynamic> MatrixXs;    ///< variable size matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 1> Vector1s;                ///< 1-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 2, 1> Vector2s;                ///< 2-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 3, 1> Vector3s;                ///< 3-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 4, 1> Vector4s;                ///< 4-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, Dynamic, 1> VectorXs;          ///< variable size vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 2> RowVector2s;             ///< 2-row-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 3> RowVector3s;             ///< 3-row-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 4> RowVector4s;             ///< 4-row-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, Dynamic> RowVectorXs;       ///< variable size row-vector of real scalar_t type
+
+    // 2. Quaternions and other rotation things
+    typedef Quaternion<laserscanutils::ScalarT> Quaternions;               ///< Quaternion of real scalar_t type
+    typedef AngleAxis<laserscanutils::ScalarT> AngleAxiss;                 ///< Angle-Axis of real scalar_t type
+}
+#endif

src/types_laser_scan_utils.h

+
+#ifndef TYPES_LASER_SCAN_UTILS_H_
+#define TYPES_LASER_SCAN_UTILS_H_
+
+//includes from Eigen lib
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+//namespace laserscanutils;
+
+//type scalar, in case working on 32-bit architectures , or benchmarking
+namespace laserscanutils
+{
+    //typedef float ScalarT;         // Use this for float, 32 bit precision
+    typedef double ScalarT;        // Use this for double, 64 bit precision
+    //typedef long double ScalarT;   // Use this for long double, 128 bit precision
+}
+
+// Eigen namespace extension using the above defined scalar
+namespace Eigen  
+{
+    // 1. Vectors and Matrices
+    typedef Matrix<laserscanutils::ScalarT, 2, 2> Matrix2s;                ///< 2x2 matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 3, 3> Matrix3s;                ///< 3x3 matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 4, 4> Matrix4s;                ///< 4x4 matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, Dynamic, Dynamic> MatrixXs;    ///< variable size matrix of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 1> Vector1s;                ///< 1-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 2, 1> Vector2s;                ///< 2-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 3, 1> Vector3s;                ///< 3-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 4, 1> Vector4s;                ///< 4-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, Dynamic, 1> VectorXs;          ///< variable size vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 2> RowVector2s;             ///< 2-row-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 3> RowVector3s;             ///< 3-row-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, 4> RowVector4s;             ///< 4-row-vector of real scalar_t type
+    typedef Matrix<laserscanutils::ScalarT, 1, Dynamic> RowVectorXs;       ///< variable size row-vector of real scalar_t type
+
+    // 2. Quaternions and other rotation things
+    typedef Quaternion<laserscanutils::ScalarT> Quaternions;               ///< Quaternion of real scalar_t type
+    typedef AngleAxis<laserscanutils::ScalarT> AngleAxiss;                 ///< Angle-Axis of real scalar_t type
+}
+#endif