Merge branch 'falko_implementation' into 'devel'

Falko implementation

See merge request !23

.gitlab-ci.yml

@@ -35,6 +35,7 @@ stages:
   - if [ -d wolf ]; then
   -   echo "directory wolf exists"
   -   cd wolf
+  -   git checkout devel
   -   git pull
   -   git checkout $WOLF_CORE_BRANCH
   - else
@@ -344,3 +345,96 @@ build_and_test_csm_falko:bionic:
     - *install_laserscanutils_definition
   script:
     - *build_and_test_definition
+
+############ UBUNTU 20.04 TESTS ############
+build_and_test_none:focal:
+  stage: none
+  image: labrobotica/wolf_deps:20.04
+  cache:
+    - key: wolf-focal
+      paths:
+      - ci_deps/wolf/
+    - key: laserscanutils-focal
+      paths:
+      - ci_deps/laser_scan_utils/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_laserscanutils_definition
+  script:
+    - *build_and_test_definition
+
+build_and_test_csm:focal:
+  stage: csm
+  image: labrobotica/wolf_deps:20.04
+  cache:
+    - key: wolf-focal
+      paths:
+      - ci_deps/wolf/
+    - key: laserscanutils-focal
+      paths:
+      - ci_deps/laser_scan_utils/
+    - key: csm-focal
+      paths:
+      - ci_deps/csm/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_csm_definition
+    - *install_laserscanutils_definition
+  script:
+    - *build_and_test_definition
+
+build_and_test_falko:focal:
+  stage: falko
+  image: labrobotica/wolf_deps:20.04
+  cache:
+    - key: wolf-focal
+      paths:
+      - ci_deps/wolf/
+    - key: laserscanutils-focal
+      paths:
+      - ci_deps/laser_scan_utils/
+    - key: falko-focal
+      paths:
+      - ci_deps/falkolib/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_falko_definition
+    - *install_laserscanutils_definition
+  script:
+    - *build_and_test_definition
+    
+build_and_test_csm_falko:focal:
+  stage: csm_falko
+  image: labrobotica/wolf_deps:20.04
+  cache:
+    - key: wolf-focal
+      paths:
+      - ci_deps/wolf/
+    - key: laserscanutils-focal
+      paths:
+      - ci_deps/laser_scan_utils/
+    - key: csm-focal
+      paths:
+      - ci_deps/csm/
+    - key: falko-focal
+      paths:
+      - ci_deps/falkolib/
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+    - *install_falko_definition
+    - *install_csm_definition
+    - *install_laserscanutils_definition
+  script:
+    - *build_and_test_definition

CMakeLists.txt

@@ -39,8 +39,8 @@ endif()
 
 if(UNIX)
   # GCC is not strict enough by default, so enable most of the warnings.
-  set(CMAKE_CXX_FLAGS
-    "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
+   set(CMAKE_CXX_FLAGS
+     "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
 endif(UNIX)
 
 
@@ -53,7 +53,7 @@ IF(NOT BUILD_DEMOS)
 ENDIF(NOT BUILD_DEMOS)
 
 IF(NOT BUILD_DOC)
-  OPTION(BUILD_DOC "Build Documentation" OFF)
+  OPTION(BUILD_DOC "Build Documentation" ON)
 ENDIF(NOT BUILD_DOC)
 
 
@@ -93,6 +93,7 @@ ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
 FIND_PACKAGE(wolfcore REQUIRED)
 FIND_PACKAGE(laser_scan_utils REQUIRED)
 FIND_PACKAGE(csm QUIET)
+FIND_PACKAGE(falkolib QUIET)
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -173,9 +174,38 @@ SET(SRCS_STATE_BLOCK
 SET(SRCS_YAML
   src/yaml/sensor_laser_2d_yaml.cpp
   )
-  
-#OPTIONALS
+
+#OPTIONAL (falko)
+if (falkolib_FOUND)
+  message("Found Falkolib. Compiling some extra classes.")
+  SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+    include/laser/processor/processor_loop_closure_falko.h
+    )
+    #OPTIONAL (falko & CSM)
+    if (csm_FOUND)
+      SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+        include/laser/processor/processor_loop_closure_falko_icp.h
+        )
+      SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+        src/processor/processor_loop_closure_falko_icp.cpp
+        )
+
+    endif()
+  SET(HDRS_FEATURE ${HDRS_FEATURE}
+    include/laser/feature/feature_scene_falko.h
+    )
+  SET(SRCS_FEATURE ${SRCS_FEATURE}
+    src/feature/feature_scene_falko.cpp
+    )
+
+  SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+    src/processor/processor_loop_closure_falko.cpp
+    )
+endif()
+
+#OPTIONAL (CSM)
 if(csm_FOUND)
+  message("Found CSM. Compiling some extra classes.")
   SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
       include/laser/processor/processor_loop_closure_icp.h
       include/laser/processor/processor_odom_icp.h
