diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5e8794fb4c1fefbc23ad65b2dfa8b3052db9663b..de2f6ef5b3dd9cdb90cdd0bad765687258995717 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/Findlaser_scan_utils.cmake b/Findlaser_scan_utils.cmake
index e54db6d717b09606e4bc3a60f281b614e56f1fbb..1af65b67e1dcd1d601bfcb19eabb40d14ab4a105 100644
--- a/Findlaser_scan_utils.cmake
+++ b/Findlaser_scan_utils.cmake
@@ -1,26 +1,26 @@
#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)
diff --git a/README.txt b/README.txt
index e3c2f1883d36b0e4c1cb030151b78ef863de609e..a7c96da94dba84fbd320adef494ce7708cf45de5 100644
--- a/README.txt
+++ b/README.txt
@@ -1 +1 @@
-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)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 16c175dcd0e2dc0f7469be464e633167d81c1d7e..882334166990292e128af0014a0d02ac00304d73 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,20 +1,13 @@
-#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.")
diff --git a/src/corners.cpp b/src/corners.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..900d42e3e7aa0d2c010f4523614515b8005fc163
--- /dev/null
+++ b/src/corners.cpp
@@ -0,0 +1,256 @@
+
+#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();
+}
diff --git a/src/corners.h b/src/corners.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9597873ee71b5e88db08c9d72bca77f5bd5e91a
--- /dev/null
+++ b/src/corners.h
@@ -0,0 +1,50 @@
+
+#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
+
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 303cde79a90342327eaa65766f73d51034b1f390..741291947b7cd12e5d565a3b6e6da4d26e81dc35 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,47 +1,5 @@
-# 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
diff --git a/src/examples/test_extract_corners.cpp b/src/examples/test_extract_corners.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25bb5d1a54a5544c7c5e83fcacc850c630310c91
--- /dev/null
+++ b/src/examples/test_extract_corners.cpp
@@ -0,0 +1,158 @@
+//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;
+}
diff --git a/src/geometry.cpp b/src/geometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a49c06ca52117aed1e8c62e922c19808621f32fb
--- /dev/null
+++ b/src/geometry.cpp
@@ -0,0 +1,27 @@
+
+#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());
+}
diff --git a/src/geometry.h b/src/geometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..433206961c9fed074c7ac4923369bc2c1e044aed
--- /dev/null
+++ b/src/geometry.h
@@ -0,0 +1,35 @@
+
+#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
diff --git a/src/scan_params.h b/src/scan_params.h
new file mode 100644
index 0000000000000000000000000000000000000000..0493087287e1376fae44c5c83634073d6c484b9b
--- /dev/null
+++ b/src/scan_params.h
@@ -0,0 +1,21 @@
+
+#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
diff --git a/src/types.h b/src/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..372b8b3d69944207ee39ede15996621e9dbfadb9
--- /dev/null
+++ b/src/types.h
@@ -0,0 +1,42 @@
+
+#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
diff --git a/src/types_laser_scan_utils.h b/src/types_laser_scan_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f759dc0ff2353757512a5336bddfa8096a0d39c
--- /dev/null
+++ b/src/types_laser_scan_utils.h
@@ -0,0 +1,41 @@
+
+#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