diff --git a/CMakeLists.txt b/CMakeLists.txt
index c854988d784c23f31f2ba48544efa554b2d0d82a..abb733b77222baf486bc7009404e1934f4e8ec70 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
if(COMMAND cmake_policy)
- cmake_policy(SET CMP0005 NEW)
+ cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# MAC OSX RPATH
@@ -47,7 +47,6 @@ if(UNIX)
endif(UNIX)
-
MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
@@ -64,8 +63,18 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-# option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+ OPTION(BUILD_DEMOS "Build Demos" OFF)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+ OPTION(BUILD_DOC "Build Documentation" OFF)
+ENDIF(NOT BUILD_DOC)
+
#############
## Testing ##
@@ -92,6 +101,8 @@ FIND_PACKAGE(wolf REQUIRED)
FIND_PACKAGE(laser_scan_utils REQUIRED)
+FIND_PACKAGE(csm QUIET)
+
#FIND_PATH(
# Suitesparse_INCLUDE_DIRS
# NAMES SuiteSparse_config.h
@@ -204,9 +215,22 @@ SET(SRCS_DTASSC
SET(SRCS_SOLVER
)
SET(SRCS_YAML
+ src/yaml/processor_odom_ICP_yaml.cpp
src/yaml/sensor_laser_2D_yaml.cpp
)
#OPTIONALS
+if(csm_FOUND)
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ # include/laser/processor/processor_closeloop_icp.h
+ include/laser/processor/processor_odom_icp.h
+ include/laser/feature/feature_icp_align.h
+ )
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ # src/processor/processor_closeloop_icp.cpp
+ src/processor/processor_odom_icp.cpp
+ src/feature/feature_icp_align.cpp
+ )
+endif(csm_FOUND)
#optional HDRS and SRCS
# ==================EXAMPLE===============
# IF (Ceres_FOUND)
@@ -216,12 +240,12 @@ SET(SRCS_YAML
# include/base/ceres_wrapper/ceres_manager.h
# include/base/ceres_wrapper/cost_function_wrapper.h
# include/base/ceres_wrapper/create_numeric_diff_cost_function.h
-# include/base/ceres_wrapper/local_parametrization_wrapper.h
+# include/base/ceres_wrapper/local_parametrization_wrapper.h
# )
# SET(SRCS_WRAPPER
# src/solver/solver_manager.cpp
# src/ceres_wrapper/ceres_manager.cpp
-# src/ceres_wrapper/local_parametrization_wrapper.cpp
+# src/ceres_wrapper/local_parametrization_wrapper.cpp
# )
# ELSE(Ceres_FOUND)
# SET(HDRS_WRAPPER)
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
new file mode 100755
index 0000000000000000000000000000000000000000..938022e920cddb287f773480596ffe497d8a0306
--- /dev/null
+++ b/cmake_modules/Findcsm.cmake
@@ -0,0 +1,64 @@
+FIND_PATH(
+ csm_INCLUDE_DIR
+ NAMES algos.h
+ PATHS /usr/local/include/csm)
+IF(csm_INCLUDE_DIR)
+ MESSAGE("Found csm include dirs: ${csm_INCLUDE_DIR}")
+ELSE(csm_INCLUDE_DIR)
+ MESSAGE("Couldn't find csm include dirs")
+ENDIF(csm_INCLUDE_DIR)
+
+FIND_LIBRARY(
+ csm_LIBRARY
+ NAMES libcsm.so libcsm.dylib
+ PATHS /usr/local/lib)
+IF(csm_LIBRARY)
+ MESSAGE("Found csm lib: ${csm_LIBRARY}")
+ELSE(csm_LIBRARY)
+ MESSAGE("Couldn't find csm lib")
+ENDIF(csm_LIBRARY)
+
+IF (csm_INCLUDE_DIR AND csm_LIBRARY)
+ SET(csm_FOUND TRUE)
+ ELSE(csm_INCLUDE_DIR AND csm_LIBRARY)
+ set(csm_FOUND FALSE)
+ENDIF (csm_INCLUDE_DIR AND csm_LIBRARY)
+
+IF (csm_FOUND)
+ IF (NOT csm_FIND_QUIETLY)
+ MESSAGE(STATUS "Found csm: ${csm_LIBRARY}")
+ ENDIF (NOT csm_FIND_QUIETLY)
+ELSE (csm_FOUND)
+ IF (csm_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find csm")
+ ENDIF (csm_FIND_REQUIRED)
+ENDIF (csm_FOUND)
+
+
+macro(csm_report_not_found REASON_MSG)
+ set(csm_FOUND FALSE)
+ unset(csm_INCLUDE_DIR)
+ unset(csm_LIBRARIES)
+
+ # Reset the CMake module path to its state when this script was called.
+ set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+ # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+ # FindPackage() use the camelcase library name, not uppercase.
+ if (csm_FIND_QUIETLY)
+ message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
+ else (csm_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
+ else()
+ # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+ # that prevents generation, but continues configuration.
+ message(SEND_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
+ endif ()
+ return()
+endmacro(csm_report_not_found)
+
+if(NOT csm_FOUND)
+ csm_report_not_found("Something went wrong while setting up csm.")
+endif(NOT csm_FOUND)
+# Set the include directories for csm (itself).
+set(csm_FOUND TRUE)
\ No newline at end of file
diff --git a/include/laser/feature/feature_icp_align.h b/include/laser/feature/feature_icp_align.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7ee1401d41192913777075dc7123b61f053a0e6
--- /dev/null
+++ b/include/laser/feature/feature_icp_align.h
@@ -0,0 +1,54 @@
+#ifndef FEATURE_ICP_ALIGN_H_
+#define FEATURE_ICP_ALIGN_H_
+
+//Wolf includes
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_odom_2D.h"
+#include "laser_scan_utils/icp.h"
+
+
+//std includes
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeatureICPAlign);
+
+//class FeatureOdom2D
+class FeatureICPAlign : public FeatureBase
+{
+ public:
+
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeatureICPAlign(const laserscanutils::icpOutput& _icp_align_res);
+
+ virtual ~FeatureICPAlign();
+
+ /** \brief Generic interface to find factors
+ *
+ * TBD
+ * Generic interface to find factors between this feature and a map (static/slam) or a previous feature
+ *
+ **/
+ virtual void findFactors();
+
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ const laserscanutils::icpOutput& getQualityAlign() const;
+
+ private:
+ laserscanutils::icpOutput icp_align_res_;
+
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..e06f241b84df4f398924c3f40c3e34202932f2c3
--- /dev/null
+++ b/include/laser/processor/processor_closeloop_icp.h
@@ -0,0 +1,56 @@
+/**************************
+ * WOLF includes *
+ **************************/
+#include <core/processor/processor_base.h>
+#include <laser/capture/capture_laser_2D.h>
+
+/**************************
+ * STD includes *
+ **************************/
+#include <iostream>
+#include <iomanip>
+#include <queue>
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(ProcessorCloseLoopICP);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCloseLoopICP);
+
+
+struct ProcessorParamsCloseLoopICP : public ProcessorParamsBase
+{
+ int match_past_key_frame;
+ ProcessorParamsCloseLoopICP() = default;
+ ProcessorParamsCloseLoopICP(std::string _unique_name, const ParamsServer& _server):
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ match_past_key_frame = _server.getParam<int>(_unique_name + "/match_past_key_frame");
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print()
+ + "match_past_key_frame: " + std::to_string(match_past_key_frame) + "\n";
+ }
+};
+
+class ProcessorCloseLoopICP : public ProcessorBase
+{
+ protected:
+ CaptureLaser2DPtr _origin_cpt;
+ int count = 0;
+ public:
+ ProcessorCloseLoopICP(ProcessorParamsBasePtr _params);
+
+ virtual void configure(SensorBasePtr _sensor) override;
+
+ virtual void process(CaptureBasePtr _capture_ptr) override;
+
+ virtual bool voteForKeyFrame() override;
+
+ static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr);
+
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr);
+
+};
+
+}
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..221160f8fe2f3a9543f5d7eac75ed5a9186f3deb
--- /dev/null
+++ b/include/laser/processor/processor_odom_icp.h
@@ -0,0 +1,126 @@
+#ifndef SRC_PROCESSOR_ODOM_ICP_H_
+#define SRC_PROCESSOR_ODOM_ICP_H_
+
+
+#include "laser/capture/capture_laser_2D.h"
+#include "laser/feature/feature_icp_align.h"
+
+#include <core/processor/processor_tracker.h>
+
+#include <laser_scan_utils/laser_scan_utils.h>
+#include <laser_scan_utils/icp.h>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomICP);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdomICP);
+
+struct ProcessorParamsOdomICP : public ProcessorParamsTracker
+{
+ // ICP Params
+ int use_point_to_line_distance;
+ int max_correspondence_dist;
+ int max_iterations;
+ int use_corr_tricks;
+ double outliers_maxPerc;
+ double outliers_adaptive_order;
+ double outliers_adaptive_mult;
+
+ // Other processor params
+ double vfk_min_dist;
+ double vfk_min_angle;
+ double vfk_min_time;
+ double vfk_min_error;
+ int vfk_max_points;
+
+ ProcessorParamsOdomICP() = default;
+ ProcessorParamsOdomICP(std::string _unique_name, const ParamsServer &_server):
+ ProcessorParamsTracker(_unique_name, _server)
+ {
+ use_point_to_line_distance = _server.getParam<int> (_unique_name + "/use_point_to_line_distance");
+ max_correspondence_dist = _server.getParam<int> (_unique_name + "/max_correspondence_dist");
+ max_iterations = _server.getParam<int> (_unique_name + "/max_iterations");
+ use_corr_tricks = _server.getParam<int> (_unique_name + "/use_corr_tricks");
+ outliers_maxPerc = _server.getParam<double> (_unique_name + "/outliers_maxPerc");
+ outliers_adaptive_order = _server.getParam<double> (_unique_name + "/outliers_adaptive_order");
+ outliers_adaptive_mult = _server.getParam<double> (_unique_name + "/outliers_adaptive_mult");
+ vfk_min_dist = _server.getParam<double> (_unique_name + "/vfk_min_dist");
+ vfk_min_angle = _server.getParam<double> (_unique_name + "/vfk_min_angle");
+ vfk_min_time = _server.getParam<double> (_unique_name + "/vfk_min_time");
+ vfk_min_error = _server.getParam<double> (_unique_name + "/vfk_min_error");
+ vfk_max_points = _server.getParam<int> (_unique_name + "/vfk_max_points");
+ }
+ std::string print() {
+ return "\n" + ProcessorParamsTracker::print() + "\n"
+ + "use_point_to_line_distance: "+ std::to_string(use_point_to_line_distance)+ "\n"
+ + "max_correspondence_dist: " + std::to_string(max_correspondence_dist) + "\n"
+ + "max_iterations: " + std::to_string(max_iterations) + "\n"
+ + "use_corr_tricks: " + std::to_string(use_corr_tricks) + "\n"
+ + "outliers_maxPerc: " + std::to_string(outliers_maxPerc) + "\n"
+ + "outliers_adaptive_order: " + std::to_string(outliers_adaptive_order) + "\n"
+ + "outliers_adaptive_mult: " + std::to_string(outliers_adaptive_mult) + "\n"
+ + "vfk_min_dist: " + std::to_string(vfk_min_dist) + "\n"
+ + "vfk_min_angle: " + std::to_string(vfk_min_angle) + "\n"
+ + "vfk_min_time: " + std::to_string(vfk_min_time) + "\n"
+ + "vfk_min_error: " + std::to_string(vfk_min_error) + "\n"
+ + "vfk_max_points: " + std::to_string(vfk_max_points) + "\n";
+ }
+};
+
+class ProcessorOdomICP : public ProcessorTracker
+{
+ protected:
+ // Useful sensor stuff
+ SensorLaser2DPtr sensor_laser_; // casted pointer to parent
+ laserscanutils::LaserScanParams laser_scan_params_;
+
+ // ICP algorithm
+ std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+
+ // temporary and carry-on transformations
+ laserscanutils::icpOutput trf_origin_last_;
+ laserscanutils::icpOutput trf_origin_incoming_;
+ laserscanutils::icpOutput trf_last_incoming_;
+
+ public:
+ laserscanutils::icpParams icp_params_;
+ ProcessorParamsOdomICPPtr proc_params_;
+
+ public:
+ ProcessorOdomICP(ProcessorParamsOdomICPPtr _params);
+ WOLF_PROCESSOR_CREATE(ProcessorOdomICP, ProcessorParamsOdomICP);
+ virtual ~ProcessorOdomICP();
+ virtual void configure(SensorBasePtr _sensor) override;
+
+ protected:
+ virtual void preProcess() override;
+
+ virtual unsigned int processKnown() override;
+ virtual unsigned int processNew(const int& _max_features) override;
+
+ virtual bool voteForKeyFrame() override;
+
+ virtual void advanceDerived() override;
+ virtual void resetDerived() override;
+
+
+ virtual void establishFactors() override;
+ FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
+
+ inline bool voteForKeyFrameDistance();
+ inline bool voteForKeyFrameAngle();
+ inline bool voteForKeyFrameTime();
+ inline bool voteForKeyFrameMatchQuality();
+
+
+
+
+};
+
+
+
+
+}
+
+#endif // SRC_PROCESSOR_ODOM_ICP_H_
diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 408b75dff1fc745a2669acdfef79f2200354f9fc..f50d33fd87074be6dd234990a63ba2cc50787a33 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -9,7 +9,6 @@
#define PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
#include "laser/processor/polyline_2D_utils.h"
-#include "core/processor/processor_tracker_feature.h"
#include "laser/landmark/landmark_polyline_2D.h"
#include "laser/landmark/landmark_match_polyline_2D.h"
#include "laser/sensor/sensor_laser_2D.h"
@@ -19,6 +18,9 @@
#include "laser/factor/factor_point_2D.h"
#include "laser/factor/factor_point_to_line_2D.h"
+#include "core/processor/processor_tracker_feature.h"
+#include "core/utils/params_server.hpp"
+
//laser_scan_utils
#include "laser_scan_utils/laser_scan.h"
#include "laser_scan_utils/line_finder_iterative.h"
@@ -36,6 +38,18 @@ typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScala
struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
{
+ ProcessorParamsTrackerFeaturePolyline2D() = default;
+ ProcessorParamsTrackerFeaturePolyline2D(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ProcessorParamsTrackerFeature(_unique_name, _server)
+ {
+ // TODO write parser!
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsTrackerFeature::print();
+ // TODO write printer!
+ }
+
laserscanutils::LineFinderIterativeParams line_finder_params;
Scalar match_feature_position_sq_norm_max;
Scalar match_feature_orientation_sq_norm_max;
@@ -81,6 +95,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
public:
ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params);
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeaturePolyline2D, ProcessorParamsTrackerFeaturePolyline2D);
virtual ~ProcessorTrackerFeaturePolyline2D();
@@ -242,10 +257,6 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
public:
- /// @brief Factory method
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params);
-
const FeatureBasePtrList& getLastNewFeatures() const
{
return untracked_features_last_;
diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h
index 71a152b098ab1f2654be2ea53e13b794c614d08b..3fd5c68f30c0896c4e25ae9f3e11ffa0d553ff91 100644
--- a/include/laser/sensor/sensor_laser_2D.h
+++ b/include/laser/sensor/sensor_laser_2D.h
@@ -20,6 +20,33 @@ struct IntrinsicsLaser2D : public IntrinsicsBase
virtual ~IntrinsicsLaser2D() = default;
laserscanutils::LaserScanParams scan_params;
+
+ IntrinsicsLaser2D() = default;
+ IntrinsicsLaser2D(std::string _unique_name, const wolf::ParamsServer & _server) :
+ IntrinsicsBase(_unique_name, _server)
+ {
+ scan_params.angle_min_ = _server.getParam<double>(_unique_name + "/LaserScanParams/angle_min");
+ scan_params.angle_max_ = _server.getParam<double>(_unique_name + "/LaserScanParams/angle_max");
+ scan_params.angle_step_ = _server.getParam<double>(_unique_name + "/LaserScanParams/angle_step");
+ scan_params.scan_time_ = _server.getParam<double>(_unique_name + "/LaserScanParams/scan_time");
+ scan_params.range_min_ = _server.getParam<double>(_unique_name + "/LaserScanParams/range_min");
+ scan_params.range_max_ = _server.getParam<double>(_unique_name + "/LaserScanParams/range_max");
+ scan_params.range_std_dev_ = _server.getParam<double>(_unique_name + "/LaserScanParams/range_std_dev");
+ scan_params.angle_std_dev_ = _server.getParam<double>(_unique_name + "/LaserScanParams/angle_std_dev");
+ }
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "\n"
+ + "LaserScanParams/angle_min: " + std::to_string(scan_params.angle_min_) + "\n"
+ + "LaserScanParams/angle_max: " + std::to_string(scan_params.angle_max_) + "\n"
+ + "LaserScanParams/angle_step: " + std::to_string(scan_params.angle_step_) + "\n"
+ + "LaserScanParams/scan_time: " + std::to_string(scan_params.scan_time_) + "\n"
+ + "LaserScanParams/range_min: " + std::to_string(scan_params.range_min_) + "\n"
+ + "LaserScanParams/range_max: " + std::to_string(scan_params.range_max_) + "\n"
+ + "LaserScanParams/range_std_dev: " + std::to_string(scan_params.range_std_dev_) + "\n"
+ + "LaserScanParams/angle_std_dev: " + std::to_string(scan_params.angle_std_dev_) + "\n";
+ }
+
};
WOLF_PTR_TYPEDEFS(SensorLaser2D);
@@ -57,6 +84,7 @@ class SensorLaser2D : public SensorBase
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
SensorLaser2D(const Eigen::VectorXs& _extrinsics, IntrinsicsLaser2DPtr _params);
+ WOLF_SENSOR_CREATE(SensorLaser2D, IntrinsicsLaser2D, 3);
virtual ~SensorLaser2D();
@@ -77,9 +105,7 @@ class SensorLaser2D : public SensorBase
const laserscanutils::LaserScanParams & getScanParams() const;
public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics);
- static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server);
- static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
+// static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml);
};
diff --git a/src/feature/feature_icp_align.cpp b/src/feature/feature_icp_align.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cbc8f8a64c97913f0ed7b12bc9920b0ddc0f45bc
--- /dev/null
+++ b/src/feature/feature_icp_align.cpp
@@ -0,0 +1,28 @@
+#include "laser/feature/feature_icp_align.h"
+
+namespace wolf {
+
+FeatureICPAlign::FeatureICPAlign(const laserscanutils::icpOutput& _icp_align_res):
+ FeatureBase("ICP ALIGN", _icp_align_res.res_transf, _icp_align_res.res_covar),
+ icp_align_res_(_icp_align_res)
+{
+
+}
+
+FeatureICPAlign::~FeatureICPAlign()
+{
+ //
+}
+
+void FeatureICPAlign::findFactors()
+{
+
+}
+
+const laserscanutils::icpOutput& FeatureICPAlign::getQualityAlign() const
+{
+ return icp_align_res_;
+}
+
+
+} // namespace wolf
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d4f410071df0de2110f27ad7a4878c8ee46ab519
--- /dev/null
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -0,0 +1,80 @@
+#include "laser/processor/processor_closeloop_icp.h"
+#include "laser_scan_utils/icp.h"
+
+using namespace wolf;
+
+// Constructor
+ProcessorCloseLoopICP::ProcessorCloseLoopICP(ProcessorParamsBasePtr _params) :
+ ProcessorBase("CL ICP", _params)
+{
+
+}
+
+void ProcessorCloseLoopICP::configure(SensorBasePtr _sensor)
+{
+
+}
+
+void ProcessorCloseLoopICP::process(CaptureBasePtr _capture_ptr)
+{
+ // std::cout << "Am I being called?" << std::endl;
+ // ++count;
+ // CaptureLaser2DPtr cpt_ptr = std::static_pointer_cast<CaptureLaser2D>(_capture_ptr);
+ // SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(_capture_ptr->getSensor());
+ // if(_origin_cpt == nullptr) _origin_cpt = cpt_ptr;
+ // if(count % 10 != 0) return;
+ // laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
+ // laserscanutils::LaserScan& last_scan = cpt_ptr->getScan();
+ // laserscanutils::LaserScan& origin_scan = _origin_cpt->getScan();
+ // std::cout << "Number of ranges: " << cpt_ptr->getScan().ranges_raw_.size() << '\n';
+ // Eigen::Vector3s transform_0;
+ // transform_0 << 0,0,0;
+ // laserscanutils::icp_output result = laserscanutils::ICP::matchPC(last_scan, origin_scan, scan_params, transform_0);
+ // std::cout << "We made it past result " << std::endl;
+ // std::cout << "My solution " << result.res_transf(0) << "," << result.res_transf(1) << "," << result.res_transf(2) << std::endl;
+ // _origin_cpt = cpt_ptr;
+}
+
+bool ProcessorCloseLoopICP::voteForKeyFrame()
+{
+ return 0;
+}
+
+ProcessorBasePtr ProcessorCloseLoopICP::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
+{
+ ProcessorCloseLoopICPPtr prc_ptr;
+
+ ProcessorParamsCloseLoopICPPtr params;
+ if (_params)
+ params = std::static_pointer_cast<ProcessorParamsCloseLoopICP>(_params);
+ else
+ params = std::make_shared<ProcessorParamsCloseLoopICP>();
+
+ prc_ptr = std::make_shared<ProcessorCloseLoopICP>(params);
+ prc_ptr->setName(_unique_name);
+
+ return prc_ptr;
+}
+
+ProcessorBasePtr ProcessorCloseLoopICP::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr)
+{
+ ProcessorCloseLoopICPPtr prc_ptr;
+ auto params = std::make_shared<ProcessorParamsCloseLoopICP>();
+ params->match_past_key_frame = _server.getParam<double>(_unique_name + "/match_past_key_frame", "5");
+
+ prc_ptr = std::make_shared<ProcessorCloseLoopICP>(params);
+ prc_ptr->setName(_unique_name);
+
+ return prc_ptr;
+}
+
+
+// Register in the SensorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("CL ICP", ProcessorCloseLoopICP)
+} /* namespace wolf */
+#include "core/processor/autoconf_processor_factory.h"
+namespace wolf {
+ WOLF_REGISTER_PROCESSOR_AUTO("CL ICP", ProcessorCloseLoopICP)
+} // namespace wolf
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0e0102c4e628136731716b4e39be20763c08e15b
--- /dev/null
+++ b/src/processor/processor_odom_icp.cpp
@@ -0,0 +1,229 @@
+#include "laser/processor/processor_odom_icp.h"
+
+using namespace wolf;
+using namespace laserscanutils;
+
+ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
+ ProcessorTracker("ODOM ICP", _params)
+{
+ proc_params_ = _params;
+
+
+ icp_params_.use_point_to_line_distance = proc_params_->use_point_to_line_distance;
+ icp_params_.max_correspondence_dist = proc_params_->max_correspondence_dist;
+ icp_params_.max_iterations = proc_params_->max_iterations;
+ icp_params_.use_corr_tricks = proc_params_->use_corr_tricks;
+ icp_params_.outliers_maxPerc = proc_params_->outliers_maxPerc;
+ icp_params_.outliers_adaptive_order = proc_params_->outliers_adaptive_order;
+ icp_params_.outliers_adaptive_mult = proc_params_->outliers_adaptive_mult;
+
+ // ICP algorithm
+ icp_tools_ptr_ = std::make_shared<ICP>();
+
+ // Frame transforms
+ trf_origin_last_.res_covar = Eigen::Matrix3s::Identity();
+ trf_origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
+ trf_last_incoming_.res_covar = Eigen::Matrix3s::Identity();
+}
+
+ProcessorOdomICP::~ProcessorOdomICP()
+{
+
+}
+
+void ProcessorOdomICP::preProcess()
+{
+
+}
+
+
+
+void ProcessorOdomICP::configure(SensorBasePtr _sensor)
+{
+ sensor_laser_ = std::static_pointer_cast<SensorLaser2D>(_sensor);
+ laser_scan_params_ = sensor_laser_->getScanParams();
+}
+
+
+
+unsigned int ProcessorOdomICP::processKnown()
+{
+ // Match the incoming with the origin, passing the transform from origin to last as initialization
+
+ if (origin_ptr_) // Make sure it's not the FIRST_TIME
+ {
+ CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_);
+ CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
+
+ trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
+ origin_ptr->getScan(),
+ this->laser_scan_params_,
+ this->icp_params_,
+ this->trf_origin_last_.res_transf); // Check order
+ }
+
+ return 0;
+}
+
+unsigned int ProcessorOdomICP::processNew(const int& _max_features)
+{
+ Eigen::Vector3s t_identity;
+ t_identity << 0, 0, 0;
+
+ // XXX: Dynamic or static? JS: static is OK.
+ CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
+ CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_);
+
+ trf_last_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
+ last_ptr->getScan(),
+ this->laser_scan_params_,
+ this->icp_params_,
+ t_identity);
+ return 0;
+}
+
+bool ProcessorOdomICP::voteForKeyFrame()
+{
+ return (voteForKeyFrameDistance() ||
+ voteForKeyFrameAngle() ||
+ voteForKeyFrameTime() ||
+ voteForKeyFrameMatchQuality());
+}
+
+inline bool ProcessorOdomICP::voteForKeyFrameDistance()
+{
+ bool vote = (trf_origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
+ if (vote)
+ {
+ std::cout << "Distance True" << '\n';
+ }
+
+ return vote;
+}
+
+inline bool ProcessorOdomICP::voteForKeyFrameAngle()
+{
+ bool vote = (fabs(trf_origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle);
+ if (vote)
+ {
+ std::cout << "Angle True" << '\n';
+ }
+
+ return vote;
+}
+
+inline bool ProcessorOdomICP::voteForKeyFrameTime()
+{
+ Scalar secs = incoming_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp();
+
+ bool vote = (secs > proc_params_->vfk_min_time);
+
+ if (vote)
+ {
+ std::cout << "Time True" << '\n';
+ }
+
+ return vote;
+}
+
+inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality()
+{
+ bool vote = (trf_origin_incoming_.error > proc_params_->vfk_min_error || trf_origin_incoming_.nvalid < proc_params_->vfk_max_points);
+ if (vote)
+ {
+ std::cout << "Quality True" << '\n';
+ }
+ return vote ;
+}
+
+
+void ProcessorOdomICP::advanceDerived()
+{
+ using namespace Eigen;
+
+ // overwrite last frame's state
+
+ Isometry2ds w_T_ro = Translation2ds(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Ds(origin_ptr_->getFrame()->getState()(2));
+ Isometry2ds ro_T_so = Translation2ds(origin_ptr_->getSensorP()->getState()) * Rotation2Ds(origin_ptr_->getSensorO()->getState()(0));
+ Isometry2ds& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
+
+ // incoming
+
+ Isometry2ds so_T_si = Translation2ds(trf_origin_incoming_.res_transf.head(2)) * Rotation2Ds(trf_origin_incoming_.res_transf(2));
+ Isometry2ds w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
+ Vector3s x_inc; x_inc << w_T_ri.translation() , Rotation2Ds(w_T_ri.rotation()).angle();
+
+ // Put the state of incoming in last
+ last_ptr_->getFrame()->setState(x_inc);
+
+ trf_origin_last_ = trf_origin_incoming_;
+}
+
+void ProcessorOdomICP::resetDerived()
+{
+
+ // Using processing_step_ to ensure that origin, last and incoming are different
+ if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK)
+ {
+ // Notation explanation:
+ // x1_R_x2: Rotation from frame x1 to frame x2
+ // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
+
+ // Rotation composition
+ Eigen::Rotation2D<Scalar> w_R_ro = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
+ Eigen::Rotation2D<Scalar> r_R_s = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
+ Eigen::Rotation2D<Scalar>& ro_R_so = r_R_s; // just a ref for bettter chaining trf. names
+ Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(trf_origin_last_.res_transf(2));
+
+ // Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*r_R_s*so_R_sl*r_R_s.inverse();
+ // ro_R_so = rl_R_sl = r_R_s
+ // Planar rotations are commutative
+ Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro * so_R_sl;
+
+ // Translation composition
+ Eigen::Vector2s w_p_w_ro = origin_ptr_->getFrame()->getState().head(2);
+ Eigen::Vector2s w_p_ro_so = w_R_ro * origin_ptr_->getSensorP()->getState();
+ Eigen::Vector2s w_p_so_sl = w_R_ro * ro_R_so * trf_origin_last_.res_transf.head(2);
+ Eigen::Vector2s w_p_sl_rl = w_R_rl * (-last_ptr_->getSensorP()->getState());
+
+ Eigen::Vector2s w_p_w_rl = w_p_w_ro + w_p_ro_so + w_p_so_sl + w_p_sl_rl;
+
+ Eigen::Vector3s curr_state;
+ curr_state.head(2) = w_p_w_rl;
+ curr_state(2) = origin_ptr_->getFrame()->getState()(2) + trf_origin_last_.res_transf(2);
+
+ last_ptr_->getFrame()->setState(curr_state);
+
+ trf_origin_last_ = trf_last_incoming_;
+ }
+}
+
+void ProcessorOdomICP::establishFactors()
+{
+ if (last_ptr_ != origin_ptr_)
+ {
+ auto ftr_ptr = emplaceFeature(last_ptr_);
+ emplaceFactor(ftr_ptr);
+ }
+}
+
+FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
+{
+ CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser);
+ return FeatureBase::emplace<FeatureICPAlign>(capture_laser,
+ trf_origin_last_);
+}
+
+FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)
+{
+ return FactorBase::emplace<FactorOdom2D>(_feature, _feature, origin_ptr_->getFrame(),
+ shared_from_this());
+}
+
+
+// Register in the SensorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+ WOLF_REGISTER_PROCESSOR("ODOM ICP", ProcessorOdomICP);
+ WOLF_REGISTER_PROCESSOR_AUTO("ODOM ICP", ProcessorOdomICP);
+} // namespace wolf
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 08a5031298200a7c045f31514423f6e712d0d0a9..404889898031bf5e548ef957625c071c0b8fe08f 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -1363,22 +1363,12 @@ void ProcessorTrackerFeaturePolyline2D::computeTransformations()
//std::cout << "R_last_incoming_ " << std::endl << R_last_incoming_ << std::endl;
}
-ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params)
-{
- auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline2D>(_params);
-
- auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline2D>(params);
-
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
-}
} /* namespace wolf */
// Register in the SensorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D)
-} /* namespace wolf */
+ WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D);
+ WOLF_REGISTER_PROCESSOR_AUTO("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D);
+} // namespace wolf
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index bff07ba6ede76c6e5cd0ac762877596bd2bd2f3f..a31115ef3c51761ad991d1a79fb97022ba8bf4a1 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -1,6 +1,7 @@
#include "laser/sensor/sensor_laser_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_angle.h"
+
+#include <core/state_block/state_block.h>
+#include <core/state_block/state_angle.h>
namespace wolf {
@@ -31,17 +32,22 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const I
//
}
-SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
- SensorLaser2D(_p_ptr, _o_ptr, *_params)
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2DPtr _params) :
+ SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
+ scan_params_(_params->scan_params)
{
//
}
-SensorLaser2D::SensorLaser2D(const Eigen::VectorXs& _extrinsics, IntrinsicsLaser2DPtr _params) :
- SensorLaser2D(std::make_shared<StateBlock>(_extrinsics.head(2), true, nullptr),
- std::make_shared<StateAngle>(_extrinsics(2), true),
- _params)
+SensorLaser2D::SensorLaser2D(const Eigen::VectorXs& _extrinsics, const IntrinsicsLaser2DPtr _params) :
+ SensorBase("LASER 2D",
+ std::make_shared<StateBlock>(_extrinsics.head(2), false),
+ std::make_shared<StateAngle>(_extrinsics(2), false),
+ nullptr,
+ 8),
+ scan_params_(_params->scan_params)
{
+ //
}
SensorLaser2D::~SensorLaser2D()
@@ -72,43 +78,11 @@ const laserscanutils::LaserScanParams& SensorLaser2D::getScanParams() const
return scan_params_;
}
-// Define the factory method
-SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
- // cast intrinsics into derived type
- IntrinsicsLaser2DPtr params = std::static_pointer_cast<IntrinsicsLaser2D>(_intrinsics);
- SensorLaser2DPtr sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params->scan_params);
- sen->setName(_unique_name);
- return sen;
-}
-
-SensorBasePtr SensorLaser2D::createAutoConf(const std::string& _unique_name, const ParamsServer& _server)
-{
- // decode extrinsics vector
- Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos");
- assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
- StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true);
-
- SensorLaser2DPtr sen;
- IntrinsicsLaser2D params;
-
- sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params.scan_params);
-
- sen->setName(_unique_name);
- return sen;
-}
} // namespace wolf
// Register in the SensorFactory and the ParameterFactory
#include "core/sensor/sensor_factory.h"
-//#include "intrinsics_factory.h"
namespace wolf {
WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D)
WOLF_REGISTER_SENSOR_AUTO("LASER 2D", SensorLaser2D)
diff --git a/src/yaml/processor_odom_ICP_yaml.cpp b/src/yaml/processor_odom_ICP_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a5557abad838122c9239b038e964f2cc66faa0d
--- /dev/null
+++ b/src/yaml/processor_odom_ICP_yaml.cpp
@@ -0,0 +1,64 @@
+/**
+ * \file processor_odom_ICP_yaml.cpp
+ *
+ * Created on: Aug 6, 2019
+ * \author: jsola
+ */
+
+
+// wolf
+#include "laser/processor/processor_odom_icp.h"
+#include "core/common/factory.h"
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// yaml library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+namespace {
+// intrinsics creator
+ProcessorParamsBasePtr createProcessorParamsOdomICP(const std::string& _filename_dot_yaml)
+{
+ WOLF_INFO("ProcessorParamsOdomICP: Parsing file: ", _filename_dot_yaml);
+
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "ODOM ICP")
+ {
+ ProcessorParamsOdomICPPtr params = std::make_shared<ProcessorParamsOdomICP>();
+
+ params->voting_active = config["voting_active"] .as<bool>();
+ params->voting_aux_active = config["voting_aux_active"] .as<bool>();
+ params->time_tolerance = config["time_tolerance"] .as<Scalar>();
+
+ params->min_features_for_keyframe = config["min_features_for_keyframe"] .as<Scalar>();
+ params->max_new_features = config["max_new_features"] .as<SizeEigen >();
+
+ params->use_point_to_line_distance = config["use_point_to_line_distance"] .as<int>();
+ params->max_correspondence_dist = config["max_correspondence_dist"] .as<int>();
+ params->max_iterations = config["max_iterations"] .as<int>();
+ params->use_corr_tricks = config["use_corr_tricks"] .as<int>();
+ params->outliers_maxPerc = config["outliers_maxPerc"] .as<Scalar>();
+ params->outliers_adaptive_order = config["outliers_adaptive_order"] .as<Scalar>();
+ params->outliers_adaptive_mult = config["outliers_adaptive_mult"] .as<Scalar>();
+ params->vfk_min_dist = config["vfk_min_dist"] .as<Scalar>();
+ params->vfk_min_angle = config["vfk_min_angle"] .as<Scalar>();
+ params->vfk_min_time = config["vfk_min_time"] .as<Scalar>();
+ params->vfk_min_error = config["vfk_min_error"] .as<Scalar>();
+ params->vfk_max_points = config["vfk_max_points"] .as<int>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No processor type found." << std::endl;
+ return nullptr;
+}
+
+// register into factory
+const bool WOLF_UNUSED registered_odom_ICP_params = ProcessorParamsFactory::get().registerCreator("ODOM ICP", createProcessorParamsOdomICP);
+
+} // namespace [void]
+} // namespace wolf
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
index d741f365f3c12239074fb5350e4b269d0a23cfc2..ec1ea6692a2c5762563a3ff3bb6dfdf2491ae640 100644
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ b/src/yaml/sensor_laser_2D_yaml.cpp
@@ -24,8 +24,31 @@ IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
{
// If required: Parse YAML
- IntrinsicsLaser2DPtr params; // dummy
- return params;
+ WOLF_INFO("IntrinsicsLaser2D: Parsing file: ", _filename_dot_yaml);
+
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "LASER 2D")
+ {
+ YAML::Node ls_params = config["LaserScanParams"];
+
+ IntrinsicsLaser2DPtr params = std::make_shared<IntrinsicsLaser2D>();
+
+ params->scan_params.angle_min_ = ls_params["angle_min"] .as<double>();
+ params->scan_params.angle_max_ = ls_params["angle_max"] .as<double>();
+ params->scan_params.angle_step_ = ls_params["angle_step"] .as<double>();
+ params->scan_params.scan_time_ = ls_params["scan_time"] .as<double>();
+ params->scan_params.range_min_ = ls_params["range_min"] .as<double>();
+ params->scan_params.range_max_ = ls_params["range_max"] .as<double>();
+ params->scan_params.range_std_dev_ = ls_params["range_std_dev"] .as<double>();
+ params->scan_params.angle_std_dev_ = ls_params["angle_std_dev"] .as<double>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file " << _filename_dot_yaml << ". No processor type found." << std::endl;
+ return nullptr;
+
}
// register into factory
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a973252f84fb6d0a3fce5a02b44540406fbee94d..ab4905e315a6adfcf0e27b85c6692d6e9ba7a3b0 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,8 +16,11 @@ target_link_libraries(gtest_example ${PLUGIN_NAME}) #
wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
target_link_libraries(gtest_landmark_polyline ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
+target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_polyline_2D gtest_polyline_2D.cpp)
target_link_libraries(gtest_polyline_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
# wolf_add_gtest(gtest_processor_tracker_feature_polyline_2D gtest_processor_tracker_feature_polyline_2D.cpp)
-# target_link_libraries(gtest_processor_tracker_feature_polyline_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
\ No newline at end of file
+# target_link_libraries(gtest_processor_tracker_feature_polyline_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..008d80f3f1e417d801dab6d95cc6590a6f4d5319
--- /dev/null
+++ b/test/gtest_processor_odom_icp.cpp
@@ -0,0 +1,187 @@
+/**
+ * \file gtest_processor_odom_icp.cpp
+ *
+ * Created on: Aug 6, 2019
+ * \author: jsola
+ */
+
+
+#include "laser/internal/config.h"
+
+#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/utils/utils_gtest.h>
+
+#include "laser/processor/processor_odom_icp.h" // THIS AT THE END OTHERWISE IT FAILS COMPILING
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+class ProcessorOdomICPPublic : public ProcessorOdomICP
+{
+ public:
+ ProcessorOdomICPPublic(const ProcessorParamsOdomICPPtr& _params) :
+ ProcessorOdomICP(_params)
+ {
+
+ }
+ virtual ~ProcessorOdomICPPublic(){}
+
+ // we can add public methods here to access protected stuff in ProcessorOdomICP
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomICPPublic);
+
+class ProcessorOdomICP_Test : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ CeresManagerPtr solver;
+ SensorLaser2DPtr lidar;
+ ProcessorOdomICPPublicPtr processor;
+
+ std::vector<float> ranges;
+
+ TimeStamp t0;
+ Vector3s x0;
+ Matrix3s P0;
+ FrameBasePtr F0, F; // keyframes
+
+ virtual void SetUp()
+ {
+ problem = Problem::create("PO", 2);
+ solver = std::make_shared<CeresManager>(problem);
+
+ auto sen = problem->installSensor("LASER 2D", "lidar", Eigen::Vector3s(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2D.yaml");
+ lidar = std::static_pointer_cast<SensorLaser2D>(sen);
+
+ auto prc = problem->installProcessor("ODOM ICP", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_ICP.yaml");
+ processor = std::static_pointer_cast<ProcessorOdomICPPublic>(prc);
+
+ ranges = std::vector<float>({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242});
+
+ t0 = 0.0;
+ x0 = Vector3s(0,0,0);
+ P0 . setIdentity();
+ F0 = problem->setPrior(x0, P0, t0, 0.1);
+ }
+ virtual void TearDown(){}
+};
+
+TEST(ProcessorParamsOdomICP, default_construct_and_print)
+{
+ auto params = std::make_shared<ProcessorParamsOdomICP>();
+
+ ASSERT_TRUE(params); // not nullptr
+
+ params->use_corr_tricks = 15;
+
+ ASSERT_EQ(params->use_corr_tricks, 15);
+
+ WOLF_INFO("params: ", params->print());
+}
+
+TEST(ProcessorParamsOdomICP, factory_and_print)
+{
+ auto params = std::static_pointer_cast<ProcessorParamsOdomICP>(ProcessorParamsFactory::get().create("ODOM ICP", laser_root_dir + "/test/yaml/processor_odom_icp.yaml"));
+
+ ASSERT_TRUE(params); // not nullptr
+
+ // check a couple of entries.
+ ASSERT_EQ (params->use_corr_tricks , 4 );
+ ASSERT_DOUBLE_EQ(params->outliers_maxPerc , 5.0 );
+
+ WOLF_INFO("params: ", params->print());
+}
+
+TEST(ProcessorOdomIcp, Constructor)
+{
+ auto params = std::make_shared<ProcessorParamsOdomICP>();
+ auto prc = std::make_shared<ProcessorOdomICP>(params);
+
+ ASSERT_TRUE(prc); // not nullptr
+ ASSERT_EQ(prc->getType(), "ODOM ICP");
+}
+
+TEST(ProcessorOdomIcp, creator_yaml)
+{
+ auto params = std::static_pointer_cast<ProcessorParamsOdomICP>(ProcessorParamsFactory::get().create("ODOM ICP", laser_root_dir + "/test/yaml/processor_odom_ICP.yaml"));
+ auto prc = std::make_shared<ProcessorOdomICP>(params);
+
+ ASSERT_TRUE(prc); // not nullptr
+ ASSERT_EQ(prc->getType(), "ODOM ICP");
+}
+
+TEST_F(ProcessorOdomICP_Test, full)
+{
+
+ TimeStamp t(t0);
+
+
+ for (int i = 0; i < 7; i++)
+ {
+ std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
+
+ CaptureLaser2DPtr scan = std::make_shared<CaptureLaser2D>(t, lidar, ranges);
+ scan->process();
+
+ F = problem->getLastFrame();
+
+ ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6);
+
+ WOLF_TRACE("F", F->id() , " ", F->getState().transpose());
+
+ if (i >= 2 && i <= 4)
+ {
+ // perturb F1
+ F->setState(0.1 * Vector3s::Random());
+ WOLF_TRACE("F", F->id() , " ", F->getState().transpose(), " perturbed!");
+ }
+
+ t += 1.0;
+ }
+
+ ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6);
+}
+
+TEST_F(ProcessorOdomICP_Test, solve)
+{
+ TimeStamp t(t0);
+
+ for (int i = 0; i < 6; i++)
+ {
+ std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
+
+ CaptureLaser2DPtr scan = std::make_shared<CaptureLaser2D>(t, lidar, ranges);
+ scan->process();
+
+ t += 1.0;
+ }
+
+ for (auto F : problem->getTrajectory()->getFrameList())
+ {
+ if (F->isKey())
+ F->setState(0.5 * Vector3s::Random());
+ }
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE(report);
+
+ for (auto F : problem->getTrajectory()->getFrameList())
+ {
+ if (F->isKey())
+ {
+ ASSERT_MATRIX_APPROX(F->getState() , x0 , 1e-6);
+ }
+ }
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "ProcessorOdomICP_Test.solve";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/yaml/processor_odom_ICP.yaml b/test/yaml/processor_odom_ICP.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6cd6081f72f53ae69464900bc4b482170de6a1ba
--- /dev/null
+++ b/test/yaml/processor_odom_ICP.yaml
@@ -0,0 +1,24 @@
+type : "ODOM ICP"
+
+# from processor base
+time_tolerance : 0.1
+voting_active : true
+voting_aux_active : false
+
+# from processor tracker
+min_features_for_keyframe : 0
+max_new_features : 0
+
+# from processor odom ICP
+use_point_to_line_distance : 1
+max_correspondence_dist : 2
+max_iterations : 3
+use_corr_tricks : 4
+outliers_maxPerc : 5
+outliers_adaptive_order : 6
+outliers_adaptive_mult : 7
+vfk_min_dist : 8
+vfk_min_angle : 9
+vfk_min_time : 2.5
+vfk_min_error : 11
+vfk_max_points : 12
diff --git a/test/yaml/sensor_laser_2D.yaml b/test/yaml/sensor_laser_2D.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8f6a26159078ceb368956f4df20cb8b8a6922dca
--- /dev/null
+++ b/test/yaml/sensor_laser_2D.yaml
@@ -0,0 +1,15 @@
+type : "LASER 2D"
+
+# from sensor base
+ # no params so far
+
+# from sensor laser 2D
+LaserScanParams:
+ angle_min : -1.57079994678
+ angle_max : 1.57079994678
+ angle_step : 0.00875097513199
+ scan_time : 0.0
+ range_min : 0.1
+ range_max : 50.0
+ range_std_dev : 0.01
+ angle_std_dev : 0.001