@@ -198,6 +228,7 @@ endif(csm_FOUND)
 # create the shared library
 ADD_LIBRARY(${PLUGIN_NAME}
   SHARED
+  ${SRCS_MATH}
   ${SRCS_CAPTURE}
   ${SRCS_FEATURE}
   ${SRCS_LANDMARK}
@@ -209,20 +240,23 @@ ADD_LIBRARY(${PLUGIN_NAME}
   
 # Set compiler options
 # ====================
-if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+IF (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
   message(STATUS "Using C++ compiler clang")
   target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
   # using Clang
-elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+ELSEIF (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   message(STATUS "Using C++ compiler gnu")
   target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
   # using GCC
-endif()
+ENDIF()
 
 #Link the created libraries
 #===============EXAMPLE=========================
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
+IF (falkolib_FOUND)
+	TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES} ${laser_scan_utils_LIBRARY} ${falkolib_LIBRARY})
+ENDIF(falkolib_FOUND)
 
 #Build tests
 #===============EXAMPLE=========================

include/laser/feature/feature_scene_falko.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FEATURE_SCENE_FALKO_H_
+#define FEATURE_SCENE_FALKO_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/feature/feature_base.h"
+#include "laser_scan_utils/scene_falko.h"
+
+// std includes
+
+namespace wolf {
+
+template <typename D> class FeatureSceneFalko : public FeatureBase
+{
+  public:
+    FeatureSceneFalko(const std::shared_ptr<laserscanutils::SceneFalko<D>> _falko_scene)
+        : FeatureBase("FeatureSceneFalko", Eigen::VectorXd::Zero(1), Eigen::MatrixXd::Ones(1, 1))
+    {
+        scene_ = _falko_scene;
+    }
+
+    ~FeatureSceneFalko() override{};
+
+    std::shared_ptr<laserscanutils::SceneFalko<D>> getScene() { return scene_; }
+
+  private:
+    std::shared_ptr<laserscanutils::SceneFalko<D>> scene_;
+};
+
+WOLF_PTR_TYPEDEFS(FeatureSceneFalkoBsc);
+
+class FeatureSceneFalkoBsc : public FeatureSceneFalko<laserscanutils::bsc>
+{
+  public:
+    FeatureSceneFalkoBsc(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::bsc>> _falko_scene)
+        : FeatureSceneFalko(_falko_scene)
+    {
+    }
+    ~FeatureSceneFalkoBsc();
+};
+
+WOLF_PTR_TYPEDEFS(FeatureSceneFalkoCgh);
+
+class FeatureSceneFalkoCgh : public FeatureSceneFalko<laserscanutils::cgh>
+{
+  public:
+    FeatureSceneFalkoCgh(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::cgh>> _falko_scene)
+        : FeatureSceneFalko(_falko_scene)
+    {
+    }
+    ~FeatureSceneFalkoCgh() override{};
+};
+
+} // namespace wolf
+
+#endif

include/laser/processor/processor_loop_closure_falko_icp.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef PROCESSOR_LOOP_CLOSURE_FALKO_ICP_H_
+#define PROCESSOR_LOOP_CLOSURE_FALKO_ICP_H_
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <laser/processor/processor_loop_closure_falko.h>
+#include <laser/feature/feature_icp_align.h>
+
+/**************************
+ *      laseScanUtils - ICP includes      *
+ **************************/
+
+#include <laser_scan_utils/icp.h>
+
+using namespace laserscanutils;
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalkoIcp);
+
+struct ParamsProcessorLoopClosureFalkoIcp : public ParamsProcessorLoopClosureFalko
+{
+
+    // ParameterLoopClosureFalko param;
+    double max_error_threshold;
+    double min_points_percent;
+
+    laserscanutils::icpParams icp_params;
+
+    ParamsProcessorLoopClosureFalkoIcp() = default;
+    ParamsProcessorLoopClosureFalkoIcp(std::string _unique_name, const ParamsServer &_server)
+        : ParamsProcessorLoopClosureFalko(_unique_name, _server)
+    {
+        // param.matcher_ddesc_th_    = _server.getParam<double>(prefix + _unique_name + "/matcher_ddesc_th");
+        // param.keypoints_number_th_ = _server.getParam<int>(prefix + _unique_name + "/keypoints_number_th");
+        max_error_threshold = _server.getParam<double>(prefix + _unique_name + "/max_error_threshold");
+        min_points_percent  = _server.getParam<double>(prefix + _unique_name + "/min_points_percent");
+
+        icp_params.use_point_to_line_distance =
+            _server.getParam<int>(prefix + _unique_name + "/icp/use_point_to_line_distance");
+        icp_params.max_correspondence_dist =
+            _server.getParam<double>(prefix + _unique_name + "/icp/max_correspondence_dist");
+        icp_params.max_iterations   = _server.getParam<int>(prefix + _unique_name + "/icp/max_iterations");
+        icp_params.use_corr_tricks  = _server.getParam<int>(prefix + _unique_name + "/icp/use_corr_tricks");
+        icp_params.outliers_maxPerc = _server.getParam<double>(prefix + _unique_name + "/icp/outliers_maxPerc");
+        icp_params.outliers_adaptive_order =
+            _server.getParam<double>(prefix + _unique_name + "/icp/outliers_adaptive_order");
+        icp_params.outliers_adaptive_mult =
+            _server.getParam<double>(prefix + _unique_name + "/icp/outliers_adaptive_mult");
+        icp_params.do_compute_covariance = _server.getParam<int>(prefix + _unique_name + "/icp/do_compute_covariance");
+        icp_params.max_angular_correction_deg =
+            _server.getParam<double>(prefix + _unique_name + "/icp/max_angular_correction_deg");
+        icp_params.max_linear_correction =
+            _server.getParam<double>(prefix + _unique_name + "/icp/max_linear_correction");
+        icp_params.epsilon_xy    = _server.getParam<double>(prefix + _unique_name + "/icp/epsilon_xy");
+        icp_params.epsilon_theta = _server.getParam<double>(prefix + _unique_name + "/icp/epsilon_theta");
+        icp_params.sigma         = _server.getParam<double>(prefix + _unique_name + "/icp/sigma");
+        icp_params.restart       = _server.getParam<int>(prefix + _unique_name + "/icp/restart");
+        icp_params.restart_threshold_mean_error =
+            _server.getParam<double>(prefix + _unique_name + "/icp/restart_threshold_mean_error");
+        icp_params.restart_dt           = _server.getParam<double>(prefix + _unique_name + "/icp/restart_dt");
+        icp_params.restart_dtheta       = _server.getParam<double>(prefix + _unique_name + "/icp/restart_dtheta");
+        icp_params.clustering_threshold = _server.getParam<double>(prefix + _unique_name + "/icp/clustering_threshold");
+        icp_params.orientation_neighbourhood =
+            _server.getParam<int>(prefix + _unique_name + "/icp/orientation_neighbourhood");
+        icp_params.do_alpha_test = _server.getParam<int>(prefix + _unique_name + "/icp/do_alpha_test");
+        icp_params.do_alpha_test_thresholdDeg =
+            _server.getParam<double>(prefix + _unique_name + "/icp/do_alpha_test_thresholdDeg");
+        icp_params.do_visibility_test = _server.getParam<int>(prefix + _unique_name + "/icp/do_visibility_test");
+        icp_params.outliers_remove_doubles =
+            _server.getParam<int>(prefix + _unique_name + "/icp/outliers_remove_doubles");
+        icp_params.debug_verify_tricks = _server.getParam<int>(prefix + _unique_name + "/icp/debug_verify_tricks");
+        icp_params.gpm_theta_bin_size_deg =
+            _server.getParam<double>(prefix + _unique_name + "/icp/gpm_theta_bin_size_deg");
+        icp_params.gpm_extend_range_deg = _server.getParam<double>(prefix + _unique_name + "/icp/gpm_extend_range_deg");
+        icp_params.gpm_interval         = _server.getParam<double>(prefix + _unique_name + "/icp/gpm_interval");
+        icp_params.min_reading          = _server.getParam<double>(prefix + _unique_name + "/icp/min_reading");
+        icp_params.max_reading          = _server.getParam<double>(prefix + _unique_name + "/icp/max_reading");
+        icp_params.use_ml_weights       = _server.getParam<int>(prefix + _unique_name + "/icp/use_ml_weights");
+        icp_params.use_sigma_weights    = _server.getParam<int>(prefix + _unique_name + "/icp/use_sigma_weights");
+    };
+
+    virtual ~ParamsProcessorLoopClosureFalkoIcp() = default;
+};
+
+/** \brief A class to implement a loop closure processor using loop closure detection from falko library
+ * The Laser data is provided by the sensor owning this processor.
+ *
+ * All the falko loop Closure functionality is implemented in the inherited class
+ *
+ * In this class is only implemented the validation of the falko loop closures with an ICP
+ *
+ * Diferent types of descriptors and matchers can be used and are specified as template parameters.
+ * \tparam D Descriptor type. <bsc> or <cgh>
+ * \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
+ * \tparam M Matcher type <nn_matcher> or <aht_matcher>
+ * \param _param_falko parameter struct with falko lib parameters
+ **/
+
+template <typename D, typename Extr, template <typename, typename> typename L>
+class ProcessorLoopClosureFalkoIcp : public ProcessorLoopClosureFalko<D, Extr, L>
+{
+
+  public:
+    ProcessorLoopClosureFalkoIcp(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+        : ProcessorLoopClosureFalko<D, Extr, L>(_params_falko)
+        , params_falko_icp_(_params_falko){
+
+          };
+
+    ~ProcessorLoopClosureFalkoIcp() = default;
+
+    WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcp, ParamsProcessorLoopClosureFalkoIcp);
+
+  protected:
+    ParamsProcessorLoopClosureFalkoIcpPtr params_falko_icp_;
+    // ICP algorithm
+    std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+
+    /** \brief Function that validates the falko loop closures with an ICP aligment.
+     *
+     * For the ICP alignment it uses the transform provided by the falko algorithm as an initial guess.
+     *
+     * \param match structure that contains scenes and capture pointers that are a match
+     **/
+    bool validateLoopClosure(MatchLoopClosurePtr match) override
+    {
+        auto start_validation = std::chrono::high_resolution_clock::now();
+        auto capture_own      = match->capture_reference; //< Capture reference
+        auto capture_other    = match->capture_target;    // Capture target
+        auto frame_own        = capture_own->getFrame();
+        auto frame_other      = capture_other->getFrame();
+        bool match_valid      = false;
+
+        auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(match);
+
+        if ((capture_own->getType() == "CaptureLaser2d") & (capture_other->getType() == "CaptureLaser2d"))
+            {
+                CaptureLaser2dPtr capture_laser_other = std::static_pointer_cast<CaptureLaser2d>(capture_other);
+                CaptureLaser2dPtr capture_laser_own   = std::static_pointer_cast<CaptureLaser2d>(capture_own);
+
+                SensorLaser2dPtr sensor_own, sensor_other;
+                sensor_own   = std::static_pointer_cast<SensorLaser2d>(capture_laser_own->getSensor());
+                sensor_other = std::static_pointer_cast<SensorLaser2d>(capture_laser_other->getSensor());
+
+                WOLF_INFO("DBG Attempting to close loop with ", frame_own->id(), " and ", frame_other->id());
+                try
+                    {
+                        laserscanutils::icpOutput trf = icp_tools_ptr_->align(
+                            capture_laser_other->getScan(), capture_laser_own->getScan(), sensor_own->getScanParams(),
+                            sensor_other->getScanParams(), params_falko_icp_->icp_params,
+                            match_falko->match_falko_->transform_vector);
+
+                        double points_coeff_used =
+                            ((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(),
+                                                               capture_laser_other->getScan().ranges_raw_.size()));
+                        double          mean_error = trf.error / trf.nvalid;
+                        Eigen::Vector3d displacement_other_own;
+                        displacement_other_own.head(2) =
+                            Eigen::Rotation2Dd(-frame_other->getState().vector("O")(0)) *
+                            (frame_own->getState().vector("P") - frame_other->getState().vector("P"));
+                        displacement_other_own(2) =
+                            frame_own->getState().vector("O")(0) - frame_other->getState().vector("O")(0);
+
+                        Eigen::Vector3d displacement_own_other;
+                        displacement_own_other.head(2) =
+                            Eigen::Rotation2Dd(-frame_own->getState().vector("O")(0)) *
+                            (frame_other->getState().vector("P") - frame_own->getState().vector("P"));
+                        displacement_own_other(2) =
+                            frame_other->getState().vector("O")(0) - frame_own->getState().vector("O")(0);
+
+                        WOLF_INFO("DBG ------------------------------");
+                        WOLF_INFO("DBG valid? ", trf.valid, "\n m_er ", mean_error,
+                                  " \n % points used :", points_coeff_used * 100);
+                        // WOLF_INFO("DBG own_POSE: ", frame_own->getState().vector("PO").transpose(),
+                        //           "\nother_POSE: ", frame_other->getState().vector("PO").transpose(),
+                        // WOLF_INFO("\ndisplacement_other_own : ", displacement_other_own.transpose(),
+                        WOLF_INFO("\ndisplacement_own_other : ", displacement_own_other.transpose(),
+                                  "\nIcp initial guess: ", match_falko->match_falko_->transform_vector.transpose(),
+                                  "\nIcp final transfr: ", trf.res_transf.transpose());
+                        // WOLF_INFO("Covariance \n", trf.res_covar);
+                        if (trf.valid == 1 and mean_error < params_falko_icp_->max_error_threshold and
+                            points_coeff_used * 100 > params_falko_icp_->min_points_percent)
+                            {
+                                WOLF_INFO("Match validated ", frame_own->id(), " and ", frame_other->id());
+                                match_valid                                 = true;
+                                match_falko->match_falko_->transform_vector = trf.res_transf;
+                                this->num_ok_ += 1;
+                                this->duration_validation_ok += std::chrono::duration_cast<std::chrono::microseconds>(
+                                    std::chrono::high_resolution_clock::now() - start_validation);
+                            }
+                        else
+                            {
+                                WOLF_INFO("Match DISCARDED ", frame_own->id(), " and ", frame_other->id());
+                                this->num_nok_ += 1;
+                                this->duration_validation_nok += std::chrono::duration_cast<std::chrono::microseconds>(
+                                    std::chrono::high_resolution_clock::now() - start_validation);
+                            }
+                    }
+                catch (std::exception &e)
+                    {
+                        this->num_nok_ += 1;
+                        WOLF_WARN(e.what())
+                    }
+            }
+        return match_valid;
+    }
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpNnBsc
+    : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoNn>
+{
+  public:
+    ProcessorLoopClosureFalkoIcpNnBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+        : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+    WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnBsc, ParamsProcessorLoopClosureFalkoIcp);
+
+    ~ProcessorLoopClosureFalkoIcpNnBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpAhtBsc
+    : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoAht>
+{
+  public:
+    ProcessorLoopClosureFalkoIcpAhtBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+        : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+    WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtBsc, ParamsProcessorLoopClosureFalkoIcp);
+
+    ~ProcessorLoopClosureFalkoIcpAhtBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnCgh);
+
+/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpNnCgh
+    : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoNn>
+{
+  public:
+    ProcessorLoopClosureFalkoIcpNnCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+        : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+    WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnCgh, ParamsProcessorLoopClosureFalkoIcp);
+
+    ~ProcessorLoopClosureFalkoIcpNnCgh() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtCgh);
+
+/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoIcpAhtCgh
+    : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoAht>
+{
+  public:
+    ProcessorLoopClosureFalkoIcpAhtCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+        : ProcessorLoopClosureFalkoIcp(_params_falko){};
+
+    WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtCgh, ParamsProcessorLoopClosureFalkoIcp);
+
+    ~ProcessorLoopClosureFalkoIcpAhtCgh() = default;
+};
+
+} // namespace wolf
+
+#endif // SRC_PROCESSOR_LOOP_CLOSURE_FALKO_ICP_H_

