diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 3696d52a048db65e2b09cd3a4a4aa42db9313b29..3dcf6dbafe78072fb683446c1fb279e5594f0cb1 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -77,21 +77,12 @@ stages:
############ LICENSE HEADERS ############
license_header:
stage: license
- image: labrobotica/wolf_deps:16.04
+ image: labrobotica/wolf_deps:20.04
before_script:
- *preliminaries_definition
script:
- *license_header_definition
-############ UBUNTU 16.04 TESTS ############
-build_and_test:xenial:
- stage: build_and_test
- image: labrobotica/wolf_deps:16.04
- except:
- - master
- script:
- - *build_and_test_definition
-
############ UBUNTU 18.04 TESTS ############
build_and_test:bionic:
stage: build_and_test
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 176bdf1a2db64c9aac6da0dfb421d10c65fb8ba3..c94b33bfb04c9751c727f62bb16557cd1ef8f508 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
+MESSAGE(STATUS "Starting WOLF CMakeLists ...")
# Pre-requisites about cmake itself
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
@@ -63,7 +63,7 @@ ENDIF(NOT BUILD_DOC)
IF(PROFILING)
add_definitions(-DPROFILING)
- message("Compiling with profiling options...")
+ message(STATUS "Compiling with profiling options...")
ENDIF(PROFILING)
if(BUILD_TESTS)
@@ -127,7 +127,6 @@ INCLUDE_DIRECTORIES("include") # In this same project
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-
# ============ HEADERS ============
SET(HDRS_CAPTURE
include/core/capture/capture_base.h
@@ -160,6 +159,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
include/core/factor/factor_relative_pose_3d.h
include/core/factor/factor_pose_3d_with_extrinsics.h
+ include/core/factor/factor_relative_pose_3d_with_extrinsics.h
include/core/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
@@ -187,6 +187,7 @@ SET(HDRS_MATH
include/core/math/covariance.h
)
SET(HDRS_MAP
+ include/core/map/factory_map.h
include/core/map/map_base.h
)
SET(HDRS_PROBLEM
@@ -197,9 +198,8 @@ SET(HDRS_PROCESSOR
include/core/processor/motion_provider.h
include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h
- include/core/processor/processor_fix_wing_model.h
+ include/core/processor/processor_fixed_wing_model.h
include/core/processor/factory_processor.h
- include/core/processor/processor_logging.h
include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
@@ -214,7 +214,7 @@ SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
include/core/sensor/factory_sensor.h
- include/core/sensor/sensor_model.h
+ include/core/sensor/sensor_motion_model.h
include/core/sensor/sensor_odom_2d.h
include/core/sensor/sensor_odom_3d.h
include/core/sensor/sensor_pose.h
@@ -249,11 +249,9 @@ SET(HDRS_UTILS
include/core/utils/check_log.h
include/core/utils/converter.h
include/core/utils/eigen_assert.h
- include/core/utils/eigen_predicates.h
include/core/utils/graph_search.h
include/core/utils/loader.h
include/core/utils/logging.h
- include/core/utils/make_unique.h
include/core/utils/params_server.h
include/core/utils/singleton.h
include/core/utils/utils_gtest.h
@@ -309,7 +307,7 @@ SET(SRCS_PROCESSOR
src/processor/motion_provider.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_fix_wing_model.cpp
+ src/processor/processor_fixed_wing_model.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
@@ -323,7 +321,7 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_model.cpp
+ src/sensor/sensor_motion_model.cpp
src/sensor/sensor_odom_2d.cpp
src/sensor/sensor_odom_3d.cpp
src/sensor/sensor_pose.cpp
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 55eb56a880cd3d407cf631ce7e052e135e9bacce..4260a8c8efb26928a6d2242edbd43830b4467875 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -17,6 +17,7 @@ config:
tree_manager:
type: "none"
+ plugin: "core"
solver:
max_num_iterations: 100
diff --git a/doc/main.dox b/doc/main.dox
index 62090c8648315200e0ba9b726d55048d74406425..efde8f91eab56973935fe8efa85d64e6dd558b48 100644
--- a/doc/main.dox
+++ b/doc/main.dox
@@ -1,258 +1,29 @@
// EDIT CAPS TEXTS
/*! \mainpage WOLF
-
- \section Description
- WOLF (Windowed Localization Frames) is a software library ...
-
- \section Main Main Features
+ WOLF (Windowed Localization Frames) is a C++ framework for state estimation based on factor graphs.
- - Uses Eigen3.2. as an algebra engine
- - Suited for Filtering and Optimization approaches
- - Allows different state spaces configurations
- - Minimizes read/write of state vectors and matrix to memory
-
- \section Contents
-
- - \link Installation Installation \endlink
- - \link how_to How To \endlink
- - \link Performance Performance \endlink
- - \link faq FAQ \endlink
- - \link todo To Do \endlink
- - \link other OTHER STUFF FTW \endlink
- .
-
-
- \section Authors
- Joan Solà (jsola (_at_) iri.upc.edu)
-
- Andreu Corominas (acorominas (_at_) iri.upc.edu)
-
- Faust Terrado (fterrado (_at_) iri.upc.edu)
+ This is the Doxygen documentation of the **WOLF core** library.
- Àngel Santamaria (asantamaria (_at_) iri.upc.edu)
-
- Joan Vallvé (jvallve (_at_) iri.upc.edu)
-
- \section License
-
- TO BE DISCUSSED :
- This package is licensed under a
- <a href="http://www.gnu.org/licenses/lgpl.html" target=â€_blankâ€>
- LGPL 3.0 License</a>.
-
- \section Disclaimer
-
- This 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/>.
-
- \section References References and External Documentation
+ If you are looking for one of the following:
- ... Nothing written here yet ...
-
-*/
-
-
-
-// HERE STARTS THE FUN (THE DIFFERENT PAGES)
-
-// TITLE
-// MINI PAGES MENU
-// CONTENT (SECTIONS...)
-
-// ADDING PAGES:
-// - add the new page in the menus of each page
-// - current page is bold "\b", not a \link!
-
-
-/*! \page Installation Installation
-\b Installation | \link how_to How To \endlink | \link Performance \endlink | \link faq FAQ \endlink | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-
-To install the WOLF library, please follow the steps below:
-
-\section Download
-
-Checkout the source code from the IRI public repository
-\code
-cd <your_developing_folder>
-svn checkout https://devel.iri.upc.edu/labrobotica/algorithms/wolf wolf
-\endcode
-
-\section Compilation
-
-After downloading, change directory to wolf/build and execute cmake,
-\code
-cd build
-cmake ..
-\endcode
-compile the shared library (wolf.so) and the example programs,
-\code
-make
-\endcode
-
-<!--
-The <em>cmake</em> only need to be executed once (make will automatically call
-<em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
--->
-
-Optionally, if you want to generate this documentation in your local machine (uses doxygen default style type),
-\code
-make doc
-\endcode <!--OTHER-->
-<!--
-- To generate this documentation with IRI-DOC style type
-\code
-cd doc
-svn checkout https://devel.iri.upc.edu/labrobotica/tools/iri_doc
-cd ../build; rm -rf *; cmake ..; make; make doc
-\endcode
--->
-
-Please note that the files in the <em>build</em> directory are genetated by <em>cmake</em>
-and <em>make</em> and can be safely removed.
-After doing so you will need to call cmake manually again.
-
-\section Installing_subsec Installing
-
-In order to be able to use the library, it is necessary to copy it into the system.
-To do that, execute, from the build directory
-\code
-sudo make install
-\endcode
-
-as root and the shared libraries will be copied to <em>/usr/local/lib/iridrivers</em> directory
-and the header files will be copied to <em>/usr/local/include/iridrivers</em> dierctory. At
-this point, the library may be used by any user.
-
-To remove the library from the system, execute
-\code
-sudo make uninstall
-\endcode
-as root, and all the associated files will be removed from the system.
-OTHER
-
-\section Configuration
-
-The default build mode is DEBUG. That is, objects and executables
-include debug information.
-
-The RELEASE build mode optimizes for speed. To build in this mode
-execute
-\code
-cmake .. -DCMAKE_BUILD_TYPE=RELEASE
-\endcode
-The release mode will be kept until next time cmake is executed.
-
-*/
-
-
-
-/*! \page how_to How to use this library
-\link Installation \endlink | \b How \b To | \link Performance \endlink | \link faq FAQ \endlink | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-You can use only the functions provided by the library after \link Installation install this library \endlink.
-
-If you need to use this library in your code, you'll have to add these lines into your CMakeLists.txt file.
-
-- first it is necessary to locate if required libraries have been installed or not using the following command (TO DO)
-\code
-FIND_PACKAGE(wolf REQUIRED)
-\endcode
-- In the case that the packages are present, it is necessary to add the header files directory to
-the include directory path by using
-\code
-INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIR})
-\endcode
-- Finally, it is also necessary to link with the desired libraries by using the following command
-\code
-TARGET_LINK_LIBRARIES(<executable name> ${wolf_LIBRARY})
-\endcode
-.
-
-<!--
-All these steps are automatically done when the new project is created following the instructions
-in <A href="http://wikiri.upc.es/index.php/Create_a_new_development_project" target=â€_blankâ€>here</a>
--->
-
-From the programming point of view the following example illustrates how to use wolf:
-\code
-#include "state_pvq.h"
-int main()
-{
- VectorXd storage(20);
- unsigned int index = 0, bsize = 4;
-
- //set storage vector
- storage << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19;
-
- VectorXd vec_pvq(StatePVQ::SIZE_);
- vec_pvq << 20, 21, 22, 23, 24, 25, 26, 27, 28, 29;
-
- StatePVQ pvq(storage, index + bsize, vec_pvq);
- cout << "storage : " << storage.transpose() << endl;
- cout << "pvq state: " << pvq.x().transpose() << endl;
- cout << "pvq pos : " << pvq.p().transpose() << endl;
- cout << "pvq vel : " << pvq.v().transpose() << endl;
- cout << "pvq quat : " << pvq.q().coeffs().transpose() << endl;
-
- VectorXd vec_pvq2(StatePVQ::SIZE_);
- vec_pvq2 << 30, 31, 32, 33, 34, 35, 36, 37, 38, 39;
- pvq.x(vec_pvq2);
- cout << "storage : " << storage.transpose() << endl;
- cout << "pvq state: " << pvq.x().transpose() << endl;
-
-
- return 0;
-}
-\endcode
-
-For further code examples, please see the files at src/examples where there are some "main()" instances that make use of the library in different useful ways.
-
-*/
-
-/*! \page Performance Performance
-\link Installation \endlink | \link how_to How To \endlink | \b Performance | \link faq FAQ \endlink | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-The following plot shows the processing time spent to compute ...
-
- <!-- \image html "images/xxx.png" -->
-*/
-
-/*! \page faq Frequently Asked Questions
-\link Installation \endlink | \link how_to How To \endlink | \link Performance \endlink | \b FAQ | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-Do I need some extra libraries to compile WOLF ?
-- Yes, please take a look at the \link Installation \endlink page.
-
-*/
-
-
-// THIS IS SHOWN BEFORE THE TO DO AUTOMATIC PAGE
-
-/*! \page todo To Do
-\link Installation \endlink | \link how_to How To \endlink | \link Performance \endlink | \link faq FAQ \endlink| \b To \b Do | \link other OTHER STUFF FTW \endlink
-- some to do
-- another to do
-*/
-
-
-
-
-/*! \page other OTHER STUFF FTW
-\link Installation \endlink | \link how_to How To \endlink | \link Performance \endlink | \link faq FAQ \endlink | \link todo To Do \endlink | \b OTHER \b STUFF \b FTW
-
-HERE IS YOUR CONTENT FOR THE WIN
-
-
+ - full documentation
+ - installation instructions
+ - access to the repositories
+ - WOLF plugins
+ - ROS packages
+
+ please visit the main WOLF page at http://www.iri.upc.edu/wolf.
+
+ WOLF is copyright 2015--2022
+
+ - Joan SolÃ
+ - Joan Vallvé
+ - Joaquim Casals,
+ - Jérémie Deray,
+ - Médéric Fourmy,
+ - Dinesh Atchuthan,
+ - Andreu Corominas-Murtra
+
+ at IRI-CSIC-UPC.
*/
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 7a7f9eeae3b50cbe7d96cd841d99ae1f07b566d3..197a6bfb1b30d79a64376f9468b9481c0a4bc3e3 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -275,6 +275,9 @@ WOLF_LIST_TYPEDEFS(FactorBase);
// Map
WOLF_PTR_TYPEDEFS(MapBase);
+// - - Map params
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsMapBase);
+
// - Landmark
WOLF_PTR_TYPEDEFS(LandmarkBase);
WOLF_LIST_TYPEDEFS(LandmarkBase);
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index 2247a898bb4d5030ed1b07ef6422d72a9ca968e8..b0eea0f2b8f91f00a9a2bc8a7bda6ce1be66dc45 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -19,8 +19,8 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
-#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+#ifndef FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_H_
+#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -37,61 +37,104 @@ WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
{
public:
+ /** \brief Class constructor Frame-Frame
+ */
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
- _top,
- _ftr_ptr,
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getCapture()->getSensorP(),
- _ftr_ptr->getCapture()->getSensorO())
- {
- assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
- assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
- //
- }
+ FactorStatus _status = FAC_ACTIVE);
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose2dWithExtrinsics() override = default;
template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
- const T* const _o2,
- const T* const _sp,
- const T* const _so,
+ bool operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
T* _residuals) const;
};
+inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePose2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+}
+
+inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePose2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _lmk_other_ptr->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+}
+
template<typename T>
-inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
- const T* const _o2,
- const T* const _ps,
- const T* const _os,
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
- Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
- Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
- Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
- T o1 = _o1[0];
- T o2 = _o2[0];
- T os = _os[0];
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
+ T o_ref = _o_ref[0];
+ T o_target = _o_target[0];
+ T o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
@@ -112,8 +155,8 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
// s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
- expected_measurement(2) = o2 - o1;
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
+ expected_measurement(2) = o_target - o_ref;
// Error
er = expected_measurement - getMeasurement().cast<T>();
diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..19245f6bcf630f737e4a01aa78b4a11030d8264a
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -0,0 +1,215 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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 _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
+#define _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/math/rotations.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/sensor/sensor_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose3dWithExtrinsics);
+
+class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativePose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4>
+{
+ public:
+
+ /** \brief Class constructor Frame-Frame
+ */
+ FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
+
+ /** \brief Class Destructor
+ */
+ ~FactorRelativePose3dWithExtrinsics() override;
+
+ /** brief : compute the residual from the state blocks being iterated by the solver.
+ **/
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const;
+
+ Eigen::Vector6d residual() const;
+ double cost() const;
+
+ // print function only for double (not Jet)
+ template<typename T, int Rows, int Cols>
+ void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const
+ {
+ // jet prints nothing
+ }
+ template<int Rows, int Cols>
+ void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const
+ {
+ // double prints stuff
+ WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
+ }
+};
+
+} // namespace wolf
+
+namespace wolf
+{
+
+inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePose3dWithExtrinsics",
+ _top,
+ _feature_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _feature_ptr->getFrame()->getP(),
+ _feature_ptr->getFrame()->getO(),
+ _feature_ptr->getCapture()->getSensorP(),
+ _feature_ptr->getCapture()->getSensorO())
+{
+}
+
+inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePose3dWithExtrinsics",
+ _top,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feature_ptr->getFrame()->getP(),
+ _feature_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP(),
+ _landmark_other_ptr->getO(),
+ _feature_ptr->getCapture()->getSensorP(),
+ _feature_ptr->getCapture()->getSensorO())
+{
+}
+
+inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
+{
+ //
+}
+
+template<typename T>
+inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
+{
+ // Maps
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_sensor);
+ Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_sensor);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_target);
+ Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
+
+ // Expected measurement
+ Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate();
+ Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l;
+ Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l);
+
+ // Measurement
+ Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>());
+ Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 );
+ Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
+ //Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
+
+ // Error
+ Eigen::Matrix<T, 6, 1> err;
+ err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l);
+ //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l); // true error function for which the covariance should be computed
+ err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
+
+ // Residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+ return true;
+}
+
+inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
+{
+ Eigen::Vector6d res;
+ double * p_camera, * o_camera, * p_ref, * o_ref, * p_target, * o_target;
+ p_camera = getCapture()->getSensorP()->getState().data();
+ o_camera = getCapture()->getSensorO()->getState().data();
+ p_ref = getCapture()->getFrame()->getP()->getState().data();
+ o_ref = getCapture()->getFrame()->getO()->getState().data();
+ p_target = getLandmarkOther()->getP()->getState().data();
+ o_target = getLandmarkOther()->getO()->getState().data();
+
+ operator() (p_camera, o_camera, p_ref, o_ref, p_target, o_target, res.data());
+
+ return res;
+}
+
+inline double FactorRelativePose3dWithExtrinsics::cost() const
+{
+ return residual().squaredNorm();
+}
+
+} // namespace wolf
+
+#endif /* _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS */
diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h
new file mode 100644
index 0000000000000000000000000000000000000000..5772e4e8a1abab280f5e6ee096161e99be39d515
--- /dev/null
+++ b/include/core/map/factory_map.h
@@ -0,0 +1,218 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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 factory_map.h
+ *
+ * Created on: Jul 25, 2016
+ * \author: jvallve
+ */
+
+#ifndef FACTORY_MAP_H_
+#define FACTORY_MAP_H_
+
+namespace wolf
+{
+class MapBase;
+struct ParamsMapBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+#include "core/utils/params_server.h"
+
+namespace wolf
+{
+
+/** \brief Map factory class
+ *
+ * This factory can create objects of classes deriving from MapBase.
+ *
+ * Specific object creation is invoked by `create(TYPE, params ... )`, and the TYPE of map is identified with a string.
+ * Currently, the following map types are implemented,
+ * - "MapGrid2dGravity" in plugin 'imu'
+ *
+ * among others.
+ *
+ * Find general Factory documentation in class Factory:
+ * - Access the factory
+ * - Register/unregister creators
+ * - Invoke object creation
+ *
+ * This documentation shows you how to use the FactoryMap specifically:
+ * - Write map creators.
+ * - Create maps
+ *
+ * #### Write map creators
+ * Map creators have the following API:
+ *
+ * \code
+ * static MapBasePtr create(ParamsMapBasePtr _params_map);
+ * \endcode
+ *
+ * They follow the general implementation shown below:
+ *
+ * \code
+ * static MapBasePtr create(ParamsMapBasePtr _params_map)
+ * {
+ * // cast map parameters to good type --- example: ParamsMapGrid
+ * auto params_ptr = std::static_pointer_cast<ParamsMapGrid>(_params_map);
+ *
+ * // Do create the Map object --- example: MapGrid
+ * return map_ptr = std::make_shared<MapGrid>(params_ptr);
+ * }
+ * \endcode
+ *
+ * #### Creating maps
+ * Note: Prior to invoking the creation of a map of a particular type,
+ * you must register the creator for this type into the factory.
+ *
+ * To create e.g. a MapGrid, you type:
+ *
+ * \code
+ * auto grid_ptr = FactoryMap::create("MapGrid", params_grid);
+ * \endcode
+ *
+ * #### See also
+ * - FactoryParamsMap: to create parameter structs deriving from ParamsMapBase directly from YAML files.
+ *
+ * #### Example 1: writing a MapGrid creator
+ * Here is an example of MapGrid::create() extracted from map_grid.cpp:
+ *
+ * \code
+ * static MapBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsMapBasePtr _intrinsics)
+ * {
+ * // check extrinsics vector
+ * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
+ *
+ * // cast instrinsics to good type
+ * auto intrinsics_ptr = std::static_pointer_cast<ParamsMapGrid>(_intrinsics);
+ *
+ * // Do create the MapGrid object, and complete its setup
+ * auto sen_ptr = std::make_shared<MapGrid>(_extrinsics_pq, intrinsics_ptr);
+ *
+ * sen_ptr->setName(_unique_name);
+ *
+ * return sen_ptr;
+ * }
+ * \endcode
+ *
+ * #### Example 2: registering a map creator into the factory
+ * Registration can be done manually or automatically. It involves the call to static functions.
+ * It is advisable to put these calls within unnamed namespaces.
+ *
+ * - __Manual registration__: you control registration at application level.
+ * Put the code either at global scope (you must define a dummy variable for this),
+ * \code
+ * namespace {
+ * const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
+ * }
+ * main () { ... }
+ * \endcode
+ * or inside your main(), where a direct call is possible:
+ * \code
+ * main () {
+ * FactoryMap::registerCreator("MapGrid", MapGrid::create);
+ * ...
+ * }
+ * \endcode
+ *
+ * - __Automatic registration__: registration is performed at library level.
+ * Put the code at the last line of the map_xxx.cpp file,
+ * \code
+ * namespace {
+ * const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
+ * }
+ * \endcode
+ * Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
+ * You are free to comment out these lines and place them wherever you consider it more convenient for your design.
+ *
+ * #### Example 2: creating maps
+ * We finally provide the necessary steps to create a map of class MapGrid in our application:
+ *
+ * \code
+ * #include "core/map/factory_map.h"
+ * #include "core/map/map_grid.h" // provides MapGrid
+ *
+ * // Note: MapGrid::create() is already registered, automatically.
+ *
+ * using namespace wolf;
+ * int main() {
+ *
+ * // To create a grid, provide:
+ * // a type = "MapGrid" and
+ * // a pointer to the intrinsics struct:
+ *
+ * // Create a pointer to the struct of map parameters stored in a YAML file ( see FactoryParamsMap )
+ * ParamsMapGridPtr params_1 =
+ * FactoryParamsMap::create("ParamsMapGrid",
+ * grid_1.yaml);
+ *
+ * MapBasePtr grid_1_ptr =
+ * FactoryMap::create ( "MapGrid" ,
+ * params_1 );
+ *
+ * return 0;
+ * }
+ * \endcode
+ *
+ */
+typedef Factory<MapBase,
+ const ParamsMapBasePtr> FactoryMap;
+
+template<>
+inline std::string FactoryMap::getClass() const
+{
+ return "FactoryMap";
+}
+
+// ParamsMap factory
+struct ParamsMapBase;
+typedef Factory<ParamsMapBase,
+ const std::string&> FactoryParamsMap;
+template<>
+inline std::string FactoryParamsMap::getClass() const
+{
+ return "FactoryParamsMap";
+}
+
+#define WOLF_REGISTER_MAP(MapType) \
+ namespace{ const bool WOLF_UNUSED MapType##Registered = \
+ FactoryMap::registerCreator(#MapType, MapType::create); } \
+
+
+typedef Factory<MapBase,
+ const ParamsServer&> AutoConfFactoryMap;
+
+template<>
+inline std::string AutoConfFactoryMap::getClass() const
+{
+ return "AutoConfFactoryMap";
+}
+
+#define WOLF_REGISTER_MAP_AUTO(MapType) \
+ namespace{ const bool WOLF_UNUSED MapType##AutoConfRegistered = \
+ AutoConfFactoryMap::registerCreator(#MapType, MapType::create); } \
+
+
+} /* namespace wolf */
+
+#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 13c870269665e595145b28d8ceca2a44aa0dac92..797d2bd3892e339c96a8c40e591eb56d39818ee5 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -32,11 +32,65 @@ class LandmarkBase;
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/common/params_base.h"
//std includes
namespace wolf {
+/*
+ * Macro for defining Autoconf map creator.
+ *
+ * Place a call to this macro inside your class declaration (in the map_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived map class, MapClass,
+ * must have a constructor available with the API:
+ *
+ * MapClass(const ParamsMapClassPtr _params);
+ *
+ * We recommend writing one of such constructors in your derived maps.
+ */
+#define WOLF_MAP_CREATE(MapClass, ParamsMapClass) \
+static \
+MapBasePtr create(const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsMapClass>(_server); \
+ \
+ return std::make_shared<MapClass>(params); \
+} \
+ \
+static \
+MapBasePtr create(const ParamsMapBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsMapClass>(_params); \
+ \
+ return std::make_shared<MapClass>(params); \
+} \
+
+
+/** \brief base struct for map parameters
+ *
+ * Derive from this struct to create structs of map parameters.
+ */
+struct ParamsMapBase: public ParamsBase
+{
+ std::string prefix = "map";
+
+ ParamsMapBase(const ParamsServer& _param_server) :
+ ParamsBase("map", _param_server)
+ {
+
+ };
+
+ ~ParamsMapBase() override = default;
+
+ std::string print() const override
+ {
+ return "";
+ }
+};
+
//class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
@@ -47,6 +101,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
MapBase();
+ MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
+ WOLF_MAP_CREATE(MapBase, ParamsMapBase);
+
~MapBase() override;
protected:
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 4ce5ab465d6e87ec8e7cacf5af05c06f2a6db32c..95631a8a6fb82ac22595b64d3e1d5b4fe7849cb4 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -102,6 +102,38 @@ inline VectorComposite identity()
return v;
}
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
+ MatrixBase<D2>& ip, typename D2::Scalar& io)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ MatrixSizeCheck<2, 1>::check(ip);
+
+ ip = SO2::exp(-o) * -p;
+ io = -o;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+ MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<3, 1>::check(d);
+ MatrixSizeCheck<3, 1>::check(id);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & d( 0 ) );
+ Map<Matrix<typename D2::Scalar, 2, 1> > ip ( & id( 0 ) );
+
+ inverse(p, d(2), ip, id(2));
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
+{
+ Matrix<typename D::Scalar, 3, 1> id;
+ inverse(d, id);
+ return id;
+}
+
template<class D1, class D2>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
@@ -408,7 +440,47 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
return res;
}
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& p1, const typename D1::Scalar& o1,
+ const MatrixBase<D2>& p2, const typename D2::Scalar& o2,
+ MatrixBase<D3>& p12, typename D3::Scalar& o12)
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<2, 1>::check(p12);
+
+ p12 = SO2::exp(-o1) * ( p2 - p1 );
+ o12 = o2 - o1;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d12)
+{
+ MatrixSizeCheck<3, 1>::check(d1);
+ MatrixSizeCheck<3, 1>::check(d2);
+ MatrixSizeCheck<3, 1>::check(d12);
+
+ typedef typename D1::Scalar T;
+
+ Map<const Matrix<T, 2, 1> > p1 ( & d1(0) );
+ Map<const Matrix<T, 2, 1> > p2 ( & d2(0) );
+ Map<Matrix<T, 2, 1> > p12 ( & d12(0) );
+
+ between(p1, d1(2), p2, d2(2), p12, d12(2));
+}
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2 )
+{
+ MatrixSizeCheck<3, 1>::check(d1);
+ MatrixSizeCheck<3, 1>::check(d2);
+ Matrix<typename D1::Scalar, 3, 1> d12;
+ between(d1, d2, d12);
+ return d12;
+}
} // namespace SE2
} // namespacs wolf
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 14f8588b7d01715a6ffb3dcd2e8be8d119aff78d..7405ae8320deb339ed6efdfe7319b4df8ec14cff 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -89,11 +89,11 @@ class Problem : public std::enable_shared_from_this<Problem>
PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
+ Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
public:
- static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
+ static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
@@ -343,6 +343,7 @@ class Problem : public std::enable_shared_from_this<Problem>
// Map branch -----------------------------------------
MapBasePtr getMap() const;
+ void setMap(MapBasePtr);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fixed_wing_model.h
similarity index 77%
rename from include/core/processor/processor_fix_wing_model.h
rename to include/core/processor/processor_fixed_wing_model.h
index 97c8ad6f3919c35744e824cde99d56c4d2b7e1b8..bccbc2317135376f7cf7dd1f70113fb933fc72b0 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fixed_wing_model.h
@@ -26,31 +26,31 @@
* Author: joanvallve
*/
-#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
-#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
#include "core/processor/processor_base.h"
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixedWingModel);
-struct ParamsProcessorFixWingModel : public ParamsProcessorBase
+struct ParamsProcessorFixedWingModel : public ParamsProcessorBase
{
Eigen::Vector3d velocity_local;
double angle_stdev;
double min_vel_norm;
- ParamsProcessorFixWingModel() = default;
- ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorFixedWingModel() = default;
+ ParamsProcessorFixedWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
- assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
+ assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized");
}
std::string print() const override
{
@@ -61,17 +61,17 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
}
};
-WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
+WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel);
-class ProcessorFixWingModel : public ProcessorBase
+class ProcessorFixedWingModel : public ProcessorBase
{
public:
- ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
+ ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params);
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
+ WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel, ParamsProcessorFixedWingModel);
- virtual ~ProcessorFixWingModel() override;
+ virtual ~ProcessorFixedWingModel() override;
void configure(SensorBasePtr _sensor) override;
protected:
@@ -105,9 +105,9 @@ class ProcessorFixWingModel : public ProcessorBase
virtual bool voteForKeyFrame() const override {return false;};
// ATTRIBUTES
- ParamsProcessorFixWingModelPtr params_processor_;
+ ParamsProcessorFixedWingModelPtr params_processor_;
};
} /* namespace wolf */
-#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_ */
diff --git a/include/core/processor/processor_logging.h b/include/core/processor/processor_logging.h
deleted file mode 100644
index 52eab893a2647dc4ef0af352b4ea7bdcd7215e33..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_logging.h
+++ /dev/null
@@ -1,52 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 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 processor_logging.h
- *
- * Created on: Oct 5, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_LOGGING_H_
-#define _WOLF_PROCESSOR_LOGGING_H_
-
-/// @brief un-comment for IDE highlights.
-
-
-#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
- static_assert(std::is_base_of<ProcessorBase, \
- typename std::remove_pointer<decltype(this)>::type>::value, \
- "This macro can be used only within the body of a " \
- "non-static " \
- "ProcessorBase (and derived) function !");
-
-#define WOLF_PROCESSOR_INFO(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED(getType(), __VA_ARGS__);
-
-#define WOLF_PROCESSOR_INFO_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED_COND(getType(), cond, __VA_ARGS__);
-
-#endif /* _WOLF_PROCESSOR_LOGGING_H_ */
diff --git a/include/core/sensor/sensor_model.h b/include/core/sensor/sensor_motion_model.h
similarity index 80%
rename from include/core/sensor/sensor_model.h
rename to include/core/sensor/sensor_motion_model.h
index 7070ff1c72da933fa44a0ca180a596224ab66ee5..68da8c7d481aa502c8352a350e03498de7f758e0 100644
--- a/include/core/sensor/sensor_model.h
+++ b/include/core/sensor/sensor_motion_model.h
@@ -19,26 +19,27 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef SRC_SENSOR_MODEL_H_
-#define SRC_SENSOR_MODEL_H_
+#ifndef SRC_SENSOR_MOTION_MODEL_H_
+#define SRC_SENSOR_MOTION_MODEL_H_
//wolf includes
#include "core/sensor/sensor_base.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(SensorModel);
-class SensorModel : public SensorBase
+WOLF_PTR_TYPEDEFS(SensorMotionModel);
+
+class SensorMotionModel : public SensorBase
{
public:
- SensorModel();
- ~SensorModel() override;
+ SensorMotionModel();
+ ~SensorMotionModel() override;
static SensorBasePtr create(const std::string& _unique_name,
const ParamsServer& _server)
{
- auto sensor = std::make_shared<SensorModel>();
+ auto sensor = std::make_shared<SensorMotionModel>();
sensor ->setName(_unique_name);
return sensor;
}
@@ -47,7 +48,7 @@ class SensorModel : public SensorBase
const Eigen::VectorXd& _extrinsics,
const ParamsSensorBasePtr _intrinsics)
{
- auto sensor = std::make_shared<SensorModel>();
+ auto sensor = std::make_shared<SensorMotionModel>();
sensor ->setName(_unique_name);
return sensor;
}
@@ -56,4 +57,4 @@ class SensorModel : public SensorBase
} /* namespace wolf */
-#endif /* SRC_SENSOR_POSE_H_ */
+#endif /* SRC_SENSOR_MOTION_MODEL_H_ */
diff --git a/include/core/utils/eigen_predicates.h b/include/core/utils/eigen_predicates.h
deleted file mode 100644
index 3a83601e84d95eba96d1193c651c220eca0ebf64..0000000000000000000000000000000000000000
--- a/include/core/utils/eigen_predicates.h
+++ /dev/null
@@ -1,121 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 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 eigen_predicates.h
- * \brief Some utils for comparing Eigen types
- * \author Jeremie Deray
- * Created on: 24/10/2017
- */
-
-#ifndef _WOLF_EIGEN_PREDICATES_H_
-#define _WOLF_EIGEN_PREDICATES_H_
-
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-/// @brief check that each Matrix/Vector elem is approx equal to value +- precision
-///
-/// @note we overload this rather than using Eigen::Matrix::isConstant() since it is
-/// defined as :
-/// abs(x - y) <= (min)(abs(x), abs(y)) * prec
-/// which does not play nice with y = 0
-auto is_constant = [](const Eigen::MatrixXd& lhs, const double val, const double precision)
- {
- for (Eigen::MatrixXd::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXd::Index i = 0; i < lhs.rows(); ++i)
- if (std::abs(lhs.coeff(i, j) - val) > precision)
- return false;
- return true;
- };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXd& lhs, const double precision)
- { return is_constant(lhs, 0, precision); };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_diff_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return pred_zero(lhs - rhs, precision); };
-
-/// @brief check that the Matrix/Vector difference norm is approx 0 +- precision
-auto pred_diff_norm_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return std::abs((lhs - rhs).norm()) <= std::abs(precision); };
-
-/// @brief check that the lhs Matrix/Vector is elem-wise approx the rhs +-precision
-auto pred_is_approx = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return lhs.isApprox(rhs, precision); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to rhs
-/// is <= precision
-auto pred_quat_is_approx = [](const Eigen::Quaterniond& lhs, const Eigen::Quaterniond& rhs, const double precision)
- { return pred_zero( log_q(rhs * lhs.inverse()), precision ); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to identity
-/// is <= precision
-auto pred_quat_identity = [](const Eigen::Quaterniond& lhs, const double precision)
- { return pred_quat_is_approx(lhs, Eigen::Quaterniond::Identity(), precision); };
-
-/// @brief check that rotation angle to get from lhs angle to rhs is <= precision
-auto pred_angle_is_approx = [](const double lhs, const double rhs, const double precision)
- { return std::abs(pi2pi(lhs - rhs)) <= std::abs(precision); };
-
-/// @brief check that rotation angle to get from lhs angle to 0 is <= precision
-auto pred_angle_zero = [](const double lhs, const double precision)
- { return pred_angle_is_approx(lhs, 0, precision); };
-
-/// @brief check that the lhs pose is approx rhs +- precision
-///
-/// @note
-/// Comparison is :
-/// d = inv(lhs) * rhs
-/// d.translation().elem_wise() ~ 0 (+- precision)
-///
-/// if pose 3d :
-/// d.rotation_as_quaternion() ~ quaternion.getIdentity (+- precision)
-///
-/// else if pose 2d:
-/// d.rotation_angle() ~ 0 (+- precision)
-///
-/// else throw std::runtime
-///
-/// @see pred_zero for translation comparison
-/// @see pred_quat_identity for 3d rotation comparison
-/// @see pred_angle_zero for 2d rotation comparison
-//auto pred_pose_is_approx = [](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs, const double precision)
-// {
-// const Eigen::MatrixXd d = lhs.inverse() * rhs;
-// const bool tok = pred_zero(d.rightCols(1), precision);
-
-// const bool qok = (lhs.rows() == 3)?
-// pred_quat_identity(Eigen::Quaterniond(d.block(0,0,3,1)),
-// precision)
-// : (lhs.rows() == 2)? pred_angle_zero(getYaw(d), precision)
-// : throw std::runtime_error("Canno't compare pose in Dim > 3 !");
-
-// return tok && qok;
-// };
-
-} // namespace wolf
-
-#endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/include/core/utils/logging.h b/include/core/utils/logging.h
index b34ceccb16c5e12567256f2749896d5369d017a8..a0e1556898fd453f5543ba90ec3645ccf282525c 100644
--- a/include/core/utils/logging.h
+++ b/include/core/utils/logging.h
@@ -222,16 +222,6 @@ inline void Logger::set_pattern(const std::string& p)
using LoggerPtr = std::unique_ptr<Logger>;
-/// dummy namespace to avoid colision with c++14
-/// @todo use std version once we move to cxx14
-namespace not_std {
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-} /* namespace not_std */
-
class LoggerManager
{
public:
@@ -265,7 +255,7 @@ protected:
/// @note would be easier with cpp17 map.try_emplace...
const bool created = existsImpl(name) ?
false :
- logger_map_.emplace(name, not_std::make_unique<Logger>(name)).second;
+ logger_map_.emplace(name, std::make_unique<Logger>(name)).second;
return created;
}
diff --git a/include/core/utils/make_unique.h b/include/core/utils/make_unique.h
deleted file mode 100644
index 0bfdbb8372a54fb59d2ab59d8ca1b78432b38dec..0000000000000000000000000000000000000000
--- a/include/core/utils/make_unique.h
+++ /dev/null
@@ -1,45 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 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 make_unique.h
- *
- * Created on: Oct 11, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_MAKE_UNIQUE_H_
-#define _WOLF_MAKE_UNIQUE_H_
-
-#include <memory>
-
-namespace wolf {
-
-/// @note this appears only in cpp14
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-
-}
-
-#endif /* _WOLF_MAKE_UNIQUE_H_ */
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index aed7fef36c99f71de9b664fa456620c7ef6523cd..ab8753351aee84c5b4f290af171b370663060499 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -27,7 +27,6 @@
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/utils/make_unique.h"
namespace wolf
{
@@ -47,8 +46,8 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
, n_interrupted_(0)
, n_no_convergence_(0)
{
- covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
- ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
+ covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
+ ceres_problem_ = std::make_unique<ceres::Problem>(params_ceres_->problem_options);
if (params_ceres_->interrupt_on_problem_change)
getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index f736ce180aa565b43ec5a68d1ec061c551f33916..8194c87d95dc05bb20b5f736afdb704a11a75560 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
// wolf
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/common/factory.h"
+//#include "core/common/factory.h"
// YAML
#include <yaml-cpp/yaml.h>
@@ -43,6 +43,12 @@ MapBase::MapBase() :
// std::cout << "constructed M"<< std::endl;
}
+MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
+ NodeBase("MAP", _type)
+{
+// std::cout << "constructed M"<< std::endl;
+}
+
MapBase::~MapBase()
{
// std::cout << "destructed -M" << std::endl;
@@ -148,3 +154,12 @@ bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _v
return _log.is_consistent_;
}
} // namespace wolf
+
+
+// Register in the FactorySensor
+#include "core/map/factory_map.h"
+namespace wolf {
+WOLF_REGISTER_MAP(MapBase);
+WOLF_REGISTER_MAP_AUTO(MapBase);
+} // namespace wolf
+
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 8d5858a6e0045372d9dfb75a702f29c018ba9d22..a3981d2f9e1a8e2b08a25f290ee2e7cf60ffd6be 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -24,7 +24,10 @@
#include "core/hardware/hardware_base.h"
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
+#include "core/map/factory_map.h"
#include "core/sensor/sensor_base.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/processor/factory_processor.h"
#include "core/processor/processor_motion.h"
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_pose.h"
@@ -32,8 +35,6 @@
#include "core/factor/factor_base.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
@@ -61,11 +62,11 @@
namespace wolf
{
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
- map_ptr_(std::make_shared<MapBase>()),
+ map_ptr_(_map),
motion_provider_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
@@ -96,12 +97,13 @@ void Problem::setup()
{
hardware_ptr_ -> setProblem(shared_from_this());
trajectory_ptr_-> setProblem(shared_from_this());
- map_ptr_ -> setProblem(shared_from_this());
+ if (map_ptr_)
+ map_ptr_ -> setProblem(shared_from_this());
}
-ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
+ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
{
- ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+ ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
p->setup();
return p->shared_from_this();
}
@@ -117,17 +119,19 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Problem structure and dimension
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
int dim = _server.getParam<int> ("problem/dimension");
- auto problem = Problem::create(frame_structure, dim);
+ auto problem = Problem::create(frame_structure, dim, nullptr);
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
auto loaders = std::vector<std::shared_ptr<Loader>>();
std::string plugins_path;
- try{
+ try
+ {
plugins_path = _server.getParam<std::string>("plugins_path");
}
- catch(MissingValueException& e){
+ catch (MissingValueException& e)
+ {
WOLF_WARN(e.what());
WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
plugins_path="/usr/local/lib/";
@@ -145,9 +149,11 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// load Packages for subscribers and publishers
std::string packages_path;
- try {
+ try
+ {
packages_path = _server.getParam<std::string>("packages_path");
- } catch (MissingValueException& e) {
+ }
+ catch (MissingValueException& e) {
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
@@ -168,9 +174,12 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// load raw libs
std::vector<std::string> raw_libs;
- try {
+ try
+ {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
- } catch (MissingValueException& e) {
+ }
+ catch (MissingValueException& e)
+ {
WOLF_TRACE("No raw libraries to load...");
raw_libs = std::vector<std::string>();
}
@@ -196,11 +205,48 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
_server);
+ // Map (optional)
+ std::string map_type, map_plugin;
+ try
+ {
+ map_type = _server.getParam<std::string>("map/type");
+ map_plugin = _server.getParam<std::string>("map/plugin");
+ }
+ catch (MissingValueException& e)
+ {
+ WOLF_TRACE("No map/type and/or map/plugin specified. Emplacing the default map: MapBase.");
+ map_type = "MapBase";
+ map_plugin = "core";
+ }
+ WOLF_TRACE("Map Type: ", map_type, " in plugin ", map_plugin);
+ if (map_plugin != "core" and map_plugin != "wolf")
+ {
+ std::string plugin = plugins_path + "libwolf" + map_plugin + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
+ auto map = AutoConfFactoryMap::create(map_type, _server);
+ map->setProblem(problem);
+ problem->setMap(map);
+
// Tree manager
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
if (tree_manager_type != "None" and tree_manager_type != "none")
+ {
+ std::string tm_plugin = _server.getParam<std::string>("problem/tree_manager/plugin");
+ if (tm_plugin != "core" and tm_plugin != "wolf")
+ {
+ std::string plugin = plugins_path + "libwolf" + tm_plugin + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
+ }
// Set problem prior -- first keyframe
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
@@ -215,9 +261,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
problem->setPriorOptions(prior_mode,
_server.getParam<VectorComposite>("problem/prior/state"),
_server.getParam<VectorComposite>("problem/prior/sigma"));
-
-
-
}
else
{
@@ -481,7 +524,15 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
for (const auto& pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
- state.insert(pair_key_vec);
+ {
+ state.insert(pair_key_vec);
+ }
+ }
+
+ //If all keys are filled return
+ if (state.size() == structure.size())
+ {
+ return state;
}
}
@@ -531,6 +582,12 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
state.insert(pair_key_vec);
}
+
+ //If all keys are filled return
+ if (state.size() == structure.size())
+ {
+ return state;
+ }
}
// check for empty blocks and fill them with the closest KF to ts, with the prior, or with zeros in the worst case
@@ -976,6 +1033,13 @@ MapBasePtr Problem::getMap() const
return map_ptr_;
}
+void Problem::setMap(MapBasePtr _map_ptr)
+{
+ assert(map_ptr_ == nullptr && "map has already been set");
+
+ map_ptr_ = _map_ptr;
+}
+
TrajectoryBasePtr Problem::getTrajectory() const
{
return trajectory_ptr_;
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
similarity index 83%
rename from src/processor/processor_fix_wing_model.cpp
rename to src/processor/processor_fixed_wing_model.cpp
index 4c75926cc84925fa827f580a2d126ac294651d4b..4f67c52a8246ad06558e92cb4e43c1910b7e3baa 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -26,7 +26,7 @@
* Author: joanvallve
*/
-#include "core/processor/processor_fix_wing_model.h"
+#include "core/processor/processor_fixed_wing_model.h"
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
@@ -35,22 +35,22 @@
namespace wolf
{
-ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
- ProcessorBase("ProcessorFixWingModel", 3, _params),
+ProcessorFixedWingModel::ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params) :
+ ProcessorBase("ProcessorFixedWingModel", 3, _params),
params_processor_(_params)
{
}
-ProcessorFixWingModel::~ProcessorFixWingModel()
+ProcessorFixedWingModel::~ProcessorFixedWingModel()
{
}
-void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
+void ProcessorFixedWingModel::configure(SensorBasePtr _sensor)
{
assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
}
-void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
+void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (_keyframe_ptr->getV()->isFixed())
return;
@@ -81,7 +81,7 @@ void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
+WOLF_REGISTER_PROCESSOR(ProcessorFixedWingModel);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixedWingModel);
} // namespace wolf
diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_motion_model.cpp
similarity index 79%
rename from src/sensor/sensor_model.cpp
rename to src/sensor/sensor_motion_model.cpp
index 292f1a05e1bfd5aaf1931998106a505afaf10df8..d2f40087622650d8a500ba2418fff892f4cad88f 100644
--- a/src/sensor/sensor_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -19,18 +19,18 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include "core/sensor/sensor_model.h"
+#include "core/sensor/sensor_motion_model.h"
namespace wolf {
-SensorModel::SensorModel() :
- SensorBase("SensorModel", nullptr, nullptr, nullptr, 6)
+SensorMotionModel::SensorMotionModel() :
+ SensorBase("SensorMotionModel", nullptr, nullptr, nullptr, 6)
{
//
}
-SensorModel::~SensorModel()
+SensorMotionModel::~SensorMotionModel()
{
//
}
@@ -40,6 +40,6 @@ SensorModel::~SensorModel()
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR(SensorModel);
-WOLF_REGISTER_SENSOR_AUTO(SensorModel);
+WOLF_REGISTER_SENSOR(SensorMotionModel);
+WOLF_REGISTER_SENSOR_AUTO(SensorMotionModel);
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a3da6a740948958190273e4526f997a6872a0cb1..9479b901549a8777580ab7455507a4f9a83a9567 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -68,10 +68,6 @@ target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
-# FeatureBase classes test
-wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
-
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
@@ -130,11 +126,16 @@ target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
+target_link_libraries(gtest_rotation ${PLUGIN_NAME})
# SE3 test
wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
+# SE2 test
+wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
+target_link_libraries(gtest_SE2 ${PLUGIN_NAME})
+
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})
@@ -217,6 +218,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM
wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
+# FactorRelativePose3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
+
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
@@ -233,11 +238,11 @@ target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
-# ProcessorDiffDriveSelfcalib class test
-wolf_add_gtest(gtest_processor_fix_wing_model gtest_processor_fix_wing_model.cpp)
-target_link_libraries(gtest_processor_fix_wing_model ${PLUGIN_NAME})
+# ProcessorFixedWingModel class test
+wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
-# ProcessorFixWingModel class test
+# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
diff --git a/test/gtest_SE2.cpp b/test/gtest_SE2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..690a5f373e2259c8cc2268c898b04f28c7c8fdc4
--- /dev/null
+++ b/test/gtest_SE2.cpp
@@ -0,0 +1,259 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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 "core/math/SE2.h"
+#include "core/utils/utils_gtest.h"
+
+using namespace Eigen;
+using namespace wolf;
+using namespace SE2;
+
+TEST(SE2, exp)
+{
+ for (auto i = 1; i<10; i++)
+ {
+ double o = Vector1d::Random()(0) * M_PI;
+ ASSERT_MATRIX_APPROX(SO2::exp(o), Rotation2Dd(o).matrix(), 1e-8);
+ }
+}
+
+TEST(SE2, invBlocks)
+{
+ Vector2d p, ip;
+ double o, io;
+
+ // zero
+ p = Vector2d::Zero();
+ o = 0;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(io, 0, 1e-8);
+
+ // zero angle
+ p = Vector2d::Random();
+ o = 0;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, -p, 1e-8);
+ ASSERT_NEAR(io, 0, 1e-8);
+
+ // zero translation
+ p = Vector2d::Zero();
+ o = Vector1d::Random()(0) * M_PI;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(io, -o, 1e-8);
+
+ // random
+ for (auto i = 0; i < 10; i++)
+ {
+ p = Vector2d::Random();
+ o = Vector1d::Random()(0) * M_PI;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, Rotation2Dd(-o).matrix() * -p, 1e-8);
+ ASSERT_NEAR(io, -o, 1e-8);
+ }
+}
+
+TEST(SE2, invVector)
+{
+ Vector3d d, id;
+
+ // zero
+ d = Vector3d::Zero();
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id, Vector3d::Zero(), 1e-8);
+
+ // zero angle
+ d = Vector3d::Random();
+ d(2) = 0;
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id.head<2>(), -d.head<2>(), 1e-8);
+ ASSERT_NEAR(id(2), 0, 1e-8);
+
+ // zero translation
+ d = Vector3d::Zero();
+ d(2) = Vector1d::Random()(0) * M_PI;
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id.head<2>(), Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(id(2), -d(2), 1e-8);
+
+ // random
+ for (auto i = 0; i < 10; i++)
+ {
+ d = Vector3d::Random();
+ d(2) *= M_PI;
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id.head<2>(), Rotation2Dd(-d(2)) * -d.head<2>(), 1e-8);
+ ASSERT_NEAR(id(2), -d(2), 1e-8);
+ }
+}
+
+TEST(SE2, betweenBlocks)
+{
+ Vector2d p1, p2, pd;
+ double o1, o2, od;
+
+ // both origin: relative pose zero
+ p1 = Vector2d::Zero();
+ p2 = p1;
+ o1 = 0;
+ o2 = o1;
+ between(p1, o1, p2, o2, pd, od);
+
+ ASSERT_MATRIX_APPROX(pd, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(od, 0, 1e-8);
+
+ // both same random pose: relative pose zero
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector2d::Random();
+ p2 = p1;
+ o1 = Vector1d::Random()(0) * M_PI;
+ o2 = o1;
+
+ between(p1, o1, p2, o2, pd, od);
+
+ ASSERT_MATRIX_APPROX(pd, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(od, 0, 1e-8);
+ }
+
+ // random relative pose
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector2d::Random();
+ o1 = Vector1d::Random()(0) * M_PI;
+ Vector2d pd_gt = Vector2d::Random();
+ double od_gt = Vector1d::Random()(0) * M_PI;
+
+ p2 = p1 + Rotation2Dd(o1) * pd_gt;
+ o2 = o1 + od_gt;
+
+ between(p1, o1, p2, o2, pd, od);
+
+ ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+ ASSERT_NEAR(od, od_gt, 1e-8);
+ }
+}
+
+TEST(SE2, betweenVectors)
+{
+ Vector3d p1, p2, pd;
+
+ // both origin: relative pose zero
+ p1 = Vector3d::Zero();
+ p2 = p1;
+ between(p1, p2, pd);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+
+ // both same random pose: relative pose zero
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ p2 = p1;
+
+ between(p1, p2, pd);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+ }
+
+ // random relative pose
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ Vector3d pd_gt = Vector3d::Random();
+ pd_gt(2) *= M_PI;
+
+ p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
+ p2(2) = p1(2) + pd_gt(2);
+
+ between(p1, p2, pd);
+
+ ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+ }
+}
+
+TEST(SE2, betweenVectorsReturn)
+{
+ Vector3d p1, p2, pd;
+
+ // both origin: relative pose zero
+ p1 = Vector3d::Zero();
+ p2 = p1;
+ pd = between(p1, p2);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+
+ // both same random pose: relative pose zero
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ p2 = p1;
+
+ pd = between(p1, p2);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+ }
+
+ // random relative pose
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ Vector3d pd_gt = Vector3d::Random();
+ pd_gt(2) *= M_PI;
+
+ p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
+ p2(2) = p1(2) + pd_gt(2);
+
+ pd = between(p1, p2);
+
+ ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
deleted file mode 100644
index 6dc7184ec5d1d39db31a2e26d06964023c26e65f..0000000000000000000000000000000000000000
--- a/test/gtest_eigen_predicates.cpp
+++ /dev/null
@@ -1,199 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 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 "core/utils/utils_gtest.h"
-
-#include "core/utils/eigen_predicates.h"
-
-TEST(TestEigenPredicates, TestEigenDynPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Zero(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = Eigen::MatrixXd::Ones(4,4) * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Zero();
- B = Eigen::Matrix4d::Random();
- C = Eigen::Matrix4d::Ones() * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
-{
- Eigen::Quaterniond A, B, C;
-
- /// @todo which version of Eigen provides this ?
-// A = Eigen::Quaterniond::UnitRandom();
-
- A.coeffs() = Eigen::Vector4d::Random().normalized();
- B.coeffs() = Eigen::Vector4d::Random().normalized();
- C = A;
-
- EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
-{
- Eigen::Quaterniond A, B;
-
- A = Eigen::Quaterniond::Identity();
- B.coeffs() = Eigen::Vector4d::Random().normalized();
-
- EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
-{
- double a = M_PI;
- double b = -M_PI;
- double c = 0;
-
- EXPECT_TRUE(wolf::pred_angle_is_approx(a, b, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_is_approx(a, c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsZero)
-{
- double a = 0;
- double b = M_PI;
- double c = 2.*M_PI;
-
- EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_angle_zero(c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f89a11003bd0e297ed6b3df283b6aba536ca5ea
--- /dev/null
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -0,0 +1,325 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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 <core/factor/factor_relative_pose_3d_with_extrinsics.h>
+#include <core/utils/utils_gtest.h>
+
+#include "core/common/wolf.h"
+#include "core/utils/logging.h"
+
+#include "core/state_block/state_quaternion.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_pose.h"
+#include "core/feature/feature_pose.h"
+
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
+ public:
+ Vector3d pos_camera, pos_robot, pos_landmark;
+ Vector3d euler_camera, euler_robot, euler_landmark;
+ Quaterniond quat_camera, quat_robot, quat_landmark;
+ Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
+ Vector7d pose_camera, pose_robot, pose_landmark;
+
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+
+ SensorBasePtr S;
+ FrameBasePtr F1;
+ CapturePosePtr c1;
+ FeaturePosePtr f1;
+ LandmarkBasePtr lmk1;
+ FactorRelativePose3dWithExtrinsicsPtr fac;
+
+ Eigen::Matrix6d meas_cov;
+
+ void SetUp() override
+ {
+ // configuration
+
+ /* We have three poses to take into account:
+ * - pose of the sensor (extrinsincs)
+ * - pose of the landmark
+ * - pose of the robot (Keyframe)
+ *
+ * The measurement provides the pose of the landmark wrt sensor's current pose in the world.
+ * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics.
+ *
+ * The robot has a sensor looking forward
+ * S: pos = (0,0,0), ori = (0, 0, 0)
+ *
+ * There is a point at the origin
+ * P: pos = (0,0,0)
+ *
+ * Therefore, P projects exactly at the origin of the sensor,
+ * f: p = (0,0)
+ *
+ * We form a Wolf tree with 1 frames F1, 1 capture C1,
+ * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1:
+ *
+ * Frame F1, Capture C1, feature f1, landmark l1, constraint c1
+ *
+ * The frame pose F1, and the sensor pose S
+ * in the robot frame are variables subject to optimization
+ *
+ * We perform a number of tests based on this configuration.*/
+
+
+ // sensor is at origin of robot and looking forward
+ // robot is at (0,0,0)
+ // landmark is right in front of sensor. Its orientation wrt sensor is identity.
+ pos_camera << 0,0,0;
+ pos_robot << 0,0,0; //robot is at origin
+ pos_landmark << 0,1,0;
+ euler_camera << 0,0,0;
+ //euler_camera << -M_PI_2, 0, -M_PI_2;
+ euler_robot << 0,0,0;
+ euler_landmark << 0,0,0;
+ quat_camera = e2q(euler_camera);
+ quat_robot = e2q(euler_robot);
+ quat_landmark = e2q(euler_landmark);
+ vquat_camera = quat_camera.coeffs();
+ vquat_robot = quat_robot.coeffs();
+ vquat_landmark = quat_landmark.coeffs();
+ pose_camera << pos_camera, vquat_camera;
+ pose_robot << pos_robot, vquat_robot;
+ pose_landmark << pos_landmark, vquat_landmark;
+
+ // Build problem
+ problem = Problem::create("PO", 3);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // Create sensor to be able to initialize (a camera for instance)
+ S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose",
+ std::make_shared<StateBlock>(pos_camera, true),
+ std::make_shared<StateQuaternion>(vquat_camera, true),
+ std::make_shared<StateBlock>(Vector4d::Zero(), false), // fixed
+ Vector1d::Zero());
+
+ // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
+ // S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera);
+ // camera = std::static_pointer_cast<SensorCamera>(S);
+
+
+ // F1 is be origin KF
+ VectorComposite x0(pose_robot, "PO", {3,4});
+ VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
+ F1 = problem->setPriorFactor(x0, s0, 0.0);
+
+
+ // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world)
+ meas_cov = Eigen::Matrix6d::Identity();
+ meas_cov.topLeftCorner(3,3) *= 1e-2;
+ meas_cov.bottomRightCorner(3,3) *= 1e-3;
+
+ //emplace feature and landmark
+ c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
+ f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
+ lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
+ std::make_shared<StateBlock>(pos_landmark),
+ std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
+ }
+};
+
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
+{
+ auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
+ lmk1,
+ nullptr,
+ false);
+
+ ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
+}
+
+/////////////////////////////////////////////
+// Tree not ok with this gtest implementation
+/////////////////////////////////////////////
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
+{
+ ASSERT_TRUE(problem->check(1));
+
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+ ASSERT_TRUE(problem->check(1));
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ // unfix F1, perturbate state
+ F1->unfix();
+ F1->getP()->perturb();
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ // unfix F1, perturbate state
+ F1->unfix();
+ F1->getO()->perturb();
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ // unfix lmk1, perturbate state
+ lmk1->unfix();
+ lmk1->getP()->perturb();
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ // unfix F1, perturbate state
+ lmk1->unfix();
+ lmk1->getO()->perturb();
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
+{
+ // Change setup
+ Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
+ Quaterniond q_w_r, q_r_c, q_c_l, q_w_l;
+ p_w_r << 1, 2, 3;
+ p_r_c << 4, 5, 6;
+ p_c_l << 7, 8, 9;
+ q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize();
+ q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize();
+ q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize();
+
+ q_w_l = q_w_r * q_r_c * q_c_l;
+ p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l);
+
+ // Change feature (remove and emplace)
+ Vector7d meas;
+ meas << p_c_l, q_c_l.coeffs();
+ f1->remove();
+ f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov));
+
+ // emplace factor
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ // Change Landmark
+ lmk1->getP()->setState(p_w_l);
+ lmk1->getO()->setState(q_w_l.coeffs());
+ ASSERT_TRUE(lmk1->getP()->stateUpdated());
+ ASSERT_TRUE(lmk1->getO()->stateUpdated());
+
+ // Change Frame
+ F1->getP()->setState(p_w_r);
+ F1->getO()->setState(q_w_r.coeffs());
+ F1->fix();
+ ASSERT_TRUE(F1->getP()->stateUpdated());
+ ASSERT_TRUE(F1->getO()->stateUpdated());
+
+ // Change sensor extrinsics
+ S->getP()->setState(p_r_c);
+ S->getO()->setState(q_r_c.coeffs());
+ ASSERT_TRUE(S->getP()->stateUpdated());
+ ASSERT_TRUE(S->getO()->stateUpdated());
+
+ Vector7d t_w_r, t_w_l;
+ t_w_r << p_w_r, q_w_r.coeffs();
+ t_w_l << p_w_l, q_w_l.coeffs();
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6);
+
+ // unfix LMK, perturbate state
+ lmk1->unfix();
+ lmk1->perturb();
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6);
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d5fa0054e26e64664dba42b3d0fc66a717bd8709..d86942de20f525f0b0995e2a5b43e18750311697 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -512,11 +512,35 @@ TEST(Problem, check)
ASSERT_TRUE(problem->check(true, std::cout));
}
+TEST(Problem, autoSetupMap)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->check(true, std::cout));
+}
+
+TEST(Problem, autoSetupNoMap)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup_no_map.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->check(true, std::cout));
+}
+
TEST(Problem, getState)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- auto parser = ParserYaml("test/yaml/params_problem_odom_3d.yaml", wolf_root);
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
auto server = ParamsServer(parser.getParams());
auto P = Problem::autoSetup(server);
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
similarity index 94%
rename from test/gtest_processor_fix_wing_model.cpp
rename to test/gtest_processor_fixed_wing_model.cpp
index 41eaed71fd6bb47d2353bb6818349daad0aae326..b2bcb1990ccd266756c7f106478b79aadff3cc88 100644
--- a/test/gtest_processor_fix_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -23,12 +23,12 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/processor/processor_fix_wing_model.h"
#include "core/state_block/state_quaternion.h"
// STL
#include <iterator>
#include <iostream>
+#include "../include/core/processor/processor_fixed_wing_model.h"
using namespace wolf;
using namespace Eigen;
@@ -58,11 +58,11 @@ class ProcessorFixWingModelTest : public testing::Test
2);
// Emplace processor
- ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
+ ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>();
params->velocity_local = (Vector3d() << 1, 0, 0).finished();
params->angle_stdev = 0.1;
params->min_vel_norm = 0;
- processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
+ processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params);
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c61eab58146a9a5917c1130a14e234afa08a01b6
--- /dev/null
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -0,0 +1,49 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6c5ed47c2efc2afc3ba960b075e60cea73b58a73
--- /dev/null
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -0,0 +1,46 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index fa43fecb397dff295b683cfcd6a282adac61cc44..59aec3c70cee0e147e38dbfb9408c8879bc203e2 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -17,6 +17,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerDummy"
+ plugin: "core"
toy_param: 0
sensors:
-
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index f37e31459d9a883aca9eb12898aa5ac285e63210..419125468ba5155eba8f0f75c972a5d52b5dbcef 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -15,7 +15,7 @@ config:
O: [0.31, 0.31, 0.31]
V: [0.31, 0.31, 0.31]
time_tolerance: 0.1
- tree_manager:
+ tree_manager:
type: "None"
sensors:
-
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 277810464d6f619ed342ce3706bec30d7ca8e5f9..61498d1b6c182a6ae95930047940c56bcc3ca4ae 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
+ plugin: "core"
n_frames: 3
n_fix_first_frames: 2
viral_remove_empty_parent: true
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index f22fdde12f065d17accb122ef7f8d1728ef6fb6c..db0a176fce7534934e9a10da0c5f84a5ae431517 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
+ plugin: "core"
n_frames: 3
n_fix_first_frames: 0
viral_remove_empty_parent: false
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index a7f0f7434fb8a71c74e3aa3f15b8dc9f6ea7c067..2b6313f5b9702ab0f7c3dc0187dbde23ae944d00 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
+ plugin: "core"
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
index cae3df67f036430481cd936ea31a9d2b4c0bca9a..609fb96f585545376756279956377ee6730dbc0d 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
+ plugin: "core"
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index 0b030b80faa8a5f63dfa4c60aa19bcfd3c81a9b0..aed7a0c7e4da1313e261501c87e3748fb64cd2b5 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -15,6 +15,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
+ plugin: "core"
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2