diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 88b7ee732f0294ece7d258692e599e6483ee5b5a..d492ed52fa980e3be84bb34201142fd51f047a57 100644
--- a/.gitlab-ci.yml
+++ b/.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
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 846ef85bd5640f895a8e797e32fdff45d4e035f9..b16e4964e99fd5092e512860d3edca8cfa5bc4ef 100644
--- a/CMakeLists.txt
+++ b/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=========================
diff --git a/include/laser/feature/feature_scene_falko.h b/include/laser/feature/feature_scene_falko.h
new file mode 100644
index 0000000000000000000000000000000000000000..6cd1b5fb232acaf627fe37eec4059cefceb19d15
--- /dev/null
+++ b/include/laser/feature/feature_scene_falko.h
@@ -0,0 +1,77 @@
+//--------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
diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7f9d6085d7e1b3ec493ad1ee79b43502e6a6cfb
--- /dev/null
+++ b/include/laser/processor/processor_loop_closure_falko.h
@@ -0,0 +1,677 @@
+//--------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_H_
+#define PROCESSOR_LOOP_CLOSURE_FALKO_H_
+
+#include <iostream>
+#include <fstream>
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/common/wolf.h"
+#include "core/processor/processor_loop_closure.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/utils/graph_search.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "laser/feature/feature_scene_falko.h"
+
+/**************************
+ * laseScanUtils includes *
+ **************************/
+#include "laser_scan_utils/loop_closure_falko.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureFalko);
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosureFalko : public MatchLoopClosure
+{
+ laserscanutils::MatchLoopClosureScenePtr match_falko_;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalko);
+
+/** \brief Struct class that store falkolib parameters and processorBase parameters
+ **/
+struct ParamsProcessorLoopClosureFalko : public ParamsProcessorLoopClosure
+{
+ int min_keypoints;
+ double score_th;
+ double distance_between_ref_frm;
+ int near_matches_th;
+ bool logging;
+ double th_kp_area;
+ double th_kp_perimeter;
+ double th_kp_maxd;
+ double th_kp_eig;
+ double th_scan_area;
+ double th_scan_eig;
+ std::string logging_filepath;
+
+ laserscanutils::ParameterLoopClosureFalko param;
+
+ ParamsProcessorLoopClosureFalko() = default;
+ ParamsProcessorLoopClosureFalko(std::string _unique_name, const ParamsServer &_server)
+ : ParamsProcessorLoopClosure(_unique_name, _server)
+ {
+
+ // processor params
+ min_keypoints = _server.getParam<int>(prefix + _unique_name + "/min_keypoints");
+ score_th = _server.getParam<double>(prefix + _unique_name + "/score_th");
+ distance_between_ref_frm = _server.getParam<double>(prefix + _unique_name + "/distance_between_ref_frm");
+ near_matches_th = _server.getParam<int>(prefix + _unique_name + "/near_matches_th");
+ th_kp_area = _server.getParam<double>(prefix + _unique_name + "/th_kp_area");
+ th_kp_perimeter = _server.getParam<double>(prefix + _unique_name + "/th_kp_perimeter");
+ th_kp_maxd = _server.getParam<double>(prefix + _unique_name + "/th_kp_maxd");
+ th_kp_eig = _server.getParam<double>(prefix + _unique_name + "/th_kp_eig");
+ th_scan_area = _server.getParam<double>(prefix + _unique_name + "/th_scan_area");
+ th_scan_eig = _server.getParam<double>(prefix + _unique_name + "/th_scan_eig");
+ logging = _server.getParam<bool>(prefix + _unique_name + "/logging");
+ if (logging)
+ logging_filepath = _server.getParam<std::string>(prefix + _unique_name + "/logging_filepath");
+
+ // Falko
+ param.matcher_distance_th_ = _server.getParam<double>(prefix + _unique_name + "/falko/matcher_distance_th");
+ param.use_descriptors_ = _server.getParam<bool>(prefix + _unique_name + "/falko/use_descriptors");
+ param.circularSectorNumber_ = _server.getParam<int>(prefix + _unique_name + "/falko/circularSectorNumber");
+ param.radialRingNumber_ = _server.getParam<int>(prefix + _unique_name + "/falko/radialRingNumber");
+ param.matcher_ddesc_th_ = _server.getParam<double>(prefix + _unique_name + "/falko/matcher_ddesc_th");
+ param.keypoints_number_th_ = _server.getParam<int>(prefix + _unique_name + "/falko/keypoints_number_th");
+ // Keypoints extractor params
+ param.enable_subbeam_ = _server.getParam<bool>(prefix + _unique_name + "/falko/enable_subbeam");
+ param.nms_radius_ = _server.getParam<double>(prefix + _unique_name + "/falko/nms_radius");
+ param.neigh_b_ = _server.getParam<double>(prefix + _unique_name + "/falko/neigh_b");
+ param.b_ratio_ = _server.getParam<double>(prefix + _unique_name + "/falko/b_ratio");
+ // Aht matcher params
+ param.xRes_ = _server.getParam<double>(prefix + _unique_name + "/falko/xRes");
+ param.yRes_ = _server.getParam<double>(prefix + _unique_name + "/falko/yRes");
+ param.thetaRes_ = _server.getParam<double>(prefix + _unique_name + "/falko/thetaRes");
+ param.xAbsMax_ = _server.getParam<double>(prefix + _unique_name + "/falko/xAbsMax");
+ param.yAbsMax_ = _server.getParam<double>(prefix + _unique_name + "/falko/yAbsMax");
+ param.thetaAbsMax_ = _server.getParam<double>(prefix + _unique_name + "/falko/thetaAbsMax");
+ };
+ virtual ~ParamsProcessorLoopClosureFalko() = default;
+
+ std::string print() const override
+ {
+
+ return ParamsProcessorLoopClosure::print() + "\n"
+ + "min_keypoints: " + std::to_string(min_keypoints) + "\n"
+ + "score_th: " + std::to_string(score_th) + "\n"
+ + "distance_between_ref_frm: " + std::to_string(distance_between_ref_frm) + "\n"
+ + "near_matches_th: " + std::to_string(near_matches_th) + "\n"
+ + "th_kp_area: " + std::to_string(th_kp_area) + "\n"
+ + "th_kp_perimeter: " + std::to_string(th_kp_perimeter) + "\n"
+ + "th_kp_maxd: " + std::to_string(th_kp_maxd) + "\n"
+ + "th_kp_eig: " + std::to_string(th_kp_eig) + "\n"
+ + "th_scan_area: " + std::to_string(th_scan_area) + "\n"
+ + "th_scan_eig: " + std::to_string(th_scan_eig) + "\n"
+ + "logging: " + std::to_string(logging) + "\n"
+ + "logging_filepath: " + logging_filepath + "\n"
+
+ + "falko/matcher_distance_th_: " + std::to_string(param.matcher_distance_th_) + "\n"
+ + "falko/use_descriptors_: " + std::to_string(param.use_descriptors_) + "\n"
+ + "falko/circularSectorNumber_: " + std::to_string(param.circularSectorNumber_) + "\n"
+ + "falko/radialRingNumber_: " + std::to_string(param.radialRingNumber_) + "\n"
+ + "falko/matcher_ddesc_th_: " + std::to_string(param.matcher_ddesc_th_) + "\n"
+ + "falko/keypoints_number_th_: " + std::to_string(param.keypoints_number_th_) + "\n"
+
+ + "falko/enable_subbeam_: " + std::to_string(param.enable_subbeam_) + "\n"
+ + "falko/nms_radius_: " + std::to_string(param.nms_radius_) + "\n"
+ + "falko/neigh_b_: " + std::to_string(param.neigh_b_) + "\n"
+ + "falko/b_ratio_: " + std::to_string(param.b_ratio_) + "\n"
+
+ + "falko/xRes_: " + std::to_string(param.xRes_) + "\n"
+ + "falko/yRes_: " + std::to_string(param.yRes_) + "\n"
+ + "falko/thetaRes_: " + std::to_string(param.thetaRes_) + "\n"
+ + "falko/xAbsMax_: " + std::to_string(param.xAbsMax_) + "\n"
+ + "falko/yAbsMax_: " + std::to_string(param.yAbsMax_) + "\n"
+ + "falko/thetaAbsMax_: " + std::to_string(param.thetaAbsMax_) + "\n";
+ };
+};
+
+/** \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.
+ *
+ * It extracts Features that contain laserscanutils::SceneFalko. The scene contain keypoints and descriptors
+ *
+ * It manages the capture and frame links in the inherited class ProcessorLoopClosure.
+ *
+ * It matches a target falko feature against a list of reference falko features.
+ *
+ * The reference features are found from a search of the previous captures
+ *
+ * The loop closures are not validated with an ICP
+ *
+ * Different 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 L Loop closure Type <LoopClosureFalkoAht> or <LoopClosureFalkoNn>
+ * \param _param_falko parameter struct with falko lib parameters
+ **/
+template <typename D, typename Extr, template <typename, typename> typename L>
+class ProcessorLoopClosureFalko : public ProcessorLoopClosure
+{
+ public:
+ ProcessorLoopClosureFalko(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosure("ProcessorLoopClosureFalko", 2, _params_falko)
+ , params_falko_(_params_falko)
+ , loop_closure_(std::make_shared<L<D, Extr>>(params_falko_->param))
+ {
+ if (!params_falko_->logging)
+ return;
+
+ std::ostringstream ss, ss1, ss2, ss3, ss4, ss5, ss6, ss7, ss8, ss9, ss10, ss11, ss12;
+ ss << std::setw(2) << std::setfill('0') << params_falko_->score_th;
+ std::string score_th(ss.str());
+
+ ss1 << std::setw(4) << std::setfill('0') << params_falko_->param.matcher_distance_th_;
+ std::string matcher_distance_th(ss1.str());
+
+ ss2 << std::setw(4) << std::setfill('0') << params_falko_->param.matcher_ddesc_th_;
+ std::string matcher_ddesc_th(ss2.str());
+
+ ss3 << std::setw(4) << std::setfill('0') << params_falko_->param.enable_subbeam_;
+ std::string enable_subbeam(ss3.str());
+
+ ss4 << std::setw(4) << std::setfill('0') << params_falko_->param.nms_radius_;
+ std::string nms_radius(ss4.str());
+
+ ss5 << std::setw(4) << std::setfill('0') << params_falko_->param.neigh_b_;
+ std::string neigh_b(ss5.str());
+
+ ss6 << std::setw(4) << std::setfill('0') << params_falko_->param.b_ratio_;
+ std::string b_ratio(ss6.str());
+
+ ss7 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_area;
+ std::string th_kp_area(ss7.str());
+
+ ss8 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_perimeter;
+ std::string th_kp_perimeter(ss8.str());
+
+ ss9 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_maxd;
+ std::string th_kp_maxd(ss9.str());
+
+ ss10 << std::setw(3) << std::setfill('0') << params_falko_->th_kp_eig;
+ std::string th_kp_eig(ss10.str());
+
+ ss11 << std::setw(3) << std::setfill('0') << params_falko_->th_scan_area;
+ std::string th_scan_area(ss11.str());
+
+ ss12 << std::setw(3) << std::setfill('0') << params_falko_->th_scan_eig;
+ std::string th_scan_eig(ss12.str());
+
+ std::string filename = params_falko_->logging_filepath + "/log" +
+ "_use_descriptors_" + std::to_string(params_falko_->param.use_descriptors_) +
+ "_score_th_" + score_th +
+ "_th_kp_area_" + th_kp_area +
+ "_th_kp_perimeter_" + th_kp_perimeter +
+ "_th_kp_maxd_" + th_kp_maxd +
+ "_th_kp_eig_" + th_kp_eig +
+ "_th_scan_area_" + th_scan_area +
+ "_th_scan_eig_" + th_scan_eig +
+ "_matcher_distance_th_" + matcher_distance_th +
+ "matcher_ddesc_th_" + matcher_ddesc_th +
+ "_enable_subbeam_" + enable_subbeam +
+ "_nms_radius_" + nms_radius +
+ "_neigh_b_" + neigh_b +
+ "_b_ratio_" + b_ratio + ".dat";
+
+ // change ~ with HOME using environment variable
+ if (filename.at(0) == '~')
+ filename = std::string(std::getenv("HOME")) + filename.substr(1);
+
+ outdata.open(filename, std::ofstream::out); // opens the file
+ if (outdata.fail())
+ WOLF_ERROR("ProcessorLoopClosureFalko: logging file '", filename, "' could not be created. \nError: ", strerror(errno));
+
+ init_outData_ = 0;
+ };
+
+ ~ProcessorLoopClosureFalko() { outdata.close(); };
+
+ std::list<MatchLoopClosurePtr> match_list_;
+
+ protected:
+ /** \brief Function not implemented
+ **/
+ bool voteFindLoopClosures(CaptureBasePtr cap) override { return true; };
+
+ /** \brief Function not implemented
+ **/
+ bool validateLoopClosure(MatchLoopClosurePtr match) override { return true; };
+
+ /** \brief It emplace factors between frames that its scenes are a match
+ * \param match structure that contains scenes and capture pointers that are a match
+ **/
+ void emplaceFactors(MatchLoopClosurePtr match) override
+ {
+ match_list_.push_back(match);
+ auto falko_match = std::static_pointer_cast<MatchLoopClosureFalko>(match);
+ auto ftr = FeatureBase::emplace<FeatureBase>(match->capture_reference, "FeatureTransformFalko",
+ falko_match->match_falko_->transform_vector,
+ Eigen::MatrixXd::Identity(3, 3));
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr, ftr, match->capture_target->getFrame(),
+ shared_from_this(), params_->apply_loss_function,
+ TOP_LOOP);
+ };
+
+ /** \brief Extracts a falko scene from a laser scan and emplaces it as a scen feature
+ * \param cap laser capture with a laser scan
+ **/
+ void emplaceFeatures(CaptureBasePtr cap) override
+ {
+ auto start_extract_scene = std::chrono::high_resolution_clock::now();
+
+ auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
+
+ assert(cap_laser && "ProcessorLoopClosureFalko::emplaceFeatures called with a capture of wrong type.");
+ // WOLF_INFO(params_falko_->print());
+ auto scene_falko = std::static_pointer_cast<laserscanutils::SceneFalko<D>>(loop_closure_->extractScene(
+ cap_laser->getScan(), std::static_pointer_cast<SensorLaser2d>(getSensor())->getScanParams()));
+
+ // Falko implementation
+ FeatureBase::emplace<FeatureSceneFalko<D>>(cap_laser, scene_falko);
+
+ WOLF_INFO("empalceFeatures: number of keypoints ", scene_falko->keypoints_list_.size())
+
+ auto duration_ms = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_extract_scene);
+
+ duration_extract_scene = duration_ms;
+ };
+
+ /** \brief It matches a target falko feature against a list of reference falko features.
+ * The reference features are found from a search of the previous captures
+ * \param cap laser capture with a laser scan and a falko feature
+ **/
+ std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
+ {
+ auto start_find_loop = std::chrono::high_resolution_clock::now();
+
+ auto current_frame = _capture->getFrame();
+ assert(_capture->getFrame() && "ProcessorLoopClosureFalko::findLoopClosures capture with no frame.");
+ assert(_capture->getFrame()->getTrajectory() &&
+ "ProcessorLoopClosureFalko::findLoopClosures capture with frame not in trajectory");
+
+ std::shared_ptr<laserscanutils::SceneFalko<D>> scene_falko_current;
+ // Find current falko scene
+ for (auto feat : _capture->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalko<D>>(feat);
+ scene_falko_current = feat_falko->getScene();
+ }
+
+ // Find reference scenes
+ std::map<laserscanutils::sceneBasePtr, CaptureBasePtr> map_scene_cap_ref;
+ std::list<laserscanutils::sceneBasePtr> list_scene_reference;
+ auto old_frame = _capture->getFrame()->getPreviousFrame();
+
+ const double& target_kp_area = scene_falko_current->area_;
+ const double& target_scan_area = scene_falko_current->area_scan_;
+ const double& target_kp_perimeter = scene_falko_current->perimeter_;
+ const double& target_kp_maxd = scene_falko_current->max_distance_;
+ const auto& target_eig_kp = scene_falko_current->eigenvalues_kp_;
+ const auto& target_eig_scan = scene_falko_current->eigenvalues_scan_;
+ GraphSearch graph_search;
+
+ int num_ref_scenes = 0;
+ int acum_score = 0;
+ while (old_frame)
+ {
+ for (auto cap : old_frame->getCaptureList())
+ {
+ for (auto feat : cap->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+
+ num_ref_scenes += 1;
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalko<D>>(feat);
+ double a_kp = feat_falko->getScene()->area_;
+ double a_min_kp = target_kp_area * params_falko_->th_kp_area;
+ double a_max_kp = target_kp_area * (1 + (1 - params_falko_->th_kp_area));
+
+ double p_kp = feat_falko->getScene()->perimeter_;
+ double p_min = target_kp_perimeter * params_falko_->th_kp_perimeter;
+ double p_max = target_kp_perimeter * (1 + (1 - params_falko_->th_kp_perimeter));
+
+ double maxd_kp = feat_falko->getScene()->max_distance_;
+ double maxd_min_kp = target_kp_maxd * params_falko_->th_kp_maxd;
+ double maxd_max_kp = target_kp_maxd * (1 + (1 - params_falko_->th_kp_maxd));
+
+ double a_scan = feat_falko->getScene()->area_scan_;
+ double a_min_scan = target_scan_area * params_falko_->th_scan_area;
+ double a_max_scan = target_scan_area * (1 + (1 - params_falko_->th_scan_area));
+
+ auto eig1_kp = feat_falko->getScene()->eigenvalues_kp_[0];
+ double eig1_min_kp = target_eig_kp[0] * params_falko_->th_kp_eig;
+ double eig1_max_kp = target_eig_kp[0] * (1 + (1 - params_falko_->th_kp_eig));
+
+ auto eig2_kp = feat_falko->getScene()->eigenvalues_kp_[1];
+ double eig2_min_kp = target_eig_kp[1] * params_falko_->th_kp_eig;
+ double eig2_max_kp = target_eig_kp[1] * (1 + (1 - params_falko_->th_kp_eig));
+
+ auto eig1_scan = feat_falko->getScene()->eigenvalues_scan_[0];
+ double eig1_min_scan = target_eig_scan[0] * params_falko_->th_scan_eig;
+ double eig1_max_scan = target_eig_scan[0] * (1 + (1 - params_falko_->th_scan_eig));
+
+ auto eig2_scan = feat_falko->getScene()->eigenvalues_scan_[1];
+ double eig2_min_scan = target_eig_scan[1] * params_falko_->th_scan_eig;
+ double eig2_max_scan = target_eig_scan[1] * (1 + (1 - params_falko_->th_scan_eig));
+
+ if (feat_falko->getScene()->keypoints_list_.size() <
+ params_falko_->min_keypoints) // min KEYPOINTS
+ continue;
+ if (((a_min_kp > a_kp) or (a_max_kp < params_falko_->th_kp_area)) and
+ a_kp != 0) // Area KEYPOINTS restriction
+ continue;
+ if (((p_min > p_kp) or (p_max < p_kp)) and
+ params_falko_->th_kp_perimeter != 0) // Perimeter KEYPOINTS restriction
+ continue;
+ if (((maxd_min_kp > maxd_kp) or (maxd_max_kp < maxd_kp)) and
+ params_falko_->th_kp_maxd != 0) // max_dist KEYPOINTS restriction
+ continue;
+ if (((a_min_scan > a_scan) or (a_max_scan < a_scan)) and
+ params_falko_->th_scan_area != 0) // Area SCAN restriction
+ continue;
+ if (((eig1_min_kp > eig1_kp) or (eig1_max_kp < eig1_kp)) and
+ params_falko_->th_kp_eig != 0) // eigenvalue 1 KEYPOINTS restriction
+ continue;
+ if (((eig2_min_kp > eig2_kp) or (eig2_max_kp < eig2_kp)) and
+ params_falko_->th_kp_eig != 0) // eigenvalue 2 KEYPOINTS restriction
+ continue;
+ if (((eig1_min_scan > eig1_scan) or (eig1_max_scan < eig1_scan)) and
+ params_falko_->th_scan_eig != 0) // eigenvalue 1 SCAN restriction
+ continue;
+ if (((eig2_min_scan > eig2_scan) or (eig2_max_scan < eig2_scan)) and
+ params_falko_->th_scan_eig != 0) // eigenvalue 2 SCAN restriction
+ continue;
+
+ auto shortest_path = graph_search.computeShortestPath(old_frame, current_frame, 10);
+
+ map_scene_cap_ref.emplace(feat_falko->getScene(), cap);
+ list_scene_reference.push_back(feat_falko->getScene());
+ }
+ }
+ old_frame = old_frame->getPreviousFrame();
+ }
+
+ WOLF_INFO("findLoopClosures: number of ref Scene ", list_scene_reference.size())
+ // find loop closure
+ std::map<double, laserscanutils::MatchLoopClosureScenePtr> match_lc_scene;
+
+ match_lc_scene = loop_closure_->findLoopClosure(list_scene_reference, scene_falko_current);
+ // fill MatchLoopClosurePtr list
+ std::map<double, MatchLoopClosurePtr> match_lc_map;
+ for (auto match_scenes : match_lc_scene)
+ {
+ if (match_scenes.first >= params_falko_->score_th)
+ {
+ WOLF_INFO("loop closure candidate score : ", match_scenes.first)
+ MatchLoopClosureFalkoPtr match_captures = std::make_shared<MatchLoopClosureFalko>();
+ match_captures->capture_reference = map_scene_cap_ref.at(match_scenes.second->scene_1);
+ match_captures->capture_target = _capture;
+ match_captures->normalized_score = match_scenes.first;
+ match_captures->match_falko_ = match_scenes.second;
+ while (match_lc_map.count(match_captures->normalized_score) != 0)
+ match_captures->normalized_score +=
+ (match_captures->normalized_score < 1.0 ? 1e-12 : -1e12);
+ acum_score += match_captures->normalized_score;
+ match_lc_map.emplace(match_captures->normalized_score, match_captures);
+
+ // TODO: calcular consistency
+ }
+ }
+
+ WOLF_INFO("findLoopClosures: lc number of candidates : ", match_lc_map.size())
+
+ duration_find_loop = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_find_loop);
+
+ if (params_falko_->logging)
+ printOutData(list_scene_reference, num_ref_scenes, match_lc_map, scene_falko_current->keypoints_list_,
+ acum_score);
+
+ if (params_falko_->near_matches_th == 1)
+ {
+ return match_lc_map;
+ }
+ // Check for robustness: more than one loop closure with nearby frames
+ std::map<double, MatchLoopClosurePtr> match_lc_map_robust;
+ // TODO: double scene_estimated_size = mean(keyoints.distance)
+ // distance_between_ref_frm_ =scene_estimated_size*factor; factor add to params
+ // near_matches_th = desapear
+ // Check angle
+ // Check in validate function: error max between transform and estimation -> consistency parameter in
+ // MatchLoopClosure if good consistency, not needed the other checks -> param for when consistency is good
+ // enough buscar papers per robustesa amb loop closures
+ for (auto match_capture_1 : match_lc_map)
+ {
+ bool consensus = true;
+ auto position_1 = match_capture_1.second->capture_reference->getFrame()->getP()->getState();
+
+ auto frame_1 = match_capture_1.second->capture_reference->getFrame();
+ // Backwards search
+ auto prev_frame_1 = frame_1->getPreviousFrame();
+ int match_prev_1_found = 0;
+ while (prev_frame_1)
+ {
+ // si surtim de la distance deixem de buscar
+ if ((position_1 - prev_frame_1->getP()->getState()).norm() >
+ params_falko_->distance_between_ref_frm)
+ {
+ break;
+ }
+ // Busquem match amb el prev_frame_1
+ for (auto match_capture_2 : match_lc_map)
+ {
+ // We look for a loop closure with the prev_frame_1
+ if (match_capture_2.second->capture_reference->getFrame() == prev_frame_1)
+ {
+ match_prev_1_found += 1;
+ }
+ }
+ prev_frame_1 = prev_frame_1->getPreviousFrame();
+ }
+
+ // Busqueda enrera
+ auto next_frame_1 = frame_1->getNextFrame();
+ int match_next_1_found = 0;
+ while (next_frame_1)
+ {
+ // si surtim de la distance deixem de buscar
+ if ((position_1 - next_frame_1->getP()->getState()).norm() >
+ params_falko_->distance_between_ref_frm)
+ {
+ break;
+ }
+ // Busquem match amb el next_frame_1
+ for (auto match_capture_2 : match_lc_map)
+ {
+ if (match_capture_2.second->capture_target->getFrame() == prev_frame_1)
+ {
+ match_next_1_found += 1;
+ }
+ }
+ next_frame_1 = next_frame_1->getNextFrame();
+ }
+
+ if (match_prev_1_found + match_next_1_found + 1 > params_falko_->near_matches_th)
+ {
+ consensus = true;
+ }
+ else
+ {
+ consensus = false;
+ }
+ if (consensus)
+ {
+ match_lc_map_robust.insert(match_capture_1);
+ }
+ }
+
+ return match_lc_map_robust;
+ }
+
+ void printOutData(std::list<laserscanutils::sceneBasePtr> &list_scene_reference, int num_ref_scenes,
+ std::map<double, MatchLoopClosurePtr> &match_lc_map, std::vector<falkolib::FALKO> &keypoints_list,
+ int acum_score)
+ {
+ if (init_outData_ == 1)
+ {
+ outdata << std::to_string(num_ok_) << ";";
+ outdata << std::to_string(num_nok_) << ";";
+ if (num_ok_ > 0)
+ {
+ outdata << std::to_string(duration_validation_ok.count() / num_ok_) << ";";
+ }
+ else
+ outdata << std::to_string(0) << ";";
+
+ if (num_nok_ > 0)
+ {
+ outdata << std::to_string(duration_validation_nok.count() / num_nok_) << ";" << std::endl;
+ }
+ else
+ outdata << std::to_string(0) << ";" << std::endl;
+ num_ok_ = 0;
+ num_nok_ = 0;
+ duration_validation_ok = std::chrono::duration_values<std::chrono::microseconds>::zero();
+ duration_validation_nok = std::chrono::duration_values<std::chrono::microseconds>::zero();
+ }
+ else
+ {
+ num_ok_ = 0;
+ num_nok_ = 0;
+ }
+ init_outData_ = 1;
+ outdata << std::to_string(list_scene_reference.size()) << ";";
+ outdata << std::to_string(num_ref_scenes) << ";";
+ outdata << std::to_string(match_lc_map.size()) << ";";
+ outdata << std::to_string(keypoints_list.size()) << ";";
+ if (match_lc_map.size() > 0)
+ {
+ outdata << std::to_string(acum_score / match_lc_map.size()) << ";";
+ }
+ else
+ {
+ outdata << std::to_string(0) << ";";
+ }
+ outdata << std::to_string(duration_find_loop.count()) << ";";
+ outdata << std::to_string(duration_extract_scene.count()) << ";";
+ }
+
+ ParamsProcessorLoopClosureFalkoPtr params_falko_;
+
+ std::shared_ptr<L<D, Extr>> loop_closure_;
+ std::ofstream outdata;
+ std::chrono::duration<long, std::micro> duration_find_loop;
+ std::chrono::duration<long, std::micro> duration_validation_ok;
+ std::chrono::duration<long, std::micro> duration_validation_nok;
+ std::chrono::duration<long, std::micro> duration_extract_scene;
+
+ public:
+ unsigned int getNStoredFrames() { return buffer_frame_.getContainer().size(); }
+ unsigned int getNStoredCaptures() { return buffer_capture_.getContainer().size(); }
+ int num_ok_;
+ int num_nok_;
+ int init_outData_;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoNnBsc
+ : public ProcessorLoopClosureFalko<laserscanutils::bsc, laserscanutils::bscExtractor,
+ laserscanutils::LoopClosureFalkoNn>
+{
+ public:
+ ProcessorLoopClosureFalkoNnBsc(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoNnBsc, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoNnBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtBsc);
+
+/** \brief A class that implements the loop closure with BSC descriptors and AHT matcher
+ **/
+class ProcessorLoopClosureFalkoAhtBsc : public ProcessorLoopClosureFalko<laserscanutils::bsc,
+ laserscanutils::bscExtractor,
+ laserscanutils::LoopClosureFalkoAht>
+{
+ public:
+ ProcessorLoopClosureFalkoAhtBsc(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoAhtBsc, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoAhtBsc() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnCgh);
+
+/** \brief A class that implements the loop closure with CGH descriptors and NN matcher
+ **/
+class ProcessorLoopClosureFalkoNnCgh : public ProcessorLoopClosureFalko<laserscanutils::cgh,
+ laserscanutils::cghExtractor,
+ laserscanutils::LoopClosureFalkoNn>
+{
+ public:
+ ProcessorLoopClosureFalkoNnCgh(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoNnCgh, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoNnCgh() = default;
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtCgh);
+
+/** \brief A class that implements the loop closure with CGH descriptors and AHT matcher
+ **/
+class ProcessorLoopClosureFalkoAhtCgh : public ProcessorLoopClosureFalko<laserscanutils::cgh,
+ laserscanutils::cghExtractor,
+ laserscanutils::LoopClosureFalkoAht>
+{
+ public:
+ ProcessorLoopClosureFalkoAhtCgh(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalko(_params_falko){};
+
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoAhtCgh, ParamsProcessorLoopClosureFalko);
+
+ ~ProcessorLoopClosureFalkoAhtCgh() = default;
+};
+
+} // namespace wolf
+
+#endif // SRC_PROCESSOR_LOOP_CLOSURE_FALKO_H_
diff --git a/include/laser/processor/processor_loop_closure_falko_icp.h b/include/laser/processor/processor_loop_closure_falko_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ed8e6bb2de33f330f43642da1d0cacadaee2158
--- /dev/null
+++ b/include/laser/processor/processor_loop_closure_falko_icp.h
@@ -0,0 +1,298 @@
+//--------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_
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index 226c19e950f2993b04f77795ab2f7ee637f62129..079ab82e3a184eebce74b466efb859b15bb31d6a 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/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
*/
diff --git a/src/feature/feature_scene_falko.cpp b/src/feature/feature_scene_falko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..113e1568808f9d8d86bdd92166943c02b9fe264f
--- /dev/null
+++ b/src/feature/feature_scene_falko.cpp
@@ -0,0 +1,27 @@
+//--------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
diff --git a/src/processor/processor_loop_closure_falko.cpp b/src/processor/processor_loop_closure_falko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..730782a8e66d7d618da8cce1b113e6bee2b9cf81
--- /dev/null
+++ b/src/processor/processor_loop_closure_falko.cpp
@@ -0,0 +1,37 @@
+//--------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
diff --git a/src/processor/processor_loop_closure_falko_icp.cpp b/src/processor/processor_loop_closure_falko_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1312f27b3353bed70570c0615a31e61acac3bf19
--- /dev/null
+++ b/src/processor/processor_loop_closure_falko_icp.cpp
@@ -0,0 +1,38 @@
+//--------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
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index b93ee3848a279510bdf5267c0f61300bbe19bd5d..b7f71dc1e831d27e58dc88f0801c0338ab62a814 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/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;
}
}
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index ad55f0aa8d949adbec78fcb0c76ba39aa581e00c..2b75292daf5c0f8e8388fca88d9e9ba786587b78 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 75cecdb4bec2513ac44e59bf0cc276f9ccdad93f..c75a1957be69ff023966f3c9d0c59dd1fd7eb0f6 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,7 +1,6 @@
# 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)
diff --git a/test/data/scan_data.h b/test/data/scan_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..bb09376965e24d8f448cd2e8f5e96d0886850d35
--- /dev/null
+++ b/test/data/scan_data.h
@@ -0,0 +1,2929 @@
+//--------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--------
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib 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.
+ *
+ * FALKOLib 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 FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+std::vector<float> testRanges1 = {250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 17.263999938964844,
+ 17.263999938964844,
+ 17.263999938964844,
+ 17.263999938964844,
+ 13.552000045776367,
+ 13.552000045776367,
+ 13.552000045776367,
+ 12.95199966430664,
+ 12.95199966430664,
+ 12.95199966430664,
+ 12.95199966430664,
+ 12.51200008392334,
+ 11.567999839782715,
+ 11.567999839782715,
+ 10.720000267028809,
+ 10.720000267028809,
+ 10.720000267028809,
+ 10.359999656677246,
+ 10.336000442504883,
+ 10.343999862670898,
+ 10.35200023651123,
+ 10.35200023651123,
+ 10.32800006866455,
+ 10.319999694824219,
+ 10.383999824523926,
+ 10.407999992370605,
+ 10.432000160217285,
+ 10.432000160217285,
+ 10.447999954223633,
+ 10.46399974822998,
+ 10.46399974822998,
+ 10.503999710083008,
+ 10.503999710083008,
+ 10.527999877929688,
+ 10.592000007629395,
+ 10.62399959564209,
+ 8.368000030517578,
+ 8.343999862670898,
+ 8.343999862670898,
+ 8.031999588012695,
+ 7.831999778747559,
+ 7.6479997634887695,
+ 7.552000045776367,
+ 7.415999889373779,
+ 7.415999889373779,
+ 7.263999938964844,
+ 7.144000053405762,
+ 7.0320000648498535,
+ 6.920000076293945,
+ 6.776000022888184,
+ 6.776000022888184,
+ 6.671999931335449,
+ 6.576000213623047,
+ 6.440000057220459,
+ 6.328000068664551,
+ 6.271999835968018,
+ 6.271999835968018,
+ 6.263999938964844,
+ 6.271999835968018,
+ 6.296000003814697,
+ 6.320000171661377,
+ 6.328000068664551,
+ 6.328000068664551,
+ 6.335999965667725,
+ 6.320000171661377,
+ 3.187999963760376,
+ 3.187999963760376,
+ 4.495999813079834,
+ 4.495999813079834,
+ 4.495999813079834,
+ 4.495999813079834,
+ 4.504000186920166,
+ 4.504000186920166,
+ 4.504000186920166,
+ 4.544000148773193,
+ 4.544000148773193,
+ 4.544000148773193,
+ 4.544000148773193,
+ 4.51200008392334,
+ 4.51200008392334,
+ 4.51200008392334,
+ 4.51200008392334,
+ 4.599999904632568,
+ 4.576000213623047,
+ 4.495999813079834,
+ 4.455999851226807,
+ 4.455999851226807,
+ 4.455999851226807,
+ 4.455999851226807,
+ 4.4079999923706055,
+ 4.368000030517578,
+ 4.320000171661377,
+ 4.271999835968018,
+ 4.271999835968018,
+ 4.223999977111816,
+ 4.184000015258789,
+ 4.168000221252441,
+ 4.159999847412109,
+ 4.168000221252441,
+ 4.176000118255615,
+ 4.176000118255615,
+ 4.184000015258789,
+ 4.199999809265137,
+ 4.199999809265137,
+ 4.208000183105469,
+ 4.216000080108643,
+ 4.216000080108643,
+ 4.223999977111816,
+ 4.23199987411499,
+ 4.23199987411499,
+ 4.176000118255615,
+ 250,
+ 250,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.099999904632568,
+ 4.099999904632568,
+ 4.099999904632568,
+ 4.079999923706055,
+ 4.0320000648498535,
+ 3.98799991607666,
+ 3.9719998836517334,
+ 3.9719998836517334,
+ 3.9560000896453857,
+ 3.9240000247955322,
+ 3.888000011444092,
+ 3.859999895095825,
+ 3.8320000171661377,
+ 3.8320000171661377,
+ 3.803999900817871,
+ 3.7880001068115234,
+ 3.259999990463257,
+ 3.240000009536743,
+ 3.2200000286102295,
+ 3.2200000286102295,
+ 3.200000047683716,
+ 3.1760001182556152,
+ 3.1559998989105225,
+ 3.1440000534057617,
+ 3.128000020980835,
+ 3.128000020980835,
+ 3.119999885559082,
+ 2.5959999561309814,
+ 2.5959999561309814,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.4839999675750732,
+ 2.4839999675750732,
+ 2.4839999675750732,
+ 2.4839999675750732,
+ 2.496000051498413,
+ 2.5320000648498535,
+ 250,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.6679999828338623,
+ 2.6679999828338623,
+ 2.6679999828338623,
+ 2.6679999828338623,
+ 2.640000104904175,
+ 2.640000104904175,
+ 2.640000104904175,
+ 2.628000020980835,
+ 2.615999937057495,
+ 2.615999937057495,
+ 2.6080000400543213,
+ 2.5959999561309814,
+ 2.5880000591278076,
+ 2.5759999752044678,
+ 2.563999891281128,
+ 2.563999891281128,
+ 2.555999994277954,
+ 2.5439999103546143,
+ 2.5320000648498535,
+ 2.5199999809265137,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.503999948501587,
+ 2.492000102996826,
+ 2.4839999675750732,
+ 2.4719998836517334,
+ 2.4639999866485596,
+ 2.4639999866485596,
+ 2.4600000381469727,
+ 2.4519999027252197,
+ 2.440000057220459,
+ 2.431999921798706,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4079999923706055,
+ 2.4079999923706055,
+ 2.427999973297119,
+ 2.444000005722046,
+ 2.444000005722046,
+ 2.4600000381469727,
+ 2.4719998836517334,
+ 2.48799991607666,
+ 2.5,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5280001163482666,
+ 2.5399999618530273,
+ 2.559999942779541,
+ 2.5799999237060547,
+ 2.5840001106262207,
+ 2.5840001106262207,
+ 3.0480000972747803,
+ 3.0480000972747803,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.624000072479248,
+ 2.615999937057495,
+ 2.611999988555908,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.5959999561309814,
+ 2.5920000076293945,
+ 2.5840001106262207,
+ 2.5759999752044678,
+ 2.572000026702881,
+ 2.572000026702881,
+ 2.563999891281128,
+ 2.555999994277954,
+ 2.5480000972747803,
+ 2.5439999103546143,
+ 2.5399999618530273,
+ 2.5399999618530273,
+ 2.5360000133514404,
+ 2.5320000648498535,
+ 2.5239999294281006,
+ 2.5199999809265137,
+ 2.507999897003174,
+ 2.507999897003174,
+ 2.503999948501587,
+ 2.5,
+ 2.496000051498413,
+ 2.492000102996826,
+ 2.48799991607666,
+ 2.48799991607666,
+ 2.4800000190734863,
+ 2.4760000705718994,
+ 2.4719998836517334,
+ 2.4679999351501465,
+ 2.4639999866485596,
+ 2.4560000896453857,
+ 2.4560000896453857,
+ 2.4519999027252197,
+ 2.447999954223633,
+ 2.447999954223633,
+ 2.444000005722046,
+ 2.436000108718872,
+ 2.436000108718872,
+ 2.431999921798706,
+ 2.427999973297119,
+ 2.4240000247955322,
+ 2.4240000247955322,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4079999923706055,
+ 2.4040000438690186,
+ 2.4040000438690186,
+ 2.4000000953674316,
+ 2.4000000953674316,
+ 2.3959999084472656,
+ 2.3919999599456787,
+ 2.3919999599456787,
+ 2.388000011444092,
+ 2.384000062942505,
+ 2.384000062942505,
+ 2.380000114440918,
+ 2.375999927520752,
+ 2.371999979019165,
+ 2.371999979019165,
+ 2.368000030517578,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.359999895095825,
+ 2.3559999465942383,
+ 2.3559999465942383,
+ 2.3559999465942383,
+ 2.6600000858306885,
+ 2.6600000858306885,
+ 2.6600000858306885,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.0,
+ 2.0,
+ 2.0,
+ 2.0,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 2.0,
+ 2.0,
+ 2.009999990463257,
+ 2.0220000743865967,
+ 2.0460000038146973,
+ 2.0460000038146973,
+ 2.0260000228881836,
+ 2.0260000228881836,
+ 2.00600004196167,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9900000095367432,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.996000051498413,
+ 1.9980000257492065,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.996000051498413,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 2.0,
+ 2.0,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.9919999837875366,
+ 1.9520000219345093,
+ 2.0320000648498535,
+ 2.0999999046325684,
+ 2.1040000915527344,
+ 2.1040000915527344,
+ 2.0280001163482666,
+ 1.9040000438690186,
+ 1.9279999732971191,
+ 1.9520000219345093,
+ 1.9559999704360962,
+ 1.9559999704360962,
+ 1.9600000381469727,
+ 1.9579999446868896,
+ 1.9600000381469727,
+ 1.9620000123977661,
+ 1.9639999866485596,
+ 1.9639999866485596,
+ 1.965999960899353,
+ 1.965999960899353,
+ 1.968000054359436,
+ 1.972000002861023,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9800000190734863,
+ 1.9839999675750732,
+ 1.9800000190734863,
+ 1.9800000190734863,
+ 1.9819999933242798,
+ 1.9880000352859497,
+ 1.996000051498413,
+ 2.009999990463257,
+ 2.00600004196167,
+ 2.00600004196167,
+ 1.99399995803833,
+ 2.687999963760376,
+ 2.687999963760376,
+ 2.687999963760376,
+ 1.74399995803833,
+ 1.74399995803833,
+ 2.063999891281128,
+ 2.115999937057495,
+ 2.196000099182129,
+ 2.191999912261963,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5199999809265137,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5360000133514404,
+ 2.5399999618530273,
+ 2.5480000972747803,
+ 2.5480000972747803,
+ 2.5480000972747803,
+ 2.5399999618530273,
+ 2.5439999103546143,
+ 2.552000045776367,
+ 2.555999994277954,
+ 2.555999994277954,
+ 2.552000045776367,
+ 2.5480000972747803,
+ 250,
+ 1.3179999589920044,
+ 1.3179999589920044,
+ 1.3179999589920044,
+ 1.3179999589920044,
+ 1.3079999685287476,
+ 1.2999999523162842,
+ 1.2940000295639038,
+ 1.2940000295639038,
+ 1.2879999876022339,
+ 1.2860000133514404,
+ 1.2940000295639038,
+ 1.3040000200271606,
+ 1.3040000200271606,
+ 1.3040000200271606,
+ 1.2999999523162842,
+ 1.2979999780654907,
+ 1.3020000457763672,
+ 1.309999942779541,
+ 1.3140000104904175,
+ 1.3140000104904175,
+ 1.3140000104904175,
+ 1.3240000009536743,
+ 1.3359999656677246,
+ 1.3420000076293945,
+ 1.343999981880188,
+ 1.343999981880188,
+ 1.2200000286102295,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.0920000076293945,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.100000023841858,
+ 1.100000023841858,
+ 1.100000023841858,
+ 1.100000023841858,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0959999561309814,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.0920000076293945,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.1019999980926514,
+ 1.1119999885559082,
+ 1.121999979019165,
+ 1.1260000467300415,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 5.791999816894531,
+ 5.791999816894531,
+ 5.791999816894531,
+ 5.791999816894531,
+ 5.776000022888184,
+ 5.776000022888184,
+ 5.776000022888184,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.736000061035156,
+ 5.679999828338623,
+ 5.671999931335449,
+ 5.671999931335449,
+ 5.631999969482422,
+ 5.631999969482422,
+ 5.552000045776367,
+ 5.552000045776367,
+ 5.552000045776367,
+ 5.552000045776367,
+ 5.559999942779541,
+ 5.584000110626221,
+ 6.104000091552734,
+ 6.783999919891357,
+ 6.831999778747559,
+ 6.831999778747559,
+ 6.831999778747559,
+ 6.760000228881836,
+ 6.616000175476074,
+ 6.455999851226807,
+ 6.343999862670898,
+ 6.343999862670898,
+ 6.320000171661377,
+ 6.311999797821045,
+ 9.192000389099121,
+ 9.175999641418457,
+ 9.15999984741211,
+ 9.15999984741211,
+ 9.128000259399414,
+ 9.119999885559082,
+ 7.992000102996826,
+ 7.9120001792907715,
+ 7.8480000495910645,
+ 7.8480000495910645,
+ 7.800000190734863,
+ 7.74399995803833,
+ 7.71999979019165,
+ 7.711999893188477,
+ 7.711999893188477,
+ 7.711999893188477,
+ 7.711999893188477,
+ 7.71999979019165,
+ 7.71999979019165,
+ 7.711999893188477,
+ 7.703999996185303,
+ 7.703999996185303,
+ 7.703999996185303,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.671999931335449,
+ 8.67199993133545,
+ 8.640000343322754,
+ 8.607999801635742,
+ 8.583999633789062,
+ 8.583999633789062,
+ 8.5600004196167,
+ 8.552000045776367,
+ 8.53600025177002,
+ 8.520000457763672,
+ 8.503999710083008,
+ 8.503999710083008,
+ 8.472000122070312,
+ 8.447999954223633,
+ 8.416000366210938,
+ 8.383999824523926,
+ 8.383999824523926,
+ 8.383999824523926,
+ 8.368000030517578,
+ 8.35200023651123,
+ 8.35200023651123,
+ 8.336000442504883,
+ 8.303999900817871,
+ 8.303999900817871,
+ 8.263999938964844,
+ 8.239999771118164,
+ 8.279999732971191,
+ 8.392000198364258,
+ 8.496000289916992,
+ 8.496000289916992,
+ 8.656000137329102,
+ 8.807999610900879,
+ 8.928000450134277,
+ 9.015999794006348,
+ 11.767999649047852,
+ 11.767999649047852,
+ 12.071999549865723,
+ 12.175999641418457,
+ 12.567999839782715,
+ 12.663999557495117,
+ 13.064000129699707,
+ 13.064000129699707,
+ 13.168000221252441,
+ 13.607999801635742,
+ 13.640000343322754,
+ 13.53600025177002,
+ 13.968000411987305,
+ 13.968000411987305,
+ 14.008000373840332,
+ 14.104000091552734,
+ 14.175999641418457,
+ 14.104000091552734,
+ 15.4399995803833,
+ 15.4399995803833,
+ 15.543999671936035,
+ 15.48799991607666,
+ 15.48799991607666,
+ 15.503999710083008,
+ 15.479999542236328,
+ 15.479999542236328,
+ 15.447999954223633,
+ 15.399999618530273,
+ 15.37600040435791,
+ 15.35200023651123,
+ 15.319999694824219,
+ 15.319999694824219,
+ 15.272000312805176,
+ 15.272000312805176,
+ 15.336000442504883,
+ 15.303999900817871,
+ 15.248000144958496,
+ 15.248000144958496,
+ 14.744000434875488,
+ 15.303999900817871,
+ 15.272000312805176,
+ 15.248000144958496,
+ 15.215999603271484,
+ 15.223999977111816,
+ 15.223999977111816,
+ 15.248000144958496,
+ 16.784000396728516,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.672000885009766,
+ 16.687999725341797,
+ 16.719999313354492,
+ 16.70400047302246,
+ 16.607999801635742,
+ 16.607999801635742,
+ 16.624000549316406,
+ 16.6560001373291,
+ 16.23200035095215,
+ 16.143999099731445,
+ 16.06399917602539,
+ 16.06399917602539,
+ 16.016000747680664,
+ 17.472000122070312,
+ 19.007999420166016,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250};
+
+std::vector<float> testRanges2 = {250,
+ 250,
+ 250,
+ 250,
+ 16.799999237060547,
+ 16.799999237060547,
+ 16.799999237060547,
+ 16.799999237060547,
+ 14.095999717712402,
+ 14.095999717712402,
+ 14.095999717712402,
+ 14.095999717712402,
+ 13.255999565124512,
+ 13.255999565124512,
+ 13.255999565124512,
+ 12.767999649047852,
+ 12.175999641418457,
+ 12.175999641418457,
+ 10.911999702453613,
+ 10.4399995803833,
+ 10.4399995803833,
+ 10.4399995803833,
+ 10.32800006866455,
+ 10.32800006866455,
+ 10.32800006866455,
+ 10.343999862670898,
+ 10.336000442504883,
+ 10.312000274658203,
+ 10.368000030517578,
+ 10.368000030517578,
+ 10.407999992370605,
+ 10.472000122070312,
+ 10.48799991607666,
+ 10.520000457763672,
+ 10.48799991607666,
+ 10.48799991607666,
+ 10.472000122070312,
+ 10.503999710083008,
+ 10.51200008392334,
+ 10.51200008392334,
+ 10.520000457763672,
+ 10.520000457763672,
+ 8.295999526977539,
+ 8.13599967956543,
+ 7.984000205993652,
+ 7.791999816894531,
+ 7.624000072479248,
+ 7.624000072479248,
+ 7.5279998779296875,
+ 7.4079999923706055,
+ 7.263999938964844,
+ 7.119999885559082,
+ 7.007999897003174,
+ 7.007999897003174,
+ 6.9120001792907715,
+ 6.760000228881836,
+ 6.639999866485596,
+ 6.544000148773193,
+ 6.415999889373779,
+ 6.415999889373779,
+ 6.311999797821045,
+ 6.271999835968018,
+ 6.263999938964844,
+ 6.288000106811523,
+ 6.303999900817871,
+ 6.303999900817871,
+ 6.320000171661377,
+ 6.335999965667725,
+ 6.343999862670898,
+ 6.335999965667725,
+ 6.303999900817871,
+ 6.303999900817871,
+ 6.271999835968018,
+ 4.5920000076293945,
+ 250,
+ 4.480000019073486,
+ 4.480000019073486,
+ 4.480000019073486,
+ 4.480000019073486,
+ 4.4720001220703125,
+ 4.504000186920166,
+ 4.519999980926514,
+ 4.5279998779296875,
+ 4.5279998779296875,
+ 4.5279998779296875,
+ 4.5279998779296875,
+ 4.567999839782715,
+ 4.559999942779541,
+ 250,
+ 4.48799991607666,
+ 4.48799991607666,
+ 4.48799991607666,
+ 4.48799991607666,
+ 4.440000057220459,
+ 4.392000198364258,
+ 4.360000133514404,
+ 4.360000133514404,
+ 4.320000171661377,
+ 4.263999938964844,
+ 4.223999977111816,
+ 4.191999912261963,
+ 4.168000221252441,
+ 4.168000221252441,
+ 4.168000221252441,
+ 4.176000118255615,
+ 4.176000118255615,
+ 4.191999912261963,
+ 4.208000183105469,
+ 4.208000183105469,
+ 4.208000183105469,
+ 4.216000080108643,
+ 4.216000080108643,
+ 4.216000080108643,
+ 4.23199987411499,
+ 4.23199987411499,
+ 4.223999977111816,
+ 4.184000015258789,
+ 250,
+ 4.136000156402588,
+ 4.136000156402588,
+ 4.136000156402588,
+ 4.136000156402588,
+ 4.111999988555908,
+ 4.111999988555908,
+ 4.0960001945495605,
+ 4.064000129699707,
+ 4.015999794006348,
+ 4.015999794006348,
+ 3.9839999675750732,
+ 3.9679999351501465,
+ 3.947999954223633,
+ 3.9119999408721924,
+ 3.875999927520752,
+ 3.875999927520752,
+ 3.8480000495910645,
+ 3.8239998817443848,
+ 3.7039999961853027,
+ 3.5920000076293945,
+ 3.259999990463257,
+ 3.259999990463257,
+ 3.240000009536743,
+ 3.2160000801086426,
+ 3.196000099182129,
+ 3.171999931335449,
+ 3.1519999504089355,
+ 3.1519999504089355,
+ 3.140000104904175,
+ 3.124000072479248,
+ 3.115999937057495,
+ 2.5959999561309814,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 250,
+ 250,
+ 250,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 250,
+ 250,
+ 250,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.6480000019073486,
+ 2.635999917984009,
+ 2.628000020980835,
+ 2.619999885559082,
+ 2.619999885559082,
+ 2.6080000400543213,
+ 2.5959999561309814,
+ 2.5880000591278076,
+ 2.5759999752044678,
+ 2.563999891281128,
+ 2.563999891281128,
+ 2.552000045776367,
+ 2.5399999618530273,
+ 2.5280001163482666,
+ 2.5199999809265137,
+ 2.51200008392334,
+ 2.51200008392334,
+ 2.5,
+ 2.48799991607666,
+ 2.4839999675750732,
+ 2.4719998836517334,
+ 2.4639999866485596,
+ 2.4639999866485596,
+ 2.4560000896453857,
+ 2.444000005722046,
+ 2.436000108718872,
+ 2.431999921798706,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4159998893737793,
+ 2.4200000762939453,
+ 2.431999921798706,
+ 2.444000005722046,
+ 2.4600000381469727,
+ 2.4600000381469727,
+ 2.4760000705718994,
+ 2.48799991607666,
+ 2.5,
+ 2.5160000324249268,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5480000972747803,
+ 2.563999891281128,
+ 2.5840001106262207,
+ 2.7079999446868896,
+ 2.684000015258789,
+ 2.684000015258789,
+ 2.5880000591278076,
+ 2.5880000591278076,
+ 2.5880000591278076,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.631999969482422,
+ 2.624000072479248,
+ 2.615999937057495,
+ 2.611999988555908,
+ 2.6040000915527344,
+ 2.6040000915527344,
+ 2.5920000076293945,
+ 2.5880000591278076,
+ 2.5840001106262207,
+ 2.5759999752044678,
+ 2.572000026702881,
+ 2.559999942779541,
+ 2.559999942779541,
+ 2.552000045776367,
+ 2.5480000972747803,
+ 2.5439999103546143,
+ 2.5399999618530273,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5160000324249268,
+ 2.507999897003174,
+ 2.503999948501587,
+ 2.503999948501587,
+ 2.5,
+ 2.492000102996826,
+ 2.492000102996826,
+ 2.4839999675750732,
+ 2.4800000190734863,
+ 2.4800000190734863,
+ 2.4760000705718994,
+ 2.4719998836517334,
+ 2.4679999351501465,
+ 2.4600000381469727,
+ 2.4600000381469727,
+ 2.4519999027252197,
+ 2.447999954223633,
+ 2.447999954223633,
+ 2.447999954223633,
+ 2.440000057220459,
+ 2.440000057220459,
+ 2.431999921798706,
+ 2.427999973297119,
+ 2.427999973297119,
+ 2.4240000247955322,
+ 2.4200000762939453,
+ 2.4200000762939453,
+ 2.4159998893737793,
+ 2.4119999408721924,
+ 2.4079999923706055,
+ 2.4040000438690186,
+ 2.4000000953674316,
+ 2.4000000953674316,
+ 2.4000000953674316,
+ 2.3959999084472656,
+ 2.3919999599456787,
+ 2.388000011444092,
+ 2.384000062942505,
+ 2.384000062942505,
+ 2.384000062942505,
+ 2.380000114440918,
+ 2.375999927520752,
+ 2.371999979019165,
+ 2.371999979019165,
+ 2.371999979019165,
+ 2.368000030517578,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.364000082015991,
+ 2.359999895095825,
+ 2.371999979019165,
+ 2.388000011444092,
+ 2.388000011444092,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.002000093460083,
+ 2.002000093460083,
+ 2.002000093460083,
+ 2.002000093460083,
+ 2.002000093460083,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.9980000257492065,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.99399995803833,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.9919999837875366,
+ 1.996000051498413,
+ 1.996000051498413,
+ 2.0,
+ 2.009999990463257,
+ 2.009999990463257,
+ 2.0320000648498535,
+ 2.0480000972747803,
+ 2.0320000648498535,
+ 2.0179998874664307,
+ 2.007999897003174,
+ 2.007999897003174,
+ 1.996000051498413,
+ 1.9919999837875366,
+ 1.9900000095367432,
+ 1.9880000352859497,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.9980000257492065,
+ 1.9980000257492065,
+ 2.0,
+ 2.002000093460083,
+ 2.0,
+ 2.002000093460083,
+ 1.9759999513626099,
+ 1.9759999513626099,
+ 1.9359999895095825,
+ 2.0360000133514404,
+ 2.0859999656677246,
+ 2.0439999103546143,
+ 1.9819999933242798,
+ 1.9819999933242798,
+ 1.9279999732971191,
+ 1.9559999704360962,
+ 1.9559999704360962,
+ 1.9600000381469727,
+ 1.9579999446868896,
+ 1.9559999704360962,
+ 1.9559999704360962,
+ 1.9579999446868896,
+ 1.9620000123977661,
+ 1.9639999866485596,
+ 1.968000054359436,
+ 1.9700000286102295,
+ 1.9700000286102295,
+ 1.972000002861023,
+ 1.9739999771118164,
+ 1.9739999771118164,
+ 1.9739999771118164,
+ 1.9780000448226929,
+ 1.9780000448226929,
+ 1.9839999675750732,
+ 1.9859999418258667,
+ 1.9859999418258667,
+ 1.9900000095367432,
+ 1.99399995803833,
+ 1.99399995803833,
+ 1.996000051498413,
+ 1.99399995803833,
+ 1.9980000257492065,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.003999948501587,
+ 2.5999999046325684,
+ 1.6720000505447388,
+ 2.0480000972747803,
+ 2.115999937057495,
+ 2.115999937057495,
+ 2.191999912261963,
+ 2.196000099182129,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5160000324249268,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5280001163482666,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5239999294281006,
+ 2.5239999294281006,
+ 2.5280001163482666,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5320000648498535,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5360000133514404,
+ 2.5399999618530273,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5439999103546143,
+ 2.5480000972747803,
+ 2.555999994277954,
+ 2.559999942779541,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.3040000200271606,
+ 1.3040000200271606,
+ 1.2979999780654907,
+ 1.2920000553131104,
+ 1.2920000553131104,
+ 1.2860000133514404,
+ 1.2899999618530273,
+ 1.2999999523162842,
+ 1.3020000457763672,
+ 1.3020000457763672,
+ 1.3020000457763672,
+ 1.2999999523162842,
+ 1.2960000038146973,
+ 1.3020000457763672,
+ 1.312000036239624,
+ 1.315999984741211,
+ 1.315999984741211,
+ 1.3179999589920044,
+ 1.3279999494552612,
+ 1.3359999656677246,
+ 1.3420000076293945,
+ 1.2200000286102295,
+ 1.2200000286102295,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.090000033378601,
+ 1.0880000591278076,
+ 1.0880000591278076,
+ 1.0880000591278076,
+ 1.0920000076293945,
+ 1.0959999561309814,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0980000495910645,
+ 1.0959999561309814,
+ 1.0959999561309814,
+ 1.0959999561309814,
+ 1.0959999561309814,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.093999981880188,
+ 1.0920000076293945,
+ 1.0920000076293945,
+ 1.0920000076293945,
+ 1.0920000076293945,
+ 1.093999981880188,
+ 1.1019999980926514,
+ 1.1139999628067017,
+ 1.1239999532699585,
+ 1.1239999532699585,
+ 1.1299999952316284,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 5.888000011444092,
+ 5.888000011444092,
+ 5.888000011444092,
+ 5.888000011444092,
+ 5.863999843597412,
+ 5.863999843597412,
+ 5.808000087738037,
+ 5.751999855041504,
+ 5.751999855041504,
+ 5.74399995803833,
+ 5.74399995803833,
+ 5.656000137329102,
+ 5.656000137329102,
+ 5.656000137329102,
+ 5.584000110626221,
+ 5.584000110626221,
+ 5.584000110626221,
+ 5.584000110626221,
+ 5.552000045776367,
+ 5.559999942779541,
+ 5.599999904632568,
+ 5.599999904632568,
+ 6.71999979019165,
+ 6.783999919891357,
+ 6.831999778747559,
+ 6.815999984741211,
+ 6.696000099182129,
+ 6.552000045776367,
+ 6.552000045776367,
+ 6.423999786376953,
+ 6.343999862670898,
+ 6.328000068664551,
+ 9.232000350952148,
+ 9.192000389099121,
+ 9.192000389099121,
+ 9.175999641418457,
+ 9.144000053405762,
+ 9.039999961853027,
+ 8.960000038146973,
+ 7.943999767303467,
+ 7.943999767303467,
+ 7.863999843597412,
+ 7.8480000495910645,
+ 7.791999816894531,
+ 7.751999855041504,
+ 7.736000061035156,
+ 7.736000061035156,
+ 7.71999979019165,
+ 7.728000164031982,
+ 7.728000164031982,
+ 7.711999893188477,
+ 7.696000099182129,
+ 7.696000099182129,
+ 7.688000202178955,
+ 7.696000099182129,
+ 7.696000099182129,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.688000202178955,
+ 7.736000061035156,
+ 7.800000190734863,
+ 8.616000175476074,
+ 8.600000381469727,
+ 8.600000381469727,
+ 8.576000213623047,
+ 8.543999671936035,
+ 8.520000457763672,
+ 8.51200008392334,
+ 8.496000289916992,
+ 8.496000289916992,
+ 8.472000122070312,
+ 8.456000328063965,
+ 8.4399995803833,
+ 8.4399995803833,
+ 8.423999786376953,
+ 8.423999786376953,
+ 8.383999824523926,
+ 8.37600040435791,
+ 8.37600040435791,
+ 8.368000030517578,
+ 8.35200023651123,
+ 8.35200023651123,
+ 8.35200023651123,
+ 8.312000274658203,
+ 8.248000144958496,
+ 8.223999977111816,
+ 8.288000106811523,
+ 8.288000106811523,
+ 8.4399995803833,
+ 8.607999801635742,
+ 8.776000022888184,
+ 8.895999908447266,
+ 9.064000129699707,
+ 9.064000129699707,
+ 9.168000221252441,
+ 11.84000015258789,
+ 12.175999641418457,
+ 12.312000274658203,
+ 12.656000137329102,
+ 12.656000137329102,
+ 12.767999649047852,
+ 13.208000183105469,
+ 13.35200023651123,
+ 13.656000137329102,
+ 13.64799976348877,
+ 13.64799976348877,
+ 13.527999877929688,
+ 14.015999794006348,
+ 13.960000038146973,
+ 14.119999885559082,
+ 14.255999565124512,
+ 14.255999565124512,
+ 13.784000396728516,
+ 15.407999992370605,
+ 15.496000289916992,
+ 15.53600025177002,
+ 15.423999786376953,
+ 15.423999786376953,
+ 15.37600040435791,
+ 15.416000366210938,
+ 15.423999786376953,
+ 15.383999824523926,
+ 15.343999862670898,
+ 15.343999862670898,
+ 15.368000030517578,
+ 15.32800006866455,
+ 15.255999565124512,
+ 15.272000312805176,
+ 15.272000312805176,
+ 15.272000312805176,
+ 15.215999603271484,
+ 15.272000312805176,
+ 15.295999526977539,
+ 15.208000183105469,
+ 15.215999603271484,
+ 15.215999603271484,
+ 15.208000183105469,
+ 15.208000183105469,
+ 15.192000389099121,
+ 15.168000221252441,
+ 15.168000221252441,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.639999389648438,
+ 16.6560001373291,
+ 16.719999313354492,
+ 16.719999313354492,
+ 16.639999389648438,
+ 16.672000885009766,
+ 16.687999725341797,
+ 16.54400062561035,
+ 16.576000213623047,
+ 16.6560001373291,
+ 16.6560001373291,
+ 16.27199935913086,
+ 16.583999633789062,
+ 16.079999923706055,
+ 16.399999618530273,
+ 16.736000061035156,
+ 16.736000061035156,
+ 16.143999099731445,
+ 250,
+ 20.304000854492188,
+ 20.304000854492188,
+ 20.304000854492188,
+ 20.304000854492188,
+ 250,
+ 250,
+ 250,
+ 20.20800018310547,
+ 20.20800018310547,
+ 20.20800018310547,
+ 20.20800018310547,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250,
+ 250};
+
+// std::vector<float> target_scan_1 = {2.03169,1.9677,2.02369,2.01969,1.9517,1.93171,1.93171,1.89171,1.92771,1.92771,1.89171,1.85172,1.83172,1.85972,1.85172,1.79173,1.55576,1.54376,1.53577,1.49577,1.49577,1.49577,1.45178,1.47977,1.47977,1.45578,1.59576,250,250,250,1.59576,1.59576,1.59576,1.59576,1.54776,1.55176,1.53577,1.53577,1.51977,1.50377,1.49577,1.48377,1.47977,1.44378,1.47178,1.47578,1.44378,1.42378,1.41978,1.42778,1.42378,1.41179,1.3198,1.26781,1.22781,1.24781,1.21981,1.20782,1.23581,1.19582,1.22781,1.19982,1.17982,1.20382,1.17582,1.17982,1.16782,1.16382,1.15582,1.15182,1.15982,1.14783,1.13183,1.14783,1.12383,1.15182,1.14383,1.13583,1.24781,1.24781,1.19582,1.2958,1.3118,1.3038,1.2958,1.2998,1.2998,1.3038,1.2978,1.2858,1.2818,1.27581,1.2838,1.2878,1.27981,1.3078,1.2898,1.27781,1.2878,1.27981,1.3018,1.2998,1.2938,1.2958,250,250,1.19582,1.21182,1.20382,1.18382,1.23581,1.19582,250,250,250,1.9777,1.9837,1.9777,1.9857,1.9917,2.09168,250,4.49132,4.48732,4.48332,4.48532,4.47732,4.47132,4.48532,4.47332,4.47532,4.46332,4.47332,4.47132,4.46332,4.49931,4.96124,5.04323,6.571,6.567,6.579,6.579,6.64299,7.55685,7.52685,7.56485,7.93079,250,250,8.70467,8.69668,8.69268,8.72067,8.72067,8.72067,8.73267,8.73667,8.76467,8.76267,8.76467,5.01924,5.01524,5.02723,5.02723,5.03123,5.05523,5.04323,5.07123,5.07123,5.07123,5.09722,5.01924,4.92325,4.68529,4.64729,4.66929,4.66529,4.68929,4.68729,4.70128,4.71728,4.70928,4.73928,4.74928,4.75128,4.76927,4.76327,4.79527,4.79527,4.80927,4.82926,4.83526,4.85326,4.86926,4.86726,4.88926,4.90725,4.92325,4.94325,4.95125,4.97124,5.00524,3.13952,3.11753,3.06953,3.03754,2.99154,2.96555,2.91156,2.87756,2.83757,2.22366,2.18967,2.16167,2.15367,2.15167,2.14167,2.17367,2.16767,2.16367,2.18567,2.32765,2.37964,2.44363,2.43163,2.41763,2.38564,2.37364,2.34164,2.30165,2.27565,2.26965,2.30565,2.31765,2.33764,2.34364,2.36164,2.37364,2.38364,2.40563,2.41163,2.41963,2.44163,2.45363,2.47362,2.48362,2.49162,2.51162,2.52961,2.55961,2.56561,2.57361,2.5996,2.6136,2.6396,2.6576,2.66959,2.69559,250,2.71959,7.15891,7.17491,7.2469,7.27889,7.37888,9.08662,9.06062,8.99063,8.90664,8.84865,8.76266,8.68668,8.67868,8.66668,8.66268,8.73067,8.81066,8.91064,9.01063,9.11461,9.2106,9.28259,9.43456,9.49056,9.62653,9.73252,9.8625,9.96848,10.0825,10.2244,10.3484,10.4724,10.6244,10.7524,10.9003,11.0623,11.2203,11.4623,11.6362,11.8122,11.9922,3.79342,3.78142,3.77343,3.76743,3.75143,3.73943,3.73743,3.71743,3.71143,3.70544,3.69144,3.68544,3.67344,3.67544,3.66144,3.63945,3.64145,3.62945,3.63345,3.61745,3.61345,3.61145,3.60345,3.60345,3.58545,3.58545,3.57746,3.55746,3.56946,3.56946,3.54746,3.55546,3.53346,3.54146,3.53346,3.52746,3.52146,3.50947,3.51547,3.50547,3.48947,3.50347,3.49947,3.50147,3.48547,3.47747,3.49147,3.47947,1.90571,1.90571,2.38764,2.37764,2.36764,3.47747,3.47747,3.45947,3.46147,3.46547,3.47747,3.45747,3.46747,3.48147,3.46747,3.47947,3.48147,3.46147,3.47947,3.47347,3.48147,3.47347,3.47147,2.38764,2.36364,1.90171,1.88771,1.88771,3.48547,3.59145,5.19121,4.08338,3.88341,3.69544,3.58345,4.18136,4.17536,3.2915,2.89156,2.83157,2.77958,2.79157,2.82357,2.81957,2.82557,2.84357,250,250,1.9677,2.0037,250,250,2.87956,2.86156,2.85956,2.87756,2.87356,2.89956,2.89956,2.89356,2.91356,2.90756,2.92955,4.41933,4.42333,4.44332,4.44332,4.47132,4.47532,1.43178,1.42778,1.37579,1.35979,1.3418,1.3298,1.3098,1.2858,1.2938,1.2818,1.25581,1.34779,1.44978,1.46778,1.47178,1.48177,1.49377,1.49177,1.51777,1.51377,1.49977,1.51977,1.52377,1.54376,1.54976,1.54376,1.55976,1.55976,1.57176,1.57576,1.57576,1.59176,1.59376,1.60975,1.61575,1.61975,1.62775,1.63775,1.65375,1.65175,1.66575,1.68174,1.68174,1.69774,1.70574,1.70574,1.73174,1.72774,1.75173,1.74973,1.76573,1.77573,1.78573,1.80573,1.81372,1.81372,1.83372,1.84572,1.86772,1.87571,1.88771,1.91171,1.92571,1.93371,1.9597,1.9637,1.9917,1.9857,2.01969,2.03169,2.03569,2.05969,2.07168,2.09368,2.11768,2.13168,2.15967,2.16967,2.19967,2.21566,2.23166,2.25966,2.28365,2.27765,2.25766,2.23566,2.23166,2.21166,2.21966,2.20766,2.19767,2.21566,2.24766,4.41933,4.39133,4.38333,4.37533,4.35134,4.32334,4.30334,4.29935,4.28335,4.29135,4.25935,4.24335,4.21536,4.21136,4.19536,4.19536,4.17137,4.15137,4.15137,4.11537,4.12737,4.09538,3.79542,3.79142,3.87541,3.9314,4.03139,4.45332,4.44332,3.04754,3.03554,3.05353,3.08753,2.98755,2.98755,3.21951,3.2715,3.07753,3.03954,3.03554,2.85357,2.86556,2.91156,2.90956,3.01554,3.03754,3.01754,3.00754,2.89556,2.90156,2.88556,3.02154,3.37349,3.33949,2.95155,2.92156,2.91956,2.91156,2.91156,2.91956,2.95155,2.99954,3.2595,3.65944,4.18736,4.18336,4.19536,4.19536,2.79557,2.93355,2.97955,3.01954,3.03554,3.05554,3.08753,3.08753,3.12552,3.12552,3.12952,3.15352,3.13952,3.17352,3.69944,3.82142,3.89141,250,2.19167,2.17967,2.13567,2.08368,2.04369,2.04369,1.9837,1.9677,1.92371,1.69174,1.64975,1.63375,1.62775,1.62775,1.62375,1.63375,1.63775,1.64175,1.65775,1.67774,1.68774,1.90371,1.9477,1.9837,1.9797,1.9877,1.9757,1.93171,1.9557,1.9437,1.9637,1.89171,1.78173,1.76173,1.75173,1.76573,1.74573,1.73974,1.76373,1.75973,1.77973,1.77373,1.75373,1.79573,4.64729,4.6193,4.63129,4.63929,4.65529,4.67529,4.70328,4.71928,4.70328,4.33934,250,4.11537,4.09938,4.07538,4.01739,3.98539,3.98339,3.9614,3.97539,4.01539,1.70574,1.67774,1.65975,1.66575,1.64775,1.63975,1.62175,1.59976,1.65175,1.68974,1.69974,1.69774,1.63975,1.64575,1.63175,1.63575,1.64975,1.62775,1.63975,1.64575,1.64375,1.65375,1.64975,1.65175,1.65575,1.65775,1.67574,1.67175,1.68974,1.68974,1.69974,1.70574,1.71774,1.72974,1.73574,1.76173,1.76973,1.78373,1.81972,1.89171,250,250,250,250,5.05923,5.16521,5.15122,5.10922,5.09722,5.05923,5.03123,5.00924,4.97724,4.95525,4.92925,4.90325,4.89126,4.85326,4.85126,4.81727,4.52731,4.51531,4.49132,4.48332,4.45932,4.43532,4.42333,4.39533,4.39333,4.37533,4.35734,4.33134,4.31334,4.30534,4.28735,4.32134,4.38733,4.43732,4.47332,4.44732,4.43332,4.42533,4.26935,3.82742,3.81542,3.79942,3.80142,3.77942,3.77543,3.76943,3.74143,3.73943,3.72143,3.71543,3.69544,3.67544,3.65944,3.64345,3.63945,3.65744,3.63945,3.63345,3.61545,3.62345,3.60345,3.60545,3.58145,3.57146,2.75158,2.77758,2.77158,2.74758,2.71159,2.70559,2.68359,2.69559,2.71359,2.73358,2.75958,2.75758,2.81357,2.82957,3.00354,3.18152,3.2755,3.2735,3.2635,3.2615,3.2595,3.23951,3.24951,3.24351,3.23151,3.23951,3.22351,2.99754,2.98355,2.98555,2.99154,2.97555,2.99954,2.99154,2.98155,2.99354,2.97155,2.97755,2.97355,2.97755,2.98355,2.98155,2.98954,2.98755,2.97555,2.98555,2.96555,2.99154,4.01739,4.02139,4.02339,3.72943,3.57946,3.44348,3.2695,3.2555,3.2695,3.2755,3.2695,3.2735,3.2915,3.2815,3.3015,3.3075,3.3095,3.31949,3.3155,3.33749,3.32749,3.32749,4.04538,4.04738,4.12537,3.9674,3.82942,3.69744,3.51147,3.46547,3.48147,3.41148,3.46947,4.21136,4.21936,4.21936,4.23136,4.24935,4.25135,4.26135,3.56346,3.50347,3.45147,3.40548,3.40348,3.38348,3.38748,3.38149,3.37749,3.39948,3.39948,3.43748,3.47347,3.49947,3.59145,3.85141,3.84541,4.47132,4.52331,4.54331,3.9334,4.5813,4.17736,4.5953,4.62929,2.25366,2.19966,2.18567,2.17967,2.19967,2.23166,250,3.41948,2.06369,2.09168,2.07168,2.06369,2.07168,1.9917,1.9917};
+// std::vector<float> reference_scan_1 = {3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144};
+
+std::vector<float> reference_scan_1 ={3.64145,3.66544,3.62545,3.62945,3.61945,3.61545,2.81157,2.77558,2.77758,2.74958,2.73158,2.72159,2.73358,2.75158,2.78158,2.78558,2.80757,2.83557,2.87756,3.2835,3.2935,3.2995,3.2955,3.2915,3.2915,3.2775,3.2835,3.2635,3.2755,3.2655,3.2535,3.2595,3.20351,3.04754,3.04354,4.01539,3.08353,3.07153,3.07753,3.06953,3.06553,3.06753,3.05553,3.06353,3.04354,3.03154,3.00954,3.00554,3.02954,250,2.99354,2.99554,4.01739,4.03539,4.03339,3.84741,3.61545,3.56346,3.35149,3.2675,3.2695,3.2815,3.2795,3.2955,3.2995,3.2955,3.3155,3.3155,3.33149,3.32349,3.32349,3.35149,3.33549,3.36549,4.08338,4.07138,4.10937,4.01539,3.86741,3.71343,3.47947,3.48547,3.48747,3.43148,3.48547,3.48147,4.22736,4.22936,4.25335,4.24735,4.24735,4.26935,4.27135,3.51946,4.27735,3.43148,3.40348,3.39148,3.39548,3.38948,3.38548,3.39748,3.40148,3.41148,3.45347,3.47147,3.51946,250,3.84541,4.37333,4.51931,4.53131,4.54331,3.9414,2.94555,2.91956,2.93555,2.21166,2.17767,2.18167,2.16767,2.17767,2.04769,1.91971,1.85572,1.9917,250,250,250,250,1.9917,1.9917,1.9917,1.9917,2.03169,1.9917,1.9517,1.9597,1.90771,1.86772,1.87971,1.87971,1.85972,1.85172,1.82772,1.79173,1.53977,1.52777,1.50777,1.50777,1.47977,1.47977,1.47977,1.45178,1.43978,1.46778,1.46778,1.42378,250,250,1.59576,250,1.59576,250,1.59576,1.52377,1.51577,1.48377,1.49577,1.47977,1.48377,1.48377,1.46778,1.44778,1.44378,1.41978,1.44778,1.41978,1.41179,1.43978,1.39579,1.40379,1.26781,1.21581,1.22381,1.20382,1.23181,1.19582,1.20382,1.18382,1.16382,1.16982,1.15582,1.16182,1.16782,1.14583,1.13983,1.13983,1.14183,1.14183,1.13183,1.12383,1.13583,1.11583,1.12383,1.13983,1.09983,1.09583,1.09983,1.09583,1.13183,1.19582,250,250,1.3158,1.27181,1.2838,1.27181,1.27181,1.27981,1.25181,1.27581,1.27781,1.26381,1.26781,1.25781,1.25781,1.25781,1.25781,1.26781,1.26781,1.25181,1.27781,1.27581,1.2958,250,250,250,1.23581,1.17982,1.16582,1.19582,1.23581,250,250,250,1.9617,1.9537,1.9397,1.9657,1.9617,1.9517,1.9597,250,4.47732,4.45932,4.45532,4.45732,4.45332,4.45532,4.45332,4.44732,4.45532,4.44332,4.44932,4.43932,4.43932,4.43932,4.43532,4.86526,4.95725,6.575,6.551,6.543,6.555,6.559,250,250,250,250,250,7.58685,8.64468,8.64468,8.65868,8.66468,8.66268,8.68068,8.69068,8.70467,8.70867,8.71067,8.73867,8.73067,4.98324,4.99124,4.98724,5.01124,5.00324,5.00924,5.01524,5.01724,5.04323,5.03923,5.04323,4.99924,4.88326,4.67129,4.6093,4.6213,4.6313,4.63329,4.64729,4.66729,4.65929,4.68129,4.69129,4.71128,4.72328,4.72928,4.75328,4.74928,4.77127,4.77527,4.78727,4.81127,4.81527,4.82726,4.84726,4.85326,4.88326,4.88126,4.90925,4.91725,4.92725,4.96124,3.09553,3.10153,3.04554,2.98755,2.94555,2.91556,2.87756,2.83157,2.79557,2.77558,2.43763,2.13767,2.13368,2.10168,2.11168,2.10368,2.13567,2.13168,2.14567,2.28165,2.31565,2.39164,2.40163,2.38564,2.36164,2.32165,2.30965,2.26566,2.22366,2.23366,2.24166,2.27765,2.29165,2.30165,2.31965,2.32165,2.33964,2.36564,2.36564,2.38764,2.39763,2.40963,2.41763,2.42563,2.44963,2.45563,2.47162,2.48962,2.50562,2.54361,2.54561,2.56361,2.58561,2.59361,2.6296,2.6456,2.6576,2.67759,2.67359,7.15891,7.17091,7.2349,7.29889,7.35888,9.08062,8.99263,8.91664,8.85065,8.76867,8.71067,8.64268,8.74867,8.58669,8.65868,8.75467,8.84265,8.91464,9.04262,9.10661,9.23859,9.33058,9.44656,9.55455,9.65853,9.77851,9.88649,10.0185,10.1425,10.2704,10.4224,10.5384,10.6944,10.8423,10.9863,11.1423,11.3983,11.5582,11.7342,11.9262,12.0302,3.76743,3.74743,3.74543,3.72543,3.73343,3.70944,3.70744,3.68944,3.68144,3.68744,3.67544,3.66144,3.66544,3.64944,3.63745,3.63545,3.61745,3.62145,3.60945,3.59745,3.59345,3.58145,3.58545,3.56746,3.57146,3.55346,3.54546,3.55746,3.53746,3.53546,3.53746,3.52546,3.52746,3.51546,3.50747,3.50747,3.48947,3.49547,3.49147,3.47547,3.48347,3.48147,3.48147,3.47747,3.46147,3.47147,3.46347,3.47147,3.45947,1.86972,1.87771,2.33164,2.37564,3.44548,3.44548,3.46347,3.45147,3.46547,3.46547,3.44148,3.45747,3.43148,3.45947,3.45747,3.44947,3.46147,3.44947,3.46547,3.45147,3.45347,3.46347,2.36564,2.37564,2.34364,1.87771,1.87971,1.87371,250,4.19936,4.01939,3.83942,3.63945,3.58545,4.16936,4.16337,3.32349,2.99154,2.82357,2.75958,2.75558,2.78558,2.80357,2.80757,2.81757,2.79957,250,250,250,2.01169,250,250,250,2.87356,2.87156,2.86556,2.88756,2.88156,2.88156,2.89556,2.90356,2.91756,2.92355,4.41333,4.43532,4.43532,4.45932,4.46932,4.46932,4.50331,4.50731,1.42378,1.40379,1.36379,1.37179,1.3378,1.3318,1.3018,1.2978,1.2858,1.26781,1.2998,1.43978,1.45178,1.48177,1.47178,1.49977,1.49377,1.50577,1.51177,1.50977,1.53177,1.53177,1.52377,1.55376,1.55176,1.56976,1.57176,1.57176,1.59576,1.59176,1.60376,1.61375,1.61175,1.62575,1.62975,1.64575,1.65175,1.64975,1.66775,1.67375,1.69374,1.70974,1.70374,1.71974,1.71774,1.72374,1.74973,1.75173,1.77573,1.77373,1.78773,1.80972,1.80972,1.82972,1.83772,1.84772,1.86572,1.86972,1.89771,1.91371,1.92171,1.9457,1.9557,1.9777,1.9917,2.00569,2.01969,2.02769,2.06769,2.07968,2.08768,2.11568,2.13168,2.16367,2.17167,2.17967,2.22366,2.23566,2.26765,2.27565,2.29565,2.28965,2.25566,2.26765,2.24366,2.23366,2.21966,2.22366,2.22566,2.22166,4.43133,4.41933,4.39133,4.35934,4.35534,4.32734,4.33134,4.29135,4.29935,4.29135,4.27535,4.27935,4.24335,4.23935,4.20336,4.19136,4.18336,4.16337,4.16737,4.13537,4.11537,4.08738,3.83142,3.86741,3.89141,4.04338,4.11137,4.47332,4.45732,4.45332,4.43532,4.43932,4.42133,4.40933,4.40333,4.38333,3.10353,3.09153,3.07153,3.08353,3.09553,3.09753,3.02154,2.98755,3.15552,3.39148,3.10553,3.09753,2.93955,2.87156,2.87156,2.89156,2.95155,3.02354,3.06753,3.04954,3.06353,3.04354,3.03954,2.97955,2.94955,3.2835,4.21536,3.00354,2.99954,2.96755,2.97155,2.96755,2.97955,2.98555,3.03154,3.57546,4.21136,4.21536,4.22336,4.21136,4.23535,2.86356,2.92755,2.99354,3.03154,3.06753,3.08553,3.09553,3.12752,3.13552,3.15752,3.16152,3.15752,3.18152,3.17152,3.85341,3.86341,3.87941,250,2.20766,2.12768,2.07168,2.05969,2.04369,1.9877,1.9557,1.85372,1.77973,1.72174,1.68174,1.65975,1.65175,1.65375,1.66375,1.65975,1.66575,1.68374,1.67574,1.71174,1.76373,1.82372,1.84372,1.84372,1.84972,1.85572,1.86372,1.87971,1.87371,1.88571,1.90171,1.86972,1.80572,1.78173,1.79373,1.78573,1.78173,1.79373,1.78373,1.79773,1.79773,1.79773,1.81172,1.86372,4.67129,4.67529,4.69129,4.71528,4.71928,4.75928,4.76727,4.36733,4.46732,4.46732,4.09938,4.06338,4.03738,4.03339,4.01339,4.00139,1.75373,1.70974,1.70774,1.68174,1.67175,1.66975,1.65375,1.66775,1.65575,1.63975,1.64775,1.62975,1.64175,1.63975,1.64775,1.64175,1.64175,1.65175,1.65375,1.66375,1.66775,1.66775,1.68974,1.68974,1.69974,1.71774,1.72574,1.73574,1.73774,1.78173,1.79573,1.81172,1.85572,1.88971,250,250,250,250,5.19121,5.2252,5.20121,5.16921,5.14122,5.09522,5.07923,5.07123,5.02723,5.01724,4.98524,4.96524,4.94725,4.90725,4.90125,4.86126,4.84526,4.82127,4.79527,4.77927,4.76327,4.73528,4.72928,4.69129,4.64329,4.48532,4.47732,4.6153,4.5973,4.5893,4.5633,4.54531,4.53731,4.51131,4.50731,4.48932,4.46932,4.45932,3.99139,3.85941,3.84341,3.82742,3.82742,3.80342,3.81142,3.78742,3.77742,3.76543,3.74343,3.74743,3.72143,3.70544,3.69744,3.67344,3.66744,3.67744,3.66344,3.67144};
+std::vector<float> target_scan_1 ={1.93771,1.93171,1.9437,1.9597,1.9537,1.9737,1.9777,1.9757,1.9937,1.9617,1.89771,1.87371,1.85772,1.86372,1.85572,1.86372,1.86172,1.86372,1.88371,1.89171,1.91971,4.73128,4.74128,4.76927,4.76927,4.78927,4.80327,4.81927,4.83526,4.85126,4.31534,4.23136,4.19536,4.16337,4.14137,4.13537,1.82572,1.79173,1.78373,1.75373,1.76173,1.74773,1.73974,1.72774,1.71574,1.71574,1.70974,1.70374,1.70974,1.70374,1.71174,1.70774,1.70374,1.71174,1.69974,1.71574,1.71374,1.70774,1.71774,1.71374,1.72374,1.72574,1.74773,1.75773,1.75373,1.78173,1.77773,1.77173,1.79573,1.79773,1.82972,1.83772,1.85572,1.87571,1.92571,1.9737,250,250,250,250,250,250,250,5.14722,5.2752,5.2692,5.2252,5.18921,5.17521,5.13322,5.12122,5.08523,5.04923,5.03123,5.00524,4.98324,4.95525,4.92525,4.91325,4.86926,4.86126,4.83926,4.81727,4.80527,4.77527,4.75528,4.67729,4.53931,4.54331,4.65929,4.65529,4.63529,4.6153,4.6033,4.5773,4.5633,4.55131,4.52131,4.52131,4.48732,3.9194,3.89541,3.88741,3.87941,3.85941,3.85941,3.83542,3.81542,3.81142,3.78742,3.78142,3.76343,3.74943,3.73343,3.70744,3.72343,3.72143,3.70344,3.70544,3.68344,3.68344,3.66744,3.65144,3.65544,2.85157,2.83557,2.81357,2.78358,2.77158,2.76358,2.76358,2.76358,2.79357,2.82157,2.79557,2.85557,2.89356,3.14552,3.2775,3.32349,3.32949,3.3075,3.2995,3.3055,3.2975,3.3055,3.2855,3.2795,3.2855,3.2675,3.04954,3.02354,3.01954,3.03354,3.01754,3.02554,3.01354,3.00354,3.02154,3.00954,3.01354,3.00354,3.00354,3.01954,2.99954,3.01154,3.00754,3.01354,3.00754,2.99154,3.01354,4.03339,4.02939,4.03539,3.82342,3.62345,3.74343,3.2695,3.2815,3.2735,3.2875,3.2895,3.2875,3.2935,3.2955,3.3115,3.3135,3.3135,3.32949,3.32149,3.34349,3.33749,4.12537,4.10737,4.07138,4.05538,4.03738,3.85941,3.44947,3.42548,3.53146,3.44348,3.43548,3.48547,4.19136,4.20336,4.21336,3.59145,3.59145,3.59145,3.59145,3.59145,3.47147,3.45147,3.42348,3.45147,3.42348,3.34749,3.34749,3.44348,250,250,250,2.95955,2.93355,2.93555,2.92955,2.91556,2.88756,2.88756,2.92355,4.50331,2.19167,2.16767,2.13368,2.04369,1.91971,1.85972,1.83172,1.85972,1.79173,1.76773,1.73174,1.79173,1.84772,1.79173,1.79173,1.86372,1.79173,1.86372,1.89171,1.84772,1.89171,1.87571,1.89171,1.84372,1.85572,1.66775,1.51577,1.45978,1.47977,1.47178,1.42778,1.43178,1.41179,1.40779,1.37979,1.39579,1.39579,1.37579,1.35579,1.35979,1.3078,1.3398,1.3278,1.39579,1.39579,1.39579,250,1.39579,1.43578,1.44778,1.39579,1.45978,1.41578,1.42778,1.43978,1.41179,1.39579,1.36779,1.37979,1.37179,1.3158,1.3318,1.21182,1.17582,1.15582,1.14783,1.11183,1.12383,1.11183,1.12183,1.09783,1.10783,1.10183,1.08783,1.08783,1.08384,1.08384,1.07384,1.06784,1.06984,1.05784,1.06184,1.05584,1.04784,1.05784,1.03784,1.05184,1.04184,1.03584,1.05584,1.01984,1.02384,1.03584,1.01585,1.00785,1.00785,1.09583,250,250,1.19582,250,1.19582,1.17982,1.19582,1.17782,1.17582,1.18782,1.17582,1.17182,1.17382,1.16582,1.17982,1.16782,1.13583,1.12783,1.09583,1.12783,1.08384,1.09583,1.08384,1.07584,1.06384,1.05184,1.03384,1.05384,1.05184,1.04784,1.04184,1.05584,1.05584,1.05984,1.06784,1.12383,1.84772,1.83972,1.85172,1.85972,1.84772,1.85572,1.85972,1.89171,250,4.36734,4.36334,4.37333,4.35534,4.36534,4.36933,4.35934,4.36733,4.35134,4.34734,4.36134,4.33734,4.34934,4.33734,4.35334,4.34134,4.77927,4.85926,4.90325,6.45502,6.45902,6.44702,6.45102,8.5347,250,7.61484,7.59484,8.5287,8.5347,8.5427,8.5447,8.5527,8.5627,8.5627,8.5727,8.57869,8.58269,8.60069,8.59469,8.61669,8.62469,4.86726,4.88326,4.88926,4.89725,4.89925,4.90725,4.92325,4.91525,4.94125,4.93525,4.94725,4.90125,4.79127,4.54931,4.52131,4.51731,4.54331,4.53731,4.55331,4.5633,4.5693,4.6033,4.6073,4.6193,4.6233,4.64729,4.65329,4.66729,4.68129,4.69528,4.72328,4.72128,4.73528,4.75728,4.75728,4.79327,4.79927,4.80727,4.82527,4.83126,4.85726,4.87526,4.89126,3.02154,2.99954,2.98155,2.91556,2.87756,2.85357,2.80757,2.78958,2.75158,2.70359,2.67559,2.6176,2.06569,2.04769,2.03569,2.04369,2.03369,2.03969,2.04969,2.06169,2.10168,2.22166,2.30765,2.32165,2.31165,2.29765,2.25766,2.24566,2.21766,2.17367,2.17767,2.16567,2.19967,2.21966,2.22566,2.24366,2.23966,2.27165,2.27965,2.29365,2.31965,2.32765,2.35564,2.35764,2.36564,2.39164,2.40163,2.41763,2.43363,2.44563,2.47162,2.47962,2.51762,2.52162,2.54161,2.56561,2.57961,2.6056,2.59161,2.6076,7.2149,7.2469,7.30689,9.00663,8.91864,8.85865,8.77066,8.71467,8.63868,8.57469,8.69468,8.5327,8.62469,8.70867,8.80066,8.90464,8.99663,9.10061,9.2046,9.29259,9.43456,9.51655,9.66053,9.75451,9.88649,10.0145,10.1425,10.2784,10.4224,10.5424,10.7024,10.8423,11.0103,11.1783,11.4323,11.6122,11.7702,11.9642,12.0702,12.0462,3.74743,3.70744,3.70944,3.70744,3.69144,3.68144,3.66544,3.66944,3.64944,3.64544,3.64744,3.62345,3.62745,3.61145,3.60545,3.60545,3.58945,3.59345,3.57346,3.57346,3.56346,3.54146,3.55546,3.53746,3.53746,3.54146,3.53346,3.52746,3.51746,3.50947,3.51147,3.49547,3.50347,3.49547,3.48747,3.48947,3.47547,3.47947,3.47347,3.46147,3.46947,3.46147,3.46947,3.46347,3.45147,3.46347,3.44548,3.45347,3.45347,3.44748,3.45147,250,1.86972,1.87172,1.86772,3.43548,3.43348,3.46547,3.44548,3.43548,3.44947,3.44348,3.46747,3.44947,3.43748,3.44747,3.44947,3.45547,3.45547,3.45547,3.46347,3.45747,3.47147,2.36764,2.37164,2.35964,3.47547,1.89771,1.88971,1.87971,1.91171,3.68144,3.59345,4.16936,4.18736,4.20936,3.2695,250,2.79158,2.77758,2.78958,2.81557,2.82357,2.81957,2.82357,2.85357,2.84357,250,250,250,2.09168,2.04369,250,250,2.89356,2.89956,2.89956,2.91556,2.91956,2.92156,2.93755,2.93555,4.46932,4.47532,4.48332,4.50331,4.51331,4.53331,4.54331,4.55131,4.5813,4.54931,4.52131,1.49577,1.45978,1.43978,1.40379,1.38379,1.37979,1.35579,1.3318,1.3218,1.3318,1.3138,1.3018,1.3118,1.49977,1.52577,1.52377,1.52777,1.53977,1.54376,1.56376,1.57176,1.56376,1.58776,1.58376,1.60376,1.60376,1.60975,1.62575,1.62975,1.64575,1.64575,1.65375,1.66775,1.66575,1.68774,1.70174,1.70374,1.71374,1.72574,1.74373,1.74573,1.75573,1.78573,1.78973,1.80173,1.81772,1.81772,1.84172,1.84972,1.86772,1.87371,1.88971,1.90971,1.91371,1.9457,1.9617,1.9737,1.9897,1.9997,2.02369,2.03169,2.04569,2.07968,2.08768,2.12168,2.13368,2.13967,2.17967,2.18767,2.21966,2.23966,2.24366,2.27965,2.29965,2.34364,2.36164,2.37564,2.34364,2.32765,2.33564,2.31165,2.29165,2.29165,2.29565,2.29165,2.29165,4.49132,4.46332,4.43532,4.43133,4.41533,4.39933,4.38333,4.36334,4.33534,4.34734,4.30334,4.29535,4.27935,4.28335,4.29135,4.24735,4.26735,4.22736,4.19936,4.08738,3.87141,3.9594,4.03139,4.23136,4.55131,4.54531,4.52931,4.51731,4.52131,4.50531,4.50131,4.48932,4.47532,4.46932,3.2755,3.21951,3.21151,3.20751,3.24351,3.24151,3.11153,3.11553,3.12552,3.13152,3.09153,3.01954,2.94755,2.96355,3.03554,3.04354,3.09753,3.09953,3.09953,3.07753,3.08953,3.07553,3.01954,3.09353,3.14352,3.09153,3.46547,3.13152,3.09953,3.09153,3.09353,3.13152,3.17352,3.76143,4.31734,4.31934,4.30934,4.31734,4.30934,4.32534,4.31334,4.30934,4.30934,3.9354,3.9174,3.61945,3.9314,3.9394,3.9374,3.9494,3.9454,3.9394,3.9654,3.9454,3.99139,2.09168,2.17167,2.19167,2.08768,2.08768,2.03169,1.88771,1.87971,1.85372,1.83372,1.78973,1.74973,1.74773,1.73774,1.75173,1.74573,1.74773,1.76173,1.75773,1.78173,1.79173};
+
+std::vector<float> reference_scan_2 ={4.86126,4.82527,4.84126,4.83326,4.85326,250,250,4.43932,4.43332,5.68313,5.04523,4.74728,4.80527,5.59715,5.01724,5.44117,5.54915,5.54516,5.51716,4.96524,5.16921,5.47317,4.28935,4.27335,5.46517,4.5893,5.43117,5.42317,5.41318,5.40318,4.68529,4.67329,5.36718,5.36518,5.36118,5.30319,5.33719,5.31519,5.38718,5.39518,5.45717,5.43317,5.44117,5.41718,5.39318,5.42117,5.39318,5.36118,5.34919,5.33719,5.35718,5.32919,5.33719,5.30519,5.32919,5.30519,5.32119,5.29319,5.30519,5.32119,5.30119,5.32119,5.2772,5.30519,5.30519,5.29319,5.2812,5.2732,5.29319,13.6039,13.5939,13.6079,13.6119,13.6159,13.6299,13.6099,13.6299,13.6179,13.6259,13.6419,11.5742,11.5642,6.51301,6.549,6.553,6.52101,6.45702,250,250,250,250,250,250,250,250,5.43317,5.38718,5.37118,5.30519,5.32519,5.31519,3.2655,3.11153,2.98155,2.78558,2.81157,2.73358,2.71359,2.6276,2.55361,2.49762,2.44163,2.37364,2.33564,2.27365,2.22766,2.17967,2.12768,2.08768,2.05369,2.01969,1.9757,1.93371,1.87971,1.86772,1.84372,1.81572,1.77573,1.76373,1.72574,1.70174,1.67575,1.65975,1.62975,1.61775,1.59176,1.57176,1.53977,1.52777,1.49577,1.48777,1.44778,1.43578,1.41578,1.39779,1.38779,1.36579,1.37579,1.35979,1.35579,1.35579,1.38179,1.39379,1.38779,1.39379,1.40779,1.40579,1.42178,1.42178,1.42178,1.43178,1.43578,1.44378,1.45178,1.45578,1.47178,1.48377,1.48977,1.48977,1.50377,1.52777,1.52577,1.55176,1.54976,1.55176,1.58176,1.57976,1.61175,1.59976,1.62775,1.62575,1.63775,1.63775,1.62975,1.64775,1.67575,1.67974,1.91571,1.82372,1.72174,1.68974,1.67774,1.69974,1.70774,1.74773,1.78373,1.79173,1.82372,1.84772,1.89371,1.91771,1.9477,1.9897,2.01769,2.05969,2.09568,2.11968,2.17167,2.20966,2.25966,2.30365,2.33564,2.40563,2.77558,2.13767,2.17167,2.23366,2.27365,2.31765,2.37764,2.78158,2.80957,2.84157,2.86556,2.90956,2.93755,2.98954,3.01754,3.04754,3.10353,3.14352,3.18352,3.19351,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,250,3.07353,2.97755,2.96755,3.03754,3.11353,3.18951,3.2615,250,1.21781,1.21981,1.21382,1.22581,1.22381,1.22381,1.21182,1.20382,1.21581,1.20382,1.20582,1.20582,1.19982,1.20582,1.20382,1.21382,1.20582,1.18982,1.18982,1.18982,1.20182,1.19782,1.18782,1.19582,1.18182,1.19982,1.19582,1.18182,1.19582,1.18182,1.20382,1.18982,1.18182,1.19582,1.19382,1.19382,1.18582,1.18382,1.19182,1.19182,1.19382,1.18782,1.18382,1.18982,1.19382,1.19382,1.16782,1.10983,0.751886,0.761884,0.761884,0.735888,0.731889,0.735888,0.731889,0.743887,0.743887,0.735888,0.751886,0.739887,0.747886,0.749886,0.743887,0.751886,0.743887,0.759884,0.753885,0.743887,0.751886,0.751886,0.749886,0.759884,0.757885,0.763884,0.763884,0.773882,0.759884,0.773882,0.763884,0.763884,0.777882,0.763884,0.777882,0.779881,0.775882,0.78788,0.775882,0.791879,0.78988,0.791879,0.791879,0.791879,0.797879,0.791879,0.795879,0.807877,0.803878,0.811876,0.811876,0.811876,0.823875,0.815876,0.831873,0.811876,0.823875,0.835873,0.831873,0.835873,0.839872,0.839872,0.85587,0.85187,0.863868,0.85587,0.85587,0.865868,0.85587,0.871867,0.867868,0.871867,0.883865,0.879866,0.897863,0.893864,0.897863,0.909861,0.901863,0.91986,0.91786,0.923859,0.931858,0.931858,0.939857,0.943856,0.953855,0.959854,0.959854,0.977851,0.971852,0.999848,0.991849,0.995848,1.00785,1.00585,1.02784,1.01984,1.02984,1.03984,1.04584,1.06384,1.06584,1.06784,1.08384,1.07984,1.10383,1.10783,1.10783,1.12383,1.12783,1.14383,1.15582,1.15582,1.17982,1.17982,1.20382,1.20982,1.21981,1.23181,1.23381,1.24981,1.26381,1.27581,1.3098,1.3078,1.3358,1.3358,1.35179,1.36579,1.37379,1.40179,1.41378,1.41778,1.44178,1.45378,1.48977,1.49177,1.50977,1.53377,1.54976,1.58176,1.59776,1.60975,1.63775,1.65375,1.68974,1.71374,1.73374,1.77573,1.79173,1.82772,1.83972,1.88771,1.90771,1.9577,1.9877,2.01569,2.05569,2.08768,2.12368,2.16567,2.25766,2.39364,2.32365,2.33764,2.39364,2.39364,250,2.46762,2.55361,250,250,250,3.40748,3.39748,3.39748,3.39548,3.40348,3.47347,3.58145,4.28535,4.45532,4.5973,4.72128,250,250,250,250,7.8888,250,250,9.11661,9.8685,10.0405,9.98848,250,12.3841,13.8739,13.8639,13.8519,17.6173,19.1251,25.3661,25.7701,25.7561,25.8161,13.8179,13.8239,13.8299,13.8359,250,9.93649,9.94449,7.56485,9.95848,9.96648,9.94849,9.98448,8.29874,8.30474,8.31273,250,250,250,250,250,250,250,250,10.1125,10.0445,250,8.18875,8.22875,250,7.34888,7.32488,7.34088,7.34888,7.37288,7.37688,7.38488,7.40887,10.2564,10.2604,250,250,10.3024,10.3844,250,10.3544,7.63684,250,7.18891,250,5.2652,5.2212,5.2332,5.19321,5.19721,5.08123,5.20521,5.19321,7.58884,4.13337,4.10138,4.08738,4.08738,4.11137,4.12137,3.9734,3.9374,3.9094,3.9294,5.12122,4.69329,4.66929,4.5933,4.54131,4.46932,4.84526,4.85726,4.86926,4.89325,4.89325,4.92125,4.98124,5.00524,4.15937,4.12537,4.04538,4.01339,3.9694,3.9214,3.88541,3.83342,3.79542,3.75143,3.70944,3.67144,3.63545,3.61745,3.57346,3.55146,3.52146,3.48347,3.45547,3.41948,3.40948,3.37749,3.32549,3.3175,3.2815,3.2695,3.24151,3.24151,3.22151,3.19351,3.21751,3.22751,3.24151,1.3118,1.2978,1.3038,1.2918,1.2958,1.2978,1.27381,1.26781,1.26181,1.24381,1.24781,1.23781,1.24181,1.21382,1.21981,1.20582,1.19582,1.19582,1.18582,1.17982,1.16582,1.15582,1.15982,1.14783,1.15382,1.14583,1.12783,1.13183,1.11983,1.12583,1.11383,1.10183,1.10183,1.09183,1.10183,1.09183,1.07784,1.07984,1.06384,1.07784,1.06384,1.05384,1.06384,1.03784,1.05384,1.04384,1.03184,1.04984,1.02784,1.03784,1.03384,1.01585,1.01984,1.00185,1.01585,1.00985,0.999848,1.00785,0.993849,1.00185,0.995848,0.993849,0.989849,0.98585,0.98585,0.977851,0.98385,0.961854,0.971852,0.963853,0.939857,0.975851,0.965853,0.953855,1.00385,1.02984,1.07784,1.10583,1.11783,1.16582,1.20182,1.23781,1.2978,1.36979,1.44978,1.50977,1.58176,1.60975,1.64375,1.70174,1.70574,1.77773,1.86572,1.73774,1.70374,1.67774,1.68774,1.68974,1.67574,1.68974,1.68974,1.70574,1.70374,1.75773,1.84172,1.82172,1.81772,1.79573,1.79573,1.80573,1.79773,1.80173,1.79373,1.81972,1.81972,1.82972,1.88171,7.93879,7.95679,7.95879,7.17291,6.68498,6.70498,6.74097,6.69698,6.73897,6.71498,6.70098,6.60899,6.64099,8.02078,8.02478,8.02078,8.04877,8.04278,7.71683,7.67083,7.40487,7.69083,7.69683,7.70083,7.72282,250,8.14876,8.15476,6.10507,6.04908,5.84111,5.73313,5.62714,5.52916,5.50916,5.53716,5.85911,5.82511,5.78112,5.73313,5.65314,5.65314,5.63714,8.45471,8.46071,8.48071,8.5087,8.5207,8.5567,5.54516,5.50916,5.48317,5.45517,5.46717,5.46317,5.46717,5.47117,5.47317,5.47717,5.50916,5.58115,5.64914,8.95664,8.99063,8.97663,250,250,2.89356,2.86556,2.85157,2.85756,2.79357,2.79357,2.79357,2.69359,2.74158,2.59361,2.59361,2.59361,2.59361,2.53361,2.49362,2.43363,2.47762,2.43363,2.46562,2.41763,2.32165,2.21366,2.18567,2.19767,2.17967,2.16967,2.22166,2.24566,2.39364,2.35764,2.39364,2.39364,2.49362,2.49362,2.59361,250,2.59361,2.59361,250,250,250,250,250,250,250,250,2.89356,2.79357,2.89356,250,250,2.91956,2.90356,2.92156,250,2.99354,3.07353,3.00554,3.01354,3.03754,3.41748,3.47747,3.43748,250,3.50947,3.42348,3.44148,3.66544,3.79342,250,3.56146,3.50147,3.59345,3.56346,5.42117,4.42133,4.38733,4.37133,4.38933,4.99324};
\ No newline at end of file
diff --git a/test/gtest_processor_loop_closure_falko.cpp b/test/gtest_processor_loop_closure_falko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0fd43107dd4ee5240a1f6304406043e09a52f821
--- /dev/null
+++ b/test/gtest_processor_loop_closure_falko.cpp
@@ -0,0 +1,719 @@
+//--------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/internal/config.h"
+
+#include "core/capture/capture_void.h"
+#include "core/problem/problem.h"
+#include "core/utils/utils_gtest.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+
+#include "laser/processor/processor_loop_closure_falko.h"
+#include "laser/sensor/sensor_laser_2d.h"
+
+#include "data/scan_data.h"
+
+// STL
+#include <iostream>
+#include <iterator>
+
+using namespace wolf;
+using namespace Eigen;
+using namespace laserscanutils;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoPublic);
+class ProcessorLoopClosureFalkoPublic : public ProcessorLoopClosureFalkoAhtBsc
+{
+ public:
+ ProcessorLoopClosureFalkoPublic(ParamsProcessorLoopClosureFalkoPtr _params_falko)
+ : ProcessorLoopClosureFalkoAhtBsc(_params_falko)
+ {
+ }
+
+ ~ProcessorLoopClosureFalkoPublic() = default;
+
+ std::shared_ptr<LoopClosureFalko<bsc, bscExtractor, aht_matcher>> getLoopClosure()
+ {
+ return loop_closure_;
+ }
+
+ void emplaceFeatures(CaptureBasePtr cap)
+ {
+ ProcessorLoopClosureFalkoAhtBsc::emplaceFeatures(cap);
+ }
+
+ std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr cap)
+ {
+ return ProcessorLoopClosureFalkoAhtBsc::findLoopClosures(cap);
+ }
+};
+
+class ProcessorLoopClosureFalkoTest : public testing::Test
+{
+ public:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ ProcessorLoopClosureFalkoPublicPtr processor;
+
+ virtual void SetUp()
+ {
+
+ // Emplace sensor
+ LaserScanParams _params;
+ _params.angle_min_ = 0;
+ _params.angle_max_ = 2.0 * M_PI;
+ _params.angle_step_ = 0.00701248;
+ sensor =
+ SensorBase::emplace<SensorLaser2d>(problem->getHardware(),
+ std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()),
+ _params);
+
+ // Emplace processor
+
+ ParserYaml parser = ParserYaml("test/yaml/params_processor_loop_closure_falko.yaml", laser_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ParamsProcessorLoopClosureFalkoPtr params =
+ std::make_shared<ParamsProcessorLoopClosureFalko>("ProcessorLoopClosureFalko", server);
+ processor = ProcessorBase::emplace<ProcessorLoopClosureFalkoPublic>(sensor, params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d &x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureBasePtr emplaceCapture(FrameBasePtr frame)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureBase>(frame, "CaptureBase", frame->getTimeStamp(), sensor);
+ }
+
+ CaptureBasePtr createCapture(TimeStamp ts)
+ {
+ // new capture
+ return std::make_shared<CaptureBase>("CaptureBase", ts, sensor);
+ }
+
+ CaptureLaser2dPtr emplaceCaptureLaser2d(FrameBasePtr frame, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), sensor, _ranges);
+ }
+
+ CaptureLaser2dPtr createCaptureLaser2d(TimeStamp ts, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return std::make_shared<CaptureLaser2d>(ts, sensor, _ranges);
+ }
+};
+
+TEST_F(ProcessorLoopClosureFalkoTest, installProcessor)
+{
+ // Stored Frames
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ // Stored Captures
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, capture_stored)
+{
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ ASSERT_TRUE(frm1 != nullptr);
+
+ std::vector<float> ranges;
+ auto cap1 = emplaceCaptureLaser2d(frm1, ranges);
+
+ // captureCallback
+ std::cout << __LINE__ << std::endl;
+ processor->captureCallback(cap1);
+ std::cout << __LINE__ << std::endl;
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(1, ranges);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(2, ranges);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ std::cout << __LINE__ << std::endl;
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ std::cout << __LINE__ << std::endl;
+ std::vector<float> ranges;
+ std::cout << __LINE__ << std::endl;
+ auto cap1 = emplaceCaptureLaser2d(frm1, ranges);
+
+ std::cout << __LINE__ << std::endl;
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+ std::cout << __LINE__ << std::endl;
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the
+ std::cout << __LINE__ << std::endl;
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ std::cout << __LINE__ << std::endl;
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+ std::cout << __LINE__ << std::endl;
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(1, ranges);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, captureCallbackMatch)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ auto frm3 = emplaceFrame(3, Vector3d::Zero());
+ auto frm4 = emplaceFrame(4, Vector3d::Zero());
+ auto frm5 = emplaceFrame(5, Vector3d::Zero());
+ // new captures
+ std::vector<float> ranges;
+ auto cap4 = createCaptureLaser2d(4, ranges);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+ problem->keyFrameCallback(frm2, nullptr);
+ problem->keyFrameCallback(frm3, nullptr);
+ problem->keyFrameCallback(frm4, nullptr);
+ problem->keyFrameCallback(frm5, nullptr);
+
+ // captureCallback
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(frm1->getCaptureList().size(), 0);
+ EXPECT_EQ(frm2->getCaptureList().size(), 0);
+ EXPECT_EQ(frm3->getCaptureList().size(), 0);
+ EXPECT_EQ(frm4->getCaptureList().size(), 1);
+ EXPECT_EQ(frm5->getCaptureList().size(), 0);
+
+ EXPECT_TRUE(cap4->getFrame() == frm4);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are
+ // removed from buffer
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, keyFrameCallbackMatch)
+{
+ // new frame
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ // new captures
+ std::vector<float> ranges;
+ auto cap1 = createCaptureLaser2d(1, ranges);
+ auto cap2 = createCaptureLaser2d(2, ranges);
+ auto cap3 = createCaptureLaser2d(3, ranges);
+ auto cap4 = createCaptureLaser2d(4, ranges);
+ auto cap5 = createCaptureLaser2d(5, ranges);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ processor->captureCallback(cap2);
+ processor->captureCallback(cap3);
+ processor->captureCallback(cap4);
+ processor->captureCallback(cap5);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm2, nullptr);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_TRUE(cap2->getFrame() == frm2);
+ EXPECT_TRUE(cap3->getFrame() == nullptr);
+ EXPECT_TRUE(cap4->getFrame() == nullptr);
+ EXPECT_TRUE(cap5->getFrame() == nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().size(), 0);
+ EXPECT_EQ(cap5->getFeatureList().size(), 0);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 4);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, emplaceFeatures)
+{
+ std::vector<float> ranges;
+
+ // add scan to capture:
+ auto cap1 = createCaptureLaser2d(1, ranges);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, findLoopClosures)
+{
+ Vector3d pos1 = {0.5, 0.3, 0};
+ auto cap1 = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), testRanges1);
+ auto cap2 = emplaceCaptureLaser2d(emplaceFrame(2, pos1), testRanges2);
+ auto cap3 = emplaceCaptureLaser2d(emplaceFrame(3, Vector3d::Zero()), testRanges1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ EXPECT_EQ(processor->match_list_.size(), 0);
+
+ processor->captureCallback(cap2);
+ EXPECT_EQ(processor->match_list_.size(), 1);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap2);
+
+ processor->captureCallback(cap3);
+ EXPECT_EQ(processor->match_list_.size(), 3);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap3);
+
+ // Verify factors
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 1);
+ EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 2);
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestExtractScene)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_1);
+ auto cap_target = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), target_scan_1);
+
+ processor->captureCallback(cap_ref);
+ processor->captureCallback(cap_target);
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+
+ auto cap_laser_1 = std::dynamic_pointer_cast<CaptureLaser2d>(cap_target);
+ auto scan_1 = cap_laser_1->getScan();
+
+ for (auto feat : cap_target->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_target = feat_falko->getScene()->keypoints_list_;
+ }
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front());
+
+ auto associations = match_falko->match_falko_->associations;
+
+ std::cout<< "transform : " << match_falko->match_falko_->transform_vector.transpose() << std::endl;
+
+ for (auto asso : associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestExtractSceneReference)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_2);
+
+ processor->captureCallback(cap_ref);
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+ std::cout << "keypoints size : " << key_ref.size() << std::endl;
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestExtractScene2)
+{
+ // Initialization
+ LaserScan scan_target, scan_ref;
+ LaserScanParams laser_params;
+
+ laser_params.angle_min_ = 0;
+ laser_params.angle_max_ = 2.0 * M_PI;
+
+ // ** TEST WITH TARGET AND REFERENCE SCENE
+ for (int i = 0; i < target_scan_1.size(); i++)
+ {
+ scan_target.ranges_raw_.push_back(target_scan_1[i]);
+ scan_ref.ranges_raw_.push_back(reference_scan_1[i]);
+ }
+ laser_params.angle_step_ = 0.00701248;
+
+ auto loop_cl_falko_2 = processor->getLoopClosure();
+
+ auto new_scene_target = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2->extractScene(scan_target, laser_params));
+ auto new_scene_reference = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2->extractScene(scan_ref, laser_params));
+
+ // std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl;
+ // std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl;
+
+ auto match_r_t = loop_cl_falko_2->matchScene(new_scene_reference, new_scene_target);
+ for (int i = 0; i < match_r_t->associations.size(); i++)
+ if (match_r_t->associations[i].second != -1)
+ {
+ std::cout << "id first : " << match_r_t->associations[i].first << std::endl;
+ std::cout << "id second : " << match_r_t->associations[i].second << std::endl;
+ }
+
+ auto key_ref = new_scene_reference->keypoints_list_;
+ auto key_target = new_scene_target->keypoints_list_;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+ for (auto asso : match_r_t->associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestEmplaceFeatures)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_1);
+ auto cap_target = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), target_scan_1);
+
+ processor->emplaceFeatures(cap_target);
+ processor->emplaceFeatures(cap_ref);
+
+ auto cap_laser_1 = std::dynamic_pointer_cast<CaptureLaser2d>(cap_target);
+ auto scan_1 = cap_laser_1->getScan();
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_target->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_target = feat_falko->getScene()->keypoints_list_;
+ }
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+ // std::cout << "reference keypoints size : " << key_ref.size() << std::endl;
+ // std::cout << "target keypoints size : " << key_target.size() << std::endl;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ auto matches = processor->findLoopClosures(cap_target);
+
+ ASSERT_TRUE(matches.size() > 0);
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(matches.begin()->second);
+
+ auto associations = match_falko->match_falko_->associations;
+
+ // std::cout << "associations.size() :" << associations.size() << std::endl;
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+
+ for (auto asso : associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+TEST_F(ProcessorLoopClosureFalkoTest, TestProcessCapture)
+{
+ auto cap_ref = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), reference_scan_1);
+ auto cap_target = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), target_scan_1);
+
+ processor->captureCallback(cap_ref);
+ auto cap_laser_1 = std::dynamic_pointer_cast<CaptureLaser2d>(cap_target);
+ auto scan_1 = cap_laser_1->getScan();
+
+ std::vector<falkolib::FALKO> key_target;
+ std::vector<falkolib::FALKO> key_ref;
+
+ for (auto feat : cap_ref->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_ref = feat_falko->getScene()->keypoints_list_;
+ }
+
+ cap_target->process();
+ //processor->process(cap_target);
+
+ for (auto feat : cap_target->getFeatureList())
+ if (feat->getType() == "FeatureSceneFalko")
+ {
+ auto feat_falko = std::static_pointer_cast<FeatureSceneFalkoBsc>(feat);
+ key_target = feat_falko->getScene()->keypoints_list_;
+ }
+
+ // std::cout << "reference keypoints size : " << key_ref.size() << std::endl;
+ // std::cout << "target keypoints size : " << key_target.size() << std::endl;
+ // std::cout << "processor->match_list_.size() : " << processor->match_list_.size() << std::endl;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
+
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
+
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front());
+ auto associations = match_falko->match_falko_->associations;
+
+ for (auto asso : associations)
+ if (asso.second != -1)
+ {
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
+
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loop_closure_falko_icp.cpp b/test/gtest_processor_loop_closure_falko_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f061bb06f13eddcec2ac6159d4b493fef74380bc
--- /dev/null
+++ b/test/gtest_processor_loop_closure_falko_icp.cpp
@@ -0,0 +1,121 @@
+//--------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/internal/config.h"
+#include "core/capture/capture_void.h"
+#include "core/problem/problem.h"
+#include "core/utils/utils_gtest.h"
+
+#include "laser/processor/processor_loop_closure_falko_icp.h"
+#include "laser/sensor/sensor_laser_2d.h"
+
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+
+#include "data/scan_data.h"
+
+// STL
+#include <iostream>
+#include <iterator>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+class ProcessorLoopClosureFalkoIcpTest : public testing::Test
+{
+ public:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ // std::shared_ptr<ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, nn_matcher>> processor;
+ ProcessorLoopClosureFalkoIcpNnBscPtr processor;
+
+ virtual void SetUp()
+ {
+
+ // Emplace sensor
+ laserscanutils::LaserScanParams _params;
+ _params.angle_min_ = 0;
+ _params.angle_max_ = 2.0 * M_PI;
+ _params.angle_step_ = 2.0 * M_PI / (testRanges1.size() - 1);
+ _params.range_min_ = 0;
+ _params.range_max_ = 200;
+ sensor =
+ SensorBase::emplace<SensorLaser2d>(problem->getHardware(), std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()), _params);
+
+ // Emplace processor
+ ParserYaml parser = ParserYaml("test/yaml/params_processor_loop_closure_falko.yaml", laser_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto params = std::make_shared<ParamsProcessorLoopClosureFalkoIcp>("ProcessorLoopClosureFalko",
+ server);
+ processor = ProcessorBase::emplace<ProcessorLoopClosureFalkoIcpNnBsc>(sensor, params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d &x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureLaser2dPtr emplaceCaptureLaser2d(FrameBasePtr frame, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), sensor, _ranges);
+ }
+
+ CaptureLaser2dPtr createCaptureLaser2d(TimeStamp ts, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return std::make_shared<CaptureLaser2d>(ts, sensor, _ranges);
+ }
+};
+
+TEST_F(ProcessorLoopClosureFalkoIcpTest, validatefindLoopClosures)
+{
+
+ auto cap1 = emplaceCaptureLaser2d(emplaceFrame(1, Vector3d::Zero()), testRanges1);
+ auto cap2 = emplaceCaptureLaser2d(emplaceFrame(2, Vector3d::Zero()), testRanges2);
+ auto cap3 = emplaceCaptureLaser2d(emplaceFrame(3, Vector3d::Zero()), testRanges1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ EXPECT_EQ(processor->match_list_.size(), 0);
+
+ processor->captureCallback(cap2);
+ EXPECT_EQ(processor->match_list_.size(), 1);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap2);
+
+ processor->captureCallback(cap3);
+ EXPECT_EQ(processor->match_list_.size(), 3);
+ EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
+ EXPECT_EQ(processor->match_list_.back()->capture_target, cap3);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loop_closure_icp.cpp b/test/gtest_processor_loop_closure_icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5941b063f42865d97b2d5d70eb64e512fb58acf
--- /dev/null
+++ b/test/gtest_processor_loop_closure_icp.cpp
@@ -0,0 +1,245 @@
+//--------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/internal/config.h"
+
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "laser/processor/processor_loop_closure_icp.h"
+#include <stdlib.h>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+class ProcessorLoopClosureIcp_Test : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorLaser2dPtr lidar;
+ ProcessorLoopClosureIcpPtr processor;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ lidar = std::static_pointer_cast<SensorLaser2d>(problem->installSensor("SensorLaser2d",
+ "lidar",
+ Eigen::Vector3d(0,0,0),
+ laser_root_dir + "/test/yaml/sensor_laser_2d.yaml"));
+ auto params = std::make_shared<ParamsProcessorLoopClosureIcp>();
+
+ params->recent_frames_ignored = 0;
+ params->frames_ignored_after_loop = 0;
+ params->max_error_threshold = 0.1;
+ params->min_points_percent = 50;
+
+ params->icp_params.use_point_to_line_distance = 1;
+ params->icp_params.max_correspondence_dist = 1e9;
+ params->icp_params.max_iterations = 1e3;
+ params->icp_params.use_corr_tricks = 1;
+ params->icp_params.outliers_maxPerc = 5;
+ params->icp_params.outliers_adaptive_order = 6;
+ params->icp_params.outliers_adaptive_mult = 7;
+ params->icp_params.do_compute_covariance = 1;
+ params->icp_params.cov_factor = 1;
+ params->icp_params.max_angular_correction_deg = 1e9;
+ params->icp_params.max_linear_correction = 1e9;
+ params->icp_params.epsilon_xy = 0.1;
+ params->icp_params.epsilon_theta = 0.1;
+ params->icp_params.sigma = 0.1;
+ params->icp_params.restart = 0;
+ params->icp_params.restart_threshold_mean_error = 0;
+ params->icp_params.restart_dt = 0;
+ params->icp_params.restart_dtheta = 0;
+ params->icp_params.clustering_threshold = 0;
+ params->icp_params.orientation_neighbourhood = 0;
+ params->icp_params.do_alpha_test = 0;
+ params->icp_params.do_alpha_test_thresholdDeg = 0;
+ params->icp_params.do_visibility_test = 0;
+ params->icp_params.outliers_remove_doubles = 0;
+ params->icp_params.debug_verify_tricks = 0;
+ params->icp_params.gpm_theta_bin_size_deg = 0;
+ params->icp_params.gpm_extend_range_deg = 0;
+ params->icp_params.gpm_interval = 0;
+ params->icp_params.min_reading = 0;
+ params->icp_params.max_reading = 100;
+ params->icp_params.use_ml_weights = 0;
+ params->icp_params.use_sigma_weights = 0;
+
+ processor = std::static_pointer_cast<ProcessorLoopClosureIcp>(problem->installProcessor("ProcessorLoopClosureIcp",
+ "prc icp",
+ lidar,
+ params));
+ }
+
+ std::vector<float> generateRanges(const int& frame_idx)
+ {
+ // generating synthetic 360º scan, half with range_A and half with range_B depending on the frame_idx. Randomly rotated.
+ // loop closures should occur (exact ranges but rotated) between pairs of frame_idx separated by 4 (0-4, 1-5, ...)
+
+ int n_ranges = std::round((lidar->getScanParams().angle_max_-lidar->getScanParams().angle_min_) / lidar->getScanParams().angle_step_);
+
+ // shape: 0-2 half circles, 1-square, 2-star-like
+ int shape = frame_idx % 3;
+
+ std::vector<float> ranges(n_ranges);
+ for (auto i = 0; i < n_ranges; i++)
+ {
+ // HALF CIRCLES (10m and 5m)
+ if (shape == 0)
+ ranges[i] = (i < n_ranges / 2 ? 10.0 : 5.0);
+
+ // SQUARE (2m)
+ else if (shape == 1)
+ ranges[i] = 2.0 * sqrt(std::pow(cos(i * lidar->getScanParams().angle_step_),2) +
+ std::pow(sin(i * lidar->getScanParams().angle_step_),2));
+
+ // STAR-LIKE (2m to 10m)
+ else if (shape == 2)
+ ranges[i] = 2.0 + (i % (n_ranges / 6)) * 8.0 / (n_ranges / 6);
+ }
+
+ // rotate randomly
+ srand (time(NULL)); // initialize random seed
+ int init = rand() % n_ranges; // half starting randomly between 0 and n_ranges-1
+ std::rotate(ranges.begin(),ranges.begin()+init,ranges.end());
+
+ return ranges;
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d &x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureLaser2dPtr emplaceCaptureLaser2d(FrameBasePtr frame, const std::vector<float> &_ranges)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureLaser2d>(frame, frame->getTimeStamp(), lidar, _ranges);
+ }
+};
+
+
+TEST_F(ProcessorLoopClosureIcp_Test, setup)
+{
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure)
+{
+ TimeStamp t(0.0);
+
+ for (int i = 0; i < 6; i++)
+ {
+ auto frm = emplaceFrame(t, Vector3d::Zero());
+ auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+ processor->keyFrameCallback(frm);
+ if (i > 2)
+ {
+ EXPECT_EQ(cap->getFeatureList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+ EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+ problem->print(4,1,1,1);
+ }
+ else
+ {
+ EXPECT_TRUE(cap->getFeatureList().empty());
+ EXPECT_TRUE(frm->getConstrainedByList().empty());
+ problem->print(4,1,1,1);
+ }
+ t += 1;
+ }
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous)
+{
+ processor->getParams()->recent_frames_ignored = 3;
+
+ TimeStamp t(0.0);
+
+ for (int i = 0; i < 6; i++)
+ {
+ auto frm = emplaceFrame(t, Vector3d::Zero());
+ auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+ processor->keyFrameCallback(frm);
+ if (i > 5)
+ {
+ EXPECT_EQ(cap->getFeatureList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+ EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+ problem->print(4,1,1,1);
+ }
+ else
+ {
+ EXPECT_TRUE(cap->getFeatureList().empty());
+ EXPECT_TRUE(frm->getConstrainedByList().empty());
+ problem->print(4,1,1,1);
+ }
+ t += 1;
+ }
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop)
+{
+ processor->getParams()->frames_ignored_after_loop = 2;
+
+ TimeStamp t(0.0);
+
+ for (int i = 0; i < 6; i++)
+ {
+ auto frm = emplaceFrame(t, Vector3d::Zero());
+ auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+ processor->keyFrameCallback(frm);
+ if (i == 3)
+ {
+ EXPECT_EQ(cap->getFeatureList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+ EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+ EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+ problem->print(4,1,1,1);
+ }
+ else
+ {
+ EXPECT_TRUE(cap->getFeatureList().empty());
+ EXPECT_TRUE(frm->getConstrainedByList().empty());
+ problem->print(4,1,1,1);
+ }
+ t += 1;
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_tracker_feature_polyline_2d.cpp b/test/gtest_processor_tracker_feature_polyline_2d.cpp
deleted file mode 100644
index 0553e9e6814e0444f22338c8aadf6de94665b359..0000000000000000000000000000000000000000
--- a/test/gtest_processor_tracker_feature_polyline_2d.cpp
+++ /dev/null
@@ -1,1203 +0,0 @@
-//--------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--------
-/**
- * \file gtest_processor_tracker_feature_polyline_2d.cpp
- *
- * Created on: Apr 3, 2019
- * \author: jvallve
- */
-
-#include "laser/processor/processor_tracker_feature_polyline_2d.h"
-#include "core/common/wolf.h"
-
-#include <core/utils/utils_gtest.h>
-#include "core/utils/logging.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
-
-#include <cmath>
-#include <iostream>
-
-using namespace Eigen;
-using namespace wolf;
-WOLF_PTR_TYPEDEFS(ProcessorPolylinePublicMethods);
-class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2d
-{
- public:
- ProcessorPolylinePublicMethods(ParamsProcessorTrackerFeaturePolyline2dPtr _params) :
- ProcessorTrackerFeaturePolyline2d(_params)
- {
-
- }
-
- unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in,
- FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_correspondences)
- {
- return ProcessorTrackerFeaturePolyline2d::trackFeatures(_features_last_in,_features_incoming_out,_feature_correspondences);
- }
-
- bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
- const FeatureBasePtr _last_feature,
- FeatureBasePtr _incoming_feature)
- {
- return ProcessorTrackerFeaturePolyline2d::correctFeatureDrift(_origin_feature,
- _last_feature,
- _incoming_feature);
- }
-
- bool voteForKeyFrame()
- {
- return ProcessorTrackerFeaturePolyline2d::voteForKeyFrame();
- }
-
- unsigned int processNew(const unsigned int& _max_features)
- {
- return ProcessorTrackerFeaturePolyline2d::processNew(_max_features);
- }
-
- unsigned int detectNewFeatures(const int& _max_new_features,
- FeatureBasePtrList& _features_last_out)
- {
- return ProcessorTrackerFeaturePolyline2d::detectNewFeatures(_max_new_features, _features_last_out);
- }
-
- void establishConstraints()
- {
- ProcessorTrackerFeaturePolyline2d::establishFactors();
- }
-
- void emplaceConstraintPointToLine(FeaturePolyline2dPtr _polyline_feature,
- LandmarkPolyline2dPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id,
- const int& _lmk_prev_point_id)
- {
- ProcessorTrackerFeaturePolyline2d::emplaceFactorPointToLine(_polyline_feature,
- _polyline_landmark,
- _ftr_point_id,
- _lmk_point_id,
- _lmk_prev_point_id);
- }
-
- void emplaceConstraintPoint(FeaturePolyline2dPtr _polyline_feature,
- LandmarkPolyline2dPtr _polyline_landmark,
- const int& _ftr_point_id,
- const int& _lmk_point_id)
- {
- ProcessorTrackerFeaturePolyline2d::emplaceFactorPoint(_polyline_feature,
- _polyline_landmark,
- _ftr_point_id,
- _lmk_point_id);
- }
-
- LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr)
- {
- return ProcessorTrackerFeaturePolyline2d::createLandmark(_feature_ptr);
- }
-
- bool modifyLandmarkAndMatch(LandmarkMatchPolyline2dPtr& lmk_match, FeaturePolyline2dPtr& pl_ftr)
- {
- return ProcessorTrackerFeaturePolyline2d::modifyLandmarkAndMatch( lmk_match, pl_ftr);
- }
-
- void tryMatchWithFeature(FeatureMatchPolyline2dScalarMap& ftr_matches,
- const FeaturePolyline2dPtr& _ftr_last,
- const FeaturePolyline2dPtr& _ftr_incoming,
- const Eigen::Matrix3d& _T_last_incoming_prior) const
- {
- ProcessorTrackerFeaturePolyline2d::tryMatchWithFeature( ftr_matches,
- _ftr_last,
- _ftr_incoming,
- _T_last_incoming_prior);
- }
-
- void tryMatchWithLandmark(LandmarkMatchPolyline2dScalarMap& lmk_matches,
- const LandmarkPolyline2dPtr& _lmk_ptr,
- const FeaturePolyline2dPtr& _feat_ptr,
- const Eigen::Matrix3d& _T_feature_landmark_prior) const
- {
- ProcessorTrackerFeaturePolyline2d::tryMatchWithLandmark(lmk_matches,
- _lmk_ptr,
- _feat_ptr,
- _T_feature_landmark_prior);
- }
-
- bool updateMatchWithLandmark(LandmarkMatchPolyline2dPtr& _lmk_match, LandmarkPolyline2dPtr& _lmk_ptr, FeaturePolyline2dPtr& _feat_ptr) const
- {
- return ProcessorTrackerFeaturePolyline2d::updateMatchWithLandmark(_lmk_match, _lmk_ptr, _feat_ptr);
- }
-
- void computeTransformations()
- {
- ProcessorTrackerFeaturePolyline2d::computeTransformations();
- }
-
- LandmarkMatchPolyline2dPtr concatenateFeatureLandmarkMatches(FeaturePolyline2dPtr pl_incoming,
- FeatureMatchPolyline2dPtr ftr_match_incoming_last,
- LandmarkMatchPolyline2dPtr lmk_match_last) const
- {
- return ProcessorTrackerFeaturePolyline2d::concatenateFeatureLandmarkMatches(pl_incoming,
- ftr_match_incoming_last,
- lmk_match_last);
- }
-};
-
-class ProcessorPolyline2dt : public testing::Test
-{
- public: //These can be accessed in fixtures
- ProcessorPolylinePublicMethodsPtr processor_pl;
- wolf::SensorBasePtr sensor_ptr;
- wolf::TimeStamp t;
- double mti_clock, tmp;
- double dt;
- Vector6d data;
- Matrix6d data_cov;
- VectorXd x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- problem = Problem::create("POV", 3);
- Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
- sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
-
- // Time and data variables
- data = Vector6d::Zero();
- data_cov = Matrix6d::Identity();
-
- // Set the origin
- x0.resize(10);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST(ProcessorIMU_constructors, ALL)
-{
- using namespace wolf;
-
- //constructor with ProcessorIMUParamsPtr argument only
- ParamsProcessorIMUPtr param_ptr = std::make_shared<ParamsProcessorIMU>();
- param_ptr->max_time_span = 2.0;
- param_ptr->max_buff_length = 20000;
- param_ptr->dist_traveled = 2.0;
- param_ptr->angle_turned = 2.0;
-
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
- ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
- ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
- ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
- ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
- //Factory constructor without yaml
- std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr problem = Problem::create("POV", 3);
- Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
- ParamsProcessorIMU params_default;
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned);
-
- //Factory constructor with yaml
- processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
-}
-
-TEST(ProcessorIMU, voteForKeyFrame)
-{
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("POV", 3);
- Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ParamsProcessorIMUPtr prc_imu_params = std::make_shared<ParamsProcessorIMU>();
- prc_imu_params->max_time_span = 1;
- prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
- prc_imu_params->voting_active = true;
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- //setting origin
- VectorXd x0(10);
- TimeStamp t(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
- //data variable and covariance matrix
- // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6d data;
- data << 1,0,0, 0,0,0;
- Matrix6d data_cov(Matrix6d::Identity());
- data_cov(0,0) = 0.5;
-
- // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
-
- // Time
- // we want more than one data to integrate otherwise covariance will be 0
- double dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- /*There should be 3 frames :
- - 1 KeyFrame at origin
- - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- - 1 frame that would be used by future data
- */
- ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3);
-
- /* if max_time_span == 2, Wolf tree should be
-
- Hardware
- S1
- pm5
- o: C2 - F2
- l: C4 - F3
- Trajectory
- KF1
- Estim, ts=0, x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
- C1 -> S1
- KF2
- Estim, ts=2.1, x = ( 2.2 0 -22 0 0 0 1 2.1 0 -21 0 0 0 0 0 0 )
- C2 -> S1
- f1
- m = ( 2.21 0 0 0 0 0 1 2.1 0 0 )
- c1 --> F1
- F3
- Estim, ts=2.1, x = ( . . .)
- C4 -> S1
- */
- //TODO : ASSERT TESTS to make sure the constraints are correctly set + check the tree above
-
-}
-
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
- using namespace wolf;
-
- t.set(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- data << 2, 0, 0, 0, 0, 0; // only acc_x
- cap_imu_ptr->setData(data);
-
- // make one step to depart from origin
- cap_imu_ptr->setTimeStamp(0.05);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
-
- // make two steps, then pretend it's just one
- cap_imu_ptr->setTimeStamp(0.10);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
-
- cap_imu_ptr->setTimeStamp(0.15);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
- mot_final.delta_ = mot_final.delta_integr_;
- Motion mot_sec = mot_final;
-
-// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
- public:
- P() : ProcessorIMU(std::make_shared<ParamsProcessorIMU>()) {}
- Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
- {
- return ProcessorIMU::interpolate(ref, sec, ts);
- }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
-}
-
-TEST_F(ProcessorIMUt, acc_x)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXd x(10);
- x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- Vector6d b; b << 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-}
-
-TEST_F(ProcessorIMUt, acc_y)
-{
- // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
- // difference hier is that we integrate over 1ms periods
-
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXd x(10);
- x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
- Vector6d b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
- x << 0,10,0, 0,0,0,1, 0,20,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, acc_z)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXd x(10);
- x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
- Vector6d b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 50; //how 0.1s
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
- x << 0,0,25, 0,0,0,1, 0,0,10;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, check_Covariance)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- VectorXd delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<double,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
-
- ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
-}
-
-TEST_F(ProcessorIMUt, gyro_x)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
-{
- // time
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
-
- // bias
- double abx = 0.002;
- Vector6d bias; bias << abx,0,0, 0,0,0;
- Vector3d acc_bias = bias.head(3);
- // state
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
-
- // init things
- problem->setPrior(x0, P0, t);
-
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
-// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
-
- // data
- double rate_of_turn = 5 * M_PI/180.0;
- data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x_true(10);
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- VectorXd x_est(10);
- x_est = problem->getCurrentState().head(10);
-
- ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- double abx(0.002), aby(0.01);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- Vector6d bias; bias << abx,aby,0, 0,0,0;
- Vector3d acc_bias = bias.head(3);
-
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
-
- double rate_of_turn = 5 * M_PI/180.0;
-// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
- data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
-// << "\n estimated state : " << x.transpose();
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
-// "\n expected state is : \n" << x.transpose() << std::endl;
-}
-
-TEST_F(ProcessorIMUt, gyro_z)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- Vector3d tmp_vec; //will be used to store rate of turn data
- Quaterniond quat_comp(Quaterniond::Identity());
- Matrix3d R0(Matrix3d::Identity());
- double time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- double tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const double dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaterniond R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4d R2quat_vec((Vector4d() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXd x(10);
- x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
-
- Quaterniond result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity and bias parts
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- Vector3d tmp_vec; //will be used to store rate of turn data
- Quaterniond quat_comp(Quaterniond::Identity());
- Matrix3d R0(Matrix3d::Identity());
- double time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- double tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const double dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaterniond R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4d R2quat_vec((Vector4d() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXd x(10);
- //rotation + translation due to initial velocity
- x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
-
- Quaterniond result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
- // v = a*dt = 0.001
- x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<1,0,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
- // v = a*dt = 1
- x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
- // v = a*dt = 0.001
- x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<0,1,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
- // v = a*dt = 1
- x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
-{
- double dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXd P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t);
-
- double rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaterniond quat_comp(Quaterniond::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXd x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
- // v = a*dt = 0.001
- x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaterniond rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<0,0,1).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
- // v = a*dt = 1
- x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/params1.yaml b/test/params1.yaml
deleted file mode 100644
index 194560afa773cb1910c1d8a94b6b80163486401f..0000000000000000000000000000000000000000
--- a/test/params1.yaml
+++ /dev/null
@@ -1,30 +0,0 @@
-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
diff --git a/test/params2.yaml b/test/params2.yaml
deleted file mode 100644
index b9ac1565e165af26bd168f4969ba520e346a8c13..0000000000000000000000000000000000000000
--- a/test/params2.yaml
+++ /dev/null
@@ -1,33 +0,0 @@
-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
diff --git a/test/params3.1.yaml b/test/params3.1.yaml
deleted file mode 100644
index bd9b495c830e56f17f11abbb5b2109d780f4e438..0000000000000000000000000000000000000000
--- a/test/params3.1.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-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
diff --git a/test/params3.yaml b/test/params3.yaml
deleted file mode 100644
index 23b9db1db2149e9f9c51255d38c629959672e436..0000000000000000000000000000000000000000
--- a/test/params3.yaml
+++ /dev/null
@@ -1,31 +0,0 @@
-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"
- jump: "@/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
diff --git a/test/yaml/params_processor_loop_closure_falko.yaml b/test/yaml/params_processor_loop_closure_falko.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c41ca1198cfa4c7e5cc6cb2e8089b830ce6ec3a0
--- /dev/null
+++ b/test/yaml/params_processor_loop_closure_falko.yaml
@@ -0,0 +1,93 @@
+ config:
+ problem:
+ dim: 2
+ processor:
+ ProcessorLoopClosureFalko:
+ time_tolerance : 0.1
+ apply_loss_function : false
+ min_keypoints : 4
+ max_loops : 3
+ distance_between_ref_frm : 0.5
+ near_matches_th : 1
+ score_th : 5
+ th_kp_area : 0.66
+ th_kp_perimeter : 0.66
+ th_kp_maxd : 0.66
+ th_kp_eig : 0
+ th_scan_area : 0
+ th_scan_eig : 0
+ logging : false
+ logging_filepath : ~/Desktop
+
+# Parameters for matcher
+ falko:
+ matcher_distance_th : 100
+ use_descriptors : true
+ circularSectorNumber : 16
+ radialRingNumber : 8
+ matcher_ddesc_th : 20
+ keypoints_number_th : 5
+ enable_subbeam : false
+ nms_radius : 0.1
+ neigh_b : 0.1
+ b_ratio : 4
+ xRes : 0.15
+ yRes : 0.15
+ thetaRes : 0.1
+ xAbsMax : 3
+ yAbsMax : 3
+ thetaAbsMax : 3
+
+# from processor base
+ keyframe_vote:
+ time_tolerance : 0.1
+ voting_active : true
+
+# from processor odom ICP
+ max_error_threshold : 0.5
+ min_points_percent : 0.5
+# from CSM
+ icp:
+ max_iterations: 50
+ max_correspondence_dist: 1
+ use_corr_tricks: 1
+ outliers_maxPerc: 0.9
+ use_point_to_line_distance: 1
+ outliers_adaptive_order: 0.7
+ outliers_adaptive_mult: 1.5
+ do_compute_covariance: 1
+
+ max_angular_correction_deg: 0
+ max_linear_correction: 0
+ epsilon_xy: 0
+ epsilon_theta: 0
+ sigma: 0.2
+ restart: 0
+ restart_threshold_mean_error: 0
+ restart_dt: 0
+ restart_dtheta: 0
+ clustering_threshold: 0
+ orientation_neighbourhood: 0
+ do_alpha_test: 0
+ do_alpha_test_thresholdDeg: 0
+ do_visibility_test: 0
+ outliers_remove_doubles: 0
+ debug_verify_tricks: 0
+ gpm_theta_bin_size_deg: 0
+ gpm_extend_range_deg: 0
+ gpm_interval: 0
+ laser_x: 0
+ laser_y: 0
+ laser_theta: 0
+ min_reading: 0.023
+ max_reading: 60
+ use_ml_weights: 0
+ use_sigma_weights: 0
+ hsm_linear_cell_size: 0.03
+ hsm_angular_cell_size_deg: 1
+ hsm_num_angular_hypotheses: 8
+ hsm_xc_directions_min_distance_deg: 10
+ hsm_xc_ndirections: 3
+ hsm_angular_hyp_min_distance_deg: 10
+ hsm_linear_xc_max_npeaks: 5
+ hsm_linear_xc_peaks_min_distance: 5
\ No newline at end of file
diff --git a/test/yaml/processor_loop_closure_icp.yaml b/test/yaml/processor_loop_closure_icp.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b0fb57d38e7e27e45fc8fc87fd72a0627c87f543
--- /dev/null
+++ b/test/yaml/processor_loop_closure_icp.yaml
@@ -0,0 +1,26 @@
+# from processor base
+time_tolerance : 0.1
+voting_active : true
+
+# from processor tracker
+min_features_for_keyframe : 0
+max_new_features : 0
+
+# from processor odom ICP
+cov_factor : 1
+use_point_to_line_distance : 1
+max_correspondence_dist : 2
+max_iterations : 3
+use_corr_tricks : 1
+outliers_maxPerc : 5
+outliers_adaptive_order : 6
+outliers_adaptive_mult : 7
+max_reading : 100
+min_reading : 0
+do_compute_covariance : 1
+vfk_min_dist : 0
+vfk_min_angle : 0
+vfk_min_time : 0
+vfk_min_error : 0
+vfk_max_points : 0
+initial_guess : "zero"