include/laser/processor/processor_loop_closure_icp.h

@@ -26,17 +26,17 @@
  *      WOLF includes     *
  **************************/
 #include "core/common/wolf.h"
-#include <core/processor/processor_loop_closure.h>
-#include <laser/capture/capture_laser_2d.h>
+#include "core/processor/processor_loop_closure.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
 
-#include <core/factor/factor_relative_pose_2d_with_extrinsics.h>
-#include <laser/feature/feature_icp_align.h>
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_icp_align.h"
 
 /**************************
  *      ICP includes      *
  **************************/
-#include <laser_scan_utils/laser_scan_utils.h>
-#include <laser_scan_utils/icp.h>
+#include "laser_scan_utils/laser_scan_utils.h"
+#include "laser_scan_utils/icp.h"
 
 namespace wolf
 {
@@ -45,8 +45,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
 
 struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure
 {
-    int recent_frames_ignored;
-    int frames_to_wait;
+    int recent_frames_ignored, frames_ignored_after_loop;
     double max_error_threshold;
     double min_points_percent;
 
@@ -58,20 +57,18 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure
         ParamsProcessorLoopClosure(_unique_name, _server)
     {
         recent_frames_ignored     = _server.getParam<int>    (prefix + _unique_name + "/recent_frames_ignored");
-        frames_to_wait            = _server.getParam<int>    (prefix + _unique_name + "/frames_to_wait");
+        frames_ignored_after_loop = _server.getParam<int>    (prefix + _unique_name + "/frames_ignored_after_loop");
         max_error_threshold       = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
         min_points_percent        = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
 
-        icp_params.cov_factor                 = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
-
-        icp_params.use_point_to_line_distance = _server.getParam<int>    (prefix + _unique_name + "/use_point_to_line_distance");
-        icp_params.max_correspondence_dist    = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
-        icp_params.max_iterations             = _server.getParam<int>    (prefix + _unique_name + "/max_iterations");
-        icp_params.use_corr_tricks            = _server.getParam<int>    (prefix + _unique_name + "/use_corr_tricks");
-        icp_params.outliers_maxPerc           = _server.getParam<double> (prefix + _unique_name + "/outliers_maxPerc");
-        icp_params.outliers_adaptive_order    = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_order");
-        icp_params.outliers_adaptive_mult     = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_mult");
-
+        icp_params.use_point_to_line_distance   = _server.getParam<int>    (prefix + _unique_name + "/use_point_to_line_distance");
+        icp_params.max_correspondence_dist      = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
+        icp_params.max_iterations               = _server.getParam<int>    (prefix + _unique_name + "/max_iterations");
+        icp_params.use_corr_tricks              = _server.getParam<int>    (prefix + _unique_name + "/use_corr_tricks");
+        icp_params.outliers_maxPerc             = _server.getParam<double> (prefix + _unique_name + "/outliers_maxPerc");
+        icp_params.outliers_adaptive_order      = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_order");
+        icp_params.outliers_adaptive_mult       = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_mult");
+        icp_params.cov_factor                   = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
         icp_params.do_compute_covariance        = _server.getParam<int>    (prefix + _unique_name + "/do_compute_covariance");
         icp_params.max_angular_correction_deg   = _server.getParam<double> (prefix + _unique_name + "/max_angular_correction_deg");
         icp_params.max_linear_correction        = _server.getParam<double> (prefix + _unique_name + "/max_linear_correction");
@@ -92,26 +89,29 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure
         icp_params.gpm_theta_bin_size_deg       = _server.getParam<double> (prefix + _unique_name + "/gpm_theta_bin_size_deg");
         icp_params.gpm_extend_range_deg         = _server.getParam<double> (prefix + _unique_name + "/gpm_extend_range_deg");
         icp_params.gpm_interval                 = _server.getParam<double> (prefix + _unique_name + "/gpm_interval");
-
-        icp_params.min_reading       = _server.getParam<double> (prefix + _unique_name + "/min_reading");
-        icp_params.max_reading       = _server.getParam<double> (prefix + _unique_name + "/max_reading");
-        icp_params.use_ml_weights    = _server.getParam<int>    (prefix + _unique_name + "/use_ml_weights");
-        icp_params.use_sigma_weights = _server.getParam<int>    (prefix + _unique_name + "/use_sigma_weights");
+        icp_params.min_reading                  = _server.getParam<double> (prefix + _unique_name + "/min_reading");
+        icp_params.max_reading                  = _server.getParam<double> (prefix + _unique_name + "/max_reading");
+        icp_params.use_ml_weights               = _server.getParam<int>    (prefix + _unique_name + "/use_ml_weights");
+        icp_params.use_sigma_weights            = _server.getParam<int>    (prefix + _unique_name + "/use_sigma_weights");
     }
     std::string print() const override
     {
-        return "\n" + ParamsProcessorLoopClosure::print()
+        return "\n" + ParamsProcessorBase::print()
             + "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
-            + "frames_to_wait: "        + std::to_string(frames_to_wait) + "\n"
-            + "max_error_threshold: "   + std::to_string(max_error_threshold) + "\n"
-            + "min_points_percent: "    + std::to_string(min_points_percent) + "\n";
+            + "frames_ignored_after_loop: " + std::to_string(frames_ignored_after_loop) + "\n"
+            + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
+            + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
     }
 };
 
-WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureIcp);
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
 struct MatchLoopClosureIcp : public MatchLoopClosure
 {
-    laserscanutils::icpOutput align_result;
+    laserscanutils::icpOutput icp_result;
 };
 
 class ProcessorLoopClosureIcp : public ProcessorLoopClosure
