diff --git a/.gitignore b/.gitignore
index fae25dce1facaccabf8aa15085cf2491d5959212..94d4918bdec0d0d02df589c529d938e6c87d72d4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -31,5 +31,5 @@ src/examples/map_apriltag_save.yaml
\.vscode/
build_release/
-
+.clangd
wolf.found
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 1477cdc248844ab181e44194a77c04573381a155..dbf139b13a33b4b14ec3125cca2621f6c9e47381 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,4 +1,4 @@
-image: segaleran/ceres
+image: labrobotica/ceres:1.14
before_script:
- ls
diff --git a/CMakeLists.txt b/CMakeLists.txt
index abc6d9039a34e2efe6eb5645a9ab4aa3bdf00d51..99ec87961134e40e963ac23176a65666a6186bfe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -116,12 +116,16 @@ ENDIF(BUILD_DEMOS OR BUILD_TESTS)
#find dependencies.
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
FIND_PACKAGE(Threads REQUIRED)
FIND_PACKAGE(Ceres REQUIRED) #Ceres is not required
+FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
+ message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
+endif()
+
# YAML with yaml-cpp
FIND_PACKAGE(YamlCpp REQUIRED)
@@ -181,6 +185,7 @@ SET(HDRS_UTILS
include/core/utils/params_server.hpp
include/core/utils/singleton.h
include/core/utils/utils_gtest.h
+ include/core/utils/converter_utils.h
)
SET(HDRS_PROBLEM
include/core/problem/problem.h
@@ -312,6 +317,7 @@ SET(SRCS_COMMON
SET(SRCS_MATH
)
SET(SRCS_UTILS
+ src/utils/converter_utils.cpp
)
SET(SRCS_CAPTURE
diff --git a/README.md b/README.md
index 69055e1fda60d7623404761607c6bc5f531c7b80..ad5473fec99242beacf3889f0818e3f4cd93caa7 100644
--- a/README.md
+++ b/README.md
@@ -1,381 +1,4 @@
WOLF - Windowed Localization Frames
===================================
-Overview
---------
-
-Wolf is a library to solve localization problems in mobile robotics, such as SLAM, map-based localization, or visual odometry. The approach contemplates the coexistence of multiple sensors of different kind, be them synchronized or not. It is thought to build state vectors formed by a set of key-frames (window), defining the robot trajectory, plus other states such as landmarks or sensor parameters for calibration, and compute error vectors given the available measurements in that window.
-
-Wolf is mainly a structure for having the data accessible and organized, plus some functionality for managing this data. It requires, on one side, several front-ends (one per sensor, or per sensor type), and a back-end constituted by the solver.
-
-Wolf may be interfaced with many kinds of solvers, including filters and nonlinear optimizers. It can be used with EKF, error-state KF, iterative EKF, and other kinds of filters such as information filters. It can also be used with nonlinear optimizers, most especially -- though not necessarily -- in their sparse flavors, and in particular the incremental ones.
-
-The task of interfacing Wolf with these solvers is relegated to wrappers, which are coded out of Wolf. We provide a wrapper to Google CERES. We also provide an experimental wrapper-less solver for incremental, sparse, nonlinear optimization based on the QR decomposition (implementing the iSAM algorithm).
-
-The basic Wolf structure is a tree of base classes reproducing the elements of the robotic problem. This is called the Wolf Tree. It has a robot, with sensors, with a trajectory formed by keyframes, and the map with its landmarks. These base classes can be derived to build the particularizations you want. You have the basic functionality in the base classes, and you add what you want on top. The Wolf Tree connectivity is augmented with the constraints linking different parts of it, becoming a real network of relations. This network is equivalent to the factor graph that would be solved by graphical models and nonlinear optimization. Wrappers are the ones transferring the Wolf structure into a factor graph that can be provided to the solver. See the documentation for a proper rationale and all the details.
-
-Wolf can be used within ROS for an easy integration. We provide examples of ROS nodes using Wolf. Wolf can also be used in other robotics frameworks.
-
-#### Features
-
-- Keyframe based
-- Multi-sensor
-- Pose-SLAM, landmark-based SLAM, or visual odometry
-- Different state types -- the state blocks
-- Different measurement models -- the constraint blocks
-- Built with polymorphic classes using virtual inheritance and templates.
-- Solver agnostic: choose your solver and build your wrapper to Wolf.
-
-#### Some preliminary documentation
-
-- You can visit this [Wolf inspiring document](https://docs.google.com/document/d/1_kBtvCIo33pdP59M3Ib4iEBleDDLcN6yCbmwJDBLtcA). Contact [Joan](mailto:jsola@iri.upc.edu) if you need permissions for the link.
-- You can also have a look at the [Wolf tree](https://docs.google.com/drawings/d/1jj5VVjQThddswpTPMLG2xv87vtT3o1jiMJo3Mk1Utjg), showing the organization of the main elements in the Wolf project. Contact [Andreu](mailto:acorominas@iri.upc.edu) if you need permissions for the link.
-- You can finally visit this [other inspiring document](https://docs.google.com/document/d/18XQlgdfTwplakYKKsfw2YAoaVuSyUEQoaU6bYHQDNpU) providing the initial motivation for the Wolf project. Contact [Joan](mailto:jsola@iri.upc.edu) if you need permissions for the link.
-
-Dependencies
-------------
-
-! Please notice that we are detailing two installation procedures below. If you are familiar with `ROS` and more especially the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/index.html) package then you may jump directly to the 'Using the `catkin_tools` package' section.
-
-#### CMake
-Building tool used by Wolf and by some of its dependencies. In order to install *cmake* please follow the instructions at [cmake site](https://cmake.org/install/)
-
-#### Autoreconf
-
- $ sudo apt install dh-autoreconf
-
-#### Eigen
-
-[Eigen](http://eigen.tuxfamily.org). Linear algebra, header library. Eigen 3.2 is also a depencency of ROS-Hydro. In case you don't have ROS in your machine, you can install Eigen by typing:
-
- $ sudo apt-get install libeigen3-dev
-
-#### Ceres (5 steps)
-
-[Ceres](http://www.ceres-solver.org/) is an optimization library. Currently, this dependency is optional, so the build procedure of Wolf skips part of compilation in case this dependency is not found on the system. **Installation** is described at [Ceres site](http://www.ceres-solver.org/building.html). However we report here an alternative step by step procedure to install Ceres.
-
-**(1)** Skip this step if Cmake 2.8.0+ and Eigen3.0+ are already installed. Otherwise install them with *apt-get*.
-
-**(2) GFLAGS**
-
-- Git clone the source:
-
- $ git clone https://github.com/gflags/gflags.git
-
-- Build and install with:
-
- $ cd gflags
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DGFLAGS_NAMESPACE="google" ..
- $ make
- $ sudo make install
-
-libgflags.a will be installed at **/usr/local/lib**
-
-**(3) GLOG**
-
-- Git clone the source:
-
- $ git clone https://github.com/google/glog.git
-
-- Build and install with:
-
- $ cd glog
- $ ./autogen.sh
- $ ./configure --with-gflags=/usr/local/
- $ make
- $ sudo make install
-
-libglog.so will be installed at **/usr/local/lib**
-
-- Troubleshooting:
-
- * If ./autogen.sh fails with './autogen.sh: autoreconf: not found'
-
- In a fresh installation you will probably need to install autoreconf running
-
- $ sudo apt-get install dh-autoreconf
-
- * If `make` command fails with the error: `/bin/bash: aclocal-1.14: command not found`
-
- Install Glog with the following commands:
-
- $ cd glog
- $ sudo apt-get install autoconf
- $ autoreconf --force --install
- $ ./configure --with-gflags=/usr/local/
- $ make
- $ sudo make install
-
-**(4) SUITESPARSE**
-
-- Easy way!:
-
- $ sudo apt-get install libsuitesparse-dev
-
-**(5) CERES**
-
-- Git clone the source:
-
- $ git clone https://ceres-solver.googlesource.com/ceres-solver
-
-- Build and install with:
-
- $ cd ceres-solver
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
- $ make
- $ sudo make install
-
-libceres.a will be installed at **/usr/local/lib** and headers at **/usr/local/include/ceres**
-
-#### Yaml-cpp
-
-Wolf uses YAML files for configuration and for saving and loading workspaces. To obtain it run:
-
-- Ubuntu:
-
- $ sudo apt-get install libyaml-cpp-dev
-
-- Mac:
-
- $ brew install yaml-cpp
-
-We are shipping the CMAKE file `FindYamlCpp.cmake` together with Wolf. Find it at `[wolf]/cmake_modules/FindYamlCpp.cmake`
-
-#### spdlog
-
-Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0.17.0. To install it:
-
-- Git clone the source:
-
- $ git clone https://github.com/gabime/spdlog.git
-
-- Build and install with:
-
- $ cd spdlog
- $ git checkout v0.17.0
- $ mkdir build
- $ cd build
- $ cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" ..
- $ make
- $ sudo make install
-
-#### Optional: Vision Utils (Install only if you want to use IRI's vision utils)
-
-This library requires OpenCV. If it is not installed in your system or you are unsure, please follow the installation steps at https://gitlab.iri.upc.edu/mobile_robotics/vision_utils
-
-**(1)** Git clone the source:
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/vision_utils.git
-
-**(2)** Build and install:
-
- $ cd vision_utils/build
- $ cmake ..
- $ make
- $ sudo make install
-
-**(3)** Optionally run tests
-
- $ ctest
-
-#### Optional: Laser Scan Utils (Install only if you want to use IRI's laser scan utils)
-
-**(1)** Git clone the source:
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/laser_scan_utils.git
-
-**(2)** Build and install:
-
- $ cd laser_scan_utils
- $ mkdir build && cd build
- $ cmake ..
- $ make
- $ sudo make install
-
-#### Optional: Raw GPS Utils (Install only if you want to use IRI's raw gps utils)
-
-**(1)** Git clone the source:
-
- $ git clone https://github.com/pt07/raw_gps_utils.git
-
-**(2)** Build and install:
-
- $ cd raw_gps_utils
- $ mkdir build && cd build
- $ cmake ..
- $ make
- $ sudo make install
-
-Download and build
-------------------
-
-#### Wolf C++ Library
-
-**Clone:**
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
-
-**Build:**
-
- $ cd wolf
- $ mkdir build
- $ cd build
- $ cmake ..
- $ make
- $ sudo make install //optional in case you want to install wolf library
-
-
-#### Wolf ROS Node
-
-- Git clone the source (inside your ROS workspace):
-
- $ git clone https://github.com/IRI-MobileRobotics/wolf_ros.git
-
-Using the `catkin_tools` package
---------------------------------
-
-**(1)** Install `catkin_tools` :
-
-[`installation webpage.`](https://catkin-tools.readthedocs.io/en/latest/installing.html)
-
-- Installing on Ubuntu with `apt-get`
-
- $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
- $ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- $ sudo apt-get update
- $ sudo apt-get install python-catkin-tools
-
-- Installing with [`pip`](https://pip.pypa.io/en/stable/installing/)
-
- $ sudo pip install -U catkin_tools
-
-**(2)** Create a `catkin workspace` :
-
- $ cd
- $ mkdir -p wolf_ws/src
- $ cd wolf_ws/src
- $ catkin_init_workspace
- $ cd ..
- $ catkin_make
-
-**(3)** Setup your `bash_rc`:
-
-Add at the end of the ~/.bashrc file with the following command:
-
- $ echo "source ~/wolf_ws/devel/setup.bash" >> ~/.bashrc
-
-Source your bash:
-
- source ~/.bashrc
-
-**(3)** Download `Ceres` :
-
-In the previously created directory `~/my_workspace_directory/wolf_ws/src/` clone `Ceres` & `wolf`.
-
- $ git clone https://github.com/artivis/ceres_solver.git
-
-**(4)** Download `wolf` :
-
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf.git
-
-At this point you might need to switch to the `catkin_build` branch of the wolf project.
-
- $ cd wolf
- $ git checkout catkin_build
-
-(optional) Download `wolf_ros` :
-
- $ git clone https://github.com/IRI-MobileRobotics/Wolf_ros.git
-
-**(5)** Let's Compile !
-
- The command below can be launch from any sub-directory in `~/my_workspace_directory/wolf_ws/`.
-
- $ catkin build
-
-**(6)** Run tests:
-
- $ catkin run_tests
-
-Troubleshooting
----------------
-
-#### Boost
-
-We have made our best to keep being boost-independent.
-However, in case you run into a boost installation issue at execution time, check that you have boost installed.
-If needed, install it with:
-
-[Boost](http://www.boost.org/). Free peer-reviewed portable C++ source libraries.
-
- $ sudo apt-get install libboost-all-dev
-
-
-
-Inspiring Links
----------------
-
-- [Basics on code optimization](http://www.eventhelix.com/realtimemantra/basics/optimizingcandcppcode.htm)
-
-- [Headers, Includes, Forward declarations, ...](http://www.cplusplus.com/forum/articles/10627/)
-
-- Using Eigen quaternion and CERES: [explanation](http://www.lloydhughes.co.za/index.php/using-eigen-quaternions-and-ceres-solver/) & [GitHub CERES extension](https://github.com/system123/ceres_extensions)
-
-Useful tools
-------------
-
-#### Profiling with Valgrind and Kcachegrind
-
-Kcachegrind is a graphical frontend for profiling your program and optimizing your code.
-
-- Ubuntu:
-
-Get the programs with
-
- $ sudo apt-get install valgrind kcachegrind
-
-- Mac OSX
-
-In Mac, you can use qcachegrind instead. To get it through Homebrew, type
-
- $ brew install valgrind qcachegrind
-
-I don't know if these packages are available through MacPorts. Try
-
- $ ports search --name valgrind
- $ ports search --name qcachegrind
-
-If they are available, just do
-
- $ sudo port install valgrind qcachegrind
-
-#### Do the profiling and watch the reports
-
-_Remember:_ For a proper profiling, compile Wolf and related libraries
-in RELEASE mode. Profiling code compiled in DEBUG mode is not going to take you
-anywhere, and in the reports you will mostly see the overhead of the DEBUG mode.
-
-Type in your `wolf/bin/` directory:
-
- $ cd bin/
- $ valgrind --tool=callgrind ./my_program <my_prg_params>
-
-this produces a log report called `callgrind.out.XXXX`, where XXXX is a number. Then type
-
-- Ubuntu
-
- ```shell
- $ kcachegrind callgrind.out.XXXX
- ```
-
-- Mac OSX
-
- ```shell
- $ qcachegrind callgrind.out.XXXX
- ```
-
-and enjoy.
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
\ No newline at end of file
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
index 90e622c1b964cfb78b1664b1ed9794f8004bff47..74ede89df7d20ffdfe00d7cd669bce317fc501a9 100644
--- a/cmake_modules/wolfConfig.cmake
+++ b/cmake_modules/wolfConfig.cmake
@@ -84,9 +84,16 @@ FIND_PACKAGE(YamlCpp REQUIRED)
list(APPEND wolf_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
list(APPEND wolf_LIBRARIES ${YAMLCPP_LIBRARY})
-if(NOT Eigen_FOUND)
+#Eigen
+# FIND_PACKAGE(Eigen3 REQUIRED)
+# list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+
+if(NOT Eigen3_FOUND)
FIND_PACKAGE(Eigen3 REQUIRED)
- list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
endif()
+if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
+ message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
+endif()
+list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
\ No newline at end of file
+SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 6130543ed715cf48dae1e13d0913adb92d073b35..b11a6b13aea90a067fec638a610ceca192c2f84e 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -12,18 +12,19 @@
* \author: jsola
*/
-#include "core/common/wolf.h"
+// wolf core includes
+#include "core/common/wolf.h"
#include "core/sensor/sensor_odom_2D.h"
#include "core/processor/processor_odom_2D.h"
+#include "core/capture/capture_odom_2D.h"
+#include "core/ceres_wrapper/ceres_manager.h"
+
+// hello wolf local includes
#include "sensor_range_bearing.h"
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
-#include "feature_range_bearing.h"
-#include "factor_range_bearing.h"
-#include "landmark_point_2D.h"
-#include "core/ceres_wrapper/ceres_manager.h"
int main()
{
@@ -123,7 +124,6 @@ int main()
params_odo->cov_det = 999;
params_odo->unmeasured_perturbation_std = 0.001;
ProcessorBasePtr processor = problem->installProcessor("ODOM 2D", "processor odo", sensor_odo, params_odo);
- ProcessorOdom2DPtr processor_odo = std::static_pointer_cast<ProcessorOdom2D>(processor);
// sensor Range and Bearing
IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
@@ -137,11 +137,17 @@ int main()
params_rb->time_tolerance = 0.01;
ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb);
+ // initialize
+ TimeStamp t(0.0); // t : 0.0
+ Vector3s x(0,0,0);
+ Matrix3s P = Matrix3s::Identity() * 0.1;
+ problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
+
// SELF CALIBRATION ===================================================
// SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
- // sensor_rb->getO()->unfix();
+ sensor_rb->getO()->unfix();
// SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
@@ -159,18 +165,12 @@ int main()
// SET OF EVENTS =======================================================
std::cout << std::endl;
- WOLF_TRACE("======== BUILD PROBLEM =======")
+ WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
// We'll do 3 steps of motion and landmark observations.
// STEP 1 --------------------------------------------------------------
- // initialize
- TimeStamp t(0.0); // t : 0.0
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
-
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
@@ -220,12 +220,13 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto sen : problem->getHardware()->getSensorList())
- for (auto sb : sen->getStateBlockVec())
+ for (auto& sen : problem->getHardware()->getSensorList())
+ for (auto& sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
+//<<<<<<< HEAD
for (auto& pair_key_sb : kf->getStateBlockMap())
if (!pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
@@ -233,6 +234,15 @@ int main()
for (auto& pair_key_sb : lmk->getStateBlockMap())
if (!pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+//=======
+// for (auto& sb : kf->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+// for (auto& lmk : problem->getMap()->getLandmarkList())
+// for (auto& sb : lmk->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+//>>>>>>> devel
problem->print(1,0,1,0);
// SOLVE again
@@ -244,19 +254,19 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
{
Eigen::MatrixXs cov;
kf->getCovariance(cov);
WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
}
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- Eigen::MatrixXs cov;
- lmk->getCovariance(cov);
- WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
- }
+ for (auto& lmk : problem->getMap()->getLandmarkList())
+ {
+ Eigen::MatrixXs cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index 9d52baaa2e57fa01c369373dc90051a4e0098029..3979a4b45a0023e70f298fcba0c0f470a11a8999 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -6,22 +6,16 @@
*/
-// hello wolf local includes
-#include "sensor_range_bearing.h"
-#include "processor_range_bearing.h"
-#include "capture_range_bearing.h"
-#include "feature_range_bearing.h"
-#include "factor_range_bearing.h"
-#include "landmark_point_2D.h"
-
// wolf core includes
#include "core/common/wolf.h"
-#include "core/internal/config.h"
+#include "core/capture/capture_odom_2D.h"
#include "core/yaml/parser_yaml.hpp"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
#include "core/ceres_wrapper/ceres_manager.h"
+// hello wolf local includes
+#include "capture_range_bearing.h"
+
+
int main()
{
/*
@@ -108,25 +102,24 @@ int main()
std::string wolf_path = std::string(_WOLF_ROOT_DIR);
// parse file into params server: each param will be retrievable from this params server:
- ParserYAML parser = ParserYAML(config_file, wolf_path);
- parser.parse();
- ParamsServer server = ParamsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
-
+ ParserYAML parser = ParserYAML(config_file, wolf_path);
+ ParamsServer server = ParamsServer(parser.getParams());
// Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
- ProblemPtr problem = Problem::autoSetup(server);
+ ProblemPtr problem = Problem::autoSetup(server);
// Print problem to see its status before processing any sensor data
problem->print(4,0,1,0);
- // recover sensor pointers for later use (access by sensor name)
- SensorBasePtr sensor_odo = problem->getSensor("sen odom");
- SensorBasePtr sensor_rb = problem->getSensor("sen rb");
-
// Solver. Configure a Ceres solver
ceres::Solver::Options options;
options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
+ // recover sensor pointers and other stuff for later use (access by sensor name)
+ SensorBasePtr sensor_odo = problem->getSensor("sen odom");
+ SensorBasePtr sensor_rb = problem->getSensor("sen rb");
+ TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp();
+
// SELF CALIBRATION ===================================================
@@ -158,9 +151,6 @@ int main()
// STEP 1 --------------------------------------------------------------
- // initialize
- TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp();
-
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
@@ -213,12 +203,13 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto sen : problem->getHardware()->getSensorList())
- for (auto sb : sen->getStateBlockVec())
+ for (auto& sen : problem->getHardware()->getSensorList())
+ for (auto& sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
+//<<<<<<< HEAD
for (auto& pair_key_sb : kf->getStateBlockMap())
if (pair_key_sb.second && !pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
@@ -226,6 +217,15 @@ int main()
for (auto& pair_key_sb : lmk->getStateBlockMap())
if (!pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+//=======
+// for (auto& sb : kf->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+// for (auto& lmk : problem->getMap()->getLandmarkList())
+// for (auto& sb : lmk->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+//>>>>>>> devel
problem->print(1,0,1,0);
// SOLVE again
@@ -237,19 +237,19 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectory()->getFrameList())
+ for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
{
Eigen::MatrixXs cov;
kf->getCovariance(cov);
WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
}
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- Eigen::MatrixXs cov;
- lmk->getCovariance(cov);
- WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
- }
+ for (auto& lmk : problem->getMap()->getLandmarkList())
+ {
+ Eigen::MatrixXs cov;
+ lmk->getCovariance(cov);
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ }
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/hello_wolf/yaml/hello_wolf_config.yaml b/hello_wolf/yaml/hello_wolf_config.yaml
index 9ca64d2479d2e29501c27ea32bb2758e3000eea2..5ac02b2f2bd369b3ecce0533c50c7f4efa520cb5 100644
--- a/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/hello_wolf/yaml/hello_wolf_config.yaml
@@ -13,12 +13,14 @@ config:
sensors:
- type: "ODOM 2D"
+ plugin: "core"
name: "sen odom"
extrinsic:
pose: [0,0, 0]
follow: "hello_wolf/yaml/sensor_odom_2D.yaml" # config parameters in this file
- type: "RANGE BEARING"
+ plugin: "core"
name: "sen rb"
extrinsic:
pose: [1,1, 0]
@@ -28,11 +30,13 @@ config:
processors:
- type: "ODOM 2D"
+ plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: hello_wolf/yaml/processor_odom_2D.yaml # config parameters in this file
- type: "RANGE BEARING"
+ plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
diff --git a/hello_wolf/yaml/processor_range_bearing.yaml b/hello_wolf/yaml/processor_range_bearing.yaml
index fea427973758203fce60dcac490747866ef29305..774795878adaa5a3ee5a3b729d51b27e30f863c6 100644
--- a/hello_wolf/yaml/processor_range_bearing.yaml
+++ b/hello_wolf/yaml/processor_range_bearing.yaml
@@ -4,4 +4,4 @@ time_tolerance: 0.1
keyframe_vote:
voting_active: true
- voting_aux_active: false
\ No newline at end of file
+ voting_aux_active: false
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 59b5123c4971be28214e50021fe1fff0dc21d537..3902dd0f05827eb30ac8b06e909a9b5272933ff0 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -13,7 +13,7 @@ namespace wolf {
* - A unique ID. The class implements the ID factory.
*
* - A unique category name, strictly within this range of possibilities:
- * - "BASE" -- should not be used
+ * - "NodeBase" -- should not be used
* - "PROBLEM"
* - "HARDWARE"
* - "SENSOR"
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 274c226ed779ced623f5ae8c0eb76c0e5bc4d291..687a24728a081025a0f80f9780cb0df925109dd6 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -53,7 +53,7 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
const FactorStatus _status = FAC_ACTIVE) :
- Base("DIFF DRIVE",
+ Base("FactorDiffDrive",
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
nullptr,
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index e5cfd57a69329ca9c82c9019f29d4ee591b27f77..ff322cf51790789313a289a3e49f5379fac6add0 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -19,7 +19,7 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
+ FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("FactorOdom2D",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index 587472541124f2d0d158b3d999c9b65bb1bc5dcb..25d751ca849a6b1ec7650bd01646846570a04680 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -75,7 +75,7 @@ inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorOdom3D, 6, 3, 4, 3, 4>("ODOM 3D", // type
+ FactorAutodiff<FactorOdom3D, 6, 3, 4, 3, 4>("FactorOdom3D", // type
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index e4caa3b2dbd2ed26ef8b23a83443b0662b1ec823..520e771471fcc2c7e8f309e8f539530ad6c30d77 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -18,7 +18,7 @@ class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
{
public:
FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ FactorAutodiff<FactorPose2D, 3, 2, 1>("FactorPose2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
// std::cout << "created FactorPose2D " << std::endl;
}
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index d36bcdb32a832695b29bbbe5a0610ded37746218..9f28b946136f75875a630388bb4674ebbf485651 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -17,7 +17,7 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
public:
FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ FactorAutodiff<FactorPose3D,6,3,4>("FactorPose3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 1bb4fe4862e774ca624338c3710ca0d87257d924..d93f0345e671248f0754dcf7552687eeb7fdb29b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -340,6 +340,10 @@ class Problem : public std::enable_shared_from_this<Problem>
bool constr_by = false, //
bool metric = true, //
bool state_blocks = false) const;
+ std::string printToString(int depth = 4, //
+ bool constr_by = false, //
+ bool metric = true, //
+ bool state_blocks = false) const;
bool check(int verbose_level = 0) const;
};
diff --git a/include/core/processor/processor_factory.h b/include/core/processor/processor_factory.h
index 1f1d8606c449f9b57625bac9c12f8f9207322d0b..24f074902f56999949d83e9b7b8a412c14e3aa41 100644
--- a/include/core/processor/processor_factory.h
+++ b/include/core/processor/processor_factory.h
@@ -69,7 +69,7 @@ namespace wolf
* that knows how to create your specific processor, e.g.:
*
* \code
- * ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
+ * ProcessorFactory::get().registerCreator("ProcessorOdom2D", ProcessorOdom2D::create);
* \endcode
*
* The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method.
@@ -96,7 +96,7 @@ namespace wolf
* For example, in processor_odom_2D.cpp we find the line:
*
* \code
- * const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
+ * const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ProcessorOdom2D", ProcessorOdom2D::create);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2D class).
@@ -107,7 +107,7 @@ namespace wolf
* It only needs to be passed the string of the processor type.
*
* \code
- * ProcessorFactory::get().unregisterCreator("ODOM 2D");
+ * ProcessorFactory::get().unregisterCreator("ProcessorOdom2D");
* \endcode
*
* #### Creating processors
@@ -117,7 +117,7 @@ namespace wolf
* To create a ProcessorOdom2D, you type:
*
* \code
- * ProcessorFactory::get().create("ODOM 2D", "main odometry", params_ptr);
+ * ProcessorFactory::get().create("ProcessorOdom2D", "main odometry", params_ptr);
* \endcode
*
* #### Example 1 : using the Factories alone
@@ -132,14 +132,14 @@ namespace wolf
* // Note: ProcessorOdom2D::create() is already registered, automatically.
*
* // First create the sensor (See SensorFactory for details)
- * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
+ * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "FactorOdom2D" , "Main odometer" , extrinsics , &intrinsics );
*
* // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct:
*
* ProcessorParamsOdom2D params({...}); // fill in the derived struct (note: ProcessorOdom2D actually has no input params)
*
* ProcessorBasePtr processor_ptr =
- * ProcessorFactory::get().create ( "ODOM 2D" , "main odometry" , ¶ms );
+ * ProcessorFactory::get().create ( "ProcessorOdom2D" , "main odometry" , ¶ms );
*
* // Bind processor to sensor
* sensor_ptr->addProcessor(processor_ptr);
@@ -158,8 +158,8 @@ namespace wolf
* #include "core/problem/problem.h"
*
* Problem problem(FRM_PO_2D);
- * problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
- * problem.installProcessor ( "ODOM 2D" , "Odometry" , "Main odometer" , ¶ms );
+ * problem.installSensor ( "SensorOdom2D" , "Main odometer" , extrinsics , &intrinsics );
+ * problem.installProcessor ( "ProcessorOdom2D" , "Odometry" , "Main odometer" , ¶ms );
* \endcode
*
* You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index dceaf3e01bce136b7841c5b9bd015203de200386..a1b4a5a86ee2082b6f44bd3eb8d37e0c3a5c88d3 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -3,6 +3,7 @@
// wolf
#include "core/common/wolf.h"
+#include "core/utils/converter_utils.h"
// Eigen
#include <eigen3/Eigen/Dense>
@@ -14,140 +15,11 @@
#include <array>
#include <vector>
#include <stack>
+#include <list>
/**
@file
*/
-namespace utils{
- //Typically we want to convert from/to list-type structures. In order to be general
- //we define a list type which is used throughout the converter. In this case this type
- //is implemented with std::vector
- template <typename A>
- using list = std::vector<A>;
- // template <typename A>
- // class toString<A>{};
- /** @Brief Splits a comma-separated string into its pieces
- * @param val is just the string of comma separated values
- * @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value
- */
- static inline std::vector<std::string> splitter(std::string val){
- std::vector<std::string> cont = std::vector<std::string>();
- std::stringstream ss(val);
- std::string token;
- while (std::getline(ss, token, ',')) {
- cont.push_back(token);
- }
- return cont;
- }
- /** @Brief Returns all the substrings of @val that match @exp
- * @param val String to be matched
- * @param exp Regular expression
- * @return <b>{std::vector<std::string>}</b> Collection of matching substrings
- */
- static inline std::vector<std::string> getMatches(std::string val, std::regex exp){
- std::smatch res;
- auto v = std::vector<std::string>();
- std::string::const_iterator searchStart( val.cbegin() );
- while ( std::regex_search( searchStart, val.cend(), res, exp ) ) {
- v.push_back(res[0]);
- searchStart = res.suffix().first;
- }
- return v;
- }
- /** @Brief Given a string representation of a matrix extracts the dimensions and the values
- * @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]"
- * @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
- */
- static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){
- std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]*\\.)?[0-9]+,?)+)\\]");
- std::regex rgxStatic("\\[((?:(?:[0-9]*\\.)?[0-9]+,?)+)\\]");
- std::smatch matches;
- std::smatch matchesStatic;
- std::array<std::string,2> values = {{"[]","[]"}};
- if(std::regex_match(matrix, matches, rgx)) {
- values[0] = "[" + matches[1].str() + "]";
- values[1] = "[" + matches[2].str() + "]";
- }else if(std::regex_match(matrix, matchesStatic, rgxStatic)){
- values[1] = "[" + matchesStatic[1].str() + "]";
- }else{
- throw std::runtime_error("Invalid string representation of a Matrix. Correct format is [([num,num],)?(num(,num)*)?]. String provided: " + matrix);
- }
- return values;
- }
- /** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation
- * @param val is just a dictionary-like string
- * @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i}
- */
- static inline std::vector<std::string> pairSplitter(std::string val){
- std::regex exp("\\{[^\\{:]:.*?\\}");
- return getMatches(val, exp);
- }
- /** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...]
- * @param str_map just a dictionary-like string
- * @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets)
- */
- static inline std::string splitMapStringRepresentation(std::string str_map){
- std::smatch mmatches;
- std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
- std::string result = "";
- if(std::regex_match(str_map, mmatches, rgxM)) {
- // v = splitter(mmatches[1].str());
- result = mmatches[1].str();
- } else {
- throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + str_map);
- }
- return result;
- }
- static inline std::vector<std::string> parseList(std::string val){
- std::stack<char> limiters;
- std::stack<std::string> word_stack;
- std::string current_word;
- std::vector<std::string> words;
- std::vector<char> chars(val.begin(), val.end());
- for(const char ¤t : chars){
- if(current == '['){
- limiters.push(current);
- word_stack.push(current_word);
- current_word = "";
- }else if(current == ']'){
- if(limiters.empty()) throw std::runtime_error("Unmatched delimiter");
- if(limiters.top() == '[') {
- if(limiters.size() > 1) {
- if(word_stack.empty()) word_stack.push("");
- current_word = word_stack.top() + "[" + current_word + "]";
- word_stack.pop();
- }else if(limiters.size() == 1 and current_word != "") words.push_back(current_word);
- else current_word += current;
- limiters.pop();
- }else throw std::runtime_error("Unmatched delimiter");
- }else if(current == '{'){
- limiters.push(current);
- word_stack.push(current_word);
- current_word = "";
- }else if(current == '}'){
- if(limiters.top() == '{') {
- if(limiters.size() > 1) {
- if(word_stack.empty()) word_stack.push("");
- current_word = word_stack.top() + "{" + current_word + "}";
- word_stack.pop();
- }else if(limiters.size() == 1) words.push_back(current_word);
- else current_word += current;
- limiters.pop();
- }else throw std::runtime_error("Unmatched delimiter");
- }else if(current == ',') {
- if(limiters.size() == 1 and current_word != "") {
- words.push_back(current_word);
- current_word = "";
- }else if(limiters.size() > 1) current_word += current;
- }else {
- if(limiters.empty()) throw std::runtime_error("Found non-delimited text");
- current_word += current;
- }
- }
- if(not limiters.empty()) throw std::runtime_error("Unclosed delimiter [] or {}");
- return words;
- }
-}
namespace wolf{
template<typename T>
@@ -218,7 +90,8 @@ struct converter<std::string>{
}
template<typename T>
static std::string convert(T val){
- throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val);
+ // throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val);
+ throw std::runtime_error("There is no general convert to string for arbitrary T !!!");
}
static std::string convert(int val){
return std::to_string(val);
@@ -235,6 +108,14 @@ struct converter<std::string>{
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
+ template <typename A> static std::string convert(std::list<A> val) {
+ std::string result = "";
+ for (auto it : val) {
+ result += "," + converter<std::string>::convert(it);
+ }
+ if (result.size() > 0) result = result.substr(1, result.size());
+ return "[" + result + "]";
+ }
template<typename A, typename B>
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
@@ -308,7 +189,7 @@ struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCo
int ncols = (int) values.size() / _Rows;
m.resize(_Rows, ncols);
}
- if(m.rows()*m.cols() != values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
+ if(m.rows()*m.cols() != (int) values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
+ "the Eigen matrix is of dimensions "
+ std::to_string(m.rows()) + "x" + std::to_string(m.cols()));
else{
diff --git a/include/core/utils/converter_utils.h b/include/core/utils/converter_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..1765c3f9091edebb4db92e66790d333e0116548a
--- /dev/null
+++ b/include/core/utils/converter_utils.h
@@ -0,0 +1,46 @@
+#ifndef CONVERTER_UTILS_H
+#define CONVERTER_UTILS_H
+
+// wolf
+#include "core/common/wolf.h"
+
+
+// std
+#include <regex>
+namespace utils{
+ //Typically we want to convert from/to list-type structures. In order to be general
+ //we define a list type which is used throughout the converter. In this case this type
+ //is implemented with std::vector
+ template <typename A>
+ using list = std::vector<A>;
+ // template <typename A>
+ // class toString<A>{};
+ /** @Brief Splits a comma-separated string into its pieces
+ * @param val is just the string of comma separated values
+ * @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value
+ */
+ std::vector<std::string> splitter(std::string val);
+ /** @Brief Returns all the substrings of @val that match @exp
+ * @param val String to be matched
+ * @param exp Regular expression
+ * @return <b>{std::vector<std::string>}</b> Collection of matching substrings
+ */
+ std::vector<std::string> getMatches(std::string val, std::regex exp);
+ /** @Brief Given a string representation of a matrix extracts the dimensions and the values
+ * @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]"
+ * @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
+ */
+ std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix);
+ /** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation
+ * @param val is just a dictionary-like string
+ * @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i}
+ */
+ std::vector<std::string> pairSplitter(std::string val);
+ /** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...]
+ * @param str_map just a dictionary-like string
+ * @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets)
+ */
+ std::string splitMapStringRepresentation(std::string str_map);
+ std::vector<std::string> parseList(std::string val);
+}
+#endif
\ No newline at end of file
diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp
index 4f36eeecb9cda277e16bb495349ea047b546b6ce..3d867d799b36bfd53fb42e93577554caf1b050c1 100644
--- a/include/core/utils/loader.hpp
+++ b/include/core/utils/loader.hpp
@@ -1,6 +1,8 @@
#ifndef LOADER_HPP
#define LOADER_HPP
#include <dlfcn.h>
+#include <string>
+#include <stdexcept>
class Loader{
protected:
std::string path_;
@@ -25,7 +27,7 @@ public:
void load(){
this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY);
if(not this->resource_)
- throw std::runtime_error("Couldn't load resource with path " + this->path_);
+ throw std::runtime_error("Couldn't load resource with path " + this->path_ + "\n" + "Error info: " + dlerror());
}
void close(){
if(this->resource_) dlclose(this->resource_);
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
index 20ade7d5d768078531954e93837b48c9a132ce2f..5dad0e8262b603c3de5eaeacecf8726f536e8eb0 100644
--- a/include/core/utils/params_server.hpp
+++ b/include/core/utils/params_server.hpp
@@ -7,43 +7,24 @@
#include <vector>
#include <regex>
#include <map>
+#include <exception>
namespace wolf{
-class ParamsServer{
- struct ParamsInitSensor{
- std::string _type;
- std::string _name;
- };
- struct ParamsInitProcessor{
- std::string _type;
- std::string _name;
- std::string _name_assoc_sensor;
- };
+class MissingValueException : public std::runtime_error
+{
+public:
+ MissingValueException(std::string msg) : std::runtime_error(msg) {}
+};
+class ParamsServer{
std::map<std::string, std::string> _params;
- std::map<std::string,ParamsInitSensor> _paramsSens;
- std::map<std::string,ParamsInitProcessor> _paramsProc;
public:
ParamsServer(){
_params = std::map<std::string, std::string>();
- _paramsSens = std::map<std::string,ParamsInitSensor>();
- _paramsProc = std::map<std::string,ParamsInitProcessor>();
}
- ParamsServer(std::map<std::string, std::string> params,
- std::vector<std::array<std::string,2>> sensors,
- std::vector<std::array<std::string,3>> procs){
+ ParamsServer(std::map<std::string, std::string> params){
_params = params;
- _paramsSens = std::map<std::string,ParamsInitSensor>();
- _paramsProc = std::map<std::string,ParamsInitProcessor>();
- for(auto it : sensors) {
- ParamsInitSensor pSensor = {it.at(0), it.at(1)};
- _paramsSens.insert(std::pair<std::string,ParamsInitSensor>(it.at(1), pSensor));
- }
- for(auto it : procs) {
- ParamsInitProcessor pProcs = {it.at(0), it.at(1), it.at(2)};
- _paramsProc.insert(std::pair<std::string,ParamsInitProcessor>(it.at(1), pProcs));
- }
}
~ParamsServer(){
//
@@ -54,15 +35,6 @@ public:
std::cout << it.first << "~~" << it.second << std::endl;
}
- void addInitParamsSensor(std::string type, std::string name){
- ParamsInitSensor params = {type, name};
- _paramsSens.insert(std::pair<std::string, ParamsInitSensor>(type + "/" + name + "/", params));
- }
-
- void addInitParamsProcessor(std::string type, std::string name, std::string name_assoc_sensor){
- ParamsInitProcessor params = {type, name, name_assoc_sensor};
- _paramsProc.insert(std::pair<std::string, ParamsInitProcessor>(type + "/" + name + "/", params));
- }
void addParam(std::string key, std::string value){
_params.insert(std::pair<std::string, std::string>(key, value));
@@ -82,21 +54,10 @@ public:
if(_params.find(key) != _params.end()){
return converter<T>::convert(_params.find(key)->second);
}else{
- throw std::runtime_error("The following key: '" + key + "' has not been found in the parameters server.");
+ throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server.");
}
}
- std::vector<ParamsInitSensor> getSensors(){
- std::vector<ParamsInitSensor> rtn = std::vector<ParamsInitSensor>();
- std::transform(this->_paramsSens.begin(), this->_paramsSens.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitSensor> v){return v.second;});
- return rtn;
- }
-
- std::vector<ParamsInitProcessor> getProcessors(){
- std::vector<ParamsInitProcessor> rtn = std::vector<ParamsInitProcessor>();
- std::transform(this->_paramsProc.begin(), this->_paramsProc.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitProcessor> v){return v.second;});
- return rtn;
- }
};
}
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index c661e078a8306acf63b98425f642460ec522dc77..99c1c8c10180318e3a84f0e307ef80be97d85c76 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -6,7 +6,9 @@
#include "yaml-cpp/yaml.h"
+#include <string>
#include <vector>
+#include <list>
#include <stack>
#include <regex>
#include <map>
@@ -168,74 +170,63 @@ class ParserYAML {
struct ParamsInitSensor{
std::string _type;
std::string _name;
+ std::string _plugin;
YAML::Node n;
};
struct ParamsInitProcessor{
std::string _type;
std::string _name;
std::string _name_assoc_sensor;
+ std::string _plugin;
+ YAML::Node n;
+ };
+ struct SubscriberManager{
+ std::string _package;
+ std::string _subscriber;
+ std::string _topic;
+ std::string _sensor_name;
YAML::Node n;
};
std::map<std::string, std::string> _params;
std::string _active_name;
std::vector<ParamsInitSensor> _paramsSens;
std::vector<ParamsInitProcessor> _paramsProc;
- std::vector<std::string> _files;
std::stack<std::string> _parsing_file;
std::string _file;
- bool _relative_path;
std::string _path_root;
- std::vector<std::array<std::string, 3>> _callbacks;
+ std::vector<SubscriberManager> _subscriber_managers;
YAML::Node problem;
std::string generatePath(std::string);
YAML::Node loadYAML(std::string);
void insert_register(std::string, std::string);
public:
- ParserYAML(){
- _params = std::map<std::string, std::string>();
- _active_name = "";
- _paramsSens = std::vector<ParamsInitSensor>();
- _paramsProc = std::vector<ParamsInitProcessor>();
- _file = "";
- _files = std::vector<std::string>();
- _parsing_file = std::stack<std::string>();
- _path_root = "";
- _relative_path = false;
- _callbacks = std::vector<std::array<std::string, 3>>();
- }
- ParserYAML(std::string file){
- _params = std::map<std::string, std::string>();
- _active_name = "";
- _paramsSens = std::vector<ParamsInitSensor>();
- _paramsProc = std::vector<ParamsInitProcessor>();
- _files = std::vector<std::string>();
- _parsing_file = std::stack<std::string>();
- _file = file;
- _path_root = "";
- _relative_path = false;
- _callbacks = std::vector<std::array<std::string, 3>>();
- }
- ParserYAML(std::string file, std::string path_root){
- _params = std::map<std::string, std::string>();
- _active_name = "";
- _paramsSens = std::vector<ParamsInitSensor>();
- _paramsProc = std::vector<ParamsInitProcessor>();
- _files = std::vector<std::string>();
- _parsing_file = std::stack<std::string>();
- _file = file;
- if(path_root != ""){
- std::regex r("/$");
- if(not std::regex_match(path_root, r)) _path_root = path_root + "/";
- else _path_root = path_root;
- _relative_path = true;
- }else{
- _relative_path = false;
- }
- _callbacks = std::vector<std::array<std::string, 3>>();
+ ParserYAML(std::string file, std::string path_root = "",
+ bool freely_parse = false) {
+ _params = std::map<std::string, std::string>();
+ _active_name = "";
+ _paramsSens = std::vector<ParamsInitSensor>();
+ _paramsProc = std::vector<ParamsInitProcessor>();
+ _subscriber_managers = std::vector<SubscriberManager>();
+ _parsing_file = std::stack<std::string>();
+ _file = file;
+ if (path_root != "") {
+ std::regex r(".*/ *$");
+ if (not std::regex_match(path_root, r))
+ _path_root = path_root + "/";
+ else
+ _path_root = path_root;
+ }
+ if(not freely_parse) this->parse();
+ else this->parse_freely();
}
- ~ParserYAML(){
+ ~ParserYAML()
+ {
//
}
+ void parse_freely();
+ std::map<std::string, std::string> getParams();
+
+ private:
void walkTree(std::string file);
void walkTree(std::string file, std::vector<std::string>& tags);
void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr);
@@ -243,21 +234,14 @@ public:
void updateActiveName(std::string tag);
void parseFirstLevel(std::string file);
std::string tagsToString(std::vector<std::string>& tags);
- std::vector<std::array<std::string, 2>> sensorsSerialization();
- std::vector<std::array<std::string, 3>> processorsSerialization();
- std::vector<std::string> getFiles();
- std::vector<std::array<std::string, 3>> getCallbacks();
- std::vector<std::array<std::string, 2>> getProblem();
- std::map<std::string,std::string> getParams();
void parse();
- void parse_freely();
};
-std::string ParserYAML::generatePath(std::string path){
+std::string ParserYAML::generatePath(std::string file){
std::regex r("^/.*");
- if(std::regex_match(path, r)){
- return path;
+ if(std::regex_match(file, r)){
+ return file;
}else{
- return _path_root + path;
+ return _path_root + file;
}
}
YAML::Node ParserYAML::loadYAML(std::string file){
@@ -312,14 +296,12 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
walkTree(str.substr(1,str.size() - 1), tags, hdr);
}else{
insert_register(hdr, n.Scalar());
- // _params.insert(std::pair<std::string,std::string>);
}
break;
}
case YAML::NodeType::Sequence : {
if(isAtomic("", n)){
insert_register(hdr, parseAtomicNode(n));
- // _params.insert(std::pair<std::string,std::string>);
}else{
for(const auto& kv : n){
walkTreeR(kv, tags, hdr);
@@ -334,7 +316,6 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
//WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
key = key.substr(1,key.size() - 1);
insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
- // _params.insert(std::pair<std::string,std::string>);
}else{
/*
If key=="follow" then the parser will assume that the value is a path and will parse
@@ -394,52 +375,54 @@ void ParserYAML::parseFirstLevel(std::string file){
if(n_config.Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + generatePath(file) + " starts with 'config:'");
if(n_config["problem"].Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + generatePath(file) + " has a 'problem' entry");
this->problem = n_config["problem"];
+ std::vector<std::map<std::string, std::string>> map_container;
try{
for(const auto& kv : n_config["sensors"]){
- ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv};
- _paramsSens.push_back(pSensor);
+ ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv};
+ _paramsSens.push_back(pSensor);
+ map_container.push_back(std::map<std::string, std::string>({
+ {"type", kv["type"].Scalar()},
+ {"name", kv["name"].Scalar()},
+ {"plugin", kv["plugin"].Scalar()}
+ }));
}
+ insert_register("sensors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
} catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type' and 'name' entries.");
+ throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
}
+
try{
for(const auto& kv : n_config["processors"]){
- ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv};
- _paramsProc.push_back(pProc);
+ ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv};
+ _paramsProc.push_back(pProc);
+ map_container.push_back(std::map<std::string, std::string>({
+ {"type", kv["type"].Scalar()},
+ {"name", kv["name"].Scalar()},
+ {"plugin", kv["plugin"].Scalar()},
+ {"sensor_name", kv["sensor_name"].Scalar()}}));
}
+ insert_register("processors",
+ wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
} catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name' and 'sensor_name' entries.");
+ throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name', 'plugin' and 'sensor_name' entries.");
}
- for(const auto& kv : n_config["callbacks"]){
- _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>(), kv[2].as<std::string>()}});
- }
- YAML::Node n_files = n["files"];
- for(const auto& kv : n_files){
- _files.push_back(kv.Scalar());
+ try {
+ for (const auto &kv : n_config["ROS subscriber managers"]) {
+ SubscriberManager pSubscriberManager = {kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv};
+ _subscriber_managers.push_back(pSubscriberManager);
+ map_container.push_back(std::map<std::string, std::string>({
+ {"package", kv["package"].Scalar()},
+ {"type", kv["type"].Scalar()},
+ {"topic", kv["topic"].Scalar()},
+ {"sensor_name", kv["sensor_name"].Scalar()}}));
+ }
+ insert_register("ROS subscriber managers", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ } catch (YAML::InvalidNode &e) {
+ throw std::runtime_error("Error parsing subscriber managers @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries.");
}
- insert_register("files", wolf::converter<std::string>::convert(_files));
- // _params.insert(std::pair<std::string,std::string>);
-}
-std::vector<std::array<std::string, 2>> ParserYAML::sensorsSerialization(){
- std::vector<std::array<std::string, 2>> aux = std::vector<std::array<std::string, 2>>();
- for(auto it : this->_paramsSens)
- aux.push_back({{it._type,it._name}});
- return aux;
-}
-std::vector<std::array<std::string, 3>> ParserYAML::processorsSerialization(){
- std::vector<std::array<std::string, 3>> aux = std::vector<std::array<std::string, 3>>();
- for(auto it : this->_paramsProc)
- aux.push_back({{it._type,it._name,it._name_assoc_sensor}});
- return aux;
-}
-std::vector<std::string> ParserYAML::getFiles(){
- return this->_files;
-}
-std::vector<std::array<std::string, 3>> ParserYAML::getCallbacks(){
- return this->_callbacks;
-}
-std::vector<std::array<std::string, 2>> ParserYAML::getProblem(){
- return std::vector<std::array<std::string, 2>>();
}
std::map<std::string,std::string> ParserYAML::getParams(){
std::map<std::string,std::string> rtn = _params;
@@ -463,8 +446,53 @@ void ParserYAML::parse(){
tags.push_back("processor");
this->walkTreeR(it.n , tags , "processor/" + it._name);
}
+ std::list<std::string> plugins, packages;
+ for (const auto& it : this->_paramsSens)
+ plugins.push_back(it._plugin);
+ for (const auto& it : this->_paramsProc)
+ plugins.push_back(it._plugin);
+ for (const auto& it : this->_subscriber_managers)
+ packages.push_back(it._package);
+ plugins.sort();
+ plugins.unique();
+ packages.sort();
+ packages.unique();
+ insert_register("plugins", wolf::converter<std::string>::convert(plugins));
+ insert_register("packages", wolf::converter<std::string>::convert(packages));
+
+ YAML::Node n;
+ n = loadYAML(generatePath(this->_file));
+
+ if (n.Type() == YAML::NodeType::Map)
+ {
+ for (auto it : n)
+ {
+ auto node = it.second;
+ // WOLF_INFO("WUT ", it.first);
+ std::vector<std::string> tags = std::vector<std::string>();
+ if(it.first.as<std::string>() != "config")
+ this->walkTreeR(node, tags, it.first.as<std::string>());
+ else
+ {
+ for (auto itt : node)
+ {
+ std::string node_key = itt.first.as<std::string>();
+ // WOLF_INFO("node_key ", node_key);
+ if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+ node_key != "ROS subscriber managers")
+ {
+ this->walkTreeR(itt.second, tags, node_key);
+ }
+ }
+ }
+ }
+ }else
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ this->walkTreeR(n, tags, "");
+ }
this->_parsing_file.pop();
-}
+ }
void ParserYAML::parse_freely(){
this->_parsing_file.push(generatePath(this->_file));
std::vector<std::string> tags = std::vector<std::string>();
@@ -472,7 +500,7 @@ void ParserYAML::parse_freely(){
this->_parsing_file.pop();
}
void ParserYAML::insert_register(std::string key, std::string value){
- // _params.insert(std::pair<std::string,std::string>);
+ if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1);
auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value));
if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")");
}
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index cc81850c8447040d21f7e6cffa4f079c5884ca47..625007b36f72d69fa4f9a7535729dcc29fae9f4e 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -13,7 +13,7 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
- CaptureMotion("DIFF DRIVE",
+ CaptureMotion("CaptureDiffDrive",
_ts,
_sensor_ptr,
_data,
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
index 36826540a27804e14c61102260bbec24a36df58f..ab9c39b639f55c6b98cc4fecd175fc02169223bb 100644
--- a/src/capture/capture_odom_2D.cpp
+++ b/src/capture/capture_odom_2D.cpp
@@ -14,7 +14,7 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2D", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
{
//
}
@@ -24,7 +24,7 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
{
//
}
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
index 84f263d2cfbd6c97e647b8504322352696d4332d..bbb529ee925c68355717afc4462a0aae8444533a 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -14,7 +14,7 @@ CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXs& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
{
//
}
@@ -24,7 +24,7 @@ CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
const Eigen::VectorXs& _data,
const Eigen::MatrixXs& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
{
//
}
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 46177687a1cbaab1a507ba3ac4e091aec7d9b7fd..3771b44fbba116020c82456804960bff07f18808 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -3,7 +3,7 @@
namespace wolf{
CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase("POSE", _ts, _sensor_ptr),
+ CaptureBase("CapturePose", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
{
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
index 00a6b796bbba9135ee8fbba1150ff353c29b72ec..14cb2626fa074450dc484b6c90c02aff647f909d 100644
--- a/src/capture/capture_velocity.cpp
+++ b/src/capture/capture_velocity.cpp
@@ -7,7 +7,7 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
const Eigen::VectorXs& _velocity,
SizeEigen _delta_size, SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity,
+ CaptureMotion("CaptureVelocity", _ts, _sensor_ptr, _velocity,
_delta_size, _delta_cov_size, _capture_origin_ptr)
{
//
@@ -22,7 +22,7 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
- CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity, _velocity_cov,
+ CaptureMotion("CaptureVelocity", _ts, _sensor_ptr, _velocity, _velocity_cov,
_delta_size, _delta_cov_size, _capture_origin_ptr,
_p_ptr, _o_ptr, _intr_ptr)
{
diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp
index cdc8ce156ab6f7309a28cc3e5c8551dd7bea1d12..dcce03c9dc66e1dc38c62fa87dcdfdd4a25b528e 100644
--- a/src/capture/capture_void.cpp
+++ b/src/capture/capture_void.cpp
@@ -3,7 +3,7 @@
namespace wolf {
CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
- CaptureBase("VOID", _ts, _sensor_ptr)
+ CaptureBase("CaptureVoid", _ts, _sensor_ptr)
{
//
}
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index ecf92ab78148361f036d7df23cae237a998a98d4..452ade3585f54eaa436226838247ddbe067e69b7 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -7,7 +7,7 @@ FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
const Eigen::MatrixXs& _delta_preintegrated_covariance,
const Eigen::VectorXs& _diff_drive_params,
const Eigen::MatrixXs& _jacobian_diff_drive_params) :
- FeatureMotion("DIFF DRIVE",
+ FeatureMotion("FeatureDiffDrive",
_delta_preintegrated,
_delta_preintegrated_covariance,
_diff_drive_params,
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
index ee6cd60ba38a16a161925d79512eaf9fa1e6212d..30bdb519152a1be4f849218fb9a88cbfd489da18 100644
--- a/src/feature/feature_odom_2D.cpp
+++ b/src/feature/feature_odom_2D.cpp
@@ -3,7 +3,7 @@
namespace wolf {
FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("ODOM 2D", _measurement, _meas_covariance)
+ FeatureBase("FeatureOdom2D", _measurement, _meas_covariance)
{
//std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
}
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index 344d230a7164d0c222567ae9ea7e1000be23cb84..7aba0e1bc02efdcad0d6f0a5115b036ea397bc80 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.cpp
@@ -3,7 +3,7 @@
namespace wolf {
FeaturePose::FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("POSE", _measurement, _meas_covariance)
+ FeatureBase("FeaturePose", _measurement, _meas_covariance)
{
//
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 9e56aaf12257b42e480a0da90f70e7e38240e7c9..1fdb53746cb4486455d0354443c4a7f80189445f 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -186,7 +186,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
}
- LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("BASE", pos_sb, ori_sb);
+ LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("LandmarkBase", pos_sb, ori_sb);
lmk->setId(id);
return lmk;
@@ -195,7 +195,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_base = LandmarkFactory::get().registerCreator("BASE", LandmarkBase::create);
+const bool WOLF_UNUSED registered_lmk_base = LandmarkFactory::get().registerCreator("LandmarkBase", LandmarkBase::create);
}
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f516ffa092dd3377f83656d542c70a458c623c95..2a01ee3c23b740eec4935cc590a71849b5496c48 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -11,6 +11,7 @@
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
+#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
#include "core/utils/loader.hpp"
@@ -20,17 +21,15 @@
// C++ includes
#include <algorithm>
#include <map>
+#include <sstream>
+#include <stdexcept>
+#include <string>
#include <vector>
+#include <unordered_set>
namespace wolf
{
-// unnamed namespace used for helper functions local to this file.
-namespace
-{
-std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
-}
-
Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
@@ -76,20 +75,64 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
+#if __APPLE__
+ std::string lib_extension = ".dylib";
+#else
+ std::string lib_extension = ".so";
+#endif
// 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);
- // cout << "PRINTING SERVER MAP" << endl;
+ //
+ // cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
- auto loaders = std::vector<Loader*>();
- for(auto it : _server.getParam<std::vector<std::string>>("files")) {
- WOLF_TRACE("Loading file " + it)
- auto l = new LoaderRaw(it);
+ auto loaders = std::vector<Loader *>();
+ std::string plugins_path;
+ try{
+ plugins_path = _server.getParam<std::string>("plugins_path");
+ }
+ catch(MissingValueException& e){
+ WOLF_WARN(e.what());
+ WOLF_WARN("Setting '/usr/local/lib/iri-algorithms/' as plugins path...");
+ plugins_path="/usr/local/lib/iri-algorithms/";
+ }
+ for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins")) {
+ if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
+ std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin);
+ auto l = new LoaderRaw(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
+ std::string packages_path;
+ try {
+ packages_path = _server.getParam<std::string>("packages_path");
+ } catch (MissingValueException& e) {
+ WOLF_WARN(e.what());
+ WOLF_WARN("Support for subscribers disabled...");
+ }
+ for (auto it : _server.getParam<std::vector<std::string>>("packages")) {
+ std::string subscriber = packages_path + "/libwolf_subscriber_" + it + lib_extension;
+ WOLF_TRACE("Loading subscriber " + subscriber);
+ auto l = new LoaderRaw(subscriber);
+ l->load();
+ loaders.push_back(l);
+ }
+ std::vector<std::string> raw_libs;
+ try {
+ raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
+ } catch (MissingValueException& e) {
+ WOLF_TRACE("No raw libraries to load...");
+ raw_libs = std::vector<std::string>();
+ }
+ for (auto lib : raw_libs) {
+ WOLF_TRACE("Loading raw lib " + lib);
+ auto l = new LoaderRaw(lib);
l->load();
loaders.push_back(l);
}
@@ -97,20 +140,20 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Install sensors and processors
auto sensorMap = std::map<std::string, SensorBasePtr>();
auto procesorMap = std::map<std::string, ProcessorBasePtr>();
-
- for(auto sen : _server.getSensors()){
- sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen._name, problem->installSensor(sen._type, sen._name, _server)));
+ auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
+ for(auto sen : sensors){
+ sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen["name"], problem->installSensor(sen["type"], sen["name"], _server)));
}
-
- for(auto prc : _server.getProcessors()){
- procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc._name, problem->installProcessor(prc._type, prc._name, prc._name_assoc_sensor, _server)));
+ auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
+ for(auto prc : processors){
+ procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
}
// Prior
- Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior/state");
- Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior/cov");
- Scalar prior_time_tolerance = _server.getParam<Scalar>("problem/prior/time_tolerance");
- Scalar prior_ts = _server.getParam<Scalar>("problem/prior/timestamp");
+ Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs> ("problem/prior/state");
+ Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs> ("problem/prior/cov");
+ Scalar prior_time_tolerance = _server.getParam<Scalar> ("problem/prior/time_tolerance");
+ Scalar prior_ts = _server.getParam<Scalar> ("problem/prior/timestamp");
WOLF_TRACE("prior timestamp:\n" , prior_ts);
WOLF_TRACE("prior state:\n" , prior_state.transpose());
@@ -121,8 +164,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Done
return problem;
-}
-
+ }
Problem::~Problem()
{
@@ -134,7 +176,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const Eigen::VectorXs& _extrinsics, //
IntrinsicsBasePtr _intrinsics)
{
- SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
+ SensorBasePtr sen_ptr = SensorFactory::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -160,7 +202,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const ParamsServer& _server)
{
- SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _server);
+ SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(_sen_type, _unique_sensor_name, _server);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -177,7 +219,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return ProcessorBasePtr();
}
- ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params);
+ ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
@@ -219,7 +261,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
- ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server);
+ ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
@@ -1082,6 +1124,265 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
cout << endl;
}
+std::string Problem::printToString(int depth, bool constr_by, bool metric, bool state_blocks) const
+{
+ using std::cout;
+ using std::endl;
+ std::stringstream result;
+
+ result << endl;
+ result << "P: wolf tree status ---------------------" << endl;
+ result << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
+ if (depth >= 1)
+ {
+ // Sensors =======================================================================================
+ for (auto S : getHardware()->getSensorList())
+ {
+ result << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
+ if (!metric && !state_blocks) result << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
+ if (depth < 2)
+ result << " -- " << S->getProcessorList().size() << "p";
+ result << endl;
+ if (metric && state_blocks)
+ {
+ for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
+ {
+ if (i==0) result << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
+ if (i==2) result << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
+ auto sb = S->getStateBlock(i);
+ if (sb)
+ {
+ result << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
+ }
+ if (i==1) result << " ]" << endl;
+ }
+ if (S->getStateBlockVec().size() > 2) result << " ]" << endl;
+ }
+ else if (metric)
+ {
+ result << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
+ if (S->getP())
+ result << S->getP()->getState().transpose();
+ if (S->getO())
+ result << " " << S->getO()->getState().transpose();
+ result << " )";
+ if (S->getIntrinsic())
+ result << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
+ result << endl;
+ }
+ else if (state_blocks)
+ {
+ result << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
+ for (auto sb : S->getStateBlockVec())
+ if (sb != nullptr)
+ result << " " << (sb->isFixed() ? "Fix" : "Est");
+ result << endl;
+ }
+ if (depth >= 2)
+ {
+ // Processors =======================================================================================
+ for (auto p : S->getProcessorList())
+ {
+ if (p->isMotion())
+ {
+ result << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
+ if (pm->getOrigin())
+ result << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
+ << pm->getOrigin()->getFrame()->id() << endl;
+ if (pm->getLast())
+ result << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
+ << pm->getLast()->getFrame()->id() << endl;
+ if (pm->getIncoming())
+ result << " i: C" << pm->getIncoming()->id() << endl;
+ }
+ else
+ {
+ result << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
+ if (pt)
+ {
+ if (pt->getOrigin())
+ result << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
+ << pt->getOrigin()->getFrame()->id() << endl;
+ if (pt->getLast())
+ result << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
+ << pt->getLast()->getFrame()->id() << endl;
+ if (pt->getIncoming())
+ result << " i: C" << pt->getIncoming()->id() << endl;
+ }
+ }
+ } // for p
+ }
+ } // for S
+ }
+ result << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
+ if (depth >= 1)
+ {
+ // Frames =======================================================================================
+ for (auto F : getTrajectory()->getFrameList())
+ {
+ result << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ if (constr_by)
+ {
+ result << " <-- ";
+ for (auto cby : F->getConstrainedByList())
+ result << "c" << cby->id() << " \t";
+ }
+ result << endl;
+ if (metric)
+ {
+ result << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ << F->getTimeStamp().get();
+ result << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ result << endl;
+ }
+ if (state_blocks)
+ {
+ result << " sb:";
+ for (auto sb : F->getStateBlockVec())
+ if (sb != nullptr)
+ result << " " << (sb->isFixed() ? "Fix" : "Est");
+ result << endl;
+ }
+ if (depth >= 2)
+ {
+ // Captures =======================================================================================
+ for (auto C : F->getCaptureList())
+ {
+ result << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+
+ if(C->getSensor() != nullptr)
+ {
+ result << " -> S" << C->getSensor()->id();
+ result << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
+ result << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
+ }
+ else
+ result << " -> S-";
+ if (C->isMotion())
+ {
+ auto CM = std::static_pointer_cast<CaptureMotion>(C);
+ if (auto OC = CM->getOriginCapture())
+ {
+ result << " -> OC" << OC->id();
+ if (auto OF = OC->getFrame())
+ result << " ; OF" << OF->id();
+ }
+ }
+
+ result << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ if (constr_by)
+ {
+ result << " <-- ";
+ for (auto cby : C->getConstrainedByList())
+ result << "c" << cby->id() << " \t";
+ }
+ result << endl;
+
+ if (state_blocks)
+ for(auto sb : C->getStateBlockVec())
+ if(sb != nullptr)
+ {
+ result << " sb: ";
+ result << (sb->isFixed() ? "Fix" : "Est");
+ if (metric)
+ result << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ result << endl;
+ }
+
+ if (C->isMotion() )
+ {
+ CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
+ result << " buffer size : " << CM->getBuffer().get().size() << endl;
+ if ( metric && ! CM->getBuffer().get().empty())
+ {
+ result << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
+ if (CM->hasCalibration())
+ {
+ result << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
+ result << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
+ result << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
+ result << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
+ }
+ }
+ }
+
+ if (depth >= 3)
+ {
+ // Features =======================================================================================
+ for (auto f : C->getFeatureList())
+ {
+ result << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
+ if (constr_by)
+ {
+ result << " <--\t";
+ for (auto cby : f->getConstrainedByList())
+ result << "c" << cby->id() << " \t";
+ }
+ result << endl;
+ if (metric)
+ result << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
+ << " )" << endl;
+ if (depth >= 4)
+ {
+ // Factors =======================================================================================
+ for (auto c : f->getFactorList())
+ {
+ result << " c" << c->id() << " " << c->getType() << " -->";
+ if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
+ result << " A";
+ if (c->getFrameOther() != nullptr)
+ result << " F" << c->getFrameOther()->id();
+ if (c->getCaptureOther() != nullptr)
+ result << " C" << c->getCaptureOther()->id();
+ if (c->getFeatureOther() != nullptr)
+ result << " f" << c->getFeatureOther()->id();
+ if (c->getLandmarkOther() != nullptr)
+ result << " L" << c->getLandmarkOther()->id();
+ result << endl;
+ } // for c
+ }
+ } // for f
+ }
+ } // for C
+ }
+ } // for F
+ }
+ result << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
+ if (depth >= 1)
+ {
+ // Landmarks =======================================================================================
+ for (auto L : getMap()->getLandmarkList())
+ {
+ result << " L" << L->id() << " " << L->getType();
+ if (constr_by)
+ {
+ result << "\t<-- ";
+ for (auto cby : L->getConstrainedByList())
+ result << "c" << cby->id() << " \t";
+ }
+ result << endl;
+ if (metric)
+ {
+ result << (L->isFixed() ? " Fix" : " Est");
+ result << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
+ result << endl;
+ }
+ if (state_blocks)
+ {
+ result << " sb:";
+ for (auto sb : L->getStateBlockVec())
+ if (sb != nullptr)
+ result << (sb->isFixed() ? " Fix" : " Est");
+ result << endl;
+ }
+ } // for L
+ }
+ result << "-----------------------------------------" << endl;
+ result << endl;
+ return result.str();
+}
bool Problem::check(int verbose_level) const
{
using std::cout;
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 79de4b220a5e3ea71233aed1a760936ba28f5042..da45cb6f62a7f0304061158e1f694e8232956400 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -21,7 +21,7 @@ ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrivePtr _params
params_prc_diff_drv_selfcal_(_params),
radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
{
- setType("DIFF DRIVE"); // overwrite odom2D setting
+ setType("ProcessorDiffDrive"); // overwrite odom2D setting
calib_size_ = 3; // overwrite odom2D setting
unmeasured_perturbation_cov_ = Matrix1s(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2D setting
}
@@ -147,7 +147,7 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
{
auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
- "DIFF DRIVE",
+ "ProcessorDiffDrive",
_capture_motion->getBuffer().get().back().delta_integr_,
_capture_motion->getBuffer().get().back().delta_integr_cov_,
_capture_motion->getCalibrationPreint(),
@@ -174,7 +174,7 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive);
-WOLF_REGISTER_PROCESSOR_AUTO("DIFF DRIVE", ProcessorDiffDrive);
+WOLF_REGISTER_PROCESSOR("ProcessorDiffDrive", ProcessorDiffDrive);
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorDiffDrive", ProcessorDiffDrive);
} // namespace wolf
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index bf825c29f5f994bb09bfbe839ef56c98d37905f4..2edeaa5256310cf04c19a75aa04d14093a96b90c 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
- ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params),
+ ProcessorMotion("ProcessorOdom2D", 3, 3, 3, 2, 0, _params),
params_odom_2D_(_params)
{
unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity();
@@ -151,7 +151,7 @@ FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_motion)
makePosDef(covariance);
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ODOM 2D",
+ "ProcessorOdom2D",
_capture_motion->getBuffer().get().back().delta_integr_,
covariance);
return key_feature_ptr;
@@ -164,6 +164,6 @@ FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_motion)
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D);
-WOLF_REGISTER_PROCESSOR_AUTO("ODOM 2D", ProcessorOdom2D);
+WOLF_REGISTER_PROCESSOR("ProcessorOdom2D", ProcessorOdom2D);
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom2D", ProcessorOdom2D);
} // namespace wolf
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 180293e74f0c19689e7ca30919a4afb542ab8726..a840e0d733cfcd83d63311c3caaaa99393e287d7 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -3,7 +3,7 @@ namespace wolf
{
ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
- ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params),
+ ProcessorMotion("ProcessorOdom3D", 7, 7, 6, 6, 0, _params),
params_odom_3D_ (_params),
k_disp_to_disp_ (0),
k_disp_to_rot_ (0),
@@ -227,7 +227,7 @@ CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own,
FeatureBasePtr ProcessorOdom3D::emplaceFeature(CaptureMotionPtr _capture_motion)
{
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ODOM 3D",
+ "ProcessorOdom3D",
_capture_motion->getBuffer().get().back().delta_integr_,
_capture_motion->getBuffer().get().back().delta_integr_cov_);
return key_feature_ptr;
@@ -246,6 +246,6 @@ FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, Cap
// Register in the SensorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ODOM 3D", ProcessorOdom3D);
-WOLF_REGISTER_PROCESSOR_AUTO("ODOM 3D", ProcessorOdom3D);
+WOLF_REGISTER_PROCESSOR("ProcessorOdom3D", ProcessorOdom3D);
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom3D", ProcessorOdom3D);
} // namespace wolf
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 76514fb7dc00451036013373a6aa26f5f6a4b93f..c0114ace1a52790c72c084f3133ced6c3f94acb8 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -73,10 +73,10 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
* When done, we need to find these new Landmarks in the incoming Capture.
*/
- // assuming cleared new lists
- assert(new_features_last_.empty());
- assert(new_features_incoming_.empty());
- assert(new_landmarks_.empty());
+ // new lists cleared
+ new_features_last_.clear();
+ new_features_incoming_.clear();
+ new_landmarks_.clear();
// We first need to populate the \b last Capture with new Features
unsigned int n = detectNewFeatures(_max_features,
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index d19ab8aa2e4d2ba5f9146bbdad2d45e5d855afc2..6e4446da2766cb670e23226fc1945d4f86af3f90 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -13,7 +13,7 @@ namespace wolf
SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
const IntrinsicsDiffDrivePtr& _intrinsics) :
- SensorBase("DIFF DRIVE",
+ SensorBase("SensorDiffDrive",
std::make_shared<StateBlock>(_extrinsics.head(2), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
std::make_shared<StateBlock>(3, false),
@@ -36,7 +36,7 @@ SensorDiffDrive::~SensorDiffDrive()
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive);
-WOLF_REGISTER_SENSOR_AUTO("DIFF DRIVE", SensorDiffDrive);
+WOLF_REGISTER_SENSOR("SensorDiffDrive", SensorDiffDrive);
+WOLF_REGISTER_SENSOR_AUTO("SensorDiffDrive", SensorDiffDrive);
} // namespace wolf
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index 44d69e34f81044c6706d952c2f1eeff1b9cdc533..8714478613edb4747ddab4285c4c1e16d7bba110 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -5,7 +5,7 @@
namespace wolf {
SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
- SensorBase("ODOM 2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+ SensorBase("SensorOdom2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_rot_to_rot_(_intrinsics.k_rot_to_rot)
{
@@ -39,6 +39,6 @@ Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D);
-WOLF_REGISTER_SENSOR_AUTO("ODOM 2D", SensorOdom2D);
+WOLF_REGISTER_SENSOR("SensorOdom2D", SensorOdom2D);
+WOLF_REGISTER_SENSOR_AUTO("SensorOdom2D", SensorOdom2D);
} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index 04de2bbd642491004a640971dba01c99032c6607..e3aba3760ee96f897e865dda5c78b32fa0867043 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3D.cpp
@@ -13,7 +13,7 @@
namespace wolf {
SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
- SensorBase("ODOM 3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ SensorBase("SensorOdom3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_disp_to_rot_(_intrinsics.k_disp_to_rot),
k_rot_to_rot_(_intrinsics.k_rot_to_rot),
@@ -42,6 +42,6 @@ SensorOdom3D::~SensorOdom3D()
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D);
-WOLF_REGISTER_SENSOR_AUTO("ODOM 3D", SensorOdom3D);
+WOLF_REGISTER_SENSOR("SensorOdom3D", SensorOdom3D);
+WOLF_REGISTER_SENSOR_AUTO("SensorOdom3D", SensorOdom3D);
}
diff --git a/src/utils/converter_utils.cpp b/src/utils/converter_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f445c572dd03b2fa10aae48bb34f4a80798ed41
--- /dev/null
+++ b/src/utils/converter_utils.cpp
@@ -0,0 +1,132 @@
+#include "core/utils/converter_utils.h"
+
+// Eigen
+#include <array>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+// STD
+#include <iostream>
+#include <list>
+#include <stack>
+#include <vector>
+
+namespace utils {
+std::vector<std::string> splitter(std::string val) {
+ std::vector<std::string> cont = std::vector<std::string>();
+ std::stringstream ss(val);
+ std::string token;
+ while (std::getline(ss, token, ',')) {
+ cont.push_back(token);
+ }
+ return cont;
+}
+std::vector<std::string> getMatches(std::string val, std::regex exp) {
+ std::smatch res;
+ auto v = std::vector<std::string>();
+ std::string::const_iterator searchStart(val.cbegin());
+ while (std::regex_search(searchStart, val.cend(), res, exp)) {
+ v.push_back(res[0]);
+ searchStart = res.suffix().first;
+ }
+ return v;
+}
+std::array<std::string, 2> splitMatrixStringRepresentation(std::string matrix) {
+ std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:-?[0-9]*(?:\\.[0-9]+)?,?)+)\\]");
+ std::regex rgxStatic("\\[((?:(?:-?[0-9]*)(?:\\.[0-9]+)?,?)+)\\]");
+ std::smatch matches;
+ std::smatch matchesStatic;
+ std::array<std::string, 2> values = {{"[]", "[]"}};
+ if (std::regex_match(matrix, matches, rgx)) {
+ values[0] = "[" + matches[1].str() + "]";
+ values[1] = "[" + matches[2].str() + "]";
+ } else if (std::regex_match(matrix, matchesStatic, rgxStatic)) {
+ values[1] = "[" + matchesStatic[1].str() + "]";
+ } else {
+ throw std::runtime_error(
+ "Invalid string representation of a Matrix. Correct format is "
+ "[([num,num],)?(num(,num)*)?]. String provided: " +
+ matrix);
+ }
+ return values;
+}
+std::vector<std::string> pairSplitter(std::string val) {
+ std::regex exp("\\{[^\\{:]:.*?\\}");
+ return getMatches(val, exp);
+}
+std::string splitMapStringRepresentation(std::string str_map) {
+ std::smatch mmatches;
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ std::string result = "";
+ if (std::regex_match(str_map, mmatches, rgxM)) {
+ result = mmatches[1].str();
+ } else {
+ throw std::runtime_error(
+ "Invalid string representation of a Map. Correct format is "
+ "[({id:value})?(,{id:value})*]. String provided: " +
+ str_map);
+ }
+ return result;
+}
+std::vector<std::string> parseList(std::string val) {
+ std::stack<char> limiters;
+ std::stack<std::string> word_stack;
+ std::string current_word;
+ std::vector<std::string> words;
+ std::vector<char> chars(val.begin(), val.end());
+ for (const char ¤t : chars) {
+ if (current == '[') {
+ limiters.push(current);
+ word_stack.push(current_word);
+ current_word = "";
+ } else if (current == ']') {
+ if (limiters.empty())
+ throw std::runtime_error("Unmatched delimiter");
+ if (limiters.top() == '[') {
+ if (limiters.size() > 1) {
+ if (word_stack.empty())
+ word_stack.push("");
+ current_word = word_stack.top() + "[" + current_word + "]";
+ word_stack.pop();
+ } else if (limiters.size() == 1 and current_word != "")
+ words.push_back(current_word);
+ else
+ current_word += current;
+ limiters.pop();
+ } else
+ throw std::runtime_error("Unmatched delimiter");
+ } else if (current == '{') {
+ limiters.push(current);
+ word_stack.push(current_word);
+ current_word = "";
+ } else if (current == '}') {
+ if (limiters.top() == '{') {
+ if (limiters.size() > 1) {
+ if (word_stack.empty())
+ word_stack.push("");
+ current_word = word_stack.top() + "{" + current_word + "}";
+ word_stack.pop();
+ } else if (limiters.size() == 1)
+ words.push_back(current_word);
+ else
+ current_word += current;
+ limiters.pop();
+ } else
+ throw std::runtime_error("Unmatched delimiter");
+ } else if (current == ',') {
+ if (limiters.size() == 1 and current_word != "") {
+ words.push_back(current_word);
+ current_word = "";
+ } else if (limiters.size() > 1)
+ current_word += current;
+ } else {
+ if (limiters.empty())
+ throw std::runtime_error("Found non-delimited text");
+ current_word += current;
+ }
+ }
+ if (not limiters.empty())
+ throw std::runtime_error("Unclosed delimiter [] or {}");
+ return words;
+}
+} // namespace utils
\ No newline at end of file
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index f9f939553af76223416385f11785fe2ee694e70d..99a3d6f2d9a80b34815075dff721aaafc77728e2 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -24,7 +24,7 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "ODOM 3D")
+ if (config["type"].as<std::string>() == "ProcessorOdom3D")
{
ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>();
@@ -48,7 +48,7 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ODOM 3D", createProcessorOdom3DParams);
+const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ProcessorOdom3D", createProcessorOdom3DParams);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_2D_yaml.cpp b/src/yaml/sensor_odom_2D_yaml.cpp
index 99220af9bcb49ff574228c059cb09b8b6a8ee1a3..47bcea92a34c78ee2fd068beccdd5ecf7ac999c8 100644
--- a/src/yaml/sensor_odom_2D_yaml.cpp
+++ b/src/yaml/sensor_odom_2D_yaml.cpp
@@ -25,7 +25,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom2D(const std::string & _filename_do
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "ODOM 2D")
+ if (config["type"].as<std::string>() == "SensorOdom2D")
{
auto params = std::make_shared<IntrinsicsOdom2D>();
@@ -40,7 +40,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom2D(const std::string & _filename_do
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_2D_intr = IntrinsicsFactory::get().registerCreator("ODOM 2D", createIntrinsicsOdom2D);
+const bool WOLF_UNUSED registered_odom_2D_intr = IntrinsicsFactory::get().registerCreator("SensorOdom2D", createIntrinsicsOdom2D);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index 51ceb2f6344d440fec93bf3417654a9f47bf3333..3d364d9114d0f90bdbbb881abee6b410ada771a8 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -24,7 +24,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "ODOM 3D")
+ if (config["type"].as<std::string>() == "SensorOdom3D")
{
IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();
@@ -42,7 +42,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_3D_intr = IntrinsicsFactory::get().registerCreator("ODOM 3D", createIntrinsicsOdom3D);
+const bool WOLF_UNUSED registered_odom_3D_intr = IntrinsicsFactory::get().registerCreator("SensorOdom3D", createIntrinsicsOdom3D);
} // namespace [unnamed]
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 83e07bb49060279665ba9002798989be5693440a..c95634eb09ffbea46dae309720c02f889458539d 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -52,7 +52,7 @@ class FactorFeatureDummy : public FactorBase
inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FEATURE DUMMY", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+ FactorBase("FactorFeatureDummy", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
{
//
}
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index 95a5c483dc4dfd336880b64972f4ecad4fcb07bf..d46f6c645cd3d3bc626fc648a2107308022da96a 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -52,7 +52,7 @@ class FactorLandmarkDummy : public FactorBase
inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& /*_feature_ptr*/, const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FEATURE DUMMY", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status)
+ FactorBase("FactorFeatureDummy", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status)
{
//
}
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 05b2f34494472cfa9fde355b91d0b48a6609d25a..6a2626e76296f0616418762f3971e4c6f4c5d74a 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -113,5 +113,5 @@ ProcessorBasePtr ProcessorTrackerFeatureDummy::create(const std::string& _unique
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
+WOLF_REGISTER_PROCESSOR("ProcessorTrackerFeatureDummy", ProcessorTrackerFeatureDummy)
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index b4fdb44a79f6d5053e258379e1eadc8811cbfae7..4f5205d152ad4b0916d5c4968e754eb8da84c023 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -110,7 +110,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
};
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
- ProcessorTrackerFeature("TRACKER FEATURE DUMMY", _params_tracker_feature_dummy),
+ ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", _params_tracker_feature_dummy),
params_tracker_feature_dummy_(_params_tracker_feature_dummy)
{
//
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 532906ac27f319486b619fd6f8e0f882d3676197..42da1408bd12e47ffcc4c10409087b38e7d3aa3c 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -13,7 +13,7 @@ namespace wolf
{
ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
- ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark_dummy),
+ ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", _params_tracker_landmark_dummy),
params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
{
//
@@ -101,7 +101,7 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
//std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
- return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), "BASE", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), "LandmarkBase", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
}
FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index da5a01b80e98cf776957422d5bca8729b89ff038..e2047009e612990818bf877724804500464a8577 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -320,7 +320,7 @@ TEST(CeresManager, AddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
@@ -342,7 +342,7 @@ TEST(CeresManager, DoubleAddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// add factor again
@@ -367,7 +367,7 @@ TEST(CeresManager, RemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
@@ -395,7 +395,7 @@ TEST(CeresManager, AddRemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// remove factor
@@ -422,7 +422,7 @@ TEST(CeresManager, DoubleRemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
@@ -549,7 +549,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
// Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
@@ -591,7 +591,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
// Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 7213c1084cf13077a0413cbbea48af0676b330d8..f6705579818494debc790f7ed397c38a661bf154 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -82,7 +82,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
- auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
+ auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV());
ASSERT_TRUE(problem_ptr->check(0));
@@ -100,7 +100,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
TEST(FactorQuatAbs, fac_block_abs_o)
{
- auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
+ auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO());
ASSERT_TRUE(problem_ptr->check(0));
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 6caa5f868adceb291690590455bea94520771b0f..7d3aeb5e822545d7ad20d2d76fb715813aae4eef 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -149,7 +149,7 @@ class FactorDiffDriveTest : public testing::Test
intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
Vector3s extr(0, 0, 0);
- auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+ auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
calib = sensor->getIntrinsic()->getState();
@@ -158,7 +158,7 @@ class FactorDiffDriveTest : public testing::Test
params->angle_turned = 1;
params->dist_traveled = 2;
params->max_buff_length = 3;
- auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
// frames
@@ -198,7 +198,7 @@ class FactorDiffDriveTest : public testing::Test
// feature
f1 = FeatureBase::emplace<FeatureMotion>(C1,
- "DIFF DRIVE",
+ "FeatureDiffDrive",
delta1,
delta1_cov,
C0->getCalibration(),
@@ -234,7 +234,7 @@ TEST_F(FactorDiffDriveTest, constructor)
{
// plain constructor
auto c1_obj = FactorDiffDrive(f1, C0, processor);
- ASSERT_EQ(c1_obj.getType(), "DIFF DRIVE");
+ ASSERT_EQ(c1_obj.getType(), "FactorDiffDrive");
// std: make_shared
c1 = std::make_shared<FactorDiffDrive>(f1,
@@ -242,7 +242,7 @@ TEST_F(FactorDiffDriveTest, constructor)
processor);
ASSERT_NE(c1, nullptr);
- ASSERT_EQ(c1->getType(), "DIFF DRIVE");
+ ASSERT_EQ(c1->getType(), "FactorDiffDrive");
// wolf: emplace
c1 = FactorBase::emplace<FactorDiffDrive>(f1,
@@ -251,7 +251,7 @@ TEST_F(FactorDiffDriveTest, constructor)
processor);
ASSERT_NE(c1, nullptr);
- ASSERT_EQ(c1->getType(), "DIFF DRIVE");
+ ASSERT_EQ(c1->getType(), "FactorDiffDrive");
ASSERT_EQ(c1->getCaptureOther()->getSensorIntrinsic(), sensor->getIntrinsic());
}
@@ -423,7 +423,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
Vector3s extr(0, 0, 0);
- auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+ auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3s calib_gt(1,1,1);
@@ -435,7 +435,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
params->max_buff_length = 99;
params->voting_active = true;
params->max_time_span = 1.5;
- auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
@@ -555,7 +555,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
intr->wheel_separation = 1.05;
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
Vector3s extr(0, 0, 0);
- auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+ auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3s calib_gt(1.0, 1.0, 1.0); // ground truth calib
@@ -567,7 +567,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
params->max_buff_length = 99;
params->voting_active = true;
params->max_time_span = 1.5;
- auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index d6eb4ddf7ea7b07cdb675b96cb7af81e3adc3c8d..4ff27b9117808265ec8b79c0c78451a7cfbcbac8 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -41,8 +41,8 @@ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), Tim
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "ODOM 3D", 1, nullptr, delta, 7, 6, nullptr);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "ODOM 3D", delta, data_cov);
+auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3D", 1, nullptr, delta, 7, 6, nullptr);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3D", delta, data_cov);
FactorOdom3DPtr ctr1 = FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr); // create and add
////////////////////////////////////////////////////////
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index 3c88521c95ab2966b839cd6ab551ef6224b27390..6be39e3471ce81fb89ec05c6ad84cfe2d696bc49 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -33,10 +33,10 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor from frm1 to frm0
-// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 2D", 0, nullptr, pose, 3, 3, nullptr);
-// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 2D", pose, data_cov);
+// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("CaptureOdom2D", 0, nullptr, pose, 3, 3, nullptr));
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2D", 0, nullptr, pose, 3, 3, nullptr);
+// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FeatureOdom2D", pose, data_cov));
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2D", pose, data_cov);
// FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
FactorPose2DPtr ctr0 = FactorBase::emplace<FactorPose2D>(fea0, fea0);
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 0b5032a27978d172432b7f68a04fdea28afc64ff..cfb89e5db2a6e1a8a6ca66d2afb83e287c5f4a73 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -39,8 +39,8 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr);
-auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 3D", pose7, data_cov);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3D", 0, nullptr, pose7, 7, 6, nullptr);
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3D", pose7, data_cov);
FactorPose3DPtr ctr0 = FactorBase::emplace<FactorPose3D>(fea0, fea0);
////////////////////////////////////////////////////////
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 28ce9d0816335bfe8861cc638e6427881dac1176..4be7353651ee5c037afa813da4b88f9d3848d897 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -71,7 +71,7 @@ TEST(FrameBase, LinksToTree)
auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
+ auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3s::Zero(), 3, 3, nullptr);
/// @todo link sensor & proccessor
ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 60ac575e2a13591b22a8ce0c0217d049124cc9e2..3c04d1567ffb7ce9d3aeb05452fe0f175582c39d 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -36,9 +36,9 @@ TEST(MapYaml, save_2D)
StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
- LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb);
- LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, o2_sb);
- LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, o3_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, o3_sb);
std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml";
@@ -159,9 +159,9 @@ TEST(MapYaml, save_3D)
auto p3_sb = std::make_shared<StateBlock>(p3, true);
auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
- LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb);
- LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, q2_sb);
- LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, q3_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, q2_sb);
+ LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, q3_sb);
std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml";
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index b4647a0a83947463c8c5f8e9a975823c2f4f9fdb..54f39079573340c9395225b5aa1a6b67b6255c2c 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -133,15 +133,15 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
// KF1 and motion from KF0
t += dt;
FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
- auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t);
- auto f1 = FeatureBase::emplace<FeatureBase>(C1, "ODOM 2D", delta, delta_cov);
+ auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2D", t);
+ auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2D", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr);
// KF2 and motion from KF1
t += dt;
FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
- auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t);
- auto f2 = FeatureBase::emplace<FeatureBase>(C2, "ODOM 2D", delta, delta_cov);
+ auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2D", t);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2D", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr);
ASSERT_TRUE(Pr->check(0));
@@ -196,7 +196,7 @@ TEST(Odom2D, VoteForKfAndSolve)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->voting_active = true;
params->dist_traveled = 100;
@@ -205,7 +205,7 @@ TEST(Odom2D, VoteForKfAndSolve)
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.00;
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
+ ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
@@ -336,7 +336,7 @@ TEST(Odom2D, KF_callback)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->dist_traveled = 100;
params->angle_turned = 6.28;
@@ -344,7 +344,7 @@ TEST(Odom2D, KF_callback)
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.000001;
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
+ ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
processor_odom2d->setTimeTolerance(dt/2);
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 177961d922ebf117b0a696fd870cfe76f48b86ad..3329f1c7250c6e5cd961c8ee4dd18fee477cd2a5 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -22,7 +22,7 @@ Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).fi
Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished());
Eigen::Vector7s prior2_extrinsics((Eigen::Vector7s() << 100, 200, 300, 0, 0, 0, 1).finished());
-SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>()));
+SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("SensorOdom3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>()));
TEST(ParameterPrior, initial_extrinsics)
{
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index bfa888fa7fc415c1689eb6b66a30b2919b599dbd..18f6b1eded0955faf7c6b823e25bbde62894203d 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -9,13 +9,6 @@ using namespace wolf;
std::string wolf_root = _WOLF_ROOT_DIR;
-ParserYAML parse(string _file, string _path_root)
-{
- ParserYAML parser = ParserYAML(_file, _path_root);
- parser.parse();
- return parser;
-}
-
//TEST(ParamsServer, Default)
//{
// auto parser = parse("test/yaml/params1.yaml", wolf_root);
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index aea2db7da463b6076179b7b8bbcd18e2a3dd2046..d464c1120e6830c9912d0f16472c53f74b4f9bac 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -10,16 +10,9 @@ std::string wolf_root = _WOLF_ROOT_DIR;
std::string sensor_prefix = "sensor/";
std::string processor_prefix = "processor/";
-ParserYAML parse(string _file, string _path_root)
-{
- ParserYAML parser = ParserYAML(_file, _path_root);
- parser.parse();
- return parser;
-}
-
TEST(ParserYAML, RegularParse)
{
- auto parser = parse("test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYAML("test/yaml/params1.yaml", wolf_root);
auto params = parser.getParams();
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
@@ -28,7 +21,7 @@ TEST(ParserYAML, RegularParse)
}
TEST(ParserYAML, ParseMap)
{
- auto parser = parse("test/yaml/params2.yaml", wolf_root);
+ auto parser = ParserYAML("test/yaml/params2.yaml", wolf_root);
auto params = parser.getParams();
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
@@ -37,28 +30,28 @@ TEST(ParserYAML, ParseMap)
}
TEST(ParserYAML, FollowFile)
{
- auto parser = parse("test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYAML("test/yaml/params1.yaml", wolf_root);
auto params = parser.getParams();
EXPECT_EQ(params[processor_prefix + "my_proc_test/max_buff_length"], "100");
EXPECT_EQ(params[processor_prefix + "my_proc_test/voting_active"], "false");
}
TEST(ParserYAML, FollowOdom3D)
{
- auto parser = parse("test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYAML("test/yaml/params1.yaml", wolf_root);
auto params = parser.getParams();
EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_buff_length"], "10");
EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_time_span"], "0.2");
}
TEST(ParserYAML, JumpFile)
{
- auto parser = parse("test/yaml/params3.yaml", wolf_root);
+ auto parser = ParserYAML("test/yaml/params3.yaml", wolf_root);
auto params = parser.getParams();
EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/max_buff_length"], "100");
EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/voting_active"], "false");
}
TEST(ParserYAML, ProblemConfig)
{
- auto parser = parse("test/yaml/params2.yaml", wolf_root);
+ auto parser = ParserYAML("test/yaml/params2.yaml", wolf_root);
auto params = parser.getParams();
EXPECT_EQ(params["problem/frame_structure"], "POV");
EXPECT_EQ(params["problem/dimension"], "2");
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 53ebf672d6104ba4bfba1484a4a87f5424c143f0..93b2dac7b09e6a27ff7ff11d5133decc3e3cf59b 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -107,16 +107,16 @@ TEST(Problem, Installers)
ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7s xs;
- SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr S = P->installSensor ("SensorOdom3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
- auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
+ auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
// check motion processor IS NOT set
ASSERT_FALSE(P->getProcessorMotion());
// install processor motion
- ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// check motion processor is set
ASSERT_TRUE(P->getProcessorMotion());
@@ -233,15 +233,15 @@ TEST(Problem, StateBlocks)
Eigen::Vector3s xs2d;
// 2 state blocks, fixed
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
// // 2 state blocks, fixed
-// SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
+// SensorBasePtr St = P->installSensor ("SensorOdom2D", "other odometer", xs2d, "");
// ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
- auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");
- auto pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
+ auto pm = P->installProcessor("ProcessorOdom3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 2 state blocks, estimated
auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
@@ -287,16 +287,16 @@ TEST(Problem, Covariances)
Eigen::Vector7s xs3d;
Eigen::Vector3s xs2d;
- SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
- SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom3D", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr St = P->installSensor ("SensorOdom2D", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
params->max_new_features = 5;
params->min_features_for_keyframe = 10;
- auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", Sm, params);
- auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", Sm, params);
+ auto pm = P->installProcessor("ProcessorOdom3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
// 4 state blocks, estimated
St->unfixExtrinsics();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 36ad812454b7eb0f21c2997762e3a3ccc3096838..3567e4d0ee0b1be16fdc526ee1b928eb8d561597 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -53,13 +53,13 @@ TEST(ProcessorBase, KeyFrameCallback)
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
- auto proc_trk = problem->installProcessor("TRACKER FEATURE DUMMY", "dummy", sens_trk);
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2D", "odometer", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odom processor", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2D", "odom processor", sens_odo, proc_odo_params);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 069f630631f03529da0d7d993f17dacb300f89a2..342f284751266099b05e358a2e09368e1fa08ff6 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -126,7 +126,7 @@ class ProcessorDiffDriveTest : public testing::Test
intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
Vector3s extr(0,0,0);
- sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("DIFF DRIVE", "sensor diff drive", extr, intr));
+ sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
// params and processor
params = std::make_shared<ProcessorParamsDiffDrive>();
@@ -136,7 +136,7 @@ class ProcessorDiffDriveTest : public testing::Test
params->max_buff_length = 3;
params->max_time_span = 2.5;
params->unmeasured_perturbation_std = 1e-4;
- processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("DIFF DRIVE", "processor diff drive", sensor, params));
+ processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("ProcessorDiffDrive", "processor diff drive", sensor, params));
}
virtual void TearDown(){}
@@ -153,7 +153,7 @@ TEST(ProcessorDiffDrive, constructor)
ASSERT_NE(prc, nullptr);
- ASSERT_EQ(prc->getType(), "DIFF DRIVE");
+ ASSERT_EQ(prc->getType(), "ProcessorDiffDrive");
}
TEST(ProcessorDiffDrive, create)
@@ -173,7 +173,7 @@ TEST(ProcessorDiffDrive, create)
ASSERT_NE(prc, nullptr);
- ASSERT_EQ(prc->getType(), "DIFF DRIVE");
+ ASSERT_EQ(prc->getType(), "ProcessorDiffDrive");
}
TEST_F(ProcessorDiffDriveTest, params)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 0977b8f87f3ba2fab515098a2afd0942f8fb6714..15e8515290eefeede88469d339b98069daa61485 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -62,7 +62,7 @@ class ProcessorMotion_test : public testing::Test{
dt = 1.0;
problem = Problem::create("PO", 2);
- sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
+ sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("SensorOdom2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->time_tolerance = 0.25;
params->dist_traveled = 100;
@@ -71,7 +71,7 @@ class ProcessorMotion_test : public testing::Test{
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.001;
processor = ProcessorBase::emplace<ProcessorOdom2DPublic>(sensor, params);
- capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
+ capture = std::make_shared<CaptureMotion>("CaptureOdom2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
Vector3s x0; x0 << 0, 0, 0;
Matrix3s P0; P0.setIdentity();
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 31dee4670b60e286a55ea315a8a1507338b3022d..9f7b0305ed0a20ba10fdb09a617d4767f089cd46 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -82,7 +82,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
+ sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
// Install processor
params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 779ac7de285105263ae1214e0e61463e311983e5..e9a2535fccb5e265727a78b5ed71fb46f1a46b37 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -100,7 +100,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
+ sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
// Install processor
params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>();
@@ -260,7 +260,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1)));
- LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("BASE",
+ LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("LandmarkBase",
std::make_shared<StateBlock>(1),
std::make_shared<StateBlock>(1)));
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 499629f8fc934eaa5853670dd7d24a69fdc8885c..ffbf013d678fd2c979d59c911c7e2f81cb92539e 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -13,7 +13,7 @@ using namespace wolf;
TEST(SensorBase, setNoiseStd)
{
- SensorBasePtr S (std::make_shared<SensorBase>("BASE", nullptr, nullptr, nullptr, 2)); // noise size 2
+ SensorBasePtr S (std::make_shared<SensorBase>("SensorBase", nullptr, nullptr, nullptr, 2)); // noise size 2
Eigen::Vector2s noise_std = Eigen::Vector2s::Ones() * 0.1;
Eigen::Matrix2s noise_cov = Eigen::Matrix2s::Identity() * 0.01;
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 2616d61040d4f224cfd65020faec182d93e02376..77003a6360294f27a249c7a4e9492753c5d66997 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -517,7 +517,7 @@ TEST(SolverManager, AddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// notification
@@ -544,7 +544,7 @@ TEST(SolverManager, RemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// update solver
@@ -578,7 +578,7 @@ TEST(SolverManager, AddRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// notification
@@ -613,7 +613,7 @@ TEST(SolverManager, DoubleRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3s::Zero(), Matrix3s::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// update solver
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 8d7cafc5f699abc2f0fa9d275f57d5b4b3677821..5bb8be67440f8cf15c8c37bf05c5b710906783c7 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -27,11 +27,11 @@ class TrackMatrixTest : public testing::Test
{
// unlinked captures
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- C0 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 0.0);
- C1 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 1.0);
- C2 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 2.0);
- C3 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 3.0);
- C4 = CaptureBase::emplace<CaptureBase>(nullptr, "BASE", 4.0);
+ C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
+ C1 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 1.0);
+ C2 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 2.0);
+ C3 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 3.0);
+ C4 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 4.0);
// unlinked frames
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
@@ -43,11 +43,11 @@ class TrackMatrixTest : public testing::Test
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
- f0 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
- f1 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
- f2 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
- f3 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
- f4 = FeatureBase::emplace<FeatureBase>(nullptr, "BASE", m, m_cov);
+ f0 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f1 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
F0->setKey();
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index 752d2c26b745b84eeb773c6bcfd760073eec6eac..70a379caf099773905033ac7f6f4b0e88d467f3e 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -6,6 +6,7 @@ config:
-
type: "ODOM 2D"
name: "odom"
+ plugin: "core"
intrinsic:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
@@ -14,24 +15,29 @@ config:
-
type: "RANGE BEARING"
name: "rb"
+ plugin: "core"
processors:
-
type: "ODOM 2D"
name: "processor1"
sensor_name: "odom"
+ plugin: "core"
-
type: "RANGE BEARING"
name: "rb_processor"
sensor_name: "rb"
+ plugin: "core"
-
type: "ODOM 2D"
name: "my_proc_test"
sensor_name: "odom"
+ plugin: "core"
follow: "test/yaml/params3.1.yaml"
-
type: "ODOM 3D"
name: "my_proc_odom3d"
sensor_name: "odom"
+ plugin: "core"
follow: "test/yaml/processor_odom_3D.yaml"
files:
- "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
index 5825166a88c5383b3d7cc4e3264cf51e9605aa5e..289488fc0e4ff0bd0d79bf26835ee72875308129 100644
--- a/test/yaml/params2.yaml
+++ b/test/yaml/params2.yaml
@@ -6,6 +6,7 @@ config:
-
type: "ODOM 2D"
name: "odom"
+ plugin: "core"
intrinsic:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
@@ -14,11 +15,13 @@ config:
-
type: "RANGE BEARING"
name: "rb"
+ plugin: "core"
processors:
-
type: "ODOM 2D"
name: "processor1"
sensor_name: "odom"
+ plugin: "core"
$mymap:
k1: v1
k2: v2
@@ -27,10 +30,12 @@ config:
type: "RANGE BEARING"
name: "rb_processor"
sensor_name: "rb"
+ plugin: "core"
-
type: "ODOM 2D"
name: "my_proc_test"
sensor_name: "odom"
+ plugin: "core"
files:
- "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index 4983d0e23734df04eb87c20422e272febbdbfb67..ec7006053c7e27d79789630f1cd161a4905aa714 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -6,6 +6,7 @@ config:
-
type: "ODOM 2D"
name: "odom"
+ plugin: "core"
intrinsic:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
@@ -15,5 +16,6 @@ config:
-
type: "ODOM 2D"
name: "my_proc_test"
+ plugin: "core"
sensor_name: "odom"
extern_params: "@test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/processor_odom_3D.yaml b/test/yaml/processor_odom_3D.yaml
index a43fb60198d51944566308f3f06b0b6f33cee8bf..25ef29e83065acfe37cff57babc06b25b3b5d57b 100644
--- a/test/yaml/processor_odom_3D.yaml
+++ b/test/yaml/processor_odom_3D.yaml
@@ -1,4 +1,4 @@
-type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
time_tolerance: 0.01 # seconds
diff --git a/test/yaml/sensor_odom_2D.yaml b/test/yaml/sensor_odom_2D.yaml
index 40677f3e1cb84d7eda527e942bd54880c0ac7e37..45271b18cfff1235f05d945c6e5f124ff33e064e 100644
--- a/test/yaml/sensor_odom_2D.yaml
+++ b/test/yaml/sensor_odom_2D.yaml
@@ -1,4 +1,4 @@
-type: "ODOM 2D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom2D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
diff --git a/test/yaml/sensor_odom_3D.yaml b/test/yaml/sensor_odom_3D.yaml
index c45adb8ca809770ff3320ec81637e7a7ea556758..9fb43d4c30c0f5c01757362f6122d0cba98b14c5 100644
--- a/test/yaml/sensor_odom_3D.yaml
+++ b/test/yaml/sensor_odom_3D.yaml
@@ -1,4 +1,4 @@
-type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
diff --git a/wolf_scripts/rename.sh b/wolf_scripts/rename.sh
new file mode 100755
index 0000000000000000000000000000000000000000..27c83e736e3b2b29a0033932f5c134f1bceb8ad0
--- /dev/null
+++ b/wolf_scripts/rename.sh
@@ -0,0 +1,62 @@
+#!/bin/bash
+file2type()
+{
+ type=$(echo $1 | sed -r "s/([^_]+)/\U\1\\ /g" | sed -r "s/_([^_]+)/\U\1/g" | sed -r "s/\\ $//" )
+}
+camel2snake ()
+{
+ camel=$1
+ new_snake=$(echo $camel | sed -r "s/([A-Z][^A-Z]+)/\L\1_/g" | sed -r "s/_$//")
+}
+snake2camel ()
+{
+ snake=$1
+ new_camel=$(echo $snake | sed -r "s/([^_]+)/\u\1/g" | sed -r "s/_([^_]+)/\u\1/g")
+}
+
+# type=$(echo $1 | sed -r "s/([^_]+)/\U\1\\ /g" | sed -r "s/_([^_]+)/\U\1/g" )
+# file2type $1
+# echo $type
+# exit 1
+getTypes ()
+{
+ for file in $(find include/ src/ test/ -type f); do
+ name=$(echo $file | rev | cut -d '/' -f1 | rev | cut -d '.' -f1)
+ extension=$(echo $name | cut -d '_' -f2- )
+ # echo $extension
+ snake2camel $extension
+ camel_extension=$new_camel
+ snake2camel $name
+ camel_file=$new_camel
+ file2type $extension
+ # echo $file " %%% " $type " %%% " $camel_file
+ # echo "VVV "$type
+ # sed -rn "s/(Feature|Capture|Landmark|Processor|Factor)(.*)\"${type}\"/\1\2\"${camel_file}\"/p" $file
+ # echo sed -rn "s/(Capture)(.*)\"${type}\"/\1\2\"${camel_file}\"/p" $file
+ # type="ODOM 3D"
+ matching=$(ag -l "\"${type}\"" --ignore "demos" . | wc -l)
+ if [ $matching -gt "0" ]; then
+ echo ${type}
+ fi
+ # sed -ri "s/\"${type}\"/\"${camel_file}\"/" $file
+ done
+}
+IFS=$'\n'
+types=$(getTypes | sort | uniq)
+# echo $types
+# for ctype in $types; do
+# ctype=$(echo $ctype | sed -r "s/([^\ 0-9])([^\ 0-9]+)\ ?/\1\L\2/g")
+# echo $ctype
+# done
+for target in $(find include/ src/ test/ -type f); do
+ for ctype in $types; do
+ filename=$(echo $target | rev | cut -d '/' -f1 | cut -d '.' -f2 | rev)
+ snake2camel $filename
+ oldtype=$ctype
+ ctype=$(echo $ctype | sed -r "s/([^\ 0-9])([^\ 0-9]+)\ ?/\1\L\2/g")
+ # echo $target $oldtype $ctype
+ # sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ sed -ri "s%\"${oldtype}\"%\"${new_camel}\"%" $target
+ done
+done