@@ -123,37 +123,45 @@ class ProcessorLoopClosureIcp : public ProcessorLoopClosure
 
         //Closeloop parameters
         ParamsProcessorLoopClosureIcpPtr params_loop_closure_icp_;
-        int frames_skipped_;
 
         // ICP algorithm
         std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
 
+        MatchLoopClosurePtr last_loop_closure_;
+
     public:
         ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params);
 
         WOLF_PROCESSOR_CREATE(ProcessorLoopClosureIcp, ParamsProcessorLoopClosureIcp);
         void configure(SensorBasePtr _sensor) override;
 
+        ParamsProcessorLoopClosureIcpPtr getParams() const;
+
     protected:
 
         /** \brief Returns if findLoopClosures() has to be called for the given capture
-         * Since there is no enough information in the capture to decide if a loop can be closed, try always.
+         *
+         * This case avoids searching if a recent loop closure
          */
         bool voteFindLoopClosures(CaptureBasePtr cap) override;
 
         /** \brief detects and emplaces all features of the given capture
-         * Since the loop closures of this processor are not based on any features, it is empty
+         *
+         * In this case, ICP does not work with features to find a loop closure.
+         * A feature will be added when emplacing the factors.
+         *
          */
-        void emplaceFeatures(CaptureBasePtr cap) override{};
+        void emplaceFeatures(CaptureBasePtr cap) override {/*nothing*/};
 
         /** \brief Find captures that correspond to loop closures with the given capture
          */
-        std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
+        std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
+
+        MatchLoopClosurePtr matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_target) const;
 
         /** \brief validates a loop closure
-         * Loops are validated just after calling ICP
          */
-        bool validateLoopClosure(MatchLoopClosurePtr) override{ return true;};
+        bool validateLoopClosure(MatchLoopClosurePtr) override {return true;};
 
         /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
          */

src/feature/feature_scene_falko.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/feature/feature_scene_falko.h"
+
+namespace wolf
+{
+
+} // namespace wolf

src/processor/processor_loop_closure_falko.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/processor_loop_closure_falko.h"
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoNnBsc);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoNnBsc);
+
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoNnCgh);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoNnCgh);
+
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoAhtBsc);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoAhtBsc);
+
+// WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoAhtCgh);
+// WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoAhtCgh);
+} // namespace wolf
\ No newline at end of file

src/processor/processor_loop_closure_falko_icp.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "laser/processor/processor_loop_closure_falko_icp.h"
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+    WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpNnBsc);
+    WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpNnBsc);
+
+    WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpAhtBsc);
+    WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpAhtBsc);
+
+    WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpNnCgh);
+    WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpNnCgh);
+
+    WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureFalkoIcpAhtCgh);
+    WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureFalkoIcpAhtCgh);
+
+} // namespace wolf
\ No newline at end of file

src/processor/processor_loop_closure_icp.cpp

@@ -23,15 +23,17 @@
 #include "laser/sensor/sensor_laser_2d.h"
 #include "core/math/covariance.h"
 
-namespace wolf
-{
+using namespace laserscanutils;
+using namespace Eigen;
+
+namespace wolf{
 
 ProcessorLoopClosureIcp::ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params) :
     ProcessorLoopClosure("ProcessorLoopClosureIcp", 2, _params),
     params_loop_closure_icp_(_params),
-    frames_skipped_(0)
+    last_loop_closure_(nullptr)
 {
-    icp_tools_ptr_ = std::make_shared<laserscanutils::ICP>();
+    icp_tools_ptr_ = std::make_shared<ICP>();
 }
 
 void ProcessorLoopClosureIcp::configure(SensorBasePtr _sensor)
@@ -40,141 +42,177 @@ void ProcessorLoopClosureIcp::configure(SensorBasePtr _sensor)
     laser_scan_params_  = sensor_laser_->getScanParams();
 }
 
+ParamsProcessorLoopClosureIcpPtr ProcessorLoopClosureIcp::getParams() const
+{
+    return params_loop_closure_icp_;
+}
+
 bool ProcessorLoopClosureIcp::voteFindLoopClosures(CaptureBasePtr cap)
 {
-    // process 1 and skip 'frames_to_wait' frames
-    if (frames_skipped_ >= params_loop_closure_icp_->frames_to_wait)
-    {
-        frames_skipped_=0;
+    if (not last_loop_closure_)
+        return true;
+
+    if (params_loop_closure_icp_->frames_ignored_after_loop <= 0)
         return true;
+
+    auto last_frm = last_loop_closure_->capture_target->getFrame();
+    for (auto i = 1; i <= params_loop_closure_icp_->frames_ignored_after_loop; i++)
+    {
+        last_frm = last_frm->getNextFrame();
+        if (not last_frm or last_frm == cap->getFrame())
+        {
+            return false;
+        }
     }
-    frames_skipped_++;
-    return false;
+
+    return true;
 }
 
-std::map<double, MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(CaptureBasePtr _capture)
+std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(CaptureBasePtr _capture)
 {
-    assert(std::dynamic_pointer_cast<CaptureLaser2d>(_capture));
-    std::map<double, MatchLoopClosurePtr> match_map;
+    std::map<double,MatchLoopClosurePtr> match_lc_map;
 
-    auto cap_own = std::static_pointer_cast<CaptureLaser2d>(_capture);
-    auto frame_own = _capture->getFrame();
-    auto frame_ref = frame_own->getPreviousFrame();
-    auto frame_distance = 1;
+    auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(_capture);
+    assert(cap_laser != nullptr);
 
-    while (frame_ref)
+    // Ignore the most recent 'recent_frames_ignored' frames
+    const auto &trajectory = getProblem()->getTrajectory();
+    int n_ignored = 0;
+    for(auto frm_it  = trajectory->rbegin();
+             frm_it != trajectory->rend();
+             frm_it++)
     {
-        // not too recent frames
-        if (frame_distance > params_loop_closure_icp_->recent_frames_ignored)
+        if (frm_it->second == _capture->getFrame())
+            continue;
+
+        if (n_ignored < params_loop_closure_icp_->recent_frames_ignored)
+        {
+            n_ignored++;
+            continue;
+        }
+
+        auto cap_ref_list = frm_it->second->getCapturesOfType<CaptureLaser2d>();
+        for (auto cap_ref : cap_ref_list)
         {
-            // search CaptureLaser2d
-            for (auto cap : frame_ref->getCaptureList())
+            auto cap_ref_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap_ref);
+            if (cap_ref_laser != nullptr)
             {
-                auto cap_ref = std::static_pointer_cast<wolf::CaptureLaser2d>(cap);
-                // try to match if no too far  (more than laser range)
-                if (cap_ref and
-                    (frame_ref->getP()->getState() - frame_own->getP()->getState()).norm() < laser_scan_params_.range_max_)
+                // Match Scans
+                auto match = matchScans(cap_ref_laser, cap_laser);
+
+                // IT'S A MATCH!
+                if (match != nullptr)
                 {
-                    // Initial guess for alignment
-                    Eigen::Vector3d transform_guess;
-                    Eigen::Isometry2d T_w_rown, T_w_rref, T_r_s, T_sref_sown;
-
-                    // Transformations
-                    T_w_rown = Eigen::Translation2d(frame_own->getP()->getState()) *
-                                                    Eigen::Rotation2Dd(frame_own->getO()->getState()(0));
-                    T_w_rref = Eigen::Translation2d(frame_ref->getP()->getState()) *
-                                                      Eigen::Rotation2Dd(frame_ref->getO()->getState()(0));
-                    T_r_s = Eigen::Translation2d(this->getSensor()->getP()->getState()) * Eigen::Rotation2Dd(this->getSensor()->getO()->getState()(0));
-                    T_sref_sown = T_r_s.inverse() * T_w_rref.inverse() * T_w_rown * T_r_s;
-
-                    // Fill up initial guess
-                    transform_guess.head(2) = T_sref_sown.translation();
-                    Eigen::Rotation2Dd R(T_sref_sown.rotation());
-                    transform_guess(2) = R.angle();
-
-                    SensorLaser2dPtr sensor_own, sensor_ref;
-                    sensor_own = std::static_pointer_cast<wolf::SensorLaser2d>(cap_own->getSensor());
-                    sensor_ref = std::static_pointer_cast<wolf::SensorLaser2d>(cap_ref->getSensor());
-
-                    // WOLF_DEBUG("LOOP CLOSURE: Aligning key frames: ", frame_own->id(), " and ", frame_ref->id(), "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
-                    // Finally align
-                    // WOLF_DEBUG("Sensor own scan params\n");
-                    // sensor_own->getScanParams().print();
-                    // WOLF_DEBUG("Sensor ref scan params\n");
-                    // sensor_ref->getScanParams().print();
-                    // WOLF_DEBUG("Icp params\n");
-                    // WOLF_DEBUG("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
-                    //           "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance,
-                    //           "\n max_iterations: ", this->icp_params_.max_iterations,
-                    //           "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
-                    //           "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order,
-                    //           "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
-                    WOLF_DEBUG("DBG Attempting to close loop with ", frame_own->id(), " and ", frame_ref->id());
-                    try
-                    {
-                        laserscanutils::icpOutput trf =
-                                icp_tools_ptr_->align(cap_own->getScan(),
-                                                      cap_ref->getScan(),
-                                                      sensor_own->getScanParams(),
-                                                      sensor_ref->getScanParams(),
-                                                      params_loop_closure_icp_->icp_params,
-                                                      transform_guess);
-
-                        double points_coeff_used = ((double)trf.nvalid / (double)fmin(cap_own->getScan().ranges_raw_.size(),
-                                                                                      cap_ref->getScan().ranges_raw_.size()));
-                        double mean_error = trf.error / trf.nvalid;
-                        WOLF_DEBUG("DBG ------------------------------");
-                        WOLF_DEBUG("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", frame_own->id(),
-                                   " ref_id: ", frame_ref->id());
-                        WOLF_DEBUG("DBG own_POSE: ", frame_own->getState().vector("PO").transpose(), " ref_POSE: ", frame_ref->getState().vector("PO").transpose(),
-                                   " Icp_guess: ", transform_guess.transpose(), " Icp_trf: ", trf.res_transf.transpose());
-
-                        // WOLF_DEBUG("Covariance \n", trf.res_covar);
-                        if (trf.valid == 1 and
-                            mean_error < params_loop_closure_icp_->max_error_threshold and
-                            points_coeff_used * 100 > params_loop_closure_icp_->min_points_percent)
-                        {
-                            WOLF_DEBUG("DBG ADDED ", frame_own->id(), " and ", frame_ref->id());
-
-                            auto match = std::make_shared<MatchLoopClosureIcp>();
-                            match->capture_reference    = cap_ref;
-                            match->capture_target       = cap_own;
-                            match->normalized_score     = 1 - mean_error / params_loop_closure_icp_->max_error_threshold;
-                            match->align_result         = trf;
-
-                            match_map.emplace(match->normalized_score, match);
-                        }
-                        else
-                        {
-                            WOLF_DEBUG("DBG DISCARDED ", frame_own->id(), " and ", frame_ref->id());
-                        }
-
-                    }
-                    catch (std::exception &e)
-                    {
-                        WOLF_WARN("ProcessorLoopClosureIcp: ICP failed with error: ", e.what());
-                    }
+                    // avoid duplicated scores (std::map)
+                    while (match_lc_map.count(match->normalized_score))
+                        match->normalized_score -= 1e-9;
+
+                    match_lc_map.emplace(match->normalized_score, match);
                 }
             }
         }
-        frame_ref = frame_ref->getPreviousFrame();
-        frame_distance++;
     }
 
-    return match_map;
+    return match_lc_map;
 }
 
-void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr _match)
+MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_tar) const
 {
-    auto match_icp = std::static_pointer_cast<MatchLoopClosureIcp>(_match);
+    auto frm_tar = cap_tar->getFrame();
+    auto frm_ref = cap_ref->getFrame();
+    auto sen_tar = std::static_pointer_cast<wolf::SensorLaser2d>(cap_tar->getSensor());
+    auto sen_ref = std::static_pointer_cast<wolf::SensorLaser2d>(cap_ref->getSensor());
+
+    // INITIAL GUESS
+    // Transformations
+    Isometry2d T_w_r_tar = Translation2d(frm_tar->getP()->getState()) * Rotation2Dd(frm_tar->getO()->getState()(0));
+    Isometry2d T_w_r_ref = Translation2d(frm_ref->getP()->getState()) * Rotation2Dd(frm_ref->getO()->getState()(0));
+    Isometry2d T_r_s_tar = Translation2d(sen_tar->getP()->getState()) * Rotation2Dd(sen_tar->getO()->getState()(0));
+    Isometry2d T_r_s_ref = Translation2d(sen_ref->getP()->getState()) * Rotation2Dd(sen_ref->getO()->getState()(0));
+    Isometry2d T_s_ref_s_tar = T_r_s_ref.inverse() * T_w_r_ref.inverse() * T_w_r_tar * T_r_s_tar;
+
+    // Fill up initial guess
+    Vector3d transform_guess;
+    transform_guess.head(2) = T_s_ref_s_tar.translation();
+    Rotation2Dd R(T_s_ref_s_tar.rotation());
+    transform_guess(2) = R.angle();
+
+    // WOLF_DEBUG("LOOP CLOSURE: Aligning key frames: ", _keyframe_tar->id(), " and ", frm_ref->id(), "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+    // WOLF_DEBUG("Sensor own scan params\n");
+    // sen_tar->getScanParams().print();
+    // WOLF_DEBUG("Sensor other scan params\n");
+    // sensor_ref->getScanParams().print();
+    // WOLF_DEBUG("Icp params\n");
+    // WOLF_DEBUG("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
+    //           "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance,
+    //           "\n max_iterations: ", this->icp_params_.max_iterations,
+    //           "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
+    //           "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order,
+    //           "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
+    WOLF_DEBUG("DBG Attempting to close loop with ", frm_tar->id(), " and ", frm_ref->id());
+    try
+    {
+        icpOutput icp_out = icp_tools_ptr_->align(cap_tar->getScan(),
+                                                  cap_ref->getScan(),
+                                                  sen_tar->getScanParams(),
+                                                  sen_ref->getScanParams(),
+                                                  params_loop_closure_icp_->icp_params,
+                                                  transform_guess);
+
+        double points_coeff_used = ((double)icp_out.nvalid / (double)fmin(cap_tar->getScan().ranges_raw_.size(),
+                                                                          cap_ref->getScan().ranges_raw_.size()));
+        double mean_error = icp_out.error / icp_out.nvalid;
+
+        WOLF_DEBUG("DBG ------------------------------");
+        WOLF_DEBUG("DBG valid? ", trf.valid,
+                   " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", frm_tar->id(),
+                   " other_id: ", frm_ref->id());
+        WOLF_DEBUG("DBG own_POSE: ", frm_tar->getState().vector("PO").transpose(),
+                   " other_POSE: ", frm_ref->getState().vector("PO").transpose(),
+                   " Icp_guess: ", transform_guess.transpose(),
+                   " Icp_trf: ", icp_out.res_transf.transpose());
+        // WOLF_DEBUG("Covariance \n", icp_out.res_covar);
+
+        // Valid output
+        if (icp_out.valid == 1 and
+            mean_error < params_loop_closure_icp_->max_error_threshold and
+            points_coeff_used * 100 > params_loop_closure_icp_->min_points_percent)
+        {
+            WOLF_DEBUG("DBG MATCH CONFIRMED ", frm_tar->id(), " and ", frm_ref->id());
+
+            auto match = std::make_shared<MatchLoopClosureIcp>();
+            match->capture_reference    = cap_ref;
+            match->capture_target       = cap_tar;
+            match->normalized_score     = points_coeff_used; // ratio points normalized (0, 1]
+            //match->normalized_score_ = exp(-mean_error); // error normalized (0, 1]
+            match->icp_result           = icp_out;
+
+            return match;
+        }
+        else
+        {
+            WOLF_DEBUG("DBG DISCARDED ", frm_tar->id(), " and ", _keyframe_ref->id());
+            return nullptr;
+        }
+
+    }
+    catch (std::exception &e)
+    {
+        WOLF_WARN("ProcessorLoopClosureIcp::matchScans ICP failed with output: ", e.what())
+    }
+    return nullptr;
+}
 
-    assert(match_icp->align_result.valid == 1);
+void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr match)
+{
+    auto match_icp = std::static_pointer_cast<MatchLoopClosureIcp>(match);
 
-    if (not isCovariance(match_icp->align_result.res_covar))
-        match_icp->align_result.res_covar = 1e-4 * Eigen::Matrix3d::Identity();
+    assert(match_icp->icp_result.valid == 1);
+    if (not isCovariance(match_icp->icp_result.res_covar))
+        match_icp->icp_result.res_covar = 1e-4 * Matrix3d::Identity();
 
     auto ftr = FeatureBase::emplace<FeatureICPAlign>(match_icp->capture_target,
-                                                     match_icp->align_result);
+                                                     match_icp->icp_result);
 
     FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr,
                                                             ftr,
@@ -182,6 +220,9 @@ void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr _match)
                                                             shared_from_this(),
                                                             params_->apply_loss_function,
                                                             TOP_LOOP);
+
+    // store last frame
+    last_loop_closure_ = match;
 }
 
 }

src/processor/processor_odom_icp.cpp

@@ -201,24 +201,10 @@ void ProcessorOdomIcp::advanceDerived()
 {
     using namespace Eigen;
 
-    // overwrite last frame's state
-
-    auto  w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
-    auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
-    const auto& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
     odom_last_ = odom_incoming_;
 
-
-    // incoming
-
-    auto       so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
-    Isometry2d  w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
-    Vector3d x_inc; x_inc << w_T_ri.translation() , Rotation2Dd(w_T_ri.rotation()).angle();
-
-    // Put the state of incoming in last
-    // last_ptr_->getFrame()->setState(x_inc, "PO", {2,1});
-
     // computing odometry
+    auto       so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
     auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
     Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
     odometry_['P'] = sl_T_si.translation();
@@ -230,36 +216,15 @@ void ProcessorOdomIcp::advanceDerived()
 
 void ProcessorOdomIcp::resetDerived()
 {
-    // WOLF_DEBUG("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
-    if (origin_ptr_ != nullptr and last_ptr_ != nullptr)
-    {
-        //
-    }
-
     // Using processing_step_ to ensure that origin, last and incoming are different
-    if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME) {
+    if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME)
+    {
         // 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
 
-
-        auto  w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
-        auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
-        const auto &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
-
-        // incoming
-
-        auto      so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
-        Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
-        Vector3d x_current;
-        x_current << w_T_ri.translation(), Rotation2Dd(w_T_ri.rotation()).angle();
-
-        // last
-
-        // last_ptr_->getFrame()->setState(x_current, "PO", {2,1});
-        // incoming_ptr_->getFrame()->setState(x_current);
-
         // computing odometry
+        auto       so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
         auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
         Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
         odometry_['P'] = sl_T_si.translation();
@@ -321,11 +286,8 @@ VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) c
     auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
     const auto& rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead
 
-    // incoming
-
     auto      so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
     Isometry2d w_T_rl = w_T_ro * ro_T_so * so_T_sl * rl_T_sl.inverse();
-    Vector3d x_current;
 
     VectorComposite state("PO", {w_T_rl.translation(), Vector1d(Rotation2Dd(w_T_rl.rotation()).angle())});
 
@@ -344,7 +306,7 @@ VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStru
 {
     // todo fix this code to get any state in the whole trajectory!
 
-
+    //
     if (last_ptr_ != nullptr)
     {
         double d = fabs(_ts - last_ptr_->getTimeStamp());
@@ -358,8 +320,9 @@ VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStru
             return getProblem()->stateZero("PO"); // return zero
         }
     }
-
-    else { // return state at origin if possible
+    // return state at origin if possible
+    else
+    {
         if (origin_ptr_ == nullptr || fabs(_ts - last_ptr_->getTimeStamp()) > params_->time_tolerance)
             return VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
         else

test/CMakeLists.txt

 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-
 # Include gtest directory.
 include_directories(${GTEST_INCLUDE_DIRS})
 
@@ -17,12 +16,22 @@ wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
 target_link_libraries(gtest_landmark_polyline ${PLUGIN_NAME})
 
 if(csm_FOUND)
-  wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
-  target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME})
+	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_processor_loop_closure_icp gtest_processor_loop_closure_icp.cpp)
+	target_link_libraries(gtest_processor_loop_closure_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
 endif(csm_FOUND)
 
 wolf_add_gtest(gtest_polyline_2d gtest_polyline_2d.cpp)
 target_link_libraries(gtest_polyline_2d ${PLUGIN_NAME})
 
-# 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})
+if(falkolib_FOUND)
+	wolf_add_gtest(gtest_processor_loop_closure_falko gtest_processor_loop_closure_falko.cpp)
+	target_link_libraries(gtest_processor_loop_closure_falko ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+	if(csm_FOUND)
+    	wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp)
+    	target_link_libraries(gtest_processor_loop_closure_falko_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+  	endif(csm_FOUND)
+endif(falkolib_FOUND)

test/params1.yaml

-config:
-  sensors: 
-    -
-      type: "ODOM 2d"
-      name: "odom"
-      intrinsic:
-        k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
-      extrinsic:
-        pos: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
-  processors:
-    -
-      type: "ODOM 2d"
-      name: "processor1"
-      sensor name: "odom"
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor name: "rb"
-    -
-      type: "ODOM 2d"
-      name: "my_proc_test"
-      sensor name: "odom"
-      follow: "/test/params3.1.yaml"
-files:
-  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
-  - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file

test/params2.yaml

-config:
-  sensors: 
-    -
-      type: "ODOM 2d"
-      name: "odom"
-      intrinsic:
-        k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
-      extrinsic:
-        pos: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
-  processors:
-    -
-      type: "ODOM 2d"
-      name: "processor1"
-      sensor name: "odom"
-      $mymap:
-        k1: v1
-        k2: v2
-        k3: [v3,v4,v5]
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor name: "rb"
-    -
-      type: "ODOM 2d"
-      name: "my_proc_test"
-      sensor name: "odom"
-files:
-  - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
-  - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file

test/params3.1.yaml

-cov_det: 100
-unmeasured_perturbation_std: 100
-angle_turned: 100
-dist_traveled: 100
-max_buff_length: 100
-max_time_span: 100
-time_tolerance: 100
-voting_active: false 
\ No newline at end of file