diff --git a/.ci_templates/.clang_format.yml b/.ci_templates/.clang_format.yml
index dc263197d73bc5bf68967dfd390ae2933bde2a31..b62073920184d98edef9e94f3b1312a8da89bcc2 100644
--- a/.ci_templates/.clang_format.yml
+++ b/.ci_templates/.clang_format.yml
@@ -11,7 +11,7 @@
# create temporary branch
- if [ `git rev-parse --verify ci_clangformat 2>/dev/null` ]; then
- - git branch --delete ci_clangformat
+ - git branch -D ci_clangformat
- fi
- export CI_NEW_BRANCH_CLANG=ci_clangformat
- echo creating new temporary branch... $CI_NEW_BRANCH_CLANG
diff --git a/.ci_templates/.license_headers.yml b/.ci_templates/.license_headers.yml
index cc3b675ccd6bf027e21971d951052563cd64d77a..f2b1db32f2a2f126eb7053ac55468df95192b255 100644
--- a/.ci_templates/.license_headers.yml
+++ b/.ci_templates/.license_headers.yml
@@ -12,7 +12,7 @@
# create temporary branch
- if [ `git rev-parse --verify ci_license_header 2>/dev/null` ]; then
- - git branch --delete ci_license_header
+ - git branch -D ci_license_header
- fi
- export CI_NEW_BRANCH_LICENSE=ci_license_header
- echo creating new temporary branch... $CI_NEW_BRANCH_LICENSE
diff --git a/.ci_templates/.yaml_schema_cpp.yml b/.ci_templates/.yaml_schema_cpp.yml
index fb7056750f841bfdd95de84630bd4f1da6127089..1e26f3d7963d9a90429e11224e4f8869d57fed14 100644
--- a/.ci_templates/.yaml_schema_cpp.yml
+++ b/.ci_templates/.yaml_schema_cpp.yml
@@ -38,7 +38,7 @@
# create temporary branch
- if [ `git rev-parse --verify ci_yamlschemacpp 2>/dev/null` ]; then
- - git branch --delete ci_yamlschemacpp
+ - git branch -D ci_yamlschemacpp
- fi
- export CI_NEW_BRANCH_YAML=ci_yamlschemacpp
- echo creating new temporary branch... $CI_NEW_BRANCH_YAML
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 6d3a51538fddaaa90f57eb7894d785887e1a4f5e..75b75aeab5826728597de4dda1456d154555fa5d 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -24,7 +24,8 @@ stages:
############ YAML ANCHORS ############
.print_variables_template: &print_variables_definition
# Print variables
- - echo $CI_COMMIT_BRANCH
+ - echo $CI_COMMIT_REF_NAME
+ - echo $WOLF_CORE_BRANCH
- echo $WOLF_IMU_BRANCH
- echo $WOLF_GNSS_BRANCH
- echo $WOLF_LASER_BRANCH
@@ -44,13 +45,10 @@ stages:
.preliminaries_template: &preliminaries_definition
## FIX VARIABLES
- - if [ "$CI_COMMIT_BRANCH" == "" ]; then
- - export CI_COMMIT_BRANCH=$CI_MERGE_REQUEST_SOURCE_BRANCH_NAME
- - fi
- - export WOLF_CORE_BRANCH=$CI_COMMIT_BRANCH
+ - export WOLF_CORE_BRANCH=$CI_COMMIT_REF_NAME
## OVERRIDE VARIABLES (for MR to be done by developer)
- #- export WOLF_CORE_BRANCH=$CI_COMMIT_BRANCH
+ #- export WOLF_CORE_BRANCH=$CI_COMMIT_REF_NAME
## PRINT VARIABLES
- *print_variables_definition
@@ -134,12 +132,12 @@ build_and_test:focal:
deploy_imu:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
@@ -149,12 +147,12 @@ deploy_imu:
deploy_gnss:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
@@ -165,12 +163,12 @@ deploy_gnss:
deploy_vision:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
DEPLOY_CI_ROS: "false"
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
@@ -180,12 +178,12 @@ deploy_vision:
deploy_laser:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
@@ -196,12 +194,12 @@ deploy_laser:
deploy_apriltag:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
@@ -212,12 +210,12 @@ deploy_apriltag:
deploy_bodydynamics:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
DEPLOY_CI_ROS: "false"
trigger:
@@ -229,7 +227,7 @@ deploy_bodydynamics:
deploy_imu_main:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
@@ -243,7 +241,7 @@ deploy_imu_main:
deploy_gnss_main:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
@@ -258,7 +256,7 @@ deploy_gnss_main:
deploy_vision_main:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
@@ -272,7 +270,7 @@ deploy_vision_main:
deploy_laser_main:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
@@ -287,7 +285,7 @@ deploy_laser_main:
deploy_apriltag_main:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
@@ -302,7 +300,7 @@ deploy_apriltag_main:
deploy_bodydynamics_main:
stage: deploy_plugins
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
@@ -325,13 +323,13 @@ no_plugins:
deploy_wolf_ros_node:
stage: deploy_ros
rules:
- - if: $CI_COMMIT_BRANCH == "main"
+ - if: $CI_COMMIT_REF_NAME == "main"
when: never
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
- if: $DEPLOY_CI_ROS == "true"
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
WOLF_GNSS_BRANCH: $WOLF_GNSS_BRANCH
WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
@@ -354,11 +352,11 @@ deploy_wolf_ros_node:
deploy_wolf_ros_node_main:
stage: deploy_ros
rules:
- - if: $CI_COMMIT_BRANCH == "main" && $DEPLOY_CI_ROS == "true"
+ - if: $CI_COMMIT_REF_NAME == "main" && $DEPLOY_CI_ROS == "true"
- if: $CI_PIPELINE_SOURCE == "merge_request_event"
when: never
variables:
- WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_CORE_BRANCH: $CI_COMMIT_REF_NAME
WOLF_IMU_BRANCH: main
WOLF_GNSS_BRANCH: main
WOLF_LASER_BRANCH: main
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f4be61e30dcbae2284856e0733b8e3a908559882..ef9b744397756151df18f32d45c9a54f3069e7fa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@
MESSAGE(STATUS "Starting WOLF CMakeLists ...")
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.16)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
@@ -130,9 +130,7 @@ SET(HDRS_COMMON
)
SET(HDRS_COMPOSITE
include/${PROJECT_NAME}/composite/composite.h
- include/${PROJECT_NAME}/composite/matrix_composite.h
- include/${PROJECT_NAME}/composite/spec_state_composite.h
- include/${PROJECT_NAME}/composite/spec_state_sensor_composite.h
+ include/${PROJECT_NAME}/composite/prior_composite.h
include/${PROJECT_NAME}/composite/type_composite.h
include/${PROJECT_NAME}/composite/vector_composite.h
)
@@ -163,6 +161,7 @@ SET(HDRS_FACTOR
SET(HDRS_FEATURE
include/${PROJECT_NAME}/feature/feature_base.h
include/${PROJECT_NAME}/feature/feature_diff_drive.h
+ include/${PROJECT_NAME}/feature/feature_landmark_external.h
include/${PROJECT_NAME}/feature/feature_match.h
include/${PROJECT_NAME}/feature/feature_motion.h
include/${PROJECT_NAME}/feature/feature_odom_2d.h
@@ -178,8 +177,7 @@ SET(HDRS_LANDMARK
include/${PROJECT_NAME}/landmark/factory_landmark.h
include/${PROJECT_NAME}/landmark/landmark_base.h
include/${PROJECT_NAME}/landmark/landmark_match.h
- include/${PROJECT_NAME}/landmark/landmark_point.h
- include/${PROJECT_NAME}/landmark/landmark_pose.h
+ include/${PROJECT_NAME}/landmark/landmark.h
)
SET(HDRS_MATH
include/${PROJECT_NAME}/math/SE2.h
@@ -276,9 +274,7 @@ SET(SRCS_COMMON
src/common/time_stamp.cpp
)
SET(SRCS_COMPOSITE
- src/composite/matrix_composite.cpp
- src/composite/spec_state_composite.cpp
- src/composite/spec_state_sensor_composite.cpp
+ src/composite/prior_composite.cpp
src/composite/vector_composite.cpp
)
SET(SRCS_FACTOR
@@ -288,6 +284,7 @@ SET(SRCS_FACTOR
SET(SRCS_FEATURE
src/feature/feature_base.cpp
src/feature/feature_diff_drive.cpp
+ src/feature/feature_landmark_external.cpp
src/feature/feature_motion.cpp
src/feature/feature_odom_2d.cpp
src/feature/feature_pose.cpp
@@ -300,8 +297,7 @@ SET(SRCS_HARDWARE
)
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
- src/landmark/landmark_point.cpp
- src/landmark/landmark_pose.cpp
+ src/landmark/landmark.cpp
)
SET(SRCS_MAP
src/map/map_base.cpp
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index 2517f01d2b8b263709a3800ce744d61eafcd8f8f..4fefb2237471d0094603b813c1d7a677d7d8ec59 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -472,3 +472,4 @@ int main(int argc, char** argv)
// exit
return 0;
}
+} // namespace wolf
\ No newline at end of file
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 7ce6c92a640b7acb77e941d8bbf3079e437cadff..4cc0318e2772468927932023a0dbcd8dd0090e40 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -124,14 +124,12 @@ int main()
// sensor odometer 2d
YAML::Node params_sen_odo;
- params_sen_odo["states"]["P"]["type"] = "StatePoint2d";
- params_sen_odo["states"]["P"]["state"] = Vector2d::Zero();
- params_sen_odo["states"]["P"]["mode"] = "fix";
- params_sen_odo["states"]["P"]["dynamic"] = false;
- params_sen_odo["states"]["O"]["type"] = "StateAngle";
- params_sen_odo["states"]["O"]["state"] = Vector1d::Zero();
- params_sen_odo["states"]["O"]["mode"] = "fix";
- params_sen_odo["states"]["O"]["dynamic"] = false;
+ params_sen_odo["states"]["P"]["value"] = Vector2d::Zero();
+ params_sen_odo["states"]["P"]["prior"]["mode"] = "fix";
+ params_sen_odo["states"]["P"]["dynamic"] = false;
+ params_sen_odo["states"]["O"]["value"] = Vector1d::Zero();
+ params_sen_odo["states"]["O"]["prior"]["mode"] = "fix";
+ params_sen_odo["states"]["O"]["dynamic"] = false;
params_sen_odo["name"] = "Sensor Odometry";
params_sen_odo["k_disp_to_disp"] = 0.1;
@@ -163,14 +161,12 @@ int main()
// sensor Range and Bearing
YAML::Node params_sen_rb;
- params_sen_rb["states"]["P"]["type"] = "StatePoint2d";
- params_sen_rb["states"]["P"]["state"] = Vector2d::Zero();
- params_sen_rb["states"]["P"]["mode"] = "fix";
- params_sen_rb["states"]["P"]["dynamic"] = false;
- params_sen_rb["states"]["O"]["type"] = "StateAngle";
- params_sen_rb["states"]["O"]["state"] = Vector1d::Zero();
- params_sen_rb["states"]["O"]["mode"] = "fix";
- params_sen_rb["states"]["O"]["dynamic"] = false;
+ params_sen_rb["states"]["P"]["value"] = Vector2d::Zero();
+ params_sen_rb["states"]["P"]["prior"]["mode"] = "fix";
+ params_sen_rb["states"]["P"]["dynamic"] = false;
+ params_sen_rb["states"]["O"]["value"] = Vector1d::Zero();
+ params_sen_rb["states"]["O"]["prior"]["mode"] = "fix";
+ params_sen_rb["states"]["O"]["dynamic"] = false;
params_sen_rb["name"] = "Sensor Range Bearing";
params_sen_rb["noise_range_metres_std"] = 0.1;
@@ -190,11 +186,13 @@ int main()
std::cout << "processor_rb created\n";
// initialize
- TimeStamp t(0.0); // t : 0.0
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(sqrt(0.1))));
- prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(sqrt(0.1))));
- FrameBasePtr KF1 = problem->setPrior(prior, t); // KF1 : (0,0,0)
+ TimeStamp t(0.0); // t : 0.0
+ FrameBasePtr KF1 =
+ problem->emplaceFirstFrame(t,
+ {{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ {{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ {{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))},
+ {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}}); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 0e3e1e6ef8d09a29d15212592dff2da8c94a604a..db86d46a5811018ac231282834a5f63cd89a39dd 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -134,7 +134,7 @@ int main()
// APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
TimeStamp t(0.0);
- FrameBasePtr KF1 = problem->applyPriorOptions(t);
+ FrameBasePtr KF1 = problem->applyFirstFrameOptions(t);
// std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
@@ -262,7 +262,7 @@ int main()
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
- * Lmk3 LandmarkPoint2d <-- Fac9
+ * Lmk3 Landmark2d <-- Fac9
* P[Est] = ( 1 2 )
*
* it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
@@ -351,11 +351,11 @@ int main()
* buffer size : 1
* delta preint : (0 0 0)
* Map
- * Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
+ * Lmk1 Landmark2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
* P[Est] = ( 1 2 )
- * Lmk2 LandmarkPoint2d <-- Fac6 Fac8
+ * Lmk2 Landmark2d <-- Fac6 Fac8
* P[Est] = ( 2 2 )
- * Lmk3 LandmarkPoint2d <-- Fac9
+ * Lmk3 Landmark2d <-- Fac9
* P[Est] = ( 3 2 )
* -----------------------------------------
* *
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 420bb24c0a5b5241d1700c0303c924c06699a9b1..151b58915918f67fc00dc36096821cf201b36d71 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -20,7 +20,7 @@
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
-#include "core/landmark/landmark_point.h"
+#include "core/landmark/landmark.h"
#include "core/state_block/state_block_derived.h"
#include "feature_range_bearing.h"
#include "factor_range_bearing.h"
@@ -74,20 +74,20 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
double bearing = capture_rb->getBearing(i);
// 4. create or recover landmark
- LandmarkPoint2dPtr lmk;
- auto lmk_it = known_lmks.find(id);
+ Landmark2dPtr lmk;
+ auto lmk_it = known_lmks.find(id);
if (lmk_it != known_lmks.end())
{
// known landmarks : recover landmark
- lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second);
+ lmk = std::static_pointer_cast<Landmark2d>(lmk_it->second);
WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
}
else
{
// new landmark:
// - create landmark
- lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(),
- std::make_shared<StatePoint2d>(invObserve(range, bearing)));
+ lmk = LandmarkBase::emplace<Landmark2d>(
+ getProblem()->getMap(), VectorComposite{{'P', invObserve(range, bearing)}}, PriorComposite());
lmk->setId(id);
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
// - add to known landmarks
diff --git a/demos/hello_wolf/schema/SensorRangeBearing.schema b/demos/hello_wolf/schema/SensorRangeBearing.schema
index 1d510289843e258df3be307754fb0b66ff203a2a..47b551f304b23d7687ad46cc741d8fd2f1a53a5d 100644
--- a/demos/hello_wolf/schema/SensorRangeBearing.schema
+++ b/demos/hello_wolf/schema/SensorRangeBearing.schema
@@ -11,12 +11,7 @@ noise_bearing_degrees_std:
_doc: Standard deviation of the noise of the bearing measurements (degrees)
states:
- keys:
- _value: PO
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
P:
- follow: SpecStateSensorP2d.schema
+ follow: StateSensorP2d.schema
O:
- follow: SpecStateSensorO2d.schema
\ No newline at end of file
+ follow: StateSensorO2d.schema
\ No newline at end of file
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 68b6aca899b87ec70c746fc9aaea17d6377e4b67..9c4dc62b50a204f31dc83a5371f1856a453f43be 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -25,7 +25,7 @@ namespace wolf
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params)
- : SensorBase("SensorRangeBearing", 2, _params),
+ : SensorBase("SensorRangeBearing", 2, {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
noise_range_metres_std_(_params["noise_range_metres_std"].as<double>()),
noise_bearing_degrees_std_(_params["noise_bearing_degrees_std"].as<double>())
{
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 267616859284e3eef37fac4f2f3fae0e944a2259..fe37ac54744548004e051742803e3f8b0dc9d151 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -5,13 +5,15 @@ problem:
first_frame:
P:
- mode: "factor"
- state: [0,0]
- noise_std: [0.31, 0.31]
+ value: [0,0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31]
O:
- mode: "factor"
- state: [0]
- noise_std: [0.31]
+ value: [0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31]
tree_manager:
type: "none"
diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 8a7a482aed9e19550178cce9e13392719adf43c8..57cad205f2416d884e82ba3ea1ace1258421099d 100644
--- a/demos/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -9,10 +9,12 @@ apply_loss_function: true
states:
P:
- mode: fix
- state: [0,0]
+ prior:
+ mode: fix
+ value: [0,0]
dynamic: false
O:
- mode: fix
- state: [0]
+ prior:
+ mode: fix
+ value: [0]
dynamic: false
\ No newline at end of file
diff --git a/demos/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
index bf221a21af8db1a1fcb256c85d6020981de728ea..5399570ac56e4fdb2831de5080768688098675bc 100644
--- a/demos/hello_wolf/yaml/sensor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -6,10 +6,12 @@ apply_loss_function: true
states:
P:
- mode: fix
- state: [1,1]
+ prior:
+ mode: fix
+ value: [1,1]
dynamic: false
O:
- mode: fix
- state: [0]
+ prior:
+ mode: fix
+ value: [0]
dynamic: false
\ No newline at end of file
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 4e03a810e2077dde864d19c737179ce37ba94807..da101c346b8a3dbf4a94342c76a76672dc09c056 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -56,17 +56,34 @@ class CaptureBase : public NodeStateBlocks
void setProblem(ProblemPtr _problem) override final;
public:
- CaptureBase(const std::string& _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr = nullptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
+ /** \brief Constructor
+ *
+ * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived type name
+ * \param _ts TimeStamp of the capture.
+ * \param _sensor_ptr Pointer to the sensor that produced the capture. If unknown: nullptr.
+ * \param _state_types Composite of types of the states that the capture can have. Probably to be hardcoded in the
+ * derived class constructor.
+ * \param _state_vectors Composite of vectors of the states of the capture. A 'StateBlock' for each key in
+ * '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are
+ * required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
+ * \param _state_priors Composite of priors (initial_guess, factor or fixed) of the states of the capture. Not all
+ * keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in
+ * '_state_vectors'.
+ *
+ **/
+ CaptureBase(const std::string& _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr = nullptr,
+ const TypeComposite& _state_types = {},
+ const VectorComposite& _state_vectors = {},
+ const PriorComposite& _state_priors = {});
~CaptureBase() override = default;
void remove(bool viral_remove_parent_without_children = false) override;
bool hasChildren() const override;
+ virtual void emplaceDrifts(CaptureBasePtr _capture_origin);
+
// Type
virtual bool isMotion() const
{
@@ -166,6 +183,7 @@ template <typename classType, typename... T>
std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
{
std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...);
+ cpt->emplacePriors();
cpt->link(_frm_ptr);
return cpt;
}
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index a8687ed63376ef774468337e5d1bd6362d35ea96..10a1d7d18f93b7d851a9cb491a629b2353dd6e92 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -43,9 +43,8 @@ class CaptureDiffDrive : public CaptureMotion
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
+ const VectorComposite& _state_vectors = {},
+ const PriorComposite& _state_priors = {});
~CaptureDiffDrive() override = default;
};
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
index e054e2abf51f01bf8d3af971d3e7a632328edb75..83a745b096d3fe145e295ab4b1c1a78859c0c75e 100644
--- a/include/core/capture/capture_landmarks_external.h
+++ b/include/core/capture/capture_landmarks_external.h
@@ -28,6 +28,7 @@ namespace wolf
struct LandmarkDetection
{
int id; // id of landmark
+ int type; // type of landmark
Eigen::VectorXd measure; // either pose or position
Eigen::MatrixXd covariance; // covariance of the measure
double quality; // [0, 1] quality of the detection
@@ -45,6 +46,7 @@ class CaptureLandmarksExternal : public CaptureBase
CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const std::vector<int>& _ids = {},
+ const std::vector<int>& _types = {},
const std::vector<Eigen::VectorXd>& _detections = {},
const std::vector<Eigen::MatrixXd>& _covs = {},
const std::vector<double>& _qualities = {});
@@ -57,6 +59,7 @@ class CaptureLandmarksExternal : public CaptureBase
};
void addDetection(const int& _id,
+ const int& _type,
const Eigen::VectorXd& _detection,
const Eigen::MatrixXd& _cov,
const double& quality);
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 0db9350f350f71df5be7aabcede379e2451acd26..0244ff76f9bc91fd97f6eccf01a6b2d4ac8acb9c 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -54,14 +54,15 @@ WOLF_PTR_TYPEDEFS(CaptureMotion);
class CaptureMotion : public CaptureBase
{
- // public interface:
-
public:
CaptureMotion(const std::string& _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr);
+ CaptureBasePtr _capture_origin_ptr,
+ const TypeComposite& _state_types = {},
+ const VectorComposite& _state_vectors = {},
+ const PriorComposite& _state_priors = {});
CaptureMotion(const std::string& _type,
const TimeStamp& _ts,
@@ -69,9 +70,9 @@ class CaptureMotion : public CaptureBase
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
+ const TypeComposite& _state_types = {},
+ const VectorComposite& _state_vectors = {},
+ const PriorComposite& _state_priors = {});
~CaptureMotion() override;
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 9a2d868f4ed7e2cd37d3ffaba4d5f3a47ee5548f..973cefc1cd21c42518ec09cd8a88ded856a32060 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -309,11 +309,13 @@ inline Factory<TypeBase, TypeInput...>::~Factory()
template <class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
{
+ if (get().callbacks_.count(_type) == 1 and get().callbacks_.at(_type) != createFn)
+ throw std::runtime_error(get().getClass() + " : For the type \"" + _type +
+ "\": Trying to register a creator but there is a different creator registered.");
+
bool reg = get().callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
+
if (reg) std::cout << std::setw(31) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
- // else
- // std::cout << std::setw(31) << std::left << get().getClass() << " X-- skipping " << _type
- // << ": already registered." << std::endl;
return reg;
}
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index cf6e74971927db9f640e396bdcec44ed9f323ecb..7b84c45ba4b5914d484c3b077c438791391f0a83 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -50,7 +50,7 @@ namespace wolf
* follow:
* - "SensorCamera"
* - "SensorLaser2d"
- * - "LandmarkPoint3d"
+ * - "Landmark3d"
* - "ProcessorLaser2d"
*
* please refer to each base class derived from NodeLinked for better examples of their types.
diff --git a/include/core/common/node_state_blocks.h b/include/core/common/node_state_blocks.h
index fece5bd183058e5e9c4fd20a2bd043833b2d7b3d..1980b46532fbe2d50723ebd72ab5324890f34fcd 100644
--- a/include/core/common/node_state_blocks.h
+++ b/include/core/common/node_state_blocks.h
@@ -22,8 +22,9 @@
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/composite/type_composite.h"
#include "core/composite/vector_composite.h"
-#include "core/composite/spec_state_composite.h"
+#include "core/composite/prior_composite.h"
#include <map>
@@ -38,11 +39,31 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
friend FactorBase;
public:
- NodeStateBlocks(const std::string& _category, const std::string& _type, const SpecStateComposite& _specs = {});
+ /** \brief Constructor
+ *
+ * \param _category TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: category name (see node_base.h)
+ * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived type name
+ * \param _state_types Composite of types of the states that the node can have. Probably to be hardcoded in the
+ * derived class constructor.
+ * \param _state_vectors Composite of vectors of the states of the node. A 'StateBlock' for each key in
+ * '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are
+ * required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
+ * \param _state_priors Composite of priors (initial_guess, factor or fixed) of the states of the node. Not all
+ * keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in
+ * '_state_vectors'. If not specified, no prior is assumed, i.e. 'initial_guess'.
+ *
+ **/
+ NodeStateBlocks(const std::string& _category,
+ const std::string& _type,
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors);
virtual ~NodeStateBlocks();
+ virtual YAML::Node toYaml() const;
+
StateKeys getKeys() const;
- SpecStateComposite getSpecs() const;
+ TypeComposite getStateTypes() const;
bool has(const char& _sb_key) const;
bool has(const std::string& _keys) const;
virtual unsigned int id() const = 0;
@@ -64,20 +85,25 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
virtual void unfix();
virtual bool isFixed() const;
- void emplaceStateBlocks(const SpecStateComposite& _specs);
- void emplaceStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors, bool _fix);
-
- // Emplace derived state blocks (angle, quaternion, etc).
- template <typename SB, typename... Args>
- std::shared_ptr<SB> emplaceStateBlock(const char& _sb_key, Args&&... _args_of_derived_state_block_constructor);
-
- virtual StateBlockPtr addStateBlock(const char& _sb_key, const StateBlockPtr& _sb);
- virtual unsigned int removeStateBlock(const char& _sb_key);
- bool hasStateBlock(const StateBlockConstPtr& _sb) const;
- bool setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
- bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
- StateBlockConstPtr getStateBlock(const char& _sb_key) const;
- StateBlockPtr getStateBlock(const char& _sb_key);
+ /** \brief Emplace a state block
+ *
+ * \param _key Key of the new state.
+ * \param _type Types of the new state.
+ * \param _vectors Vector of the new state.
+ * \param _fixed If the new state is fixed (i.e. not estimated).
+ **/
+ StateBlockPtr emplaceStateBlock(const char key,
+ const std::string& _type,
+ const Eigen::VectorXd& _vector,
+ const bool& _fixed);
+
+ // virtual StateBlockPtr addStateBlock(const char& _sb_key, const StateBlockPtr& _sb);
+ virtual unsigned int removeStateBlock(const char& _sb_key);
+ bool hasStateBlock(const StateBlockConstPtr& _sb) const;
+ bool setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
+ bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
+ StateBlockConstPtr getStateBlock(const char& _sb_key) const;
+ StateBlockPtr getStateBlock(const char& _sb_key);
// Register/remove state blocks to/from wolf::Problem
void setProblem(ProblemPtr _problem) override;
@@ -105,7 +131,7 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0);
- void emplacePriors(const SpecStateComposite& _specs);
+ void emplacePriors();
FactorBaseConstPtrSet getFactoredBySet() const;
FactorBasePtrSet getFactoredBySet();
@@ -130,11 +156,11 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
const std::list<SizeEigen>& _sizes,
const bool _notify = true);
void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify = true);
- void setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify = true);
+ void setState(const StateKeys& _keys, const std::list<Eigen::VectorXd>& _vectors, const bool _notify = true);
- VectorXd getStateVector(const StateKeys& _keys) const;
- unsigned int getSize(const StateKeys& _keys = "") const;
- unsigned int getLocalSize(const StateKeys& _keys = "") const;
+ Eigen::VectorXd getStateVector(const StateKeys& _keys) const;
+ unsigned int getSize(const StateKeys& _keys = "") const;
+ unsigned int getLocalSize(const StateKeys& _keys = "") const;
// Covariance
bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
@@ -160,10 +186,12 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const;
- CheckLog checkStatesAndFactoredBy(bool _verbose, std::ostream& _stream, std::string _tabs) const;
+ CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const;
private:
- Composite<StateBlockPtr> state_block_composite_;
+ Composite<StateBlockPtr> state_blocks_;
+ PriorComposite state_priors_;
+ TypeComposite state_types_;
FactorBasePtrSet factored_by_set_;
FactorBasePtrSet factor_sensory_set_;
@@ -185,29 +213,34 @@ inline NodeStateBlocks::~NodeStateBlocks()
inline StateKeys NodeStateBlocks::getKeys() const
{
- return state_block_composite_.getKeys();
+ return state_blocks_.getKeys();
+}
+
+inline TypeComposite NodeStateBlocks::getStateTypes() const
+{
+ return state_types_;
}
inline bool NodeStateBlocks::has(const char& _sb_key) const
{
- return state_block_composite_.has(_sb_key);
+ return state_blocks_.has(_sb_key);
}
inline bool NodeStateBlocks::has(const std::string& _keys) const
{
- return state_block_composite_.has(_keys);
+ return state_blocks_.has(_keys);
}
inline std::map<char, StateBlockConstPtr> NodeStateBlocks::getStateBlockMap() const
{
std::map<char, StateBlockConstPtr> map_const;
- for (auto&& pair : state_block_composite_) map_const[pair.first] = pair.second;
+ for (auto&& pair : state_blocks_) map_const[pair.first] = pair.second;
return map_const;
}
inline std::map<char, StateBlockPtr> NodeStateBlocks::getStateBlockMap()
{
- return state_block_composite_;
+ return state_blocks_;
}
inline std::vector<StateBlockConstPtr> NodeStateBlocks::getStateBlockVec() const
@@ -226,44 +259,30 @@ inline std::vector<StateBlockPtr> NodeStateBlocks::getStateBlockVec()
inline unsigned int NodeStateBlocks::removeStateBlock(const char& _sb_key)
{
- return state_block_composite_.erase(_sb_key);
-}
-
-template <typename SB, typename... Args>
-inline std::shared_ptr<SB> NodeStateBlocks::emplaceStateBlock(const char& _sb_key,
- Args&&... _args_of_derived_state_block_constructor)
-{
- assert(state_block_composite_.count(_sb_key) == 0 and
- "Trying to add a state block with an existing type! Use setStateBlock instead.");
- std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
-
- addStateBlock(_sb_key, sb);
-
- return sb;
+ return state_blocks_.erase(_sb_key);
}
inline bool NodeStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
{
- assert(state_block_composite_.count(_sb_key) > 0 and
- "Cannot set an inexistent state block! Use addStateBlock instead.");
+ assert(state_blocks_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead.");
WOLF_WARN_COND(
factor_prior_map_.count(_sb_key),
"NodeStateBlocks::setStateBlock: this state block has an absolute factor, setting state but solver may "
"change it again...");
- state_block_composite_.at(_sb_key) = _sb;
+ state_blocks_.at(_sb_key) = _sb;
return true; // success
}
inline wolf::StateBlockConstPtr NodeStateBlocks::getStateBlock(const char& _sb_key) const
{
- auto it = state_block_composite_.find(_sb_key);
- return it != state_block_composite_.end() ? it->second : nullptr;
+ auto it = state_blocks_.find(_sb_key);
+ return it != state_blocks_.end() ? it->second : nullptr;
}
inline wolf::StateBlockPtr NodeStateBlocks::getStateBlock(const char& _sb_key)
{
- auto it = state_block_composite_.find(_sb_key);
- return it != state_block_composite_.end() ? it->second : nullptr;
+ auto it = state_blocks_.find(_sb_key);
+ return it != state_blocks_.end() ? it->second : nullptr;
}
inline wolf::StateBlockConstPtr NodeStateBlocks::getP() const
@@ -288,13 +307,13 @@ inline wolf::StateBlockPtr NodeStateBlocks::getO()
inline void NodeStateBlocks::fix()
{
- for (auto pair_key_sbp : state_block_composite_)
+ for (auto pair_key_sbp : state_blocks_)
if (pair_key_sbp.second != nullptr) pair_key_sbp.second->fix();
}
inline void NodeStateBlocks::unfix()
{
- for (auto pair_key_sbp : state_block_composite_)
+ for (auto pair_key_sbp : state_blocks_)
if (pair_key_sbp.second != nullptr) pair_key_sbp.second->unfix();
}
diff --git a/include/core/composite/composite.h b/include/core/composite/composite.h
index 5a35424c245f4a72bf2c8debb1e15756b08270d7..b3982180104ada586357e107cea2d93d7854f87d 100644
--- a/include/core/composite/composite.h
+++ b/include/core/composite/composite.h
@@ -41,12 +41,12 @@ class Composite : public map<char, T>
using CompositeType = T;
using map<char, T>::map;
- Composite(const YAML::Node& _n, std::string _keys = ALL_KEYS);
+ Composite(const YAML::Node& _n,
+ const std::string& _field,
+ const bool _optional,
+ const std::string& _keys = ALL_KEYS);
virtual ~Composite() = default;
- template <typename CompositeOther>
- CompositeOther cast() const;
-
bool has(char _key) const;
bool has(const StateKeys& _structure) const;
StateKeys getKeys() const;
@@ -55,7 +55,7 @@ class Composite : public map<char, T>
{
for (auto&& pair : _x)
{
- _os << pair.first << ": " << pair.second;
+ _os << pair.first << ": " << pair.second << std::endl;
}
return _os;
}
@@ -65,20 +65,13 @@ class Composite : public map<char, T>
YAML::Node toYaml() const;
};
-template <typename T>
-template <typename CompositeOther>
-inline CompositeOther Composite<T>::cast() const
-{
- CompositeOther casted_composite;
- for (auto&& pair : *this)
- {
- casted_composite.emplace(pair.first, static_cast<typename CompositeOther::CompositeType>(pair.second));
- }
- return casted_composite;
-}
+typedef Composite<bool> BoolComposite;
template <typename T>
-inline Composite<T>::Composite(const YAML::Node& _n, std::string _keys)
+inline Composite<T>::Composite(const YAML::Node& _n,
+ const std::string& _field,
+ const bool _required,
+ const std::string& _keys)
{
if (_n.IsDefined())
{
@@ -87,24 +80,18 @@ inline Composite<T>::Composite(const YAML::Node& _n, std::string _keys)
throw std::runtime_error("Composite: constructor with a non-map yaml node");
}
- // load _keys from node or input param
- if (_keys == ALL_KEYS and _n["keys"]) _keys = _n["keys"].as<std::string>();
-
// check existence of _keys in node
if (_keys != ALL_KEYS)
for (auto key : _keys)
- if (not _n[key])
+ if (not _n[key] and _required)
{
- throw std::runtime_error(std::string("Composite: In constructor. Missing state for key '") + key +
- "'.");
+ throw std::runtime_error(std::string("Composite ") + typeid(T).name() +
+ " constructor: Missing node for key '" + key + "'.");
}
// iterate node map pairs
for (auto node_pair : _n)
{
- // ignore "keys"
- if (node_pair.first.as<std::string>() == "keys") continue;
-
// check that keys are char type
char key;
try
@@ -113,7 +100,8 @@ inline Composite<T>::Composite(const YAML::Node& _n, std::string _keys)
}
catch (const std::exception& e)
{
- throw std::runtime_error("In Composite constructor: There is a key of type different than char: " +
+ throw std::runtime_error(std::string("In Composite ") + typeid(T).name() +
+ " constructor: There is a key of type different than char: " +
node_pair.first.as<std::string>() + ". Error: " + e.what());
}
@@ -122,17 +110,46 @@ inline Composite<T>::Composite(const YAML::Node& _n, std::string _keys)
_keys.find(key) == std::string::npos) //
continue;
- try
+ // check field
+ if (not _field.empty())
{
- this->emplace(key, node_pair.second.as<T>());
+ if (node_pair.second[_field])
+ {
+ try
+ {
+ this->emplace(key, node_pair.second[_field].as<T>());
+ }
+ catch (const std::exception& e)
+ {
+ YAML::Emitter em;
+ em << node_pair.second[_field];
+ throw std::runtime_error("In Composite constructor: Failed to emplace the entry " + _field +
+ " for key '" + key + "' with error: " + e.what() + "\nNode:\n" +
+ em.c_str());
+ }
+ }
+ else if (_required)
+ {
+ YAML::Emitter em;
+ em << node_pair.second;
+ throw std::runtime_error("In Composite constructor: Missing mandatory field '" + _field +
+ "' for key '" + key + "'\nNode:\n" + em.c_str());
+ }
}
- catch (const std::exception& e)
+ else
{
- YAML::Emitter em;
- em << node_pair.second;
- throw std::runtime_error(
- std::string("Composite: In constructor. Failed to emplace the entry for key '") + key +
- "' with error: " + e.what() + "\nNode:\n" + em.c_str());
+ try
+ {
+ this->emplace(key, node_pair.second.as<T>());
+ }
+ catch (const std::exception& e)
+ {
+ YAML::Emitter em;
+ em << node_pair.second;
+ throw std::runtime_error("In Composite constructor: Failed to emplace the entry for key '" +
+ std::string(1, key) + "' with error: " + e.what() + "\nNode:\n" +
+ em.c_str());
+ }
}
}
}
diff --git a/include/core/composite/matrix_composite.h b/include/core/composite/matrix_composite.h
deleted file mode 100644
index b3c33a5702e06d67318c9b3b3348e8905962a406..0000000000000000000000000000000000000000
--- a/include/core/composite/matrix_composite.h
+++ /dev/null
@@ -1,213 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include "core/common/wolf.h"
-#include "core/composite/vector_composite.h"
-
-#include <unordered_map>
-#include <iostream>
-
-namespace wolf
-{
-class MatrixComposite : public std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >
-{
- private:
- std::unordered_map<char, unsigned int> size_rows_, size_cols_;
- unsigned int rows() const;
- unsigned int cols() const;
- void sizeAndIndices(const StateKeys& _struct_rows,
- const StateKeys& _struct_cols,
- std::unordered_map<char, unsigned int>& _indices_rows,
- std::unordered_map<char, unsigned int>& _indices_cols,
- unsigned int& _rows,
- unsigned int& _cols) const;
-
- public:
- MatrixComposite(){};
- MatrixComposite(const StateKeys& _row_structure, const StateKeys& _col_structure);
- MatrixComposite(const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes);
-
- /**
- * \brief Construct from Eigen::VectorXd and structure
- *
- * Usage:
- *
- * VectorXd vec_eigen(1,2,3,4,5);
- *
- * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
- *
- * result:
- *
- * vec_comp["a"].transpose(); // = (1,2);
- * vec_comp["b"].transpose(); // = (3,4,5);
- */
- MatrixComposite(const MatrixXd& _m,
- const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes);
- ~MatrixComposite() = default;
-
- bool check() const;
- static MatrixComposite zero(const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes);
-
- static MatrixComposite identity(const StateKeys& _structure, const std::list<int>& _sizes);
-
- unsigned int count(const char& _row, const char& _col) const;
-
- bool emplace(const char& _row, const char& _col, const Eigen::MatrixXd& _mat_blk);
-
- // throw error if queried block is not present
- bool at(const char& _row, const char& _col, Eigen::MatrixXd& _mat_blk) const;
- const MatrixXd& at(const char& _row, const char& _col) const;
- MatrixXd& at(const char& _row, const char& _col);
-
- // make some shadowed API available
- using std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >::at;
- using std::unordered_map<char, std::unordered_map<char, Eigen::MatrixXd> >::count;
-
- MatrixXd matrix(const StateKeys& _struct_rows, const StateKeys& _struct_cols) const;
-
- MatrixComposite operator*(double scalar_right) const;
- MatrixComposite operator+(const MatrixComposite& _second) const;
- MatrixComposite operator-(const MatrixComposite& _second) const;
- MatrixComposite operator-(void) const;
-
- /**
- * \brief Matrix product
- *
- * This function computes the matrix product M * N
- *
- * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
- * both in their structure and their individual sizes.
- *
- * For example: Let us call 'this' matrix with the name 'M'.
- *
- * The matrix 'M' maps the "PO" space into a new space "VW":
- * M["V"]["P"] M["V"]["O"]
- * M["W"]["P"] M["W"]["O"]
- *
- * The second matrix N has blocks "PO" in vertical arrangement,
- * and "XY" in horizontal arrangement:
- * N["P"]["X"] N["P"]["Y"]
- * N["O"]["X"] N["O"]["Y"]
- *
- * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
- *
- * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
- * S["V"]["X"] S["V"]["Y"]
- * S["W"]["X"] S["W"]["Y"]
- */
- MatrixComposite operator*(const MatrixComposite& _second) const;
-
- /**
- * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
- *
- * This function computes the product J * Q * J.transpose.
- *
- * It considers that this is a Jacobian matrix, and that the provided matrix
- * is a proper covariance (e.g. symmmetric and semi-positive).
- * It also considers that the Jacobian blocks are compatible with the blocks
- * of the provided covariance, both in their structure and their individual sizes.
- *
- * If these conditions are not satisfied, the product will fail and throw,
- * or give aberrant results at best.
- *
- * -----
- *
- * For example: Let us call 'this' a Jacobian matrix with the name 'J'.
- *
- * This Jacobian 'J' maps the "PO" blocks into "VW":
- *
- * J["V"]["P"] J["V"]["O"]
- * J["W"]["P"] J["W"]["O"]
- *
- * The provided matrix is a covariances matrix Q.
- * Q has blocks "P", "O" in both vertical and horizontal arrangements:
- *
- * Q["P"]["P"] Q["P"]["O"]
- * Q["O"]["P"] Q["O"]["O"]
- *
- * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
- *
- * Now, upon a call to
- *
- * MatrixComposite S = J.propagate(Q);
- *
- * the result is a covariances matrix S with blocks "V" and "W"
- *
- * S["V"]["V"] S["V"]["W"]
- * S["W"]["V"] S["W"]["W"]
- */
- MatrixComposite propagate(const MatrixComposite& _Cov); // This performs this * _Cov * this.tr
-
- /**
- * \brief Matrix-vector product
- *
- * This function computes the matrix product M * N
- *
- * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
- * both in their structure and their individual sizes.
- *
- * For example: Let us call 'this' matrix with the name 'M'.
- *
- * The matrix 'M' maps the "PO" space into a new space "VW":
- * M["V"]["P"] M["V"]["O"]
- * M["W"]["P"] M["W"]["O"]
- *
- * The second vector V has blocks "PO" in vertical arrangement,
- * V["P"]
- * V["O"]
- *
- * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
- *
- * Then, the result is a vector S = M * V with block arrangement "VW"
- * S["V"]
- * S["W"]
- */
- VectorComposite operator*(const VectorComposite& _second) const;
-
- friend std::ostream& operator<<(std::ostream& _os, const MatrixComposite& _M);
-
- friend MatrixComposite operator*(double scalar_left, const MatrixComposite& M);
-};
-
-//////////// IMPLEMENTATION ////////////
-
-inline unsigned int MatrixComposite::count(const char& _row, const char& _col) const
-{
- const auto& rit = this->find(_row);
-
- if (rit == this->end())
- return 0;
-
- else
- return rit->second.count(_col);
-}
-
-} // namespace wolf
diff --git a/include/core/composite/prior_composite.h b/include/core/composite/prior_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..63742f486632e7c3dba03dff4d29a16de152ad59
--- /dev/null
+++ b/include/core/composite/prior_composite.h
@@ -0,0 +1,105 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include "core/composite/composite.h"
+
+#include <string>
+#include <eigen3/Eigen/Dense>
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+class Prior
+{
+ protected:
+ std::string prior_mode_; // Prior mode can be 'initial_guess', 'fix' or 'factor'
+ Eigen::VectorXd factor_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix
+ // (ONLY IF prior_mode_ == 'factor')
+
+ public:
+ Prior() = default;
+ Prior(const std::string& _prior_mode, const Eigen::VectorXd& _factor_std = Eigen::VectorXd(0));
+ Prior(const YAML::Node& _n);
+
+ virtual ~Prior() = default;
+
+ const std::string& getPriorMode() const;
+ const Eigen::VectorXd& getFactorStd() const;
+ bool isInitialGuess() const;
+ bool isFixed() const;
+ bool isFactor() const;
+
+ virtual void check() const;
+
+ friend std::ostream& operator<<(std::ostream& _os, const wolf::Prior& _x);
+
+ YAML::Node toYaml() const;
+};
+
+typedef Composite<Prior> PriorComposite;
+
+inline const std::string& Prior::getPriorMode() const
+{
+ return prior_mode_;
+}
+
+inline const Eigen::VectorXd& Prior::getFactorStd() const
+{
+ return factor_std_;
+}
+
+inline bool Prior::isInitialGuess() const
+{
+ return prior_mode_ == "initial_guess";
+}
+
+inline bool Prior::isFixed() const
+{
+ return prior_mode_ == "fix";
+}
+
+inline bool Prior::isFactor() const
+{
+ return prior_mode_ == "factor";
+}
+
+} // namespace wolf
+
+// CONVERSION TO YAML
+namespace YAML
+{
+template <>
+struct convert<wolf::Prior>
+{
+ static Node encode(const wolf::Prior& spec)
+ {
+ return spec.toYaml();
+ }
+
+ static bool decode(const Node& node, wolf::Prior& spec)
+ {
+ spec = wolf::Prior(node);
+
+ return true;
+ }
+};
+} // namespace YAML
diff --git a/include/core/composite/spec_state_composite.h b/include/core/composite/spec_state_composite.h
deleted file mode 100644
index 6d7e4267bb51b952015bc9a5c5dee494e0c5bb21..0000000000000000000000000000000000000000
--- a/include/core/composite/spec_state_composite.h
+++ /dev/null
@@ -1,189 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include "core/composite/composite.h"
-#include "core/composite/type_composite.h"
-#include "core/composite/vector_composite.h"
-
-#include <string>
-#include <eigen3/Eigen/Dense>
-#include "yaml-cpp/yaml.h"
-
-namespace wolf
-{
-class SpecState
-{
- protected:
- std::string type_; // State type
- Eigen::VectorXd state_; // state values
- std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
- Eigen::VectorXd
- noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
-
- public:
- SpecState() = default;
- SpecState(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode,
- const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
-
- SpecState(const YAML::Node& _n);
-
- virtual ~SpecState() = default;
-
- const std::string& getType() const;
- const std::string& getMode() const;
- const Eigen::VectorXd& getState() const;
- const Eigen::VectorXd& getNoiseStd() const;
- bool isFixed() const;
- bool isInitialGuess() const;
- bool isFactor() const;
-
- virtual void check() const;
-
- virtual std::string print(const std::string& _spaces = "") const;
- friend std::ostream& operator<<(std::ostream& _os, const wolf::SpecState& _x);
-
- YAML::Node toYaml() const;
-};
-
-template <typename T>
-class SpecBaseComposite : public Composite<T>
-{
- public:
- using Composite<T>::Composite;
- SpecBaseComposite(const TypeComposite& _types, const VectorComposite& _vectors, bool _fix);
- virtual ~SpecBaseComposite() = default;
-
- VectorComposite getVectorComposite() const;
- TypeComposite getTypeComposite() const;
-};
-
-typedef SpecBaseComposite<SpecState> SpecStateComposite;
-
-template <typename T>
-inline SpecBaseComposite<T>::SpecBaseComposite(const TypeComposite& _types, const VectorComposite& _vectors, bool _fix)
-{
- if (not _types.has(_vectors.getKeys()) or not _vectors.has(_types.getKeys()))
- {
- throw std::runtime_error(
- "SpecBaseComposite<T>: in constructor, _types and _vectors don't have the same structure");
- }
-
- // join types and vectors into specs of template type T
- for (auto type_pair : _types)
- {
- this->emplace(type_pair.first,
- T(type_pair.second, _vectors.at(type_pair.first), _fix ? "fix" : "initial_guess"));
- }
-}
-
-template <>
-inline VectorComposite SpecBaseComposite<SpecState>::getVectorComposite() const
-{
- VectorComposite states;
- for (auto&& pair : *this)
- {
- states.emplace(pair.first, pair.second.getState());
- }
- return states;
-}
-
-template <typename T>
-inline VectorComposite SpecBaseComposite<T>::getVectorComposite() const
-{
- throw std::runtime_error("Impossible to build a VectorComposite from this Composite");
-}
-
-template <>
-inline TypeComposite SpecBaseComposite<SpecState>::getTypeComposite() const
-{
- TypeComposite types;
- for (auto&& pair : *this)
- {
- types.emplace(pair.first, pair.second.getType());
- }
- return types;
-}
-
-template <typename T>
-inline TypeComposite SpecBaseComposite<T>::getTypeComposite() const
-{
- throw std::runtime_error("Impossible to build a TypeComposite from this Composite");
-}
-
-inline const std::string& SpecState::getType() const
-{
- return type_;
-}
-
-inline const std::string& SpecState::getMode() const
-{
- return mode_;
-}
-
-inline const Eigen::VectorXd& SpecState::getState() const
-{
- return state_;
-}
-
-inline const Eigen::VectorXd& SpecState::getNoiseStd() const
-{
- return noise_std_;
-}
-
-inline bool SpecState::isFixed() const
-{
- return mode_ == "fix";
-}
-
-inline bool SpecState::isInitialGuess() const
-{
- return mode_ == "initial_guess";
-}
-
-inline bool SpecState::isFactor() const
-{
- return mode_ == "factor";
-}
-
-} // namespace wolf
-
-// CONVERSION TO YAML
-namespace YAML
-{
-template <>
-struct convert<wolf::SpecState>
-{
- static Node encode(const wolf::SpecState& spec)
- {
- return spec.toYaml();
- }
-
- static bool decode(const Node& node, wolf::SpecState& spec)
- {
- spec = wolf::SpecState(node);
-
- return true;
- }
-};
-} // namespace YAML
diff --git a/include/core/composite/spec_state_sensor_composite.h b/include/core/composite/spec_state_sensor_composite.h
deleted file mode 100644
index a5a513591ce8e91c84b86f88a7b2f41f4f441866..0000000000000000000000000000000000000000
--- a/include/core/composite/spec_state_sensor_composite.h
+++ /dev/null
@@ -1,90 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include "core/composite/spec_state_composite.h"
-
-namespace wolf
-{
-class SpecStateSensor : public SpecState
-{
- protected:
- bool dynamic_; // State dynamic
- Eigen::VectorXd
- drift_std_; // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
-
- public:
- SpecStateSensor() = default;
- SpecStateSensor(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode,
- const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
- bool _dynamic = false,
- const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
-
- SpecStateSensor(const YAML::Node& _n);
-
- virtual ~SpecStateSensor() = default;
-
- bool isDynamic() const;
- const Eigen::VectorXd& getDriftStd() const;
-
- void check() const override;
-
- std::string print(const std::string& _spaces = "") const override;
- friend std::ostream& operator<<(std::ostream& _os, const wolf::SpecStateSensor& _x);
-
- YAML::Node toYaml() const;
-};
-
-typedef Composite<SpecStateSensor> SpecStateSensorComposite;
-
-inline bool SpecStateSensor::isDynamic() const
-{
- return dynamic_;
-}
-
-inline const Eigen::VectorXd& SpecStateSensor::getDriftStd() const
-{
- return drift_std_;
-}
-
-} // namespace wolf
-
-// CONVERSION TO YAML
-namespace YAML
-{
-template <>
-struct convert<wolf::SpecStateSensor>
-{
- static Node encode(const wolf::SpecStateSensor& spec)
- {
- return spec.toYaml();
- }
-
- static bool decode(const Node& node, wolf::SpecStateSensor& spec)
- {
- spec = wolf::SpecStateSensor(node);
-
- return true;
- }
-};
-} // namespace YAML
diff --git a/include/core/composite/vector_composite.h b/include/core/composite/vector_composite.h
index 46e53dd981d85062fa805be21e5be420738b7ae8..0f357cbcc154d93aca2c81e6bbd8a65383a6ccaf 100644
--- a/include/core/composite/vector_composite.h
+++ b/include/core/composite/vector_composite.h
@@ -23,61 +23,16 @@
#include "core/common/wolf.h"
#include "core/composite/composite.h"
-#include "yaml-schema-cpp/yaml_conversion.hpp"
-
-#include <unordered_map>
-#include <iostream>
-
namespace wolf
{
-using namespace Eigen;
-
class VectorComposite : public Composite<Eigen::VectorXd>
{
public:
using Composite<Eigen::VectorXd>::Composite;
-
- /**
- * \brief Construct from Eigen::VectorXd and keys
- *
- * Usage:
- *
- * VectorXd vec_eigen(1,2,3,4,5);
- *
- * VectorComposite vec_comp("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !!
- *
- * result:
- *
- * vec_comp["a"].transpose(); // = (1,2);
- * vec_comp["b"].transpose(); // = (3,4,5);
- */
- VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
- VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
+ VectorComposite(const Composite<Eigen::VectorXd>& _composite);
Eigen::VectorXd vector(const StateKeys& _keys) const;
- /**
- * \brief set from Eigen::VectorXd and keys
- *
- * Usage:
- *
- * Eigen::VectorXd vec_eigen;
- * wolf::VectorComposite vec_comp;
- *
- * vec_comp.set("ab", vec_eigen, {2,3} ); // vec_eigen must be at least size 5 !!
- *
- * result:
- *
- * vec_comp["a"].transpose(); // = (1,2);
- * vec_comp["b"].transpose(); // = (3,4,5);
- */
- void set(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
- void set(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
-
- void setZero();
-
- VectorComposite operator()(const std::string& _keys) const;
-
friend std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x);
};
diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
index 7e7ced2213407507c46034b7e3b77ede835c7c8f..ea2e3a0f54b55f5e77b5000b477921f7e2899323 100644
--- a/include/core/factor/factor_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.h
@@ -25,7 +25,7 @@
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
-//#include "ceres/jet.h"
+// #include "ceres/jet.h"
namespace wolf
{
@@ -113,9 +113,9 @@ inline Eigen::Vector6d FactorPose3dWithExtrinsics::error() const
Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
Eigen::Quaterniond w_q_m(getMeasurement().data() + 3); // measurements
- Vector6d err;
- Eigen::Map<Vector3d> p_err(err.data() + 0);
- Eigen::Map<Vector3d> o_err(err.data() + 3);
+ Eigen::Vector6d err;
+ Eigen::Map<Eigen::Vector3d> p_err(err.data() + 0);
+ Eigen::Map<Eigen::Vector3d> o_err(err.data() + 3);
error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm, w_q_m, p_err, o_err);
@@ -139,9 +139,9 @@ inline bool FactorPose3dWithExtrinsics::operator()(const T* const _p,
Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements
Eigen::Quaterniond w_q_m(getMeasurement().data() + 3); // measurements
- Eigen::Matrix<T, 6, 1> err; // error
- Eigen::Map<Matrix<T, 3, 1> > p_err(err.data() + 0);
- Eigen::Map<Matrix<T, 3, 1> > o_err(err.data() + 3);
+ Eigen::Matrix<T, 6, 1> err; // error
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > p_err(err.data() + 0);
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > o_err(err.data() + 3);
error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm.cast<T>(), w_q_m.cast<T>(), p_err, o_err);
// Residuals
diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
index d66cd8d5e574e1014b69eec5203006a81120c4e8..4bb48fc1245d8f9681c117daf1078146eb8a9f13 100644
--- a/include/core/factor/factor_velocity_local_direction_3d.h
+++ b/include/core/factor/factor_velocity_local_direction_3d.h
@@ -94,7 +94,7 @@ FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(
// measurement: elevation & azimuth (w.r.t. selected plane)
measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_), orthogonal_axis_);
- measurement_sqrt_information_upper_ = Matrix2d::Identity() * measurement_sqrt_information_upper_(0, 0);
+ measurement_sqrt_information_upper_ = Eigen::Matrix2d::Identity() * measurement_sqrt_information_upper_(0, 0);
}
template <typename T>
diff --git a/include/core/feature/feature_landmark_external.h b/include/core/feature/feature_landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..893d28511c3a27514e2f35b76d89df74662ab4e3
--- /dev/null
+++ b/include/core/feature/feature_landmark_external.h
@@ -0,0 +1,61 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+// Wolf includes
+#include "core/feature/feature_base.h"
+
+// std includes
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(FeatureLandmarkExternal);
+
+// class FeatureLandmarkExternal
+class FeatureLandmarkExternal : public FeatureBase
+{
+ protected:
+ int external_id_; ///< The id of the landmark assigned by an external classifier
+ int external_type_; ///< The type of the landmark assigned by an external classifier
+
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _external_id the id assigned by an external tracker (-1 for not tracked)
+ * \param _external_type the type index of the landmark detection (-1 for not classified)
+ *
+ */
+ FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_covariance,
+ const int& _external_id = -1,
+ const int& _external_type = -1);
+
+ ~FeatureLandmarkExternal() override;
+
+ int getExternalType() const;
+ void setExternalType(const int& _external_type);
+ int getExternalId() const;
+ void setExternalId(const int& _external_id);
+};
+
+} // namespace wolf
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 0c9902109070ca5df35a9af53fc926c201d601b5..96fa788f55d0b458a133384483188508c0a00919 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -54,35 +54,18 @@ class FrameBase : public NodeStateBlocks
TimeStamp time_stamp_; ///< frame time stamp
public:
- /** \brief Constructor with type, time stamp and state pointer
- *
- * Constructor with type, time stamp and state pointer
- * \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _p_ptr StateBlock pointer to the position (default: nullptr)
- * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
- **/
- FrameBase(const TimeStamp& _ts,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _v_ptr = nullptr);
-
- /** \brief Constructor time stamp and specs composite
- *
- * Constructor with time stamp and specs composite
- * \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _frame_specs SpecStateComposite containing all information needed to create the state blocs.
- **/
- FrameBase(const TimeStamp& _ts, const SpecStateComposite& _frame_specs);
-
/** \brief Constructor time stamp and specs composite
*
- * Constructor with time stamp and specs composite
+ * Constructor with time stamp and specs (types, vectors, priors)
* \param _ts is the time stamp associated to this frame, provided in seconds
* \param _frame_types TypeComposite containing the types of the state blocs.
* \param _frame_vectors VectorComposite containing the vectors of the the state blocs.
+ * \param _frame_priors PriorComposite containing the priors of the the state blocs.
**/
- FrameBase(const TimeStamp& _ts, const TypeComposite& _frame_types, const VectorComposite& _frame_vectors);
+ FrameBase(const TimeStamp& _ts,
+ const TypeComposite& _frame_types,
+ const VectorComposite& _frame_vectors,
+ const PriorComposite& _frame_priors = {});
~FrameBase() override = default;
@@ -207,6 +190,7 @@ template <typename classType, typename... T>
std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
{
std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
+ frm->emplacePriors();
frm->link(_ptr);
return frm;
}
diff --git a/include/core/landmark/landmark_point.h b/include/core/landmark/landmark.h
similarity index 52%
rename from include/core/landmark/landmark_point.h
rename to include/core/landmark/landmark.h
index 99968e2a9e34dec6255601aabd5a6c21498f661f..2d0da68881f32615a418bb7cda47c7b872e72372 100644
--- a/include/core/landmark/landmark_point.h
+++ b/include/core/landmark/landmark.h
@@ -26,68 +26,66 @@
namespace wolf
{
template <unsigned int DIM>
-class LandmarkPoint : public LandmarkBase
+class Landmark : public LandmarkBase
{
public:
- /** \brief Constructor stateblock
- *
- * Constructor with stateblock
- * \param _state_p StateBlock of the point
- **/
- LandmarkPoint(StateBlockPtr _state_p);
-
/** \brief Constructor specs composite
*
* Constructor with specs composite
- * \param _state_specs SpecStateComposite containing all information needed to create the state blocs.
+ * \param _state_vectors Composite of keys and vectors of the landmark states.
+ * All keys ('P' and 'O') are required.
+ * \param _state_priors Composite of keys and priors (initial_guess, factor or fixed) of the landmark states.
+ * Not all the keys are required. If not specified, no prior is assumed, i.e. 'initial_guess'.
**/
- LandmarkPoint(const SpecStateComposite& _state_specs);
+ Landmark(const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors = {},
+ const int _external_id = -1,
+ const int _external_type = -1);
/** \brief Constructor with YAML node (to be used by the derived classes YAML node constructors)
*
* Constructor with YAML node
* \param _n YAML node containing the necessary information
**/
- LandmarkPoint(const YAML::Node& _n);
+ Landmark(const YAML::Node& _n);
- WOLF_LANDMARK_TEMPLATE_DIM_CREATE(LandmarkPoint, DIM);
+ WOLF_LANDMARK_TEMPLATE_DIM_CREATE(Landmark, DIM);
- ~LandmarkPoint() override = default;
+ ~Landmark() override = default;
};
-typedef LandmarkPoint<2> LandmarkPoint2d;
-typedef LandmarkPoint<3> LandmarkPoint3d;
+typedef Landmark<2> Landmark2d;
+typedef Landmark<3> Landmark3d;
-WOLF_DECLARED_PTR_TYPEDEFS(LandmarkPoint2d);
-WOLF_DECLARED_PTR_TYPEDEFS(LandmarkPoint3d);
+WOLF_DECLARED_PTR_TYPEDEFS(Landmark2d);
+WOLF_DECLARED_PTR_TYPEDEFS(Landmark3d);
////////////////////////////////////////////////////////////////////////////////////////////////
// IMPLEMENTATION //
////////////////////////////////////////////////////////////////////////////////////////////////
template <unsigned int DIM>
-inline LandmarkPoint<DIM>::LandmarkPoint(StateBlockPtr _state_p)
- : LandmarkBase("LandmarkPoint" + toString(DIM) + "d", SpecStateComposite({}))
-{
- static_assert(DIM == 2 or DIM == 3);
-
- if (_state_p->getType() != (DIM == 2 ? "StatePoint2d" : "StatePoint3d"))
- throw std::runtime_error("LandmarkPoint bad stateblock type for 'P': " + _state_p->getType() + " (should be " +
- (DIM == 2 ? "StatePoint2d)" : "StatePoint3d)"));
-
- addStateBlock('P', _state_p);
-}
-
-template <unsigned int DIM>
-inline LandmarkPoint<DIM>::LandmarkPoint(const SpecStateComposite& _state_specs)
- : LandmarkBase("LandmarkPoint" + toString(DIM) + "d", _state_specs)
+inline Landmark<DIM>::Landmark(const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors,
+ const int _external_id,
+ const int _external_type)
+ : LandmarkBase(
+ "Landmark" + toString(DIM) + "d",
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
+ _state_vectors,
+ _state_priors,
+ _external_id,
+ _external_type)
{
static_assert(DIM == 2 or DIM == 3);
}
template <unsigned int DIM>
-inline LandmarkPoint<DIM>::LandmarkPoint(const YAML::Node& _n)
- : LandmarkBase("LandmarkPoint" + toString(DIM) + "d", _n)
+inline Landmark<DIM>::Landmark(const YAML::Node& _n)
+ : LandmarkBase(
+ "Landmark" + toString(DIM) + "d",
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
+ _n)
{
static_assert(DIM == 2 or DIM == 3);
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index d4902325654b071c7ebe10936243fed69c3fca1c..2476497e8a379340ece2f6280b69510eef0285b1 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -56,7 +56,7 @@ namespace wolf
} \
} \
auto lmk = std::make_shared<LandmarkClass>(_node); \
- lmk->emplacePriors(SpecStateComposite(_node["states"])); \
+ lmk->emplacePriors(); \
return lmk; \
}
@@ -74,7 +74,7 @@ namespace wolf
} \
} \
auto lmk = std::make_shared<LandmarkClass<Dim>>(_node); \
- lmk->emplacePriors(SpecStateComposite(_node["states"])); \
+ lmk->emplacePriors(); \
return lmk; \
}
@@ -88,32 +88,51 @@ class LandmarkBase : public NodeStateBlocks
static unsigned int landmark_id_count_;
protected:
+ unsigned int landmark_id_; ///< landmark unique id
+ unsigned int track_id_; ///< associated track id
+ int external_id_; ///< Id provided by an external tracker (-1 untracked)
+ int external_type_; ///< Type provided by an external classifier (-1 unclassified)
+ TimeStamp stamp_; ///< stamp of the creation of the landmark
+
// Navigate wolf tree
- void setProblem(ProblemPtr _problem) override final;
- unsigned int landmark_id_; ///< landmark unique id
- unsigned int track_id_; ///< associated track id
- TimeStamp stamp_; ///< stamp of the creation of the landmark
+ void setProblem(ProblemPtr _problem) override final;
public:
- /** \brief Constructor type and specs composite
+ /** \brief Constructor type and state specs composites (types, vectors, priors)
*
* Constructor with type and specs composite
- * \param _type indicates landmark type.(types defined at wolf.h)
- * \param _state_specs SpecStateComposite containing all information needed to create the state blocs.
+ * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived landmark class name
+ * \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Composite of types of the landmark states.
+ * \param _state_vectors Composite of vectors of the states of the node. A 'StateBlock' for each key in
+ * '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are
+ * required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
+ * \param _state_priors Composite of priors (initial_guess, factor or fixed) of the states of the node. Not all
+ * keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in
+ * '_state_vectors'. If not specified, no prior is assumed, i.e. 'initial_guess'.
+ * \param _external_id ID provided by an external tracker (default: -1 untracked)
+ * \param _external_type TYPE provided by an external classifier (default: -1 unclassified)
**/
- LandmarkBase(const std::string& _type, const SpecStateComposite& _state_specs);
+ LandmarkBase(const std::string& _type,
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors,
+ const int _external_id = -1,
+ const int _external_type = -1);
- /** \brief Constructor with type and a YAML node (to be used by the derived classes YAML node constructors)
+ /** \brief Constructor with type, state types composite and a YAML node (to be used by the derived classes YAML
+ *node constructors)
*
* Constructor with type, and YAML node
- * \param _type indicates landmark type.(types defined at wolf.h)
+ * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived landmark class name
+ * \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Composite of types of the landmark states.
* \param _n YAML node containing the necessary information
**/
- LandmarkBase(const std::string& _type, const YAML::Node& _n);
+ LandmarkBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _n);
~LandmarkBase() override = default;
- void remove(bool viral_remove_parent_without_children = false) override;
- virtual YAML::Node toYaml() const;
+ void remove(bool viral_remove_parent_without_children = false) override;
+
+ YAML::Node toYaml() const override;
// Properties
unsigned int id() const override;
@@ -122,7 +141,11 @@ class LandmarkBase : public NodeStateBlocks
unsigned int trackId(); // get track ID
void setTrackId(unsigned int _track_id); // set track ID
- public:
+ void setExternalId(const int& _external_id);
+ int getExternalId() const;
+ void setExternalType(const int& _external_type);
+ int getExternalType() const;
+
MapBaseConstPtr getMap() const;
MapBasePtr getMap();
@@ -151,14 +174,8 @@ class LandmarkBase : public NodeStateBlocks
void setMap(const MapBasePtr _map_ptr);
public:
- LandmarkBasePtr shared_from_this_landmark()
- {
- return std::static_pointer_cast<LandmarkBase>(shared_from_this());
- };
- LandmarkBaseConstPtr shared_from_this_landmark() const
- {
- return std::static_pointer_cast<const LandmarkBase>(shared_from_this());
- };
+ LandmarkBasePtr shared_from_this_landmark();
+ LandmarkBaseConstPtr shared_from_this_landmark() const;
};
} // namespace wolf
@@ -172,6 +189,7 @@ template <typename classType, typename... T>
std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
{
std::shared_ptr<classType> lmk = std::make_shared<classType>(std::forward<T>(all)...);
+ lmk->emplacePriors();
lmk->link(_map_ptr);
return lmk;
}
@@ -212,4 +230,34 @@ inline void LandmarkBase::setTrackId(unsigned int _track_id)
track_id_ = _track_id;
}
+inline void LandmarkBase::setExternalId(const int& _external_id)
+{
+ external_id_ = _external_id;
+}
+
+inline int LandmarkBase::getExternalId() const
+{
+ return external_id_;
+}
+
+inline void LandmarkBase::setExternalType(const int& _external_type)
+{
+ external_type_ = _external_type;
+}
+
+inline int LandmarkBase::getExternalType() const
+{
+ return external_type_;
+}
+
+inline LandmarkBasePtr LandmarkBase::shared_from_this_landmark()
+{
+ return std::static_pointer_cast<LandmarkBase>(shared_from_this());
+}
+
+inline LandmarkBaseConstPtr LandmarkBase::shared_from_this_landmark() const
+{
+ return std::static_pointer_cast<const LandmarkBase>(shared_from_this());
+}
+
} // namespace wolf
diff --git a/include/core/landmark/landmark_pose.h b/include/core/landmark/landmark_pose.h
deleted file mode 100644
index 0b14ea983f08861f3a973d809dafde3a2b67149f..0000000000000000000000000000000000000000
--- a/include/core/landmark/landmark_pose.h
+++ /dev/null
@@ -1,100 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-// Wolf includes
-#include "core/landmark/landmark_base.h"
-
-namespace wolf
-{
-template <unsigned int DIM>
-class LandmarkPose : public LandmarkBase
-{
- public:
- /** \brief Constructor stateblock
- *
- * Constructor with stateblock
- * \param _state_p StateBlock of the position
- * \param _state_o StateBlock of the orientation
- **/
- LandmarkPose(StateBlockPtr _state_p, StateBlockPtr _state_o);
-
- /** \brief Constructor specs composite
- *
- * Constructor with specs composite
- * \param _state_specs SpecStateComposite containing all information needed to create the state blocs.
- **/
- LandmarkPose(const SpecStateComposite& _state_specs);
-
- /** \brief Constructor with YAML node (to be used by the derived classes YAML node constructors)
- *
- * Constructor with YAML node
- * \param _n YAML node containing the necessary information
- **/
- LandmarkPose(const YAML::Node& _n);
-
- WOLF_LANDMARK_TEMPLATE_DIM_CREATE(LandmarkPose, DIM);
-
- ~LandmarkPose() override = default;
-};
-
-typedef LandmarkPose<2> LandmarkPose2d;
-typedef LandmarkPose<3> LandmarkPose3d;
-
-WOLF_DECLARED_PTR_TYPEDEFS(LandmarkPose2d);
-WOLF_DECLARED_PTR_TYPEDEFS(LandmarkPose3d);
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-// IMPLEMENTATION //
-////////////////////////////////////////////////////////////////////////////////////////////////
-
-template <unsigned int DIM>
-inline LandmarkPose<DIM>::LandmarkPose(StateBlockPtr _state_p, StateBlockPtr _state_o)
- : LandmarkBase("LandmarkPose" + toString(DIM) + "d", SpecStateComposite({}))
-{
- static_assert(DIM == 2 or DIM == 3);
-
- if (_state_p->getType() != (DIM == 2 ? "StatePoint2d" : "StatePoint3d"))
- throw std::runtime_error("LandmarkPose bad stateblock type for 'P': " + _state_p->getType() + " (should be " +
- (DIM == 2 ? "StatePoint2d)" : "StatePoint3d)"));
-
- if (_state_o->getType() != (DIM == 2 ? "StateAngle" : "StateQuaternion"))
- throw std::runtime_error("LandmarkPose bad stateblock type for 'O': " + _state_p->getType() + " (should be " +
- (DIM == 2 ? "StateAngle)" : "StateQuaternion)"));
-
- addStateBlock('P', _state_p);
- addStateBlock('O', _state_o);
-}
-
-template <unsigned int DIM>
-inline LandmarkPose<DIM>::LandmarkPose(const SpecStateComposite& _state_specs)
- : LandmarkBase("LandmarkPose" + toString(DIM) + "d", _state_specs)
-{
- static_assert(DIM == 2 or DIM == 3);
-}
-
-template <unsigned int DIM>
-inline LandmarkPose<DIM>::LandmarkPose(const YAML::Node& _n) : LandmarkBase("LandmarkPose" + toString(DIM) + "d", _n)
-{
- static_assert(DIM == 2 or DIM == 3);
-}
-
-} // namespace wolf
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 411a6e38bb73868f986e7b05066f982516562523..2422fe6496cd799c6c3a2eedb2d72fb8ea73f459 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -23,7 +23,7 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/composite/vector_composite.h"
-#include "core/composite/matrix_composite.h"
+// #include "core/composite/matrix_composite.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
@@ -58,7 +58,7 @@ inline Eigen::Matrix<T, 2, 2> skew(const T& t)
/** \brief Compute [1]_x * t = (-t.y ; t.x)
*/
template <class D>
-inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
+inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const Eigen::MatrixBase<D>& t)
{
return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
}
@@ -82,20 +82,23 @@ inline Eigen::Matrix2d V_helper(const T theta)
c_1 = theta / T(2.0); // cos(x) ~= 1 - x^2/2
}
- return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
+ return (Eigen::Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
}
inline VectorComposite identity()
{
VectorComposite v;
- v['P'] = Vector2d::Zero();
- v['O'] = Vector1d::Zero();
+ v['P'] = Eigen::Vector2d::Zero();
+ v['O'] = Eigen::Vector1d::Zero();
return v;
}
template <typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, MatrixBase<D2>& ip, typename D2::Scalar& io)
+inline void inverse(const Eigen::MatrixBase<D1>& p,
+ const typename D1::Scalar& o,
+ Eigen::MatrixBase<D2>& ip,
+ typename D2::Scalar& io)
{
MatrixSizeCheck<2, 1>::check(p);
MatrixSizeCheck<2, 1>::check(ip);
@@ -105,7 +108,10 @@ inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, Matri
}
template <typename D1, typename D2, typename D3, typename D4>
-inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o, MatrixBase<D3>& ip, MatrixBase<D4>& io)
+inline void inverse(const Eigen::MatrixBase<D1>& p,
+ const Eigen::MatrixBase<D2>& o,
+ Eigen::MatrixBase<D3>& ip,
+ Eigen::MatrixBase<D4>& io)
{
MatrixSizeCheck<2, 1>::check(p);
MatrixSizeCheck<1, 1>::check(o);
@@ -116,21 +122,21 @@ inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o, MatrixBase
}
template <typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id)
+inline void inverse(const Eigen::MatrixBase<D1>& d, Eigen::MatrixBase<D2>& id)
{
MatrixSizeCheck<3, 1>::check(d);
MatrixSizeCheck<3, 1>::check(id);
- Map<const Matrix<typename D1::Scalar, 2, 1> > p(&d(0));
- Map<Matrix<typename D2::Scalar, 2, 1> > ip(&id(0));
+ Eigen::Map<const Eigen::Matrix<typename D1::Scalar, 2, 1> > p(&d(0));
+ Eigen::Map<Eigen::Matrix<typename D2::Scalar, 2, 1> > ip(&id(0));
inverse(p, d(2), ip, id(2));
}
template <typename D>
-inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
+inline Eigen::Matrix<typename D::Scalar, 3, 1> inverse(const Eigen::MatrixBase<D>& d)
{
- Matrix<typename D::Scalar, 3, 1> id;
+ Eigen::Matrix<typename D::Scalar, 3, 1> id;
inverse(d, id);
return id;
}
@@ -164,9 +170,9 @@ inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T
MatrixSizeCheck<2, 1>::check(p);
// Jacobian of t = V(theta)*p wrt theta -- developed in Matlab symbolic, and copied here.
- T x = p(0);
- T y = p(1);
- Matrix<T, 2, 1> J_Vp_th;
+ T x = p(0);
+ T y = p(1);
+ Eigen::Matrix<T, 2, 1> J_Vp_th;
if (fabs(theta) > T(Constants::EPS))
{
@@ -186,7 +192,7 @@ inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T
}
template <class D1, class D2, class D3>
-inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
+inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta, Eigen::MatrixBase<D3>& _J_delta_tau)
{
MatrixSizeCheck<3, 1>::check(_tau);
MatrixSizeCheck<3, 1>::check(_delta);
@@ -210,12 +216,12 @@ inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D
inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
{
// [1] eq. 156
- const auto& p = _tau.at('P');
- const auto& theta = pi2pi(_tau.at('O')(0));
- Matrix2d V = V_helper(theta);
+ const auto& p = _tau.at('P');
+ const auto& theta = pi2pi(_tau.at('O')(0));
+ Eigen::Matrix2d V = V_helper(theta);
_delta['P'] = V * p;
- _delta['O'] = Matrix1d(theta);
+ _delta['O'] = Eigen::Matrix1d(theta);
}
inline VectorComposite exp(const VectorComposite& tau)
@@ -227,31 +233,31 @@ inline VectorComposite exp(const VectorComposite& tau)
return res;
}
-inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
-{
- // [1] eq. 156
- const auto& p = _tau.at('P');
- const auto& theta = pi2pi(_tau.at('O')(0));
- Matrix2d V = V_helper(theta);
-
- _delta['P'] = V * p;
- _delta['O'] = Matrix1d(theta);
-
- // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
- // J_Vp_p = V: see V_helper, eq. 158
- // J_Vp_theta: see fcn helper
- // J_theta_theta = 1; eq. 126
- _J_delta_tau.clear();
- _J_delta_tau.emplace('P', 'P', V);
- _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
- _J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0));
- _J_delta_tau.emplace('O', 'O', Matrix1d(1));
-}
+// inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
+// {
+// // [1] eq. 156
+// const auto& p = _tau.at('P');
+// const auto& theta = pi2pi(_tau.at('O')(0));
+// Matrix2d V = V_helper(theta);
+
+// _delta['P'] = V * p;
+// _delta['O'] = Eigen::Matrix1d(theta);
+
+// // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
+// // J_Vp_p = V: see V_helper, eq. 158
+// // J_Vp_theta: see fcn helper
+// // J_theta_theta = 1; eq. 126
+// _J_delta_tau.clear();
+// _J_delta_tau.emplace('P', 'P', V);
+// _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
+// _J_delta_tau.emplace('O', 'P', Eigen::RowVector2d(0.0, 0.0));
+// _J_delta_tau.emplace('O', 'O', Eigen::Matrix1d(1));
+// }
template <class D1, class D2, class D3>
-inline void compose(const MatrixBase<D1>& _delta1,
- const MatrixBase<D2>& _delta2,
- MatrixBase<D3>& _delta1_compose_delta2)
+inline void compose(const Eigen::MatrixBase<D1>& _delta1,
+ const Eigen::MatrixBase<D2>& _delta2,
+ Eigen::MatrixBase<D3>& _delta1_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
@@ -263,19 +269,20 @@ inline void compose(const MatrixBase<D1>& _delta1,
}
template <class D1, class D2>
-inline Matrix<typename D1::Scalar, 3, 1> compose(const MatrixBase<D1>& _delta1, const MatrixBase<D2>& _delta2)
+inline Eigen::Matrix<typename D1::Scalar, 3, 1> compose(const Eigen::MatrixBase<D1>& _delta1,
+ const Eigen::MatrixBase<D2>& _delta2)
{
- Matrix<typename D1::Scalar, 3, 1> delta1_compose_delta2;
+ Eigen::Matrix<typename D1::Scalar, 3, 1> delta1_compose_delta2;
compose(_delta1, _delta2, delta1_compose_delta2);
return delta1_compose_delta2;
}
template <class D1, class D2, class D3, class D4, class D5>
-inline void compose(const MatrixBase<D1>& _delta1,
- const MatrixBase<D2>& _delta2,
- MatrixBase<D3>& _delta1_compose_delta2,
- MatrixBase<D4>& _J_compose_delta1,
- MatrixBase<D5>& _J_compose_delta2)
+inline void compose(const Eigen::MatrixBase<D1>& _delta1,
+ const Eigen::MatrixBase<D2>& _delta2,
+ Eigen::MatrixBase<D3>& _delta1_compose_delta2,
+ Eigen::MatrixBase<D4>& _J_compose_delta1,
+ Eigen::MatrixBase<D5>& _J_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
@@ -283,7 +290,7 @@ inline void compose(const MatrixBase<D1>& _delta1,
MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
- Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
+ Eigen::Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
// tc = t1 + R1 t2
_delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>();
@@ -338,7 +345,7 @@ inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, Vect
_c['P'] = p1 + R1 * p2;
// Rc = R1 R2 --> theta = theta1 + theta2
- _c['O'] = Matrix1d(pi2pi(a1 + a2));
+ _c['O'] = Eigen::Matrix1d(pi2pi(a1 + a2));
}
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
@@ -348,75 +355,76 @@ inline VectorComposite compose(const VectorComposite& x1, const VectorComposite&
return c;
}
-inline void compose(const VectorComposite& _x1,
- const VectorComposite& _x2,
- VectorComposite& _c,
- MatrixComposite& _J_c_x1,
- MatrixComposite& _J_c_x2)
-{
- const auto& p1 = _x1.at('P');
- const auto& a1 = _x1.at('O')(0); // angle
- Matrix2d R1 = SO2::exp(a1); // need temporary
-
- const auto& p2 = _x2.at('P');
- const auto& a2 = _x2.at('O')(0); // angle
-
- /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
- *
- * resulting delta is:
- *
- * tc = t1 + R1 t2 (*)
- * Rc = R1 R2 (**)
- *
- * Jacobians have the form:
- *
- * [ J_t_t J_t_R ]
- * [ J_r_t J_R_R ]
- *
- * Jacobian blocks are:
- *
- * J_tc_t1 = I trivial from (*)
- * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
- *
- * J_Rc_t1 = (0 0) trivial from (**)
- * J_Rc_R1 = 1 see [1]: eq. (125)
- *
- * J_tc_t2 = R1 see [1]: eq. (130)
- * J_tc_R2 = 0 trivial from (*)
- *
- * J_Rc_t2 = 0 trivial from (**)
- * J_Rc_R2 = 1 see [1]: eq. (125)
- */
-
- _J_c_x1.clear();
- _J_c_x1.emplace('P', 'P', Matrix2d::Identity());
- _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)));
- _J_c_x1.emplace('O', 'P', RowVector2d(0, 0));
- _J_c_x1.emplace('O', 'O', Matrix1d(1));
-
- _J_c_x2.clear();
- _J_c_x2.emplace('P', 'P', R1);
- _J_c_x2.emplace('P', 'O', Vector2d(0, 0));
- _J_c_x2.emplace('O', 'P', RowVector2d(0, 0));
- _J_c_x2.emplace('O', 'O', Matrix1d(1));
-
- // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or
- // 'x2'.
-
- // tc = t1 + R1 t2
- _c['P'] = p1 + R1 * p2;
-
- // Rc = R1 R2 --> theta = theta1 + theta2
- _c['O'] = Matrix1d(pi2pi(a1 + a2));
-}
+// inline void compose(const VectorComposite& _x1,
+// const VectorComposite& _x2,
+// VectorComposite& _c,
+// MatrixComposite& _J_c_x1,
+// MatrixComposite& _J_c_x2)
+// {
+// const auto& p1 = _x1.at('P');
+// const auto& a1 = _x1.at('O')(0); // angle
+// Eigen::Matrix2d R1 = SO2::exp(a1); // need temporary
+
+// const auto& p2 = _x2.at('P');
+// const auto& a2 = _x2.at('O')(0); // angle
+
+// /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+// *
+// * resulting delta is:
+// *
+// * tc = t1 + R1 t2 (*)
+// * Rc = R1 R2 (**)
+// *
+// * Jacobians have the form:
+// *
+// * [ J_t_t J_t_R ]
+// * [ J_r_t J_R_R ]
+// *
+// * Jacobian blocks are:
+// *
+// * J_tc_t1 = I trivial from (*)
+// * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+// *
+// * J_Rc_t1 = (0 0) trivial from (**)
+// * J_Rc_R1 = 1 see [1]: eq. (125)
+// *
+// * J_tc_t2 = R1 see [1]: eq. (130)
+// * J_tc_R2 = 0 trivial from (*)
+// *
+// * J_Rc_t2 = 0 trivial from (**)
+// * J_Rc_R2 = 1 see [1]: eq. (125)
+// */
+
+// _J_c_x1.clear();
+// _J_c_x1.emplace('P', 'P', Eigen::Matrix2d::Identity());
+// _J_c_x1.emplace('P', 'O', Eigen::MatrixXd(R1 * skewed(p2)));
+// _J_c_x1.emplace('O', 'P', Eigen::RowVector2d(0, 0));
+// _J_c_x1.emplace('O', 'O', Eigen::Matrix1d(1));
+
+// _J_c_x2.clear();
+// _J_c_x2.emplace('P', 'P', R1);
+// _J_c_x2.emplace('P', 'O', Eigen::Vector2d(0, 0));
+// _J_c_x2.emplace('O', 'P', Eigen::RowVector2d(0, 0));
+// _J_c_x2.emplace('O', 'O', Eigen::Matrix1d(1));
+
+// // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1'
+// or
+// // 'x2'.
+
+// // tc = t1 + R1 t2
+// _c['P'] = p1 + R1 * p2;
+
+// // Rc = R1 R2 --> theta = theta1 + theta2
+// _c['O'] = Eigen::Matrix1d(pi2pi(a1 + a2));
+// }
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& p1,
- const MatrixBase<D2>& q1,
- const MatrixBase<D4>& p2,
- const MatrixBase<D5>& o2,
- MatrixBase<D7>& plus_p,
- MatrixBase<D8>& plus_q)
+inline void plus(const Eigen::MatrixBase<D1>& p1,
+ const Eigen::MatrixBase<D2>& q1,
+ const Eigen::MatrixBase<D4>& p2,
+ const Eigen::MatrixBase<D5>& o2,
+ Eigen::MatrixBase<D7>& plus_p,
+ Eigen::MatrixBase<D8>& plus_q)
{
MatrixSizeCheck<2, 1>::check(p1);
MatrixSizeCheck<1, 1>::check(q1);
@@ -430,22 +438,22 @@ inline void plus(const MatrixBase<D1>& p1,
}
template <typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus)
+inline void plus(const Eigen::MatrixBase<D1>& d1, const Eigen::MatrixBase<D2>& d2, Eigen::MatrixBase<D3>& d_plus)
{
- Map<const Matrix<typename D1::Scalar, 2, 1> > p1(&d1(0));
- Map<const Matrix<typename D1::Scalar, 1, 1> > q1(&d1(2));
- Map<const Matrix<typename D2::Scalar, 2, 1> > p2(&d2(0));
- Map<const Matrix<typename D2::Scalar, 1, 1> > o2(&d2(2));
- Map<Matrix<typename D3::Scalar, 2, 1> > p_p(&d_plus(0));
- Map<Matrix<typename D1::Scalar, 1, 1> > q_p(&d_plus(2));
+ Eigen::Map<const Eigen::Matrix<typename D1::Scalar, 2, 1> > p1(&d1(0));
+ Eigen::Map<const Eigen::Matrix<typename D1::Scalar, 1, 1> > q1(&d1(2));
+ Eigen::Map<const Eigen::Matrix<typename D2::Scalar, 2, 1> > p2(&d2(0));
+ Eigen::Map<const Eigen::Matrix<typename D2::Scalar, 1, 1> > o2(&d2(2));
+ Eigen::Map<Eigen::Matrix<typename D3::Scalar, 2, 1> > p_p(&d_plus(0));
+ Eigen::Map<Eigen::Matrix<typename D1::Scalar, 1, 1> > q_p(&d_plus(2));
plus(p1, q1, p2, o2, p_p, q_p);
}
template <typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
+inline Eigen::Matrix<typename D1::Scalar, 3, 1> plus(const Eigen::MatrixBase<D1>& d1, const Eigen::MatrixBase<D2>& d2)
{
- Matrix<typename D1::Scalar, 3, 1> d_plus;
+ Eigen::Matrix<typename D1::Scalar, 3, 1> d_plus;
plus(d1, d2, d_plus);
return d_plus;
}
@@ -465,12 +473,12 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
}
template <typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& p1,
- const typename D1::Scalar& o1,
- const MatrixBase<D2>& p2,
- const typename D2::Scalar& o2,
- MatrixBase<D3>& p12,
- typename D3::Scalar& o12)
+inline void between(const Eigen::MatrixBase<D1>& p1,
+ const typename D1::Scalar& o1,
+ const Eigen::MatrixBase<D2>& p2,
+ const typename D2::Scalar& o2,
+ Eigen::MatrixBase<D3>& p12,
+ typename D3::Scalar& o12)
{
MatrixSizeCheck<2, 1>::check(p1);
MatrixSizeCheck<2, 1>::check(p2);
@@ -481,7 +489,7 @@ inline void between(const MatrixBase<D1>& p1,
}
template <typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12)
+inline void between(const Eigen::MatrixBase<D1>& d1, const Eigen::MatrixBase<D2>& d2, Eigen::MatrixBase<D3>& d12)
{
MatrixSizeCheck<3, 1>::check(d1);
MatrixSizeCheck<3, 1>::check(d2);
@@ -489,19 +497,20 @@ inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBa
typedef typename D1::Scalar T;
- Map<const Matrix<T, 2, 1> > p1(&d1(0));
- Map<const Matrix<T, 2, 1> > p2(&d2(0));
- Map<Matrix<T, 2, 1> > p12(&d12(0));
+ Eigen::Map<const Eigen::Matrix<T, 2, 1> > p1(&d1(0));
+ Eigen::Map<const Eigen::Matrix<T, 2, 1> > p2(&d2(0));
+ Eigen::Map<Eigen::Matrix<T, 2, 1> > p12(&d12(0));
between(p1, d1(2), p2, d2(2), p12, d12(2));
}
template <typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
+inline Eigen::Matrix<typename D1::Scalar, 3, 1> between(const Eigen::MatrixBase<D1>& d1,
+ const Eigen::MatrixBase<D2>& d2)
{
MatrixSizeCheck<3, 1>::check(d1);
MatrixSizeCheck<3, 1>::check(d2);
- Matrix<typename D1::Scalar, 3, 1> d12;
+ Eigen::Matrix<typename D1::Scalar, 3, 1> d12;
between(d1, d2, d12);
return d12;
}
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index e152d5943abe045a213645e7c0f4ef568c2367c0..3d291b571bd4979b704d42027396df08fe28f7af 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -23,7 +23,6 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/composite/vector_composite.h"
-#include "core/composite/matrix_composite.h"
/*
* The functions in this file are related to manipulations of Delta motion magnitudes used in 3d motion.
@@ -135,7 +134,8 @@ inline void inverse(const VectorComposite& v, VectorComposite& c)
inline VectorComposite inverse(const VectorComposite& v)
{
- VectorComposite c("PO", Vector7d::Zero(), {3, 4});
+ VectorComposite c(
+ {{'P', Vector3d::Zero()}, {'O', Vector4d::Zero()}}); // no need to normalize, will be overwritten
inverse(v, c);
return c;
}
@@ -242,37 +242,39 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
- VectorComposite c("PO", Vector7d::Zero(), {3, 4});
+ VectorComposite c(
+ {{'P', Vector3d::Zero()}, {'O', Vector4d::Zero()}}); // no need to normalize, will be overwritten
compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
return c;
}
-inline void compose(const VectorComposite& x1,
- const VectorComposite& x2,
- VectorComposite& c,
- MatrixComposite& J_c_x1,
- MatrixComposite& J_c_x2)
-{
- // Some useful temporaries
- const auto R1 = q2R(x1.at('O')); // q1.matrix(); // make temporary
- const auto& R2 = q2R(x2.at('O')); // q2.matrix(); // do not make temporary, only reference
-
- // Jac wrt first delta
- J_c_x1.emplace('P', 'P', Matrix3d::Identity());
- J_c_x1.emplace('P', 'O', -R1 * skew(x2.at('P')));
- J_c_x1.emplace('O', 'P', Matrix3d::Zero());
- J_c_x1.emplace('O', 'O', R2.transpose());
-
- // Jac wrt second delta
- J_c_x2.emplace('P', 'P', R1);
- J_c_x2.emplace('P', 'O', Matrix3d::Zero());
- J_c_x2.emplace('O', 'P', Matrix3d::Zero());
- J_c_x2.emplace('O', 'O', Matrix3d::Identity());
-
- // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same
- // variable
- compose(x1, x2, c);
-}
+// inline void compose(const VectorComposite& x1,
+// const VectorComposite& x2,
+// VectorComposite& c,
+// MatrixComposite& J_c_x1,
+// MatrixComposite& J_c_x2)
+// {
+// // Some useful temporaries
+// const auto R1 = q2R(x1.at('O')); // q1.matrix(); // make temporary
+// const auto& R2 = q2R(x2.at('O')); // q2.matrix(); // do not make temporary, only reference
+
+// // Jac wrt first delta
+// J_c_x1.emplace('P', 'P', Matrix3d::Identity());
+// J_c_x1.emplace('P', 'O', -R1 * skew(x2.at('P')));
+// J_c_x1.emplace('O', 'P', Matrix3d::Zero());
+// J_c_x1.emplace('O', 'O', R2.transpose());
+
+// // Jac wrt second delta
+// J_c_x2.emplace('P', 'P', R1);
+// J_c_x2.emplace('P', 'O', Matrix3d::Zero());
+// J_c_x2.emplace('O', 'P', Matrix3d::Zero());
+// J_c_x2.emplace('O', 'O', Matrix3d::Identity());
+
+// // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the
+// same
+// // variable
+// compose(x1, x2, c);
+// }
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>& p1,
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 644b37bef7f0e2f4a480712e3e1e9f6eebd09f38..db4ee82ee708fdf3afd71669f679f80dac672650 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -40,7 +40,8 @@ struct ParamsProcessorBase;
#include "core/common/node_state_blocks.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/composite/spec_state_composite.h"
+#include "core/composite/prior_composite.h"
+#include "core/composite/type_composite.h"
#include "core/composite/vector_composite.h"
#include "core/processor/motion_provider.h"
@@ -74,10 +75,12 @@ class Problem : public std::enable_shared_from_this<Problem>
TreeManagerBasePtr tree_manager_;
std::map<int, MotionProviderPtr> motion_provider_map_;
- SizeEigen dim_;
- TypeComposite frame_types_;
- SpecStateComposite prior_options_;
- bool prior_applied_;
+ SizeEigen dim_;
+ TypeComposite frame_types_;
+ TypeComposite first_frame_types_;
+ VectorComposite first_frame_values_;
+ PriorComposite first_frame_priors_;
+ unsigned int first_frame_status_; ///< 0: not set, 1: set, 2: applied
std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_;
@@ -112,7 +115,7 @@ class Problem : public std::enable_shared_from_this<Problem>
protected:
static void searchAndLoadPlugins(const YAML::Node& _node, LoaderPtr _loader);
- static std::set<std::string> searchPlugins(const YAML::Node& _node, std::set<YAML::Node>& _visited_nodes);
+ static std::set<std::string> searchPlugins(const YAML::Node& _node, std::list<YAML::Node>& _visited_nodes);
static void loadPlugin(LoaderPtr _loader, const std::string& plugin_name);
void loadPlugin(const std::string& plugin_name);
@@ -206,16 +209,22 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBaseConstPtr getTrajectory() const;
TrajectoryBasePtr getTrajectory();
- // Prior
- bool isPriorApplied() const;
- void setPriorOptions(const SpecStateComposite& priors);
- FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
- FrameBasePtr setPrior(const SpecStateComposite& priors, const TimeStamp& _ts);
+ // First Frame
+ bool isFirstFrameApplied() const;
+ void setFirstFrameOptions(const TypeComposite& _first_frame_types,
+ const VectorComposite& _first_frame_values,
+ const PriorComposite& _first_frame_priors);
+ FrameBasePtr applyFirstFrameOptions(const TimeStamp& _ts);
+ FrameBasePtr emplaceFirstFrame(const TimeStamp& _ts,
+ const TypeComposite& _first_frame_types,
+ const VectorComposite& _first_frame_values,
+ const PriorComposite& _first_frame_priors);
/** \brief Emplace frame from timestamp, types and and state
* \param _time_stamp Time stamp of the frame
* \param _frame_types TypeComposite indicating the types of each key.
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ * \param _frame_state VectorComposite with the states
+ * \param _frame_priors PriorComposite with the priors on the states (not all keys required)
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
@@ -224,7 +233,8 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
const TypeComposite& _frame_types,
- const VectorComposite& _frame_state);
+ const VectorComposite& _frame_state,
+ const PriorComposite& _frame_priors = {});
/** \brief Emplace frame from timestamp and keys
* \param _time_stamp Time stamp of the frame
@@ -269,21 +279,6 @@ class Problem : public std::enable_shared_from_this<Problem>
const StateKeys& _frame_keys,
const Eigen::VectorXd& _frame_state);
- /** \brief Emplace frame from state specs composite
- * \param _time_stamp Time stamp of the frame
- * \param _frame_spec_composite SpecStateComposite; each state must match in size and type with the
- * problem dimension and the corresponding key, for example, 'P' must be a 'StatePoint2d' in a 2D problem.
- *
- * - The structure is taken from _frame_spec_composite
- * - The state sizes and types are taken from _frame_spec_composite
- *
- * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
- * - Create a Frame
- * - Add it to Trajectory
- * - If it is key-frame, update state-block lists in Problem
- */
- FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const SpecStateComposite& _frame_spec_composite);
-
// Frame getters
FrameBaseConstPtr getLastFrame() const;
FrameBasePtr getLastFrame();
@@ -426,9 +421,9 @@ inline TreeManagerBasePtr Problem::getTreeManager()
return tree_manager_;
}
-inline bool Problem::isPriorApplied() const
+inline bool Problem::isFirstFrameApplied() const
{
- return prior_applied_;
+ return first_frame_status_ == 2;
}
inline std::map<int, MotionProviderConstPtr> Problem::getMotionProviderMap() const
diff --git a/include/core/processor/buffer.h b/include/core/processor/buffer.h
index 33a391f0dc3d2ac33e470733b49763be03d51559..3e558bc25728e68fad97cc68934063ad6e1e0ef4 100644
--- a/include/core/processor/buffer.h
+++ b/include/core/processor/buffer.h
@@ -22,13 +22,6 @@
// Wolf includes
#include "core/common/wolf.h"
-#include "core/capture/capture_base.h"
-#include "core/frame/frame_base.h"
-#include "core/common/time_stamp.h"
-
-// std
-#include <memory>
-#include <map>
namespace wolf
{
@@ -145,11 +138,15 @@ class BufferCapture : public Buffer<CaptureBasePtr>
{
};
-/////////////////////////////////////////////////////////////////////////////////////////
+} // namespace wolf
+#include "core/common/time_stamp.h"
+
+namespace wolf
+{
template <typename T>
-typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
- const double& _time_tolerance) const
+inline typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
+ const double& _time_tolerance) const
{
Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
@@ -185,7 +182,8 @@ typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _ti
}
template <typename T>
-typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
+inline typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
+ const double& _time_tolerance)
{
Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
@@ -221,7 +219,7 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
}
template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+inline T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
if (container_.empty()) return nullptr;
@@ -238,7 +236,7 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
}
template <typename T>
-T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+inline T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty()) return nullptr;
@@ -255,7 +253,7 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
}
template <typename T>
-T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+inline T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
// There is no element
if (container_.empty()) return nullptr;
@@ -273,7 +271,7 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
}
template <typename T>
-T Buffer<T>::selectFirst() const
+inline T Buffer<T>::selectFirst() const
{
// There is no element
if (container_.empty()) return nullptr;
@@ -283,7 +281,7 @@ T Buffer<T>::selectFirst() const
}
template <typename T>
-T Buffer<T>::selectLast() const
+inline T Buffer<T>::selectLast() const
{
// There is no element
if (container_.empty()) return nullptr;
@@ -293,19 +291,19 @@ T Buffer<T>::selectLast() const
}
template <typename T>
-void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
+inline void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
{
container_.emplace(_time_stamp, _element);
}
template <typename T>
-const std::map<TimeStamp, T>& Buffer<T>::getContainer() const
+inline const std::map<TimeStamp, T>& Buffer<T>::getContainer() const
{
return container_;
}
template <typename T>
-std::map<TimeStamp, T>& Buffer<T>::getContainer()
+inline std::map<TimeStamp, T>& Buffer<T>::getContainer()
{
return container_;
}
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 94f86169ceb95654424513b8673bc6d37ee98cad..9825c82fc1802fc4003fc45f88cbfb12f0c510ce 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -20,15 +20,10 @@
#pragma once
-// Fwd refs
-namespace wolf
-{
-class SensorBase;
-}
-
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/processor/buffer.h"
#include "core/processor/motion_provider.h"
#include "core/processor/factory_processor.h"
#include "core/frame/frame_base.h"
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index f4f354fe1a0945e671b8bbbcf04ed2bb3afbdfbd..caeac1811039f7599dee92a4134061d17214815c 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -23,6 +23,8 @@
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
#include "core/processor/track_matrix.h"
+#include "core/landmark/landmark_base.h"
+#include "core/feature/feature_landmark_external.h"
namespace wolf
{
@@ -41,8 +43,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
void configure(SensorBasePtr _sensor) override{};
protected:
- TrackMatrix track_matrix_;
- std::set<SizeStd> lmks_ids_origin_;
+ TrackMatrix track_matrix_;
/** Pre-process incoming Capture
*
@@ -90,13 +91,13 @@ class ProcessorLandmarkExternal : public ProcessorTracker
/** \brief Emplaces a landmark or modifies (if needed) a landmark
* \param _feature_ptr pointer to the Feature
*/
- LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+ LandmarkBasePtr emplaceLandmark(FeatureLandmarkExternalPtr _feature_ptr);
/** \brief Modifies (if needed) a landmark
* \param _landmark pointer to the landmark
* \param _feature pointer to the Feature.
*/
- void modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature);
+ void modifyLandmark(LandmarkBasePtr _landmark, FeatureLandmarkExternalPtr _feature);
/** Post-process
*
@@ -115,28 +116,42 @@ class ProcessorLandmarkExternal : public ProcessorTracker
const VectorComposite& _pose2,
const VectorComposite& _pose_sen) const;
- // Params
- bool use_orientation_; ///< use orientation measure or not when emplacing factors
- double match_dist_th_; ///< for considering tracked detection: distance threshold to previous detection
+ double detectionDistance(FeatureBasePtr _ftr,
+ LandmarkBasePtr _lmk,
+ const VectorComposite& _pose_frm,
+ const VectorComposite& _pose_sen) const;
+
+ // PROCESSOR PARAMETERS
+ bool use_orientation_; ///< use orientation measure or not when emplacing factors
unsigned int new_features_for_keyframe_; ///< for keyframe voting: amount of new features with respect to origin
///< (sufficient condition if more than min_features_for_keyframe)
- double filter_quality_th_; ///< min quality to consider the detection
- unsigned int filter_track_length_th_; ///< length of the track necessary to consider the detection
- double time_span_; ///< for keyframe voting: time span since last frame (sufficient condition if more than
- ///< min_features_for_keyframe)
+ double time_span_; ///< for keyframe voting: time span since last frame (sufficient condition if more than
+ ///< min_features_for_keyframe)
+
+ double quality_th_; ///< min quality to consider the detection
+
+ // Matching distance threshold to previous detection considering motion (necessary condition)
+ double match_dist_th_id_; ///< Match by ID
+ double match_dist_th_type_; ///< Match by TYPE
+ double match_dist_th_unknown_; ///< No ID/TYPE information
+
+ unsigned int track_length_th_; ///< Track length threshold to emplace factors (necessary condition)
+
+ bool close_loops_by_id_; ///< Close loop if ID matches (ID unchanged guaranteed)
+ bool close_loops_by_type_; ///< Close loop if TYPE matches (also distance check)
};
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(const YAML::Node& _params)
: ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params),
- lmks_ids_origin_(),
use_orientation_(_params["use_orientation"].as<bool>()),
- match_dist_th_(_params["match_dist_th"].as<double>()),
new_features_for_keyframe_(_params["keyframe_vote"]["new_features_for_keyframe"].as<unsigned int>()),
- filter_quality_th_(_params["filter"]["quality_th"].as<double>()),
- filter_track_length_th_(_params["filter"]["track_length_th"].as<unsigned int>()),
- time_span_(_params["keyframe_vote"]["time_span"].as<double>())
-{
- //
-}
-
-} // namespace wolf
+ time_span_(_params["keyframe_vote"]["time_span"].as<double>()),
+ quality_th_(_params["quality_th"].as<double>()),
+ match_dist_th_id_(_params["match_dist_th_id"].as<double>()),
+ match_dist_th_type_(_params["match_dist_th_type"].as<double>()),
+ match_dist_th_unknown_(_params["match_dist_th_unknown"].as<double>()),
+ track_length_th_(_params["track_length_th"].as<unsigned int>()),
+ close_loops_by_id_(_params["close_loops_by_id"].as<bool>()),
+ close_loops_by_type_(_params["close_loops_by_type"].as<bool>()){};
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 9cfa830b6708bafaef8465824913926510287644..7efda2a977e51f53eb3d6d06d10366202cd004ab 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -145,8 +145,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
// Queries to the processor:
TimeStamp getTimeStamp() const override;
- VectorComposite getState(StateKeys _structure = "") const override;
- VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const override;
+ VectorComposite getState(StateKeys _keys = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, StateKeys _keys = "") const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 4df7bab5d594504388f57e07e36a80fd23e29455..910b625d783622f61622e50b1609cb1f29c0afbe 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -31,7 +31,6 @@ class StateBlock;
// Wolf includes
#include "core/common/wolf.h"
#include "core/sensor/factory_sensor.h"
-#include "core/composite/spec_state_sensor_composite.h"
#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
#include "yaml-cpp/yaml.h"
@@ -70,7 +69,7 @@ namespace wolf
} \
} \
std::shared_ptr<SensorClass> sen(new SensorClass(_input_node)); \
- sen->emplacePriors(SpecStateComposite(_input_node["states"])); \
+ sen->emplacePriors(); \
return sen->shared_from_this_sensor(); \
} \
static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
@@ -98,7 +97,7 @@ namespace wolf
} \
} \
std::shared_ptr<SensorClass> sen(new SensorClass<Dim>(_input_node)); \
- sen->emplacePriors(SpecStateComposite(_input_node["states"])); \
+ sen->emplacePriors(); \
return sen->shared_from_this_sensor(); \
} \
static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
@@ -133,16 +132,20 @@ class SensorBase : public NodeStateBlocks
void setProblem(ProblemPtr _problem) override final;
- /** \brief Constructor with Prior and Params
+ /** \brief Constructor
*
- * Constructor with parameter vector
- * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: type name
- * \param _dim_compatible TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR:
- * Which dimension is the sensor compatible (2: 2D, 3: 3D, -1: both)
+ * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived class name
+ * \param _dim_compatible TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Which dimension is the
+ * sensor compatible with (2: 2D, 3: 3D, -1: both)
+ * \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Composite of keys and types of the states
+ * of the sensor.
* \param _params params yaml node
*
**/
- SensorBase(const std::string& _type, const int& _dim_compatible, const YAML::Node& _params);
+ SensorBase(const std::string& _type,
+ const int& _dim_compatible,
+ const TypeComposite& _state_types,
+ const YAML::Node& _params);
public:
~SensorBase() override;
@@ -174,7 +177,6 @@ class SensorBase : public NodeStateBlocks
bool process(const CaptureBasePtr capture_ptr);
// State blocks
- StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
StateBlockConstPtr getStateBlockDynamic(const char& _key) const;
StateBlockPtr getStateBlockDynamic(const char& _key);
StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const;
@@ -271,6 +273,7 @@ std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _
const std::vector<std::string>& _folders_schema)
{
std::shared_ptr<classType> sen = std::static_pointer_cast<classType>(classType::create(_params, _folders_schema));
+ sen->emplacePriors();
sen->link(_hwd_ptr);
return sen;
}
@@ -312,13 +315,6 @@ inline CaptureBasePtr SensorBase::getLastCapture()
return last_capture_;
}
-inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
-{
- NodeStateBlocks::addStateBlock(_key, _sb_ptr);
- setStateBlockDynamic(_key, _dynamic);
- return _sb_ptr;
-}
-
inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic)
{
if (not has(_key))
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index caca8d2833d9c99480d3d41ef7e3a5641601b5ec..ed861c847c0db2087d8de9ce9ffeb0e42a62a5c7 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -30,7 +30,12 @@ template <unsigned int DIM>
class SensorOdom : public SensorBase
{
protected:
- SensorOdom(const YAML::Node& _params) : SensorBase("SensorOdom" + toString(DIM) + "d", DIM, _params)
+ SensorOdom(const YAML::Node& _params)
+ : SensorBase(
+ "SensorOdom" + toString(DIM) + "d",
+ DIM,
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params)
{
static_assert(DIM == 2 or DIM == 3);
}
@@ -121,9 +126,9 @@ inline Eigen::MatrixXd SensorOdom<DIM>::computeNoiseCov(const Eigen::VectorXd& _
// return
if (DIM == 2)
- return (Vector2d() << dvar, rvar).finished().asDiagonal();
+ return (Eigen::Vector2d() << dvar, rvar).finished().asDiagonal();
else
- return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
+ return (Eigen::Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
}
typedef SensorOdom<2> SensorOdom2d;
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index cdeb26de51263d5f4f419d335e9668a916f8e7a3..3ee0487541f11623a2e5d3041ef395230c42eb2a 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -30,7 +30,12 @@ template <unsigned int DIM>
class SensorPose : public SensorBase
{
protected:
- SensorPose(const YAML::Node& _params) : SensorBase("SensorPose" + toString(DIM) + "d", DIM, _params)
+ SensorPose(const YAML::Node& _params)
+ : SensorBase(
+ "SensorPose" + toString(DIM) + "d",
+ DIM,
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params)
{
static_assert(DIM == 2 or DIM == 3);
}
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 731302c2fdf627585806fdc359fd0ac25a4a1b50..5b0811ec8372d9988f90c1a5d9738a4cb7da75ed 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -29,7 +29,7 @@ class StateAngle : public StateBlock
{
public:
StateAngle(double _angle, bool _fixed = false, bool _transformable = true);
- StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true);
+ StateAngle(const Eigen::VectorXd& _angle, bool _fixed = false, bool _transformable = true);
static Eigen::VectorXd Identity();
@@ -46,7 +46,7 @@ inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable)
state_(0) = pi2pi(_angle);
}
-inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable)
+inline StateAngle::StateAngle(const Eigen::VectorXd& _angle, bool _fixed, bool _transformable)
: StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
if (_angle.size() != 1) throw std::runtime_error("The angle state vector must be of size 1!");
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
index b46ac22714216bf5017fadf18bbad66278f3aad3..837a8aea9890e8736097574c31a24bc66149bcb7 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/include/core/state_block/state_block_derived.h
@@ -65,6 +65,16 @@ typedef StateParams<7> StateParams7;
typedef StateParams<8> StateParams8;
typedef StateParams<9> StateParams9;
typedef StateParams<10> StateParams10;
+typedef StateParams<11> StateParams11;
+typedef StateParams<12> StateParams12;
+typedef StateParams<13> StateParams13;
+typedef StateParams<14> StateParams14;
+typedef StateParams<15> StateParams15;
+typedef StateParams<16> StateParams16;
+typedef StateParams<17> StateParams17;
+typedef StateParams<18> StateParams18;
+typedef StateParams<19> StateParams19;
+typedef StateParams<20> StateParams20;
/**
* @brief State block for general 2D points and positions
@@ -91,7 +101,8 @@ class StatePoint2d : public StateBlock
WOLF_STATE_BLOCK_CREATE(StatePoint2d);
void transform(const VectorComposite& _transformation) override
{
- if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
+ if (transformable_)
+ setState(_transformation.at('P') + Eigen::Rotation2Dd(_transformation.at('O')(0)) * getState());
}
};
@@ -121,7 +132,7 @@ class StateVector2d : public StateBlock
WOLF_STATE_BLOCK_CREATE(StateVector2d);
void transform(const VectorComposite& _transformation) override
{
- if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
+ if (transformable_) setState(Eigen::Rotation2Dd(_transformation.at('O')(0)) * getState());
}
};
@@ -151,7 +162,7 @@ class StatePoint3d : public StateBlock
void transform(const VectorComposite& _transformation) override
{
if (transformable_)
- setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
+ setState(_transformation.at('P') + Eigen::Quaterniond(_transformation.at('O').data()) * getState());
}
};
@@ -181,7 +192,7 @@ class StateVector3d : public StateBlock
WOLF_STATE_BLOCK_CREATE(StateVector3d);
void transform(const VectorComposite& _transformation) override
{
- if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
+ if (transformable_) setState(Eigen::Quaterniond(_transformation.at('O').data()) * getState());
}
};
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 1a3ea64516d7e12bf89a714268f4b01206b3d3e3..a5d1ecbee55a14b1b426326168ea9a64643f7d27 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -77,7 +77,7 @@ inline Eigen::VectorXd StateHomogeneous3d::Identity()
inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
{
if (transformable_)
- setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data()))
+ setState((Eigen::Quaterniond(_transformation.at('O').data()) * Eigen::Quaterniond(getState().data()))
.coeffs()); // TODO is this correct?????? probably not!!!
}
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 56041c0fa157982dbe9b1c3af092de8e93af2a5f..f38d3f7e573e60fd3e44e50e6aa40fd1e2254dac 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -103,7 +103,8 @@ inline Eigen::VectorXd StateQuaternion::identity() const
inline void StateQuaternion::transform(const VectorComposite& _transformation)
{
if (transformable_)
- setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs());
+ setState(
+ (Eigen::Quaterniond(_transformation.at('O').data()) * Eigen::Quaterniond(getState().data())).coeffs());
}
} // namespace wolf
diff --git a/schema/TypeAndPlugin.schema b/schema/TypeAndPlugin.schema
index 26163f23a90dc57569a4e2337b64c01f59e96b66..1a98e1d9541a72fdf5358be24528126d3c04bf68 100644
--- a/schema/TypeAndPlugin.schema
+++ b/schema/TypeAndPlugin.schema
@@ -6,4 +6,4 @@ type:
plugin:
_mandatory: true
_type: string
- _doc: plugin where it is implemented the class
+ _doc: plugin where the class is implemented
diff --git a/schema/landmark/Landmark2d.schema b/schema/landmark/Landmark2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..1bb1894382bcd5af0b9fe53184ccfcecd6f87959
--- /dev/null
+++ b/schema/landmark/Landmark2d.schema
@@ -0,0 +1,12 @@
+follow: LandmarkBase.schema
+
+states:
+ P:
+ _type: StateP2d
+ _mandatory: false
+ _doc: Specifications of the position state.
+
+ O:
+ _type: StateO2d
+ _mandatory: false
+ _doc: Specifications of the orientation state.
\ No newline at end of file
diff --git a/schema/landmark/Landmark3d.schema b/schema/landmark/Landmark3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..c9a69b1d24dc6327ea1bcdd514fde83116862165
--- /dev/null
+++ b/schema/landmark/Landmark3d.schema
@@ -0,0 +1,12 @@
+follow: LandmarkBase.schema
+
+states:
+ P:
+ _type: StateP3d
+ _mandatory: false
+ _doc: Specifications of the position state.
+
+ O:
+ _type: StateO3d
+ _mandatory: false
+ _doc: Specifications of the orientation state.
\ No newline at end of file
diff --git a/schema/landmark/LandmarkBase.schema b/schema/landmark/LandmarkBase.schema
index f575611e05a9b5804a6000034ca192a1125fb4b8..0ef2acb92f8458d1365edf2624c534a1ccab169f 100644
--- a/schema/landmark/LandmarkBase.schema
+++ b/schema/landmark/LandmarkBase.schema
@@ -14,8 +14,12 @@ id:
_type: int
_doc: Unique id of the landmark.
-states:
- keys:
- _mandatory: true
- _type: string
- _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
\ No newline at end of file
+external_id:
+ _mandatory: false
+ _type: int
+ _doc: "Id given by an external tracker via capture. Missing or -1 means untracked."
+
+external_type:
+ _mandatory: false
+ _type: int
+ _doc: "Type given by an external identifier via capture (for example: 1: chair, 2: person...). Missing or -1 means uknown."
\ No newline at end of file
diff --git a/schema/landmark/LandmarkPoint2d.schema b/schema/landmark/LandmarkPoint2d.schema
deleted file mode 100644
index f5fce4aa46359c128e5cdabeccaaabf1da931c63..0000000000000000000000000000000000000000
--- a/schema/landmark/LandmarkPoint2d.schema
+++ /dev/null
@@ -1,10 +0,0 @@
-follow: LandmarkBase.schema
-
-states:
- keys:
- _mandatory: false
- _type: string
- _value: P
- _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
- P:
- follow: SpecStateP2d.schema
\ No newline at end of file
diff --git a/schema/landmark/LandmarkPoint3d.schema b/schema/landmark/LandmarkPoint3d.schema
deleted file mode 100644
index 6e5358563c88b35f800d3aff2688fce2227218db..0000000000000000000000000000000000000000
--- a/schema/landmark/LandmarkPoint3d.schema
+++ /dev/null
@@ -1,10 +0,0 @@
-follow: LandmarkBase.schema
-
-states:
- keys:
- _mandatory: false
- _type: string
- _value: P
- _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
- P:
- follow: SpecStateP3d.schema
\ No newline at end of file
diff --git a/schema/landmark/LandmarkPose2d.schema b/schema/landmark/LandmarkPose2d.schema
deleted file mode 100644
index ba6ba0c8bba84da6693cc839452c94ed77fb4eed..0000000000000000000000000000000000000000
--- a/schema/landmark/LandmarkPose2d.schema
+++ /dev/null
@@ -1,12 +0,0 @@
-follow: LandmarkBase.schema
-
-states:
- keys:
- _mandatory: false
- _type: string
- _value: PO
- _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
- P:
- follow: SpecStateP2d.schema
- O:
- follow: SpecStateO2d.schema
\ No newline at end of file
diff --git a/schema/landmark/LandmarkPose3d.schema b/schema/landmark/LandmarkPose3d.schema
deleted file mode 100644
index 5117cc1886b64599ea2bb424fb773460f9bedb4c..0000000000000000000000000000000000000000
--- a/schema/landmark/LandmarkPose3d.schema
+++ /dev/null
@@ -1,12 +0,0 @@
-follow: LandmarkBase.schema
-
-states:
- keys:
- _mandatory: false
- _type: string
- _value: PO
- _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
- P:
- follow: SpecStateP3d.schema
- O:
- follow: SpecStateO3d.schema
\ No newline at end of file
diff --git a/schema/problem/Problem2d.schema b/schema/problem/Problem2d.schema
index d80d8320e19f81c03d4705c2bcb344b611ae1c67..ab42adc11bdc80aec1ccd2b11b36affa22709a44 100644
--- a/schema/problem/Problem2d.schema
+++ b/schema/problem/Problem2d.schema
@@ -3,16 +3,17 @@ problem:
first_frame:
P:
_mandatory: false
- _type: SpecStateP2d
- _doc: "specification for the first frame P state"
+ _type: StateP2d
+ _doc: "specification for the first frame position state"
O:
_mandatory: false
- _type: SpecStateO2d
- _doc: "specification for the first frame O state"
+ _type: StateO2d
+ _doc: "specification for the first frame orientation state"
V:
_mandatory: false
- _type: SpecStateV2d
- _doc: "specification for the first frame V state"
+ _type: StateV2d
+ _doc: "specification for the first frame linear velocity state"
+
dimension:
_type: int
_mandatory: true
diff --git a/schema/problem/Problem3d.schema b/schema/problem/Problem3d.schema
index ac45046f8720dec4f7c0942875ecf5422144cb72..b4f875f8e69faca9f05305a01f69ba239e2d14ca 100644
--- a/schema/problem/Problem3d.schema
+++ b/schema/problem/Problem3d.schema
@@ -3,16 +3,16 @@ problem:
first_frame:
P:
_mandatory: false
- _type: SpecStateP3d
- _doc: "specification for the first frame P state"
+ _type: StateP3d
+ _doc: "specification for the first frame position state"
O:
_mandatory: false
- _type: SpecStateO3d
- _doc: "specification for the first frame O state"
+ _type: StateO3d
+ _doc: "specification for the first frame orientation state"
V:
_mandatory: false
- _type: SpecStateV3d
- _doc: "specification for the first frame V state"
+ _type: StateV3d
+ _doc: "specification for the first frame linear velocity state"
dimension:
_type: int
_mandatory: true
diff --git a/schema/problem/StateO2d.schema b/schema/problem/StateO2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..3b2ce5ff8a9b8f4cceec90b529d8acde3d245c0d
--- /dev/null
+++ b/schema/problem/StateO2d.schema
@@ -0,0 +1,22 @@
+type:
+ _type: string
+ _mandatory: false
+ _value: StateAngle
+ _doc: "The type of the state for orientation in 2D: StateAngle. Not required to be filled by the user"
+
+value:
+ _type: Vector1d
+ _mandatory: true
+ _doc: A vector containing the orientation, yaw [rad].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: It can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector1d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad].
\ No newline at end of file
diff --git a/schema/problem/StateO3d.schema b/schema/problem/StateO3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..30ac4c6ebc4e4ef2f221999c61a43af9075683dc
--- /dev/null
+++ b/schema/problem/StateO3d.schema
@@ -0,0 +1,22 @@
+type:
+ _type: string
+ _mandatory: false
+ _value: StateQuaternion
+ _doc: "The type of the state for orientation in 3D: StateQuaternion. Not required to be filled by the user"
+
+value:
+ _type: Vector4d
+ _mandatory: true
+ _doc: A vector containing the quaternion values (x, y, z, w)
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad].
\ No newline at end of file
diff --git a/schema/problem/StateP2d.schema b/schema/problem/StateP2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..5aaa8609d4f65814c2414cd8c6a6e9ab38fe4699
--- /dev/null
+++ b/schema/problem/StateP2d.schema
@@ -0,0 +1,22 @@
+type:
+ _type: string
+ _mandatory: false
+ _value: StatePoint2d
+ _doc: "The type of the state for position in 2D: StatePoint2d. Not required to be filled by the user"
+
+value:
+ _type: Vector2d
+ _mandatory: true
+ _doc: A vector containing the position (x, y) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector2d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
\ No newline at end of file
diff --git a/schema/problem/StateP3d.schema b/schema/problem/StateP3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..d62b95f162e3164b11ff35392ae8e676bf92faa0
--- /dev/null
+++ b/schema/problem/StateP3d.schema
@@ -0,0 +1,22 @@
+type:
+ _type: string
+ _mandatory: false
+ _value: StatePoint3d
+ _doc: "The type of the state for position in 3D: StatePoint3d. Not required to be filled by the user"
+
+value:
+ _type: Vector3d
+ _mandatory: true
+ _doc: A vector containing the position (x, y, z) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
\ No newline at end of file
diff --git a/schema/problem/StateV2d.schema b/schema/problem/StateV2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..91bbd8345f3d97a5f2c39b44979bfdcf39546c1c
--- /dev/null
+++ b/schema/problem/StateV2d.schema
@@ -0,0 +1,22 @@
+type:
+ _type: string
+ _mandatory: false
+ _value: StateVector2d
+ _doc: "The type of the state for linear velocity in 2D: StateVector2d. Not required to be filled by the user"
+
+value:
+ _type: Vector2d
+ _mandatory: true
+ _doc: A vector containing the linear velocity components (x, y) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector2d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
\ No newline at end of file
diff --git a/schema/problem/StateV3d.schema b/schema/problem/StateV3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..158fe5b088ea1a6d39b1da092ea970124dd14849
--- /dev/null
+++ b/schema/problem/StateV3d.schema
@@ -0,0 +1,22 @@
+type:
+ _type: string
+ _mandatory: false
+ _value: StateVector3d
+ _doc: "The type of the state for linear velocity in 3D: StateVector3d. Not required to be filled by the user"
+
+value:
+ _type: Vector3d
+ _mandatory: true
+ _doc: A vector containing the linear velocity components (x, y, z) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
\ No newline at end of file
diff --git a/schema/processor/ProcessorLandmarkExternal.schema b/schema/processor/ProcessorLandmarkExternal.schema
index c8afe136ee3be6f5c850dc6fbef1962d6aeddfeb..c929de2307515879a73a2eb51d839f82aafc939e 100644
--- a/schema/processor/ProcessorLandmarkExternal.schema
+++ b/schema/processor/ProcessorLandmarkExternal.schema
@@ -1,15 +1,14 @@
follow: ProcessorTracker.schema
-filter:
- quality_th:
- _mandatory: true
- _type: double
- _doc: "The minimum quality to consider the detection"
+quality_th:
+ _mandatory: true
+ _type: double
+ _doc: "The minimum quality to consider the detection"
- track_length_th:
- _mandatory: true
- _type: unsigned int
- _doc: "The minimum track length to consider the detection"
+track_length_th:
+ _mandatory: true
+ _type: unsigned int
+ _doc: "Minimum track length to emplace factors (necessary condition)"
keyframe_vote:
time_span:
@@ -27,7 +26,27 @@ use_orientation:
_type: bool
_doc: "If the orientation measurement is considered when emplacing factors"
-match_dist_th:
+match_dist_th_id:
+ _mandatory: true
+ _type: double
+ _doc: "Matching distance threshold to previous detection considering motion (necessary condition) when ID matches"
+
+match_dist_th_type:
_mandatory: true
_type: double
- _doc: "the threshold in distance for considering a match between landmarks and detections"
\ No newline at end of file
+ _doc: "Matching distance threshold to previous detection considering motion (necessary condition) when TYPE matches"
+
+match_dist_th_unknown:
+ _mandatory: true
+ _type: double
+ _doc: "Matching distance threshold to previous detection considering motion (necessary condition) when no ID/TYPE information"
+
+close_loops_by_id:
+ _mandatory: true
+ _type: bool
+ _doc: "Close loop if ID matches"
+
+close_loops_by_type:
+ _mandatory: true
+ _type: bool
+ _doc: "Close loop if TYPE matches if distance check holds (match_dist_th_type)"
diff --git a/schema/sensor/SensorBase.schema b/schema/sensor/SensorBase.schema
index 18319a7e7875a8003d89787b56c311b03450b0ad..e9aa53fb7ab04389eb6de20cc184a6ab43d04273 100644
--- a/schema/sensor/SensorBase.schema
+++ b/schema/sensor/SensorBase.schema
@@ -3,10 +3,4 @@ follow: TypeAndPlugin.schema
name:
_mandatory: true
_type: string
- _doc: The sensor's name. It has to be unique.
-
-states:
- keys:
- _mandatory: true
- _type: string
- _doc: The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas.
\ No newline at end of file
+ _doc: The sensor's name. It has to be unique.
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
index 14ff63ce81264765f6cc8dd36cc36839df1c35b0..c504b1758600534fef518cff1770fa87b6b15eee 100644
--- a/schema/sensor/SensorDiffDrive.schema
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -3,7 +3,7 @@ follow: SensorBase.schema
ticks_per_wheel_revolution:
_mandatory: true
_type: double
- _doc: ratio of displacement variance to displacement, for odometry noise calculation.
+ _doc: Amount of sensor ticks of a whole wheel revolution (if measurement is directly radiants, put 2*PI).
ticks_std_factor:
_mandatory: true
@@ -11,31 +11,35 @@ ticks_std_factor:
_doc: ratio of displacement variance to rotation, for odometry noise calculation.
states:
- keys:
- _value: POI
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
P:
- follow: SpecStateSensorP2d.schema
+ follow: StateSensorP2d.schema
O:
- follow: SpecStateSensorO2d.schema
+ follow: StateSensorO2d.schema
I:
- follow: SpecStateSensor.schema
- type:
- _type: string
- _mandatory: false
- _value: StateParams3
- _doc: The type of the SensorDiffDrive intrinsic parameters is StateParam3.
- state:
- _type: Vector3d
+ dynamic:
+ _type: bool
_mandatory: true
- _doc: A vector containing the intrinsic state values. 0=left wheel radius (m), 1=right wheel radius (m), 2=wheel separation (m)
- noise_std:
+ _doc: "If the intrinsic state is dynamic, i.e. it changes along time (0: left wheel radius [m], 1: right wheel radius [m], 2: wheel separation [m])."
+
+ value:
_type: Vector3d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
+ _mandatory: true
+ _doc: "A vector containing the intrinsic state values (0: left wheel radius [m], 1: right wheel radius [m], 2: wheel separation [m])."
+
+ prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: "Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values."
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: "A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]."
+
drift_std:
_type: Vector3d
_mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
+ _doc: "A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]."
+
\ No newline at end of file
diff --git a/schema/sensor/SensorMotionModel.schema b/schema/sensor/SensorMotionModel.schema
index 536be42fbee0425ba36cf7c5bd8c0cd2ecdceb7a..9ff4a8f839cc1bec658f6c111fc6214889d29b27 100644
--- a/schema/sensor/SensorMotionModel.schema
+++ b/schema/sensor/SensorMotionModel.schema
@@ -1,7 +1 @@
-follow: SensorBase.schema
-states:
- keys:
- _value: ""
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
\ No newline at end of file
+follow: SensorBase.schema
\ No newline at end of file
diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema
index 5e74d3d7aeba8d549cbca036e234bca69b3dfc7d..cea2bc8e33161955d98bc320dd2744a3b5ad0d89 100644
--- a/schema/sensor/SensorOdom2d.schema
+++ b/schema/sensor/SensorOdom2d.schema
@@ -26,13 +26,8 @@ min_rot_var:
_doc: minimum rotation variance, for odometry noise calculation.
states:
- keys:
- _value: PO
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
P:
- follow: SpecStateSensorP2d.schema
+ follow: StateSensorP2d.schema
O:
- follow: SpecStateSensorO2d.schema
+ follow: StateSensorO2d.schema
diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema
index dca332c76e77781eee42b0f890125fd11e85256b..5a2f1c1200393af20b0b4fcb10f4785191ce9abf 100644
--- a/schema/sensor/SensorOdom3d.schema
+++ b/schema/sensor/SensorOdom3d.schema
@@ -26,12 +26,7 @@ min_rot_var:
_doc: minimum rotation variance, for odometry noise calculation.
states:
- keys:
- _value: PO
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
P:
- follow: SpecStateSensorP3d.schema
+ follow: StateSensorP3d.schema
O:
- follow: SpecStateSensorO3d.schema
\ No newline at end of file
+ follow: StateSensorO3d.schema
\ No newline at end of file
diff --git a/schema/sensor/SensorPose2d.schema b/schema/sensor/SensorPose2d.schema
index b597a4036ebf3099425f63c35c75e9c3840d48f4..dc53b350a87a83011b19b737fe0e2e8e22d1fccc 100644
--- a/schema/sensor/SensorPose2d.schema
+++ b/schema/sensor/SensorPose2d.schema
@@ -6,13 +6,8 @@ std_noise:
_doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
states:
- keys:
- _value: PO
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
P:
- follow: SpecStateSensorP2d.schema
+ follow: StateSensorP2d.schema
O:
- follow: SpecStateSensorO2d.schema
+ follow: StateSensorO2d.schema
diff --git a/schema/sensor/SensorPose3d.schema b/schema/sensor/SensorPose3d.schema
index f044419086964a0abe2be7cb4d28825b94077974..4864eb85791430e8970f23175f4cabdf9ebc97d0 100644
--- a/schema/sensor/SensorPose3d.schema
+++ b/schema/sensor/SensorPose3d.schema
@@ -6,13 +6,8 @@ std_noise:
_doc: Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal).
states:
- keys:
- _value: PO
- _mandatory: false
- _type: string
- _doc: The keys corresponding to the states of the sensor.
P:
- follow: SpecStateSensorP3d.schema
+ follow: StateSensorP3d.schema
O:
- follow: SpecStateSensorO3d.schema
+ follow: StateSensorO3d.schema
diff --git a/schema/sensor/SpecStateSensor.schema b/schema/sensor/SpecStateSensor.schema
deleted file mode 100644
index 7295f6cd73ae49b20b1c3b86b74562967f2991d5..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensor.schema
+++ /dev/null
@@ -1,11 +0,0 @@
-follow: SpecState.schema
-
-dynamic:
- _type: bool
- _mandatory: true
- _doc: If the state is dynamic, i.e. it changes along time.
-
-drift_std:
- _type: VectorXd
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorO2d.schema b/schema/sensor/SpecStateSensorO2d.schema
deleted file mode 100644
index 4fa80b9cd425f1ee00b70f23c646a81dce081569..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorO2d.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateO2d.schema
-
-drift_std:
- _type: Vector1d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorO3d.schema b/schema/sensor/SpecStateSensorO3d.schema
deleted file mode 100644
index 74747dfd1b301070f32b83aca127160ff6c8f821..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorO3d.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateO3d.schema
-
-drift_std:
- _type: Vector3d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorP2d.schema b/schema/sensor/SpecStateSensorP2d.schema
deleted file mode 100644
index 5391ee50ebb67987ee836034f0846a2d71f79b03..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorP2d.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateP2d.schema
-
-drift_std:
- _type: Vector2d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorP3d.schema b/schema/sensor/SpecStateSensorP3d.schema
deleted file mode 100644
index 7fbc5cba5b32f7948b34ccbf07f4c23d8a503ca3..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorP3d.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateP3d.schema
-
-drift_std:
- _type: Vector3d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams1.schema b/schema/sensor/SpecStateSensorParams1.schema
deleted file mode 100644
index f20932d671813e04aa7504331dd7d98ef379af90..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams1.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams1.schema
-
-drift_std:
- _type: Vector1d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams10.schema b/schema/sensor/SpecStateSensorParams10.schema
deleted file mode 100644
index d1771ca4de9ae95b59fec71a2c44631d0836e1fa..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams10.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams10.schema
-
-drift_std:
- _type: Vector10d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams2.schema b/schema/sensor/SpecStateSensorParams2.schema
deleted file mode 100644
index 8e2a8c598b6d7ba662f1a113727598b38f5c9446..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams2.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams2.schema
-
-drift_std:
- _type: Vector2d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams3.schema b/schema/sensor/SpecStateSensorParams3.schema
deleted file mode 100644
index 3d37203ab1c6c2e57de7a3661511ad93dfe50fb9..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams3.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams3.schema
-
-drift_std:
- _type: Vector3d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams4.schema b/schema/sensor/SpecStateSensorParams4.schema
deleted file mode 100644
index 5899d21181fb7188bd97228cc64350a5e2d3c73d..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams4.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams4.schema
-
-drift_std:
- _type: Vector4d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams5.schema b/schema/sensor/SpecStateSensorParams5.schema
deleted file mode 100644
index f718f767f1cfc64c1181ecd4621e1c78cf8dddf2..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams5.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams5.schema
-
-drift_std:
- _type: Vector5d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams6.schema b/schema/sensor/SpecStateSensorParams6.schema
deleted file mode 100644
index 64bc565d093a18e17b08560cfc3100624ca8bd13..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams6.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams6.schema
-
-drift_std:
- _type: Vector6d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams7.schema b/schema/sensor/SpecStateSensorParams7.schema
deleted file mode 100644
index edf40251ae8f6e2f551ab72709123dd2947985db..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams7.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams7.schema
-
-drift_std:
- _type: Vector7d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams8.schema b/schema/sensor/SpecStateSensorParams8.schema
deleted file mode 100644
index dd3f1dae3ea83d96e0c3d9de1ea889e21a0544e4..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams8.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams8.schema
-
-drift_std:
- _type: Vector8d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/SpecStateSensorParams9.schema b/schema/sensor/SpecStateSensorParams9.schema
deleted file mode 100644
index d1b3ccdcf5a8332335e5279bd527665af88897ba..0000000000000000000000000000000000000000
--- a/schema/sensor/SpecStateSensorParams9.schema
+++ /dev/null
@@ -1,7 +0,0 @@
-follow: SpecStateSensor.schema
-follow: SpecStateParams9.schema
-
-drift_std:
- _type: Vector9d
- _mandatory: false
- _doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/sensor/StateSensorO2d.schema b/schema/sensor/StateSensorO2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..fbc0225766c0fa481dfc5ea923bd95110239aaa1
--- /dev/null
+++ b/schema/sensor/StateSensorO2d.schema
@@ -0,0 +1,26 @@
+dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the orientation is dynamic, i.e. it changes along time.
+
+value:
+ _type: Vector1d
+ _mandatory: true
+ _doc: A vector containing the orientation, yaw [rad].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: It can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector1d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad].
+
+drift_std:
+ _type: Vector1d
+ _mandatory: false
+ _doc: A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) [rad/sqrt(s)].
\ No newline at end of file
diff --git a/schema/sensor/StateSensorO3d.schema b/schema/sensor/StateSensorO3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..aefcaa8ccb96fb96230091f432efe951b3af6505
--- /dev/null
+++ b/schema/sensor/StateSensorO3d.schema
@@ -0,0 +1,26 @@
+dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the orientation is dynamic, i.e. it changes along time.
+
+value:
+ _type: Vector4d
+ _mandatory: true
+ _doc: A vector containing the quaternion values (x, y, z, w)
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad].
+
+drift_std:
+ _type: Vector3d
+ _mandatory: false
+ _doc: A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix [rad/sqrt(s)].
\ No newline at end of file
diff --git a/schema/sensor/StateSensorP2d.schema b/schema/sensor/StateSensorP2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..ec1928268500a07c1aefe85bf9adc405a03d0822
--- /dev/null
+++ b/schema/sensor/StateSensorP2d.schema
@@ -0,0 +1,26 @@
+dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the position is dynamic, i.e. it changes along time.
+
+value:
+ _type: Vector2d
+ _mandatory: true
+ _doc: A vector containing the position (x, y) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector2d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
+
+drift_std:
+ _type: Vector2d
+ _mandatory: false
+ _doc: A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)].
\ No newline at end of file
diff --git a/schema/sensor/StateSensorP3d.schema b/schema/sensor/StateSensorP3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..c4fed2f29a09cf2395d985617c42e849fba413fc
--- /dev/null
+++ b/schema/sensor/StateSensorP3d.schema
@@ -0,0 +1,26 @@
+dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the position is dynamic, i.e. it changes along time.
+
+value:
+ _type: Vector3d
+ _mandatory: true
+ _doc: A vector containing the position (x, y, z) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
+
+drift_std:
+ _type: Vector3d
+ _mandatory: false
+ _doc: A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)].
\ No newline at end of file
diff --git a/schema/state/SpecState.schema b/schema/state/SpecState.schema
deleted file mode 100644
index 51c1d9c425fabbeb9f8a884b8c5577a9cc397878..0000000000000000000000000000000000000000
--- a/schema/state/SpecState.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-type:
- _type: string
- _mandatory: true
- _doc: The derived type of the StateBlock
-state:
- _type: VectorXd
- _mandatory: true
- _doc: A vector containing the state values
-mode:
- _type: string
- _mandatory: true
- _options: ["fix", "factor", "initial_guess"]
- _doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-noise_std:
- _type: VectorXd
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateO2d.schema b/schema/state/SpecStateO2d.schema
deleted file mode 100644
index 2c9754a2a6a482b37634242928470838d701a7cc..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateO2d.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateAngle
- _doc: The derived type of the State in 'O'
-
-state:
- _type: Vector1d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector1d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateO3d.schema b/schema/state/SpecStateO3d.schema
deleted file mode 100644
index 09aebe56106076db6b01dbcb8835481aac90591e..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateO3d.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateQuaternion
- _doc: The derived type of the State in 'O'
-
-state:
- _type: Vector4d
- _mandatory: true
- _doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized)
-
-noise_std:
- _type: Vector3d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateP2d.schema b/schema/state/SpecStateP2d.schema
deleted file mode 100644
index 53b65665dc9e47dcf328a5e0ef57115d5e173d1f..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateP2d.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StatePoint2d
- _doc: The derived type of the state in 'P'
-
-state:
- _type: Vector2d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector2d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateP3d.schema b/schema/state/SpecStateP3d.schema
deleted file mode 100644
index 08803fc22e8733621bf587ceb19d21eb04682ebd..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateP3d.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _mandatory: false
- _type: string
- _value: StatePoint3d
- _doc: The derived type of the state in 'P'
-
-state:
- _type: Vector3d
- _mandatory: true
- _doc: A vector containing the state 'P' values
-
-noise_std:
- _type: Vector3d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams1.schema b/schema/state/SpecStateParams1.schema
deleted file mode 100644
index 427bb3fe538c2fd8cd6a2938d514901da722df75..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams1.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams1
- _doc: The derived type of the state for params of size 1
-
-state:
- _type: Vector1d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector1d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams10.schema b/schema/state/SpecStateParams10.schema
deleted file mode 100644
index 4af637998978a89a7f41557753ea2798d26efc8d..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams10.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams10
- _doc: The derived type of the state for params of size 10
-
-state:
- _type: Vector10d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector10d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams2.schema b/schema/state/SpecStateParams2.schema
deleted file mode 100644
index b9afc8d8993d592cce9f7de392e1e9b01c90d00f..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams2.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams2
- _doc: The derived type of the state for params of size 2
-
-state:
- _type: Vector2d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector2d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams3.schema b/schema/state/SpecStateParams3.schema
deleted file mode 100644
index 15203c8e867655684f63d8988e868c00c30bb57a..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams3.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams3
- _doc: The derived type of the state for params of size 3
-
-state:
- _type: Vector3d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector3d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams4.schema b/schema/state/SpecStateParams4.schema
deleted file mode 100644
index 971d9e7c01a0037e7ff0dc281aa4da5a967046f0..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams4.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams4
- _doc: The derived type of the state for params of size 4
-
-state:
- _type: Vector4d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector4d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams5.schema b/schema/state/SpecStateParams5.schema
deleted file mode 100644
index 75091b913f6e4aa47326b66212c8cd15c9e85e62..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams5.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams5
- _doc: The derived type of the state for params of size 5
-
-state:
- _type: Vector5d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector5d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams6.schema b/schema/state/SpecStateParams6.schema
deleted file mode 100644
index 52ad88a16ab223da42771c7c35fb7f046323070b..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams6.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams6
- _doc: The derived type of the state for params of size 6
-
-state:
- _type: Vector6d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector6d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams7.schema b/schema/state/SpecStateParams7.schema
deleted file mode 100644
index daa2b1ec50dc5e496dcd70594f62254ba5b5cc4d..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams7.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams7
- _doc: The derived type of the state for params of size 7
-
-state:
- _type: Vector7d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector7d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams8.schema b/schema/state/SpecStateParams8.schema
deleted file mode 100644
index affde4bedc8dc48526f5b83bd81a57ce85af622a..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams8.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams8
- _doc: The derived type of the state for params of size 8
-
-state:
- _type: Vector8d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector8d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateParams9.schema b/schema/state/SpecStateParams9.schema
deleted file mode 100644
index 521add8da196ec4c5b4773f12ddbc30fdd31b2a2..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateParams9.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateParams9
- _doc: The derived type of the state for params of size 9
-
-state:
- _type: Vector9d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector9d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateV2d.schema b/schema/state/SpecStateV2d.schema
deleted file mode 100644
index 3e9d3e7f5cbc4d2ee5bbabfc69537a80c3cd18f5..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateV2d.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateVector2d
- _doc: The derived type of the state in 'V'
-
-state:
- _type: Vector2d
- _mandatory: true
- _doc: A vector containing the state values
-
-noise_std:
- _type: Vector2d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/SpecStateV3d.schema b/schema/state/SpecStateV3d.schema
deleted file mode 100644
index cb899721e8e2572c1e95c12d4e17ac3c8347e526..0000000000000000000000000000000000000000
--- a/schema/state/SpecStateV3d.schema
+++ /dev/null
@@ -1,17 +0,0 @@
-follow: SpecState.schema
-
-type:
- _type: string
- _mandatory: false
- _value: StateVector3d
- _doc: The derived type of the state in 'V'
-
-state:
- _type: Vector3d
- _mandatory: true
- _doc: A vector containing the state 'V' values
-
-noise_std:
- _type: Vector3d
- _mandatory: $mode == 'factor'
- _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/state/StateO2d.schema b/schema/state/StateO2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..e7473a5a68c86e9ad668bcab7d26fb84e8f28bd7
--- /dev/null
+++ b/schema/state/StateO2d.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector1d
+ _mandatory: false
+ _doc: A vector containing the orientation, yaw [rad].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: It can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector1d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad].
\ No newline at end of file
diff --git a/schema/state/StateO3d.schema b/schema/state/StateO3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..a11e36a8ca092bdf9f7c303a27e6fff8ad37e0df
--- /dev/null
+++ b/schema/state/StateO3d.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector4d
+ _mandatory: false
+ _doc: A vector containing the quaternion values (x, y, z, w)
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad].
diff --git a/schema/state/StateP2d.schema b/schema/state/StateP2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..2d9875e469d8b434fcae21c347fe9804a085121a
--- /dev/null
+++ b/schema/state/StateP2d.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector2d
+ _mandatory: true
+ _doc: A vector containing the position (x, y) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector2d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
diff --git a/schema/state/StateP3d.schema b/schema/state/StateP3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..84121396effe8d492792bb4a491af7db76356e3f
--- /dev/null
+++ b/schema/state/StateP3d.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector3d
+ _mandatory: false
+ _doc: A vector containing the position (x, y, z) [m].
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
diff --git a/schema/state/StateParams1.schema b/schema/state/StateParams1.schema
new file mode 100644
index 0000000000000000000000000000000000000000..7bc77e64109a2c80218c54e62c88f566ffba6730
--- /dev/null
+++ b/schema/state/StateParams1.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector1d
+ _mandatory: true
+ _doc: A vector containing the state value.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector1d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams10.schema b/schema/state/StateParams10.schema
new file mode 100644
index 0000000000000000000000000000000000000000..3646cc9a7d394f9db0dac523b883772c182abde9
--- /dev/null
+++ b/schema/state/StateParams10.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector10d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector10d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams2.schema b/schema/state/StateParams2.schema
new file mode 100644
index 0000000000000000000000000000000000000000..51d30a93d6bf9804589839e7582ef071d7a7a567
--- /dev/null
+++ b/schema/state/StateParams2.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector2d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector2d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams3.schema b/schema/state/StateParams3.schema
new file mode 100644
index 0000000000000000000000000000000000000000..867810176b50c1fc45ce31bc22df52e10a1ae052
--- /dev/null
+++ b/schema/state/StateParams3.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector3d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector3d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams4.schema b/schema/state/StateParams4.schema
new file mode 100644
index 0000000000000000000000000000000000000000..8298ddfc74e01508f279c36caf33c8981e650f13
--- /dev/null
+++ b/schema/state/StateParams4.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector4d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector4d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams5.schema b/schema/state/StateParams5.schema
new file mode 100644
index 0000000000000000000000000000000000000000..2b83c55929331a91d744735b8e747599f47c74dd
--- /dev/null
+++ b/schema/state/StateParams5.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector5d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector5d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams6.schema b/schema/state/StateParams6.schema
new file mode 100644
index 0000000000000000000000000000000000000000..a7cb2e9c77c4f77d2cbb827544c79aa72cab90e5
--- /dev/null
+++ b/schema/state/StateParams6.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector6d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector6d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams7.schema b/schema/state/StateParams7.schema
new file mode 100644
index 0000000000000000000000000000000000000000..f97d2cad13ae94575cbf71d3e00a7e24da177b8c
--- /dev/null
+++ b/schema/state/StateParams7.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector7d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector7d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams8.schema b/schema/state/StateParams8.schema
new file mode 100644
index 0000000000000000000000000000000000000000..0bdc87a4c3d9718c6e5ccca4f8c75b1a79c71eb0
--- /dev/null
+++ b/schema/state/StateParams8.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector8d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector8d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/schema/state/StateParams9.schema b/schema/state/StateParams9.schema
new file mode 100644
index 0000000000000000000000000000000000000000..2c8e3fbdc8a46bad4de55746add74289a762460d
--- /dev/null
+++ b/schema/state/StateParams9.schema
@@ -0,0 +1,16 @@
+value:
+ _type: Vector9d
+ _mandatory: true
+ _doc: A vector containing the state values.
+
+prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+
+ factor_std:
+ _type: Vector9d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 0184e90527bb92522aa3d72c389884781d4f1386..aede1ee3caf975108f2f229eb2f16c5f422d94f0 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -27,75 +27,48 @@ using namespace Eigen;
unsigned int CaptureBase::capture_id_count_ = 0;
-CaptureBase::CaptureBase(const std::string& _type,
- const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr)
- : NodeStateBlocks("CAPTURE", _type),
+CaptureBase::CaptureBase(const std::string& _type,
+ const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors)
+ : NodeStateBlocks("CAPTURE", _type, _state_types, _state_vectors, _state_priors),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
{
assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!");
- if (_sensor_ptr)
- {
- if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic('P'))
- {
- WOLF_ERROR_COND(not _p_ptr,
- "In CaptureBase constructor of type ",
- _type,
- " the sensor ",
- _sensor_ptr->getType(),
- " ",
- _sensor_ptr->getName(),
- " has state P dynamic but provided _p_ptr is nullptr")
- assert(_p_ptr && "Pointer to dynamic position params is null!");
- addStateBlock('P', _p_ptr);
- }
- else
- assert(_p_ptr == nullptr &&
- "Provided dynamic sensor position but the sensor position is static or doesn't exist");
- if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic('O'))
- {
- WOLF_ERROR_COND(not _o_ptr,
- "In CaptureBase constructor of type ",
- _type,
- " the sensor ",
- _sensor_ptr->getType(),
- " ",
- _sensor_ptr->getName(),
- " has state O dynamic but provided _o_ptr is nullptr")
- assert(_o_ptr && "Pointer to dynamic orientation params is null!");
- addStateBlock('O', _o_ptr);
- }
- else
- assert(_o_ptr == nullptr &&
- "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
+ // states provided but no sensor
+ WOLF_ERROR_COND(
+ not _sensor_ptr and not _state_vectors.empty(),
+ "In CaptureBase constructor of type " + _type + ": sensor not provided (nullptr) but states provided");
- if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic('I'))
+ // Check that provided states are dynamic
+ if (_sensor_ptr)
+ {
+ for (auto pair_vector : _state_vectors)
{
- WOLF_ERROR_COND(not _intr_ptr,
- "In CaptureBase constructor of type ",
- _type,
- " the sensor ",
- _sensor_ptr->getType(),
- " ",
- _sensor_ptr->getName(),
- " has state I dynamic but provided _i_ptr is nullptr")
- assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
- addStateBlock('I', _intr_ptr);
+ char key = pair_vector.first;
+ // missing state
+ if (not _sensor_ptr->getStateBlock(key))
+ throw std::runtime_error(std::string("In CaptureBase constructor of type ") + _type + " the sensor " +
+ _sensor_ptr->getType() + " " + _sensor_ptr->getName() +
+ " do not have state " + std::string(1, key));
+ // static state
+ if (not _sensor_ptr->isStateBlockDynamic(key))
+ throw std::runtime_error(std::string("In CaptureBase constructor of type ") + _type + " the sensor " +
+ _sensor_ptr->getType() + " " + _sensor_ptr->getName() + " state " +
+ std::string(1, key) + " is static");
+ // wrong type
+ if (_state_types.at(key) != _sensor_ptr->getStateTypes().at(key))
+ throw std::runtime_error(std::string("In CaptureBase constructor of type ") + _type + " the sensor " +
+ _sensor_ptr->getType() + " " + _sensor_ptr->getName() + " state " +
+ std::string(1, key) + " has type " + _sensor_ptr->getStateTypes().at(key) +
+ " (provided " + _state_types.at(key) + ")");
}
- else
- assert(_intr_ptr == nullptr &&
- "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
- }
- else if (_p_ptr || _o_ptr || _intr_ptr)
- {
- WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
}
@@ -140,6 +113,14 @@ bool CaptureBase::hasChildren() const
return NodeStateBlocks::hasChildren() or not feature_list_.empty();
}
+void CaptureBase::emplaceDrifts(CaptureBasePtr _capture_origin)
+{
+ if (not getSensor())
+ throw std::runtime_error("Attempting to call emplaceDrifts to a capture with no associated sensor.");
+
+ // TODO (inspiration from ProcessorImu)
+}
+
bool CaptureBase::process()
{
assert(getSensor() != nullptr &&
@@ -394,7 +375,7 @@ CheckLog CaptureBase::localCheck(bool _verbose, std::ostream& _stream, std::stri
}
// check NodeStateBlocks
- auto node_log = checkStatesAndFactoredBy(_verbose, _stream, _tabs);
+ auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
log.compose(node_log);
return log;
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index fc3aa3afcbcb4580ff2dc553fecc0ada76705e04..ee5c2a5042e51fef09afbc9bc4fec2b522215ae3 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -28,18 +28,17 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr)
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors)
: CaptureMotion("CaptureDiffDrive",
_ts,
_sensor_ptr,
_data,
_data_cov,
_capture_origin_ptr,
- _p_ptr,
- _o_ptr,
- _intr_ptr)
+ {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'I', "StateParams3"}},
+ _state_vectors,
+ _state_priors)
{
//
}
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
index b7b222d96f6fde58eaf411373e3500dfc50300a2..c1cbbd0ddd8d8a5d00c8782ad8d5d31d85788ba5 100644
--- a/src/capture/capture_landmarks_external.cpp
+++ b/src/capture/capture_landmarks_external.cpp
@@ -25,6 +25,7 @@ namespace wolf
CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const std::vector<int>& _ids,
+ const std::vector<int>& _types,
const std::vector<Eigen::VectorXd>& _detections,
const std::vector<Eigen::MatrixXd>& _covs,
const std::vector<double>& _qualities)
@@ -36,7 +37,7 @@ CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp&
"size.");
for (auto i = 0; i < _detections.size(); i++)
- addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
+ addDetection(_ids.at(i), _types.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
}
CaptureLandmarksExternal::~CaptureLandmarksExternal()
@@ -45,11 +46,12 @@ CaptureLandmarksExternal::~CaptureLandmarksExternal()
}
void CaptureLandmarksExternal::addDetection(const int& _id,
+ const int& _type,
const Eigen::VectorXd& _detection,
const Eigen::MatrixXd& _cov,
const double& _quality)
{
- detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
+ detections_.push_back(LandmarkDetection{_id, _type, _detection, _cov, _quality});
}
} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 1748a2fac139aedf443cf221534e0b565a38c288..210a193e195ddb8d692b3102300adf79c75c41fc 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -26,8 +26,11 @@ CaptureMotion::CaptureMotion(const std::string& _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- CaptureBasePtr _capture_origin_ptr)
- : CaptureBase(_type, _ts, _sensor_ptr),
+ CaptureBasePtr _capture_origin_ptr,
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors)
+ : CaptureBase(_type, _ts, _sensor_ptr, _state_types, _state_vectors, _state_priors),
data_(_data),
data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data)
: Eigen::MatrixXd::Zero(
@@ -36,7 +39,6 @@ CaptureMotion::CaptureMotion(const std::string& _type,
buffer_(),
capture_origin_ptr_(_capture_origin_ptr)
{
- //
}
CaptureMotion::CaptureMotion(const std::string& _type,
@@ -45,16 +47,15 @@ CaptureMotion::CaptureMotion(const std::string& _type,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr)
- : CaptureBase(_type, _ts, _sensor_ptr, _p_ptr, _o_ptr, _intr_ptr),
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors)
+ : CaptureBase(_type, _ts, _sensor_ptr, _state_types, _state_vectors, _state_priors),
data_(_data),
data_cov_(_data_cov),
buffer_(),
capture_origin_ptr_(_capture_origin_ptr)
{
- //
}
CaptureMotion::~CaptureMotion()
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
index 48826bd7f0712398329d015ef547fb4bea9af614..76ad1ac9ce15fcf78f36e731c922191c5a40ca03 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -24,14 +24,31 @@
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
+using namespace Eigen;
+
namespace wolf
{
-NodeStateBlocks::NodeStateBlocks(const std::string& _category,
- const std::string& _type,
- const SpecStateComposite& _specs)
- : NodeBase(_category, _type)
+NodeStateBlocks::NodeStateBlocks(const std::string& _category,
+ const std::string& _type,
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors)
+ : NodeBase(_category, _type), state_priors_(_state_priors), state_types_(_state_types)
{
- emplaceStateBlocks(_specs);
+ if (not _state_types.has(_state_vectors.getKeys()))
+ {
+ throw std::runtime_error("NodeStateBlocks: In constructor of type " + _type +
+ ", provided any '_state_vectors' (" + _state_vectors.getKeys() +
+ ") that is not in '_state_types' (" + _state_types.getKeys() + ").");
+ }
+
+ checkTypeComposite(_state_types);
+
+ for (auto vec_pair : _state_vectors)
+ emplaceStateBlock(vec_pair.first,
+ _state_types.at(vec_pair.first),
+ vec_pair.second,
+ _state_priors.count(vec_pair.first) ? _state_priors.at(vec_pair.first).isFixed() : false);
}
void NodeStateBlocks::remove(bool viral_remove_parent_without_children)
@@ -61,79 +78,45 @@ void NodeStateBlocks::remove(bool viral_remove_parent_without_children)
factor_prior_map_.clear();
}
-SpecStateComposite NodeStateBlocks::getSpecs() const
+StateBlockPtr NodeStateBlocks::emplaceStateBlock(const char _key,
+ const std::string& _type,
+ const VectorXd& _vector,
+ const bool& _fixed)
{
- SpecStateComposite specs;
- for (auto&& state_pair : state_block_composite_)
+ // check key available
+ if (state_blocks_.count(_key))
{
- specs.emplace(state_pair.first,
- SpecState(state_pair.second->getType(),
- state_pair.second->getState(),
- state_pair.second->isFixed() ? "fix" : "initial_guess"));
+ throw std::runtime_error(std::string("NodeStateBlocks::emplaceStateBlock: key ") + _key +
+ " already in the node.");
}
- return specs;
-}
-
-void NodeStateBlocks::emplaceStateBlocks(const SpecStateComposite& _specs)
-{
- for (auto spec_pair : _specs)
+ // check correct type
+ checkTypeComposite({{_key, _type}});
+ if (state_types_.count(_key))
{
- auto key = spec_pair.first;
- auto prior = spec_pair.second;
-
- // checks
- if (key == 'P' and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d")
- {
- throw std::runtime_error(
- "NodeStateBlocks::emplaceStateBlocks: in key 'P', the state block should be of type 'StatePoint2d' or "
- "'StatePoint3d'");
- }
- if (key == 'O' and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion")
- {
+ if (state_types_.at(_key) != _type)
throw std::runtime_error(
- "NodeStateBlocks::emplaceStateBlocks: in key 'O', the state block should be of type 'StateAngle' or "
- "'StateQuaternion'");
- }
- if (key == 'V' and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d")
- {
- throw std::runtime_error(
- "NodeStateBlocks::emplaceStateBlocks: in key 'V', the state block should be of type 'StateVector2d' "
- "or "
- "'StateVector3d'");
- }
- if (state_block_composite_.count(key))
- {
- throw std::runtime_error(std::string("NodeStateBlocks::emplaceStateBlocks: key ") + key +
- " already in the node.");
- }
-
- // create and add state block
- addStateBlock(key, FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed()));
+ std::string("NodeStateBlocks::emplaceStateBlock: Wrong input type: " + _type + " for key ") + _key +
+ ". It should be " + state_types_.at(_key));
}
-}
-
-void NodeStateBlocks::emplaceStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors, bool _fix)
-{
- emplaceStateBlocks(SpecStateComposite(_types, _vectors, _fix));
-}
+ else
+ state_types_[_key] = _type;
-StateBlockPtr NodeStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb)
-{
- assert(state_block_composite_.count(_sb_key) == 0 and
- "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ // create state block
+ auto sb = FactoryStateBlock::create(_type, _vector, _fixed);
- state_block_composite_.emplace(_sb_key, _sb);
+ // store state block
+ state_blocks_.emplace(_key, sb);
- // conditionally register to problem
- if (getProblem()) getProblem()->notifyStateBlock(_sb, ADD);
+ // register to problem (if linked)
+ if (getProblem()) getProblem()->notifyStateBlock(sb, ADD);
- return _sb;
+ return sb;
}
-void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
- const Eigen::VectorXd& _x,
- const Eigen::MatrixXd& _cov,
- unsigned int _start_idx)
+void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
+ const VectorXd& _x,
+ const MatrixXd& _cov,
+ unsigned int _start_idx)
{
assert(isCovariance(_cov) and
"NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix");
@@ -150,13 +133,12 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
// not segment of state not allowed
if (_start_idx != 0)
throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different "
- "from "
- "size "
- "not allowed");
- // state size
+ "NodeStateBlocks::emplaceFactorStateBlock: Prior factor for a segment of a state with local size "
+ "different "
+ "from size not allowed");
+ // meas size
if (_x.size() != _sb->getSize())
- throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad state size");
+ throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad measurement size");
// cov size
if (_cov.cols() != _sb->getLocalSize())
throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad covariance size");
@@ -168,19 +150,15 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
throw std::runtime_error(
"NodeStateBlocks::emplaceFactorStateBlock: Inconsistend measurement and covariance sizes");
- // state size
+ // meas size
if (_x.size() + _start_idx > _sb->getSize())
- throw std::runtime_error(
- "NodeStateBlocks::emplaceFactorStateBlock: Prior for a segment of a state with local size different "
- "from "
- "size "
- "not allowed");
+ throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad measurement size");
}
// existing prior
if (factor_prior_map_.count(_key))
throw std::runtime_error(
- std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior in this key ") +
+ std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior for this key ") +
_key + ", factor " + toString(factor_prior_map_.at(_key)->id()));
// SET STATE
@@ -188,7 +166,7 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
_sb->setState(_x);
else
{
- Eigen::VectorXd new_x = _sb->getState();
+ VectorXd new_x = _sb->getState();
new_x.segment(_start_idx, _x.size()) = _x;
_sb->setState(new_x);
}
@@ -214,68 +192,90 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
}
}
-void NodeStateBlocks::emplacePriors(const SpecStateComposite& _specs)
+void NodeStateBlocks::emplacePriors()
{
- for (auto spec_pair : _specs)
+ WOLF_INFO_COND(state_priors_.empty(),
+ "NodeStateBlocks: Emplacing priors of ",
+ this->getType(),
+ " named ",
+ this->getName(),
+ ". No priors stored (either not specified or already applied)");
+
+ for (auto prior_pair : state_priors_)
{
- auto key = spec_pair.first;
- auto prior = spec_pair.second;
-
- // checks
- if (key == 'P' and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d")
- {
- throw std::runtime_error(
- "NodeStateBlocks::emplacePriors: in key 'P', the state block should be of type 'StatePoint2d' "
- "or "
- "'StatePoint3d'");
- }
- if (key == 'O' and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion")
- {
- throw std::runtime_error(
- "NodeStateBlocks::emplacePriors: in key 'O', the state block should be of type 'StateAngle' or "
- "'StateQuaternion'");
- }
- if (key == 'V' and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d")
- {
- throw std::runtime_error(
- "NodeStateBlocks::emplacePriors: in key 'V', the state block should be of type 'StateVector2d' "
- "or "
- "'StateVector3d'");
- }
+ auto key = prior_pair.first;
+ auto prior = prior_pair.second;
// ignore priors to missing keys
- if (not state_block_composite_.count(key))
+ if (not state_blocks_.count(key))
{
WOLF_WARN("NodeStateBlocks::emplacePriors: key ", key, " not in the node. Ignoring prior for this key.");
continue;
}
// fix (if specified)
- if (prior.isFixed()) state_block_composite_.at(key)->fix();
+ if (prior.isFixed()) state_blocks_.at(key)->fix();
// emplace factor prior (if specified)
+ WOLF_DEBUG_COND(prior.isFactor(),
+ "NodeStateBlocks::emplacePriors: Emplacing factor in '",
+ key,
+ "' with factor_std: ",
+ prior.getFactorStd().transpose());
if (prior.isFactor())
- emplaceFactorStateBlock(key, prior.getState(), prior.getNoiseStd().cwiseAbs2().asDiagonal());
+ emplaceFactorStateBlock(
+ key, state_blocks_.at(key)->getState(), prior.getFactorStd().cwiseAbs2().asDiagonal());
+ }
+ state_priors_.clear();
+}
+
+YAML::Node NodeStateBlocks::toYaml() const
+{
+ YAML::Node node;
- // set state (already set inside emplaceFactorStateBlock)
+ node["type"] = NodeBase::node_type_;
+
+ for (auto&& state_pair : state_blocks_)
+ {
+ auto key_str = std::string(1, state_pair.first);
+ node["states"][key_str]["type"] = state_pair.second->getType();
+ node["states"][key_str]["value"] = state_pair.second->getState();
+ if (factor_prior_map_.count(state_pair.first) != 0)
+ {
+ node["states"][key_str]["prior"]["mode"] = "factor";
+ MatrixXd cov_sqrt =
+ factor_prior_map_.at(state_pair.first)->getMeasurementSquareRootInformationUpper().inverse();
+ node["states"][key_str]["prior"]["factor_std"] =
+ VectorXd((cov_sqrt * cov_sqrt.transpose()).diagonal().cwiseSqrt());
+ WOLF_DEBUG("info_sqrt: \n",
+ factor_prior_map_.at(state_pair.first)->getMeasurementSquareRootInformationUpper());
+ WOLF_DEBUG("cov_sqrt: \n", cov_sqrt);
+ WOLF_DEBUG("cov: \n", cov_sqrt * cov_sqrt.transpose());
+ WOLF_DEBUG("factor_std: \n", VectorXd((cov_sqrt * cov_sqrt.transpose()).diagonal()).transpose());
+ }
else
- state_block_composite_.at(key)->setState(prior.getState());
+ node["states"][key_str]["prior"]["mode"] = state_pair.second->isFixed() ? "fix" : "initial_guess";
}
+
+ return node;
}
void NodeStateBlocks::setProblem(ProblemPtr _problem)
{
+ if (getProblem() or not _problem) return;
+
NodeBase::setProblem(_problem);
// register state blocks
- if (_problem != nullptr)
- {
- for (auto pair_key_sbp : getStateBlockMap())
- if (pair_key_sbp.second != nullptr) _problem->notifyStateBlock(pair_key_sbp.second, ADD);
- }
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr) _problem->notifyStateBlock(pair_key_sbp.second, ADD);
+
+ // register prior factors
+ for (auto pair_key_prior : getPriorFactorMap())
+ if (pair_key_prior.second != nullptr) _problem->notifyFactor(pair_key_prior.second, ADD);
}
-bool NodeStateBlocks::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
+bool NodeStateBlocks::getCovariance(const StateKeys& _keys, MatrixXd& _cov) const
{
if (not this->has(_keys))
{
@@ -342,7 +342,7 @@ void NodeStateBlocks::setState(const VectorComposite& _state, const bool _notify
}
}
-void NodeStateBlocks::setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify)
+void NodeStateBlocks::setState(const VectorXd& _state, const StateKeys& _keys, const bool _notify)
{
int size = getSize(_keys);
assert(_state.size() == size and "In FrameBase::setState wrong state size");
@@ -359,7 +359,7 @@ void NodeStateBlocks::setState(const Eigen::VectorXd& _state, const StateKeys& _
}
}
-void NodeStateBlocks::setState(const Eigen::VectorXd& _state,
+void NodeStateBlocks::setState(const VectorXd& _state,
const StateKeys& _keys,
const std::list<SizeEigen>& _sizes,
const bool _notify)
@@ -418,9 +418,9 @@ VectorComposite NodeStateBlocks::getState(const StateKeys& _keys) const
for (const auto key : keys)
{
- auto state_it = state_block_composite_.find(key);
+ auto state_it = state_blocks_.find(key);
- if (state_it != state_block_composite_.end()) state.emplace(key, state_it->second->getState());
+ if (state_it != state_blocks_.end()) state.emplace(key, state_it->second->getState());
}
return state;
@@ -446,27 +446,27 @@ unsigned int NodeStateBlocks::getSize(const StateKeys& _keys) const
bool NodeStateBlocks::isFixed() const
{
- return std::all_of(state_block_composite_.cbegin(),
- state_block_composite_.cend(),
+ return std::all_of(state_blocks_.cbegin(),
+ state_blocks_.cend(),
[](const std::pair<char, StateBlockPtr>& sb_pair) { return sb_pair.second->isFixed(); });
}
bool NodeStateBlocks::hasStateBlock(const StateBlockConstPtr& _sb) const
{
- const auto& it = std::find_if(state_block_composite_.cbegin(),
- state_block_composite_.cend(),
+ const auto& it = std::find_if(state_blocks_.cbegin(),
+ state_blocks_.cend(),
[_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
- return it != state_block_composite_.cend();
+ return it != state_blocks_.cend();
}
bool NodeStateBlocks::stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const
{
- const auto& it = std::find_if(state_block_composite_.cbegin(),
- state_block_composite_.cend(),
+ const auto& it = std::find_if(state_blocks_.cbegin(),
+ state_blocks_.cend(),
[_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
- if (it != state_block_composite_.cend())
+ if (it != state_blocks_.cend())
{
_key = it->first;
return true;
@@ -522,7 +522,7 @@ void NodeStateBlocks::removeFactor(FactorBasePtr _fac_ptr)
void NodeStateBlocks::perturb(double amplitude)
{
- for (const auto& pair_key_sb : state_block_composite_)
+ for (const auto& pair_key_sb : state_blocks_)
{
auto& sb = pair_key_sb.second;
if (!sb->isFixed()) sb->perturb(amplitude);
@@ -531,7 +531,7 @@ void NodeStateBlocks::perturb(double amplitude)
void NodeStateBlocks::transform(const VectorComposite& _transformation)
{
- for (auto& pair_key_sb : state_block_composite_) pair_key_sb.second->transform(_transformation);
+ for (auto& pair_key_sb : state_blocks_) pair_key_sb.second->transform(_transformation);
}
void NodeStateBlocks::printState(bool _factored_by,
@@ -586,7 +586,7 @@ void NodeStateBlocks::printState(bool _factored_by,
}
}
-CheckLog NodeStateBlocks::checkStatesAndFactoredBy(bool _verbose, std::ostream& _stream, std::string _tabs) const
+CheckLog NodeStateBlocks::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
diff --git a/src/composite/matrix_composite.cpp b/src/composite/matrix_composite.cpp
deleted file mode 100644
index e56388e006c3db55f8390394fa20cf9d1d39bd24..0000000000000000000000000000000000000000
--- a/src/composite/matrix_composite.cpp
+++ /dev/null
@@ -1,571 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/composite/matrix_composite.h"
-
-namespace wolf
-{
-bool MatrixComposite::emplace(const char& _row, const char& _col, const Eigen::MatrixXd& _mat_blk)
-{
- // check rows
- if (size_rows_.count(_row) == 0)
- size_rows_[_row] = _mat_blk.rows();
- else
- assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
-
- // check cols
- if (size_cols_.count(_col) == 0)
- size_cols_[_col] = _mat_blk.cols();
- else
- assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
-
- // now it's safe to use [] operator. the first one is a function-like call to [] -- just weird for this->[]
- this->operator[](_row)[_col] = _mat_blk;
- return true;
-}
-
-bool MatrixComposite::at(const char& _row, const char& _col, Eigen::MatrixXd& _mat_blk) const
-{
- auto row_it = this->find(_row);
- if (row_it != this->end()) return false;
-
- auto col_it = row_it->second.find(_col);
- if (col_it != row_it->second.end()) return false;
-
- _mat_blk = col_it->second;
-
- return true;
-}
-
-Eigen::MatrixXd& MatrixComposite::at(const char& _row, const char& _col)
-{
- auto row_it = this->find(_row);
- assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
-
- auto col_it = row_it->second.find(_col);
- assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
-
- return col_it->second;
-}
-
-const Eigen::MatrixXd& MatrixComposite::at(const char& _row, const char& _col) const
-{
- auto row_it = this->find(_row);
- assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
-
- auto col_it = row_it->second.find(_col);
- assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
-
- return col_it->second;
-}
-
-wolf::MatrixComposite MatrixComposite::operator*(const MatrixComposite& _N) const
-{
- MatrixComposite MN;
- for (const auto& pair_i_Mi : (*this))
- {
- const auto& i = pair_i_Mi.first;
- const auto& Mi = pair_i_Mi.second;
-
- for (const auto& pair_k_Nk : _N)
- {
- const auto& k = pair_k_Nk.first;
- const auto& Nk = pair_k_Nk.second;
-
- for (const auto& pair_j_Nkj : Nk)
- {
- const auto& j = pair_j_Nkj.first;
- const auto& Nkj = pair_j_Nkj.second;
- const auto& Mik = Mi.at(k);
-
- if (MN.count(i, j) == 0)
- MN.emplace(i, j, Mik * Nkj);
- else
- MN.at(i, j) += Mik * Nkj;
- }
- }
- }
- return MN;
-}
-
-wolf::MatrixComposite MatrixComposite::operator+(const MatrixComposite& _N) const
-{
- MatrixComposite MpN;
- for (const auto& pair_i_Mi : *this)
- {
- const auto& i = pair_i_Mi.first;
- const auto& Mi = pair_i_Mi.second;
-
- for (const auto& pair_j_Mij : Mi)
- {
- const auto& j = pair_j_Mij.first;
- const auto& Mij = pair_j_Mij.second;
- const auto& Nij = _N.at(i, j);
-
- MpN.emplace(i, j, Mij + Nij);
- }
- }
- return MpN;
-}
-
-wolf::MatrixComposite MatrixComposite::operator-(const MatrixComposite& _N) const
-{
- MatrixComposite MpN;
- for (const auto& pair_i_Mi : *this)
- {
- const auto& i = pair_i_Mi.first;
- const auto& Mi = pair_i_Mi.second;
-
- for (const auto& pair_j_Mij : Mi)
- {
- const auto& j = pair_j_Mij.first;
- const auto& Mij = pair_j_Mij.second;
- const auto& Nij = _N.at(i, j);
-
- MpN.emplace(i, j, Mij - Nij);
- }
- }
- return MpN;
-}
-
-MatrixComposite MatrixComposite::operator-(void) const
-{
- MatrixComposite m;
-
- for (const auto& pair_rkey_row : *this)
- {
- m.unordered_map<char, unordered_map<char, MatrixXd>>::emplace(pair_rkey_row.first,
- unordered_map<char, MatrixXd>());
- for (const auto& pair_ckey_blk : pair_rkey_row.second)
- {
- m[pair_rkey_row.first].emplace(pair_ckey_blk.first, -pair_ckey_blk.second);
- }
- }
- return m;
-}
-
-wolf::VectorComposite MatrixComposite::operator*(const VectorComposite& _x) const
-{
- VectorComposite y;
- for (const auto& pair_rkey_row : *this)
- {
- const auto& rkey = pair_rkey_row.first;
- const auto& row = pair_rkey_row.second;
-
- for (const auto& pair_ckey_mat : row)
- {
- const auto& ckey = pair_ckey_mat.first;
- const auto& J_r_c = pair_ckey_mat.second;
-
- const auto& it = y.find(rkey);
- if (it != y.end())
- it->second += J_r_c * _x.at(ckey);
- else
- y.emplace(rkey, J_r_c * _x.at(ckey));
- }
- }
- return y;
-}
-
-MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
-{
- // simplify names of operands
- const auto& J = *this;
- const auto& Q = _Cov;
-
- MatrixComposite S; // S = J * Q * J.tr
-
- /* Covariance propagation
- *
- * 1. General formula for the output block S(i,j)
- *
- * S_ij = sum_n (J_in * sum_k(Q_nk * J_jk^T)) (A)
- *
- * which develops as
- *
- * S_ij = sum_n (J_in * QJt_nj)
- *
- * with:
- *
- * QJt_nj = sum_k (Q_nk * J_jk^T) (B)
- *
- *
- * 2. Algorithm to accomplish the above:
- *
- * for i = 1 : S.rows = J.rows (1) // iterate for i and j
- * {
- * J_i = J[i]
- * for j = i : S.cols = J.rows (2) // j starts at i: do not loop over the symmetrical blocks
- * {
- * S_ij = 0 (3) // start formula (A) for S_ij
- * for n = 1 : Q.rows (4)
- * {
- * J_in = J[i][n]
- * QJt_nj = 0 (5) // start formula (B) for QJt_nj
- * for k = 1 : Q.cols (6)
- * {
- * J_jk = J[j][k]
- * QJt_nj += Q_nk * J_jk^T (7) // sum on QJt_nj
- * }
- * S_ij += J_in * QJt_nj (8) // sum on S_ij
- * }
- * S[i][j] = S_ij // write block in output composite
- * if (i != j)
- * S[j][i] = S_ij^T (9) // write symmetrical block in output composite
- * }
- * }
- */
-
- // Iterate on the output matrix S first, row i, col j.
- for (const auto& pair_i_Si : J) // (1)
- {
- const auto& i = pair_i_Si.first;
- const auto& J_i = pair_i_Si.second;
-
- double i_size = J_i.begin()->second.rows();
-
- for (const auto& pair_j_Sj : J) // (2)
- {
- const auto& j = pair_j_Sj.first;
- const auto& J_j = pair_j_Sj.second;
-
- double j_size = J_j.begin()->second.rows();
-
- MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3)
-
- for (const auto& pair_n_Qn : Q) // (4)
- {
- const auto& n = pair_n_Qn.first;
- const auto& Q_n = pair_n_Qn.second;
-
- double n_size = Q_n.begin()->second.rows();
-
- const auto& J_in = J_i.at(n);
-
- MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5)
-
- for (const auto& pair_k_Qnk : Q_n) // (6)
- {
- const auto& k = pair_k_Qnk.first;
- const auto& Q_nk = pair_k_Qnk.second;
-
- const auto& J_jk = J_j.at(k);
-
- QJt_nj += Q_nk * J_jk.transpose(); // (7)
- }
-
- S_ij += J_in * QJt_nj; // (8)
- }
-
- S.emplace(i, j, S_ij);
- if (j == i)
- break; // avoid computing the symmetrical block!
- else
- S.emplace(j, i, S_ij.transpose()); // (9)
- }
- }
-
- return S;
-}
-
-MatrixComposite MatrixComposite::operator*(double scalar_right) const
-{
- MatrixComposite S(*this);
- for (const auto& pair_rkey_row : *this)
- {
- const auto& rkey = pair_rkey_row.first;
- for (const auto& pair_ckey_block : pair_rkey_row.second)
- {
- const auto ckey = pair_ckey_block.first;
- S[rkey][ckey] *= scalar_right;
- }
- }
- return S;
-}
-
-MatrixComposite operator*(double scalar_left, const MatrixComposite& M)
-{
- MatrixComposite S;
- S = M * scalar_left;
- return S;
-}
-
-MatrixXd MatrixComposite::matrix(const StateKeys& _struct_rows, const StateKeys& _struct_cols) const
-{
- std::unordered_map<char, unsigned int> indices_rows;
- std::unordered_map<char, unsigned int> indices_cols;
- unsigned int rows, cols;
-
- sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
-
- MatrixXd M(MatrixXd::Zero(rows, cols));
-
- for (const auto& pair_row_rband : (*this))
- {
- const auto& row = pair_row_rband.first;
- const auto& rband = pair_row_rband.second;
- for (const auto& pair_col_blk : rband)
- {
- const auto& col = pair_col_blk.first;
- const auto& blk = pair_col_blk.second;
-
- const unsigned int& blk_rows = size_rows_.at(row);
- const unsigned int& blk_cols = size_cols_.at(col);
-
- assert(blk.rows() == blk_rows && "MatrixComposite block size mismatch! Wrong number of rows.");
- assert(blk.cols() == blk_cols && "MatrixComposite block size mismatch! Wrong number of cols.");
-
- M.block(indices_rows.at(row), indices_cols.at(col), blk_rows, blk_cols) = blk;
- }
- }
-
- return M;
-}
-
-unsigned int MatrixComposite::rows() const
-{
- unsigned int rows = 0;
- for (const auto& pair_row_size : this->size_rows_) rows += pair_row_size.second;
- return rows;
-}
-
-unsigned int MatrixComposite::cols() const
-{
- unsigned int cols = 0;
- for (const auto& pair_col_size : this->size_rows_) cols += pair_col_size.second;
- return cols;
-}
-
-void MatrixComposite::sizeAndIndices(const StateKeys& _struct_rows,
- const StateKeys& _struct_cols,
- std::unordered_map<char, unsigned int>& _indices_rows,
- std::unordered_map<char, unsigned int>& _indices_cols,
- unsigned int& _rows,
- unsigned int& _cols) const
-{
- _rows = 0;
- _cols = 0;
- for (const auto& row : _struct_rows)
- {
- _indices_rows[row] = _rows;
- _rows += size_rows_.at(row);
- }
- for (const auto& col : _struct_cols)
- {
- _indices_cols[col] = _cols;
- _cols += size_cols_.at(col);
- }
-}
-
-MatrixComposite::MatrixComposite(const StateKeys& _row_structure, const StateKeys& _col_structure)
-{
- for (const auto& rkey : _row_structure)
- for (const auto& ckey : _col_structure) this->emplace(rkey, ckey, MatrixXd(0, 0));
-}
-
-MatrixComposite::MatrixComposite(const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes)
-{
- assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
- assert(_col_structure.size() == _col_sizes.size() &&
- "Column structure and sizes have different number of elements!");
-
- auto rsize_it = _row_sizes.begin();
- for (const auto& rkey : _row_structure)
- {
- auto csize_it = _col_sizes.begin();
- for (const auto& ckey : _col_structure)
- {
- this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
-
- csize_it++;
- }
- rsize_it++;
- }
-}
-
-MatrixComposite::MatrixComposite(const MatrixXd& _m,
- const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes)
-{
- assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
- assert(_col_structure.size() == _col_sizes.size() &&
- "Column structure and sizes have different number of elements!");
-
- SizeEigen rindex = 0, cindex;
- auto rsize_it = _row_sizes.begin();
- for (const auto& rkey : _row_structure)
- {
- assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
-
- cindex = 0;
- auto csize_it = _col_sizes.begin();
-
- for (const auto& ckey : _col_structure)
- {
- assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns");
-
- this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
-
- cindex += *csize_it;
- csize_it++;
- }
-
- assert(_m.cols() == cindex && "Provided matrix has too many columns");
-
- rindex += *rsize_it;
- rsize_it++;
- }
-
- assert(_m.rows() == rindex && "Provided matrix has too many rows");
-}
-
-MatrixComposite MatrixComposite::zero(const StateKeys& _row_structure,
- const std::list<int>& _row_sizes,
- const StateKeys& _col_structure,
- const std::list<int>& _col_sizes)
-{
- MatrixComposite m;
-
- assert(_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
- assert(_col_structure.size() == _col_sizes.size() &&
- "Column structure and sizes have different number of elements!");
-
- auto rsize_it = _row_sizes.begin();
- for (const auto& rkey : _row_structure)
- {
- auto csize_it = _col_sizes.begin();
- for (const auto& ckey : _col_structure)
- {
- m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
-
- csize_it++;
- }
- rsize_it++;
- }
- return m;
-}
-
-MatrixComposite MatrixComposite::identity(const StateKeys& _structure, const std::list<int>& _sizes)
-{
- MatrixComposite m;
-
- assert(_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!");
-
- auto rsize_it = _sizes.begin();
- auto rkey_it = _structure.begin();
-
- while (rkey_it != _structure.end())
- {
- const auto& rkey = *rkey_it;
- const auto rsize = *rsize_it;
-
- m.emplace(rkey, rkey, MatrixXd::Identity(rsize, rsize)); // diagonal block
-
- auto csize_it = rsize_it;
- auto ckey_it = rkey_it;
-
- csize_it++;
- ckey_it++;
-
- while (ckey_it != _structure.end())
- {
- const auto& ckey = *ckey_it;
- const auto& csize = *csize_it;
-
- m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
- m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block
-
- csize_it++;
- ckey_it++;
- }
-
- rsize_it++;
- rkey_it++;
- }
- return m;
-}
-
-bool MatrixComposite::check() const
-{
- bool size_ok = true;
-
- // first see matrix itself, check that sizes are OK
- for (const auto& pair_rkey_row : *this)
- {
- const auto& rkey = pair_rkey_row.first;
- const auto& row = pair_rkey_row.second;
- for (const auto& pair_ckey_blk : row)
- {
- const auto& ckey = pair_ckey_blk.first;
- const auto& blk = pair_ckey_blk.second;
-
- if (size_rows_.count(rkey) != 0)
- {
- if (size_rows_.at(rkey) != blk.rows())
- {
- WOLF_ERROR("row size for row ", rkey, " has wrong size");
- size_ok = false;
- }
- }
- else
- {
- WOLF_ERROR("row size for row ", rkey, " does not exist");
- size_ok = false;
- }
- if (size_cols_.count(ckey) != 0)
- {
- if (size_cols_.at(ckey) != blk.cols())
- {
- WOLF_ERROR("col size for col ", rkey, " has wrong size");
- size_ok = false;
- }
- }
- else
- {
- WOLF_ERROR("col size for col ", ckey, " does not exist");
- size_ok = false;
- }
- }
- }
- return size_ok;
-}
-
-std::ostream& operator<<(std::ostream& _os, const MatrixComposite& _M)
-{
- for (const auto& pair_rkey_row : _M)
- {
- const auto rkey = pair_rkey_row.first;
-
- for (const auto& pair_ckey_mat : pair_rkey_row.second)
- {
- const auto& ckey = pair_ckey_mat.first;
-
- _os << "\n[" << rkey << "," << ckey << "]: \n" << pair_ckey_mat.second;
- }
- }
- return _os;
-}
-
-} // namespace wolf
diff --git a/src/composite/prior_composite.cpp b/src/composite/prior_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ee85eebc9e24b9cd084a92a48050430061e5767
--- /dev/null
+++ b/src/composite/prior_composite.cpp
@@ -0,0 +1,67 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "core/composite/prior_composite.h"
+
+namespace wolf
+{
+Prior::Prior(const std::string& _prior_mode, const Eigen::VectorXd& _factor_std)
+ : prior_mode_(_prior_mode), factor_std_(_factor_std)
+{
+ Prior::check();
+}
+
+Prior::Prior(const YAML::Node& _n)
+{
+ prior_mode_ = _n["mode"].as<std::string>();
+ if (prior_mode_ == "factor")
+ factor_std_ = _n["factor_std"].as<Eigen::VectorXd>();
+ else
+ factor_std_ = Eigen::VectorXd(0);
+
+ Prior::check();
+}
+
+void Prior::check() const
+{
+ if (prior_mode_ != "initial_guess" and prior_mode_ != "fix" and prior_mode_ != "factor")
+ throw std::runtime_error("Prior::check() wrong prior_mode_ value: " + prior_mode_ +
+ ", it should be: 'initial_guess', 'fix' or 'factor'. \n");
+}
+
+std::ostream& operator<<(std::ostream& _os, const wolf::Prior& _x)
+{
+ if (_x.getPriorMode() == "factor")
+ _os << "mode: " + _x.getPriorMode() << " factor_std: " << _x.getFactorStd().transpose();
+ else
+ _os << "mode: " + _x.getPriorMode();
+ return _os;
+}
+
+YAML::Node Prior::toYaml() const
+{
+ YAML::Node node;
+ node["mode"] = prior_mode_;
+ if (prior_mode_ == "factor") node["factor_std"] = factor_std_;
+
+ return node;
+}
+
+} // namespace wolf
diff --git a/src/composite/spec_state_composite.cpp b/src/composite/spec_state_composite.cpp
deleted file mode 100644
index a217d72a9f899021ebf00f57c7186fa252be19fe..0000000000000000000000000000000000000000
--- a/src/composite/spec_state_composite.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/composite/spec_state_composite.h"
-#include "core/state_block/factory_state_block.h"
-
-namespace wolf
-{
-SpecState::SpecState(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode,
- const Eigen::VectorXd& _noise_std)
- : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std)
-{
- SpecState::check();
-}
-
-SpecState::SpecState(const YAML::Node& _n)
-{
- type_ = _n["type"].as<std::string>();
- state_ = _n["state"].as<Eigen::VectorXd>();
- mode_ = _n["mode"].as<std::string>();
- if (mode_ == "factor")
- noise_std_ = _n["noise_std"].as<Eigen::VectorXd>();
- else
- noise_std_ = Eigen::VectorXd(0);
-
- SpecState::check();
-}
-
-void SpecState::check() const
-{
- if (mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor")
- throw std::runtime_error("SpecState::check() wrong mode value: " + mode_ +
- ", it should be: 'initial_guess', 'fix' or 'factor'. \n" + print());
-
- // try to create a state block and check for local parameterization and dimensions consistency
- StateBlockPtr sb;
- try
- {
- sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
- }
- catch (const std::exception& e)
- {
- throw std::runtime_error("SpecState::check() could not create state block from prior with error: " +
- std::string(e.what()) + "\n" + print());
- }
-
- // check factor sigma size
- if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize())
- throw std::runtime_error("SpecState::check() SpecState " + type_ +
- " noise_std size different of StateBlock local size. \n" + print());
-}
-
-std::string SpecState::print(const std::string& _spaces) const
-{
- return _spaces + "type: " + type_ + "\n" + _spaces + "mode: " + mode_ + "\n" + _spaces +
- "state: " + toString(state_) + "\n" +
- (mode_ == "factor" ? _spaces + "noise_std: " + toString(noise_std_) + "\n" : "");
-}
-
-std::ostream& operator<<(std::ostream& _os, const wolf::SpecState& _x)
-{
- _os << _x.print();
- return _os;
-}
-
-YAML::Node SpecState::toYaml() const
-{
- YAML::Node node;
- node["type"] = type_;
- node["state"] = state_;
- node["mode"] = mode_;
- if (mode_ == "factor") node["noise_std"] = noise_std_;
-
- return node;
-}
-
-} // namespace wolf
diff --git a/src/composite/spec_state_sensor_composite.cpp b/src/composite/spec_state_sensor_composite.cpp
deleted file mode 100644
index 8c7b0db0b72bf8a6404a7cff064deb8e1386cf19..0000000000000000000000000000000000000000
--- a/src/composite/spec_state_sensor_composite.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/composite/spec_state_sensor_composite.h"
-#include "core/state_block/factory_state_block.h"
-
-namespace wolf
-{
-SpecStateSensor::SpecStateSensor(const std::string& _type,
- const Eigen::VectorXd& _state,
- const std::string& _mode,
- const Eigen::VectorXd& _noise_std,
- bool _dynamic,
- const Eigen::VectorXd& _drift_std)
- : SpecState(_type, _state, _mode, _noise_std), dynamic_(_dynamic), drift_std_(_drift_std)
-{
- check();
-}
-
-SpecStateSensor::SpecStateSensor(const YAML::Node& prior_node) : SpecState(prior_node)
-{
- dynamic_ = prior_node["dynamic"].as<bool>();
-
- if (dynamic_ and prior_node["drift_std"])
- drift_std_ = prior_node["drift_std"].as<Eigen::VectorXd>();
- else
- drift_std_ = Eigen::VectorXd(0);
-
- check();
-}
-
-void SpecStateSensor::check() const
-{
- SpecState::check();
-
- auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); // already tried in SpecState::check()
-
- // check drift sigma size
- if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize())
- throw std::runtime_error("SpecStateSensor::check() Prior " + type_ +
- " drift_std size different of StateBlock local size. " + print());
-}
-
-std::string SpecStateSensor::print(const std::string& _spaces) const
-{
- return SpecState::print(_spaces) + _spaces + "dynamic: " + toString(dynamic_) + "\n" +
- (drift_std_.size() > 0 ? _spaces + "drift_std: " + toString(drift_std_) + "\n" : "");
-}
-
-std::ostream& operator<<(std::ostream& _os, const wolf::SpecStateSensor& _x)
-{
- _os << _x.print();
- return _os;
-}
-
-YAML::Node SpecStateSensor::toYaml() const
-{
- YAML::Node node = SpecState::toYaml();
- node["dynamic"] = dynamic_;
- if (dynamic_ and drift_std_.size() > 0) node["drift_std"] = drift_std_;
-
- return node;
-}
-
-} // namespace wolf
diff --git a/src/composite/vector_composite.cpp b/src/composite/vector_composite.cpp
index 87fe5c589e9af5e55adf1848e9a9458d797eff1f..c1826367cd068342a0623fcc4d1b60b74aba7821 100644
--- a/src/composite/vector_composite.cpp
+++ b/src/composite/vector_composite.cpp
@@ -20,44 +20,13 @@
#include "core/composite/vector_composite.h"
-namespace wolf
-{
-VectorComposite::VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes)
-{
- assert(_keys.size() == _sizes.size() && "Keys and _sizes list size mismatch");
-
- int index = 0;
- auto size_it = _sizes.begin();
- for (const auto& key : _keys)
- {
- const auto& size = *size_it;
-
- if (_v.size() < index + size) throw std::runtime_error("VectorComposite: provided vector _v is too short");
-
- (*this)[key] = _v.segment(index, size);
-
- index += size;
- size_it++;
- }
-
- if (_v.size() != index) throw std::runtime_error("VectorComposite: provided vector _v is too long");
-}
+using namespace Eigen;
-VectorComposite::VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors)
+namespace wolf
{
- if (_keys.size() != _vectors.size())
- throw std::runtime_error("VectorComposite: _keys and _vectors list size mismatch");
-
- auto vector_it = _vectors.begin();
-
- for (const auto& key : _keys)
- {
- this->emplace(key, *vector_it);
- vector_it++;
- }
-}
+VectorComposite::VectorComposite(const Composite<Eigen::VectorXd>& _composite) : Composite<VectorXd>(_composite) {}
-Eigen::VectorXd VectorComposite::vector(const StateKeys& _keys) const
+VectorXd VectorComposite::vector(const StateKeys& _keys) const
{
// traverse once with unordered_map access
std::vector<const VectorXd*> vp;
@@ -68,7 +37,7 @@ Eigen::VectorXd VectorComposite::vector(const StateKeys& _keys) const
size += vp.back()->size();
}
- Eigen::VectorXd x(size);
+ VectorXd x(size);
// traverse once linearly
unsigned int index = 0;
@@ -80,22 +49,6 @@ Eigen::VectorXd VectorComposite::vector(const StateKeys& _keys) const
return x;
}
-VectorComposite VectorComposite::operator()(const std::string& _keys) const
-{
- if (not this->has(_keys))
- {
- throw std::runtime_error("VectorComposite::operator() required keys " + _keys +
- " are not available, only have " + getKeys());
- }
-
- VectorComposite output;
- for (auto key : _keys)
- {
- output.emplace(key, this->at(key));
- }
- return output;
-}
-
std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x)
{
for (auto&& pair : _x)
@@ -105,42 +58,4 @@ std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x)
return _os;
}
-void VectorComposite::set(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes)
-{
- int index = 0;
- auto size_it = _sizes.begin();
- for (const auto& key : _keys)
- {
- const auto& size = *size_it;
-
- if (_v.size() < index + size)
- throw std::runtime_error("VectorComposite::set: provided vector _v is too short");
-
- (*this)[key] = _v.segment(index, size);
-
- index += size;
- size_it++;
- }
-}
-
-void VectorComposite::set(const StateKeys& _keys, const std::list<VectorXd>& _vectors)
-{
- if (_keys.size() != _vectors.size())
- throw std::runtime_error("VectorComposite: _keys and _vectors list size mismatch");
-
- auto vector_it = _vectors.begin();
- for (const auto& key : _keys)
- {
- WOLF_DEBUG_COND(count(key), "VectorComposite::set: overriding state for key ", key);
-
- (*this)[key] = *vector_it;
- vector_it++;
- }
-}
-
-void VectorComposite::setZero()
-{
- for (auto& pair_key_vec : *this) pair_key_vec.second.setZero();
-}
-
} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 8bd39ccbfd7b3dd8926d80ece1c2257466cdb0ab..0d5ba19a555975354182d0f073c3d295e6b03a89 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -270,7 +270,7 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
assert(this->getFeature() == nullptr && "linking an already linked factor");
// not link if nullptr
- WOLF_DEBUG_COND(_ftr_ptr == nullptr, "FactorBase::link _ftr_ptr is nullptr");
+ WOLF_DEBUG_COND(_ftr_ptr == nullptr, "FactorBase::link: ", getType(), " id: ", id(), " _ftr_ptr is nullptr");
if (_ftr_ptr)
{
// link with feature
@@ -291,12 +291,21 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
for (auto& node_w : node_state_blocks_list_)
{
auto node = node_w.lock();
+ WOLF_WARN_COND(node == nullptr,
+ "FactorBase::link: ",
+ getType(),
+ " id: ",
+ id(),
+ " node_state_blocks weak pointer is not valid");
if (node != nullptr)
{
+ WOLF_DEBUG_COND(node->getProblem() == nullptr,
+ "FactorBase::link: ",
+ getType(),
+ " id: ",
+ id(),
+ " node_state_blocks getProblem() is nullptr");
if (not getProblem()) this->setProblem(node->getProblem());
-
- assert(node->getProblem() == getProblem() &&
- "FactorBase::link: linking a factor with problem set to a floating NodeStateBlocks.");
node->addFactor(shared_from_this());
}
}
diff --git a/src/feature/feature_landmark_external.cpp b/src/feature/feature_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..336b8df302f86ad8a68a27f90ee422b818a787ba
--- /dev/null
+++ b/src/feature/feature_landmark_external.cpp
@@ -0,0 +1,60 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "core/feature/feature_landmark_external.h"
+
+namespace wolf
+{
+FeatureLandmarkExternal::FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_covariance,
+ const int& _external_id,
+ const int& _external_type)
+ : FeatureBase("FeatureLandmarkExternal", _measurement, _meas_covariance),
+ external_id_(_external_id),
+ external_type_(_external_type)
+{
+}
+
+FeatureLandmarkExternal::~FeatureLandmarkExternal()
+{
+ //
+}
+
+int FeatureLandmarkExternal::getExternalType() const
+{
+ return external_type_;
+}
+
+void FeatureLandmarkExternal::setExternalType(const int& _external_type)
+{
+ external_type_ = _external_type;
+}
+
+int FeatureLandmarkExternal::getExternalId() const
+{
+ return external_id_;
+}
+
+void FeatureLandmarkExternal::setExternalId(const int& _external_id)
+{
+ external_id_ = _external_id;
+}
+
+} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 7e1a6935b965b90d9eba4597a219960208f7a0a6..3f9e957876265cf0d31be96dce3ac7eab5747437 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -31,35 +31,11 @@ namespace wolf
{
unsigned int FrameBase::frame_id_count_ = 0;
-FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr)
- : NodeStateBlocks("FRAME", "FrameBase"), trajectory_ptr_(), frame_id_(++frame_id_count_), time_stamp_(_ts)
-{
- if (_p_ptr)
- {
- addStateBlock('P', _p_ptr);
- }
- if (_o_ptr)
- {
- addStateBlock('O', _o_ptr);
- }
- if (_v_ptr)
- {
- addStateBlock('V', _v_ptr);
- }
-}
-
-FrameBase::FrameBase(const TimeStamp& _ts, const SpecStateComposite& _frame_specs)
- : NodeStateBlocks("FRAME", "FrameBase", _frame_specs),
- trajectory_ptr_(),
- frame_id_(++frame_id_count_),
- time_stamp_(_ts)
-{
-}
-
-FrameBase::FrameBase(const TimeStamp& _ts, const TypeComposite& _frame_types, const VectorComposite& _frame_vectors)
- : NodeStateBlocks("FRAME",
- "FrameBase",
- SpecStateComposite(_frame_types, _frame_vectors, false)), // false -> not fixed
+FrameBase::FrameBase(const TimeStamp& _ts,
+ const TypeComposite& _frame_types,
+ const VectorComposite& _frame_vectors,
+ const PriorComposite& _frame_priors)
+ : NodeStateBlocks("FRAME", "FrameBase", _frame_types, _frame_vectors, _frame_priors),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
@@ -357,7 +333,7 @@ CheckLog FrameBase::localCheck(bool _verbose, std::ostream& _stream, std::string
}
// check NodeStateBlocks
- auto node_log = checkStatesAndFactoredBy(_verbose, _stream, _tabs);
+ auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
log.compose(node_log);
return log;
diff --git a/src/landmark/landmark_pose.cpp b/src/landmark/landmark.cpp
similarity index 89%
rename from src/landmark/landmark_pose.cpp
rename to src/landmark/landmark.cpp
index aa7e87887ab78b387b83a8d1f625dc11ab6cf57f..f4432db4bf432aa8a39eb6e3e89946ff3eca73f9 100644
--- a/src/landmark/landmark_pose.cpp
+++ b/src/landmark/landmark.cpp
@@ -18,12 +18,12 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
namespace wolf
{
// Register in the FactoryLandmark
-WOLF_REGISTER_LANDMARK(LandmarkPose2d);
-WOLF_REGISTER_LANDMARK(LandmarkPose3d);
+WOLF_REGISTER_LANDMARK(Landmark2d);
+WOLF_REGISTER_LANDMARK(Landmark3d);
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 06155d4d80e1cc3be4b68e75b707b470c4c03197..7fe42e99623614aab7d15addbaf0b177fa02b9c7 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -21,26 +21,38 @@
#include "core/landmark/landmark_base.h"
#include "core/factor/factor_base.h"
#include "core/map/map_base.h"
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_angle.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/state_block/factory_state_block.h"
-#include "core/common/factory.h"
#include "yaml-schema-cpp/yaml_conversion.hpp"
namespace wolf
{
unsigned int LandmarkBase::landmark_id_count_ = 0;
-LandmarkBase::LandmarkBase(const std::string& _type, const SpecStateComposite& _state_specs)
- : NodeStateBlocks("LANDMARK", _type, _state_specs), map_ptr_(), landmark_id_(++landmark_id_count_)
+LandmarkBase::LandmarkBase(const std::string& _type,
+ const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors,
+ const int _external_id,
+ const int _external_type)
+ : NodeStateBlocks("LANDMARK", _type, _state_types, _state_vectors, _state_priors),
+ map_ptr_(),
+ landmark_id_(++landmark_id_count_),
+ track_id_(0),
+ external_id_(_external_id),
+ external_type_(_external_type)
{
}
-LandmarkBase::LandmarkBase(const std::string& _type, const YAML::Node& _n)
- : NodeStateBlocks("LANDMARK", _type, SpecStateComposite(_n["states"])),
+LandmarkBase::LandmarkBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _n)
+ : NodeStateBlocks("LANDMARK",
+ _type,
+ _state_types,
+ VectorComposite(_n["states"], "value", false, _state_types.getKeys()),
+ PriorComposite(_n["states"], "prior", false, _state_types.getKeys())),
map_ptr_(),
- landmark_id_(_n["id"].as<int>())
+ landmark_id_(_n["id"].as<int>()),
+ track_id_(_n["track_id"] ? _n["track_id"].as<unsigned int>() : 0),
+ external_id_(_n["external_id"] ? _n["external_id"].as<int>() : -1),
+ external_type_(_n["external_type"] ? _n["external_type"].as<int>() : -1)
{
}
@@ -66,11 +78,13 @@ void LandmarkBase::remove(bool viral_remove_parent_without_children)
YAML::Node LandmarkBase::toYaml() const
{
- YAML::Node node;
+ YAML::Node node = NodeStateBlocks::toYaml();
+
node["id"] = landmark_id_;
- node["type"] = node_type_;
- node["states"] = NodeStateBlocks::getSpecs().toYaml();
- node["transformable"] = getP()->isTransformable();
+ node["track_id"] = track_id_;
+ node["external_id"] = external_id_;
+ node["external_type"] = external_type_;
+
return node;
}
@@ -153,7 +167,7 @@ CheckLog LandmarkBase::localCheck(bool _verbose, std::ostream& _stream, std::str
log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
// check NodeStateBlocks
- auto node_log = checkStatesAndFactoredBy(_verbose, _stream, _tabs);
+ auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
log.compose(node_log);
return log;
diff --git a/src/landmark/landmark_point.cpp b/src/landmark/landmark_point.cpp
deleted file mode 100644
index 80de18daeca4573869075df69542ac62639287c0..0000000000000000000000000000000000000000
--- a/src/landmark/landmark_point.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/landmark/landmark_point.h"
-
-namespace wolf
-{
-// Register in the FactoryLandmark
-WOLF_REGISTER_LANDMARK(LandmarkPoint2d);
-WOLF_REGISTER_LANDMARK(LandmarkPoint3d);
-
-} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 142ad33672516f25c9931a1b4f964951874f7dc3..f53c38e5e1d326b595c87262fce0d3649d359cbe 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -109,7 +109,7 @@ bool MapBase::save(std::string _map_file_yaml, const std::string& _map_name) con
for (LandmarkBaseConstPtr lmk_ptr : getLandmarkList())
{
- emitter << YAML::Flow << lmk_ptr->toYaml();
+ emitter << /*YAML::Flow <<*/ lmk_ptr->toYaml();
}
emitter << YAML::EndSeq << YAML::EndMap;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ef78ba624fa2b95b473d466fcd208d2f97124591..dc4f719c0cd9f12adfec8608e34108b760090fab 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -49,8 +49,10 @@ Problem::Problem(const TypeComposite& _frame_types, SizeEigen _dim, LoaderPtr _l
motion_provider_map_(),
dim_(_dim),
frame_types_(_frame_types),
- prior_options_(),
- prior_applied_(false),
+ first_frame_types_(),
+ first_frame_values_(),
+ first_frame_priors_(),
+ first_frame_status_(0),
transformed_(false),
loader_(_loader)
{
@@ -67,8 +69,10 @@ Problem::Problem(const StateKeys& _frame_keys, SizeEigen _dim, LoaderPtr _loader
motion_provider_map_(),
dim_(_dim),
frame_types_(),
- prior_options_(),
- prior_applied_(false),
+ first_frame_types_(),
+ first_frame_values_(),
+ first_frame_priors_(),
+ first_frame_status_(0),
transformed_(false),
loader_(_loader)
{
@@ -174,8 +178,10 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node, LoaderPtr _loader)
// First frame
WOLF_DEBUG("Loading problem first frame parameters...");
- SpecStateComposite priors(problem_node["first_frame"]);
- problem->setPriorOptions(priors);
+ TypeComposite ff_types(problem_node["first_frame"], "type", true);
+ VectorComposite ff_values(problem_node["first_frame"], "value", true);
+ PriorComposite ff_priors(problem_node["first_frame"], "prior", true);
+ problem->setFirstFrameOptions(ff_types, ff_values, ff_priors);
// SENSORS ===============================================================================
// load plugin if sensor is not registered
@@ -209,18 +215,18 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node, LoaderPtr _loader)
void Problem::searchAndLoadPlugins(const YAML::Node& _node, LoaderPtr _loader)
{
- std::set<YAML::Node> visited_nodes;
- auto plugins = searchPlugins(_node, visited_nodes);
+ std::list<YAML::Node> visited_nodes;
+ auto plugins = searchPlugins(_node, visited_nodes);
for (auto plugin : plugins) loadPlugin(_loader, plugin);
}
-std::set<std::string> Problem::searchPlugins(const YAML::Node& _node, std::set<YAML::Node>& _visited_nodes)
+std::set<std::string> Problem::searchPlugins(const YAML::Node& _node, std::list<YAML::Node>& _visited_nodes)
{
std::set<std::string> plugins;
- if (_visited_nodes.count(_node)) return {};
+ if (std::find(_visited_nodes.begin(), _visited_nodes.end(), _node) != _visited_nodes.end()) return {};
- _visited_nodes.insert(_node);
+ _visited_nodes.push_back(_node);
if (_node.IsMap())
{
@@ -543,13 +549,29 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
const TypeComposite& _frame_types,
- const VectorComposite& _frame_state)
+ const VectorComposite& _frame_state,
+ const PriorComposite& _frame_priors)
{
// check input _frame_specs
checkTypeComposite(_frame_types);
- return FrameBase::emplace<FrameBase>(
- getTrajectory(), _time_stamp, SpecStateComposite(_frame_types, _frame_state, false)); // false -> not fixed
+ // Check _frame_types are consistent with this->frame_types_
+ for (auto type_pair : frame_types_)
+ {
+ if (_frame_types.count(type_pair.first) and
+ _frame_types.at(type_pair.first) != frame_types_.at(type_pair.first))
+ {
+ throw std::runtime_error("Problem::emplaceFrame: provided _frame_specs specify the type " +
+ _frame_types.at(type_pair.first) + " for the key " +
+ std::string(1, type_pair.first) +
+ " but in problem, this key is specified as: " + frame_types_.at(type_pair.first));
+ }
+ }
+
+ // add the types of the keys that are not in frame_types_
+ appendToFrameTypes(frame_types_);
+
+ return FrameBase::emplace<FrameBase>(getTrajectory(), _time_stamp, _frame_types, _frame_state, _frame_priors);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys)
@@ -569,8 +591,9 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const VectorCom
{
if (not frame_types_.has(_frame_state.getKeys()))
{
- throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_state.getKeys() +
- " but only have " + frame_types_.getKeys());
+ throw std::runtime_error("Problem::emplaceFrame: any unknown type, given vectors of " +
+ _frame_state.getKeys() + " but problem only know the types of " +
+ frame_types_.getKeys());
}
return Problem::emplaceFrame(_time_stamp, getFrameTypes(_frame_state.getKeys()), _frame_state);
@@ -582,8 +605,8 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
{
if (not frame_types_.has(_frame_keys))
{
- throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_keys +
- " but only have " + frame_types_.getKeys());
+ throw std::runtime_error("Problem::emplaceFrame: any unknown type, given vectors of " + _frame_keys +
+ " but problem only know the types of " + frame_types_.getKeys());
}
VectorComposite vec_composite;
@@ -609,29 +632,6 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
return Problem::emplaceFrame(_time_stamp, getFrameTypes(vec_composite.getKeys()), vec_composite);
}
-FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const SpecStateComposite& _frame_specs)
-{
- // check input _frame_specs
- checkTypeComposite(_frame_specs.getTypeComposite());
-
- // Check _frame_specs are consistent with frame_types_
- auto input_types = _frame_specs.getTypeComposite();
- for (auto type_pair : frame_types_)
- {
- if (input_types.count(type_pair.first) and input_types.at(type_pair.first) != input_types.at(type_pair.first))
- {
- throw std::runtime_error("Problem::emplaceFrame: provided _frame_specs specify the type " +
- input_types.at(type_pair.first) + " for key " + std::string(1, type_pair.first) +
- " but in problem, this key is specified as: " + input_types.at(type_pair.first));
- }
- }
-
- // add the types of the keys that are not in frame_types_
- appendToFrameTypes(_frame_specs.getTypeComposite());
-
- return FrameBase::emplace<FrameBase>(getTrajectory(), _time_stamp, _frame_specs);
-}
-
TimeStamp Problem::getTimeStamp() const
{
TimeStamp ts = TimeStamp::Invalid();
@@ -688,8 +688,8 @@ VectorComposite Problem::getState(const StateKeys& _keys) const
if (last_kf)
state_last = last_kf->getState(keys);
- else if (not prior_options_.empty())
- state_last = prior_options_.getVectorComposite();
+ else if (not first_frame_values_.empty())
+ state_last = first_frame_values_;
for (auto key : keys)
{
@@ -743,8 +743,8 @@ VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _struct
if (last_kf)
state_last = last_kf->getState(structure);
- else if (not prior_options_.empty())
- state_last = prior_options_.getVectorComposite();
+ else if (not first_frame_values_.empty())
+ state_last = first_frame_values_;
for (auto key : structure)
{
@@ -784,18 +784,13 @@ VectorComposite Problem::getOdometry(const StateKeys& _structure) const
}
// check for empty blocks and fill them with the prior, or with zeros in the worst case
-
- VectorComposite state_last;
-
- if (not prior_options_.empty()) state_last = prior_options_.getVectorComposite();
-
for (auto key : structure)
{
if (odom_state.count(key) == 0)
{
- auto state_last_it = state_last.find(key);
+ auto state_last_it = first_frame_values_.find(key);
- if (state_last_it != state_last.end())
+ if (state_last_it != first_frame_values_.end())
odom_state.emplace(key, state_last_it->second);
else
@@ -1267,50 +1262,57 @@ FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts)
return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
-void Problem::setPriorOptions(const SpecStateComposite& _priors)
+void Problem::setFirstFrameOptions(const TypeComposite& _first_frame_types,
+ const VectorComposite& _first_frame_values,
+ const PriorComposite& _first_frame_priors)
{
- if (not prior_options_.empty())
+ if (isFirstFrameApplied())
{
throw std::runtime_error("prior options have already been applied");
}
+ WOLF_WARN_COND(first_frame_status_ == 1, "Overriding prior options.");
// add the types of the keys that are not in frame_types_
- appendToFrameTypes(_priors.getTypeComposite());
+ appendToFrameTypes(first_frame_types_);
- prior_options_ = _priors;
+ first_frame_types_ = _first_frame_types;
+ first_frame_values_ = _first_frame_values;
+ first_frame_priors_ = _first_frame_priors;
+ first_frame_status_ = 1; // options set
}
-FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
+FrameBasePtr Problem::applyFirstFrameOptions(const TimeStamp& _ts)
{
- if (isPriorApplied())
+ if (isFirstFrameApplied())
{
- throw std::runtime_error("applyPriorOptions can be called once!");
+ throw std::runtime_error("applyFirstFrameOptions can be called once!");
}
- // No prior options set
- if (prior_options_.empty())
- {
- prior_applied_ = true;
- return nullptr;
- }
+ FrameBasePtr first_frame(nullptr);
- // Create first frame with the prior options
- FrameBasePtr first_frame = emplaceFrame(_ts, prior_options_);
- first_frame->emplacePriors(prior_options_);
+ // prior options set
+ if (first_frame_status_ == 1)
+ {
+ // Create first frame with the prior options
+ first_frame = emplaceFrame(_ts, first_frame_types_, first_frame_values_, first_frame_priors_);
- // notify all processors
- keyFrameCallback(first_frame, nullptr);
+ // notify all processors
+ keyFrameCallback(first_frame, nullptr);
+ }
- // raise flag
- prior_applied_ = true;
+ // first frame status: APPLIED
+ first_frame_status_ = 2;
return first_frame;
}
-FrameBasePtr Problem::setPrior(const SpecStateComposite& _priors, const TimeStamp& _ts)
+FrameBasePtr Problem::emplaceFirstFrame(const TimeStamp& _ts,
+ const TypeComposite& _frame_types,
+ const VectorComposite& _frame_state,
+ const PriorComposite& _frame_priors)
{
- setPriorOptions(_priors);
- return applyPriorOptions(_ts);
+ setFirstFrameOptions(_frame_types, _frame_state, _frame_priors);
+ return applyFirstFrameOptions(_ts);
}
bool Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index baf2b2ed5233c13656f3c96009a867c670c01028..dbb10437a04741b465465bbe6c1aba535b51a644 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -122,7 +122,8 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture)
prev_capture_stamp_ = _capture->getTimeStamp();
// apply prior in problem if not done (very first capture)
- if (getProblem() and not getProblem()->isPriorApplied()) getProblem()->applyPriorOptions(_capture->getTimeStamp());
+ if (getProblem() and not getProblem()->isFirstFrameApplied())
+ getProblem()->applyFirstFrameOptions(_capture->getTimeStamp());
// asking if capture should be stored
if (storeCapture(_capture)) buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index 806706d96ec6dc23ab4e144764d015aabdb1ca0d..88efc85560463149b805fb89edd58ee073dfb53f 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -20,13 +20,11 @@
#include "core/processor/processor_landmark_external.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/feature/feature_base.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
-#include "core/landmark/landmark_point.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
@@ -48,8 +46,7 @@ void ProcessorLandmarkExternal::preProcess()
"ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
// Extract features from capture detections
- auto landmark_detections = cap_landmarks->getDetections();
- WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: landmark_detections ", landmark_detections.size());
+ auto landmark_detections = cap_landmarks->getDetections();
std::set<int> ids;
for (auto detection : landmark_detections)
{
@@ -57,7 +54,7 @@ void ProcessorLandmarkExternal::preProcess()
"ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
// Filter by quality
- if (detection.quality < filter_quality_th_ or ids.count(detection.id)) continue;
+ if (detection.quality < quality_th_ or ids.count(detection.id)) continue;
// measure and covariance
VectorXd meas;
@@ -81,17 +78,16 @@ void ProcessorLandmarkExternal::preProcess()
}
// emplace feature
- FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks, "FeatureLandmarkExternal", meas, cov);
- ftr->setLandmarkId(detection.id);
-
- if (detection.id != -1 and detection.id != 0) ids.insert(detection.id);
-
- // unmatched_detections_incoming_.push_back(ftr);
+ FeatureBasePtr ftr =
+ FeatureBase::emplace<FeatureLandmarkExternal>(cap_landmarks, meas, cov, detection.id, detection.type);
+ if (detection.id >= 0) ids.insert(detection.id);
// add new feature
new_features_incoming_.push_back(ftr);
}
- WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ", new_features_incoming_.size(), " new features");
+ WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
+ new_features_incoming_.size(),
+ " features in incoming capture");
}
unsigned int ProcessorLandmarkExternal::processKnown()
@@ -100,37 +96,214 @@ unsigned int ProcessorLandmarkExternal::processKnown()
known_features_incoming_.clear();
// Track features from last_ptr_ to incoming_ptr_
- if (last_ptr_)
+ if (not last_ptr_) return 0;
+
+ WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+ auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+
+ for (auto feat_last : known_features_last_)
{
- WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
- auto pose_sen = getSensor()->getState("PO");
- auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
- auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+ auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
+ bool matched = false;
+ WOLF_DEBUG("Tracking feature last: ",
+ feat_lmk_last->id(),
+ " - ID: ",
+ feat_lmk_last->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_last->getExternalType(),
+ " meas: ",
+ feat_lmk_last->getMeasurement().transpose());
+
+ // First we try to match by EXTERNAL_ID
+ if (feat_lmk_last->getExternalId() != -1)
+ {
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
+ {
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+
+ WOLF_DEBUG("Feature incoming candidate (by ID): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Same EXTERNAL_ID
+ // 2. Compatible TYPE (either not defined or same)
+ // 3. Detections close enough
+ if (feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId() and // cond 1
+ (feat_lmk_incoming->getExternalType() == -1 or // cond 2
+ feat_lmk_last->getExternalType() == -1 or //
+ feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
+ detectionDistance( // cond 3
+ feat_lmk_last,
+ feat_lmk_incoming,
+ pose_last,
+ pose_incoming,
+ pose_sen) < match_dist_th_id_)
+ {
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID if defined
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
+ }
+ else
+ feature_incoming_it++;
+ }
+ }
+ // skip search (already found match)
+ if (matched) continue;
- for (auto feat_last : known_features_last_)
+ // Second we try to match by TYPE (if defined)
+ if (feat_lmk_last->getExternalType() != -1)
{
- auto feat_candidate_it = new_features_incoming_.begin();
- while (feat_candidate_it != new_features_incoming_.end())
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
{
- if ((*feat_candidate_it)->landmarkId() == feat_last->landmarkId() and
- detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < match_dist_th_)
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+ WOLF_DEBUG("Feature incoming candidate (by TYPE): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Compatible EXTERNAL_ID (either not defined or same)
+ // 2. Same TYPE
+ // 3. Detections close enough
+ if ((feat_lmk_incoming->getExternalId() == -1 or // cond 1
+ feat_lmk_last->getExternalId() == -1 or //
+ feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
+ feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType() and // cond 2
+ detectionDistance( // cond 3
+ feat_lmk_last,
+ feat_lmk_incoming,
+ pose_last,
+ pose_incoming,
+ pose_sen) < match_dist_th_type_)
{
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID last -> incoming
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // set EXTERNAL_ID last -> incoming
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_incoming->setExternalId(feat_lmk_last->getExternalId());
+
// grow track
- track_matrix_.add(feat_last, *feat_candidate_it);
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
// feature is known
- known_features_incoming_.push_back((*feat_candidate_it));
+ known_features_incoming_.push_back(feat_lmk_incoming);
// remove from unmatched
- feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
break;
}
else
- feat_candidate_it++;
+ feature_incoming_it++;
+ }
+ }
+ // skip search (already found match)
+ if (matched) continue;
+
+ // Finally, we try to match by distance
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
+ {
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+
+ WOLF_DEBUG("Feature incoming candidate (by distance): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Compatible EXTERNAL_ID (either not defined or same)
+ // 2. Compatible TYPE (either not defined or same)
+ // 3. Detections close enough
+ if ((feat_lmk_incoming->getExternalId() == -1 or // cond 1
+ feat_lmk_last->getExternalId() == -1 or //
+ feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
+ (feat_lmk_incoming->getExternalType() == -1 or // cond 2
+ feat_lmk_last->getExternalType() == -1 or //
+ feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
+ detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
+ match_dist_th_unknown_)
+ {
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID last -> incoming
+ if (feat_lmk_last->landmarkId() != 0) feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // set TYPE last -> incoming
+ if (feat_lmk_last->getExternalType() != -1)
+ feat_lmk_incoming->setExternalType(feat_lmk_last->getExternalType());
+
+ // set EXTERNAL_ID last -> incoming
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_incoming->setExternalId(feat_lmk_last->getExternalId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
}
+ else
+ feature_incoming_it++;
}
- WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
+ WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked.");
}
+ WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
// Add new features (not tracked) as known features
WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
@@ -175,39 +348,70 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
{
if (getProblem()->getDim() == 2)
{
- auto pose_s1 = SE2::compose(_pose1, _pose_sen);
- auto pose_s2 = SE2::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
- auto p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+ VectorComposite pose_s1 = SE2::compose(_pose1, _pose_sen);
+ VectorComposite pose_s2 = SE2::compose(_pose2, _pose_sen);
+
+ Eigen::Vector2d p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ Eigen::Vector2d p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+
return (p1 - p2).norm();
}
else
{
- auto pose_s1 = SE3::compose(_pose1, _pose_sen);
- auto pose_s2 = SE3::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
- auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+ VectorComposite pose_s1 = SE3::compose(_pose1, _pose_sen);
+ VectorComposite pose_s2 = SE3::compose(_pose2, _pose_sen);
+ Eigen::Vector3d p1 =
+ pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ Eigen::Vector3d p2 =
+ pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+
return (p1 - p2).norm();
}
}
return 0; // non reachable
}
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr,
+ LandmarkBasePtr _lmk,
+ const VectorComposite& _pose_frm,
+ const VectorComposite& _pose_sen) const
+{
+ // Needed all poses
+ if (not _pose_frm.has("PO") or not _pose_sen.has("PO") or not _lmk->has('P'))
+ {
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::detectionDistance: Missing any required geometric information");
+ }
+
+ if (getProblem()->getDim() == 2)
+ {
+ auto pose_s = SE2::compose(_pose_frm, _pose_sen);
+ Eigen::Vector2d p = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>();
+ return (p - _lmk->getP()->getState().head<2>()).norm();
+ }
+ else
+ {
+ auto pose_s = SE3::compose(_pose_frm, _pose_sen);
+ Eigen::Vector3d p = pose_s.at('P') + Quaterniond(Vector4d(pose_s.at('O'))) * _ftr->getMeasurement().head<3>();
+ return (p - _lmk->getP()->getState().head<3>()).norm();
+ }
+}
+
bool ProcessorLandmarkExternal::voteForKeyFrame() const
{
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
- // no tracks longer than filter_track_length_th
+ // number of tracks longer than track_length_th
auto n_tracks = 0;
auto n_new_tracks = 0;
for (auto track_id : track_ids_last)
{
- if (track_matrix_.trackSize(track_id) >= filter_track_length_th_)
+ if (track_matrix_.trackSize(track_id) >= track_length_th_)
{
n_tracks++;
- if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId())) n_new_tracks++;
+ if (track_matrix_.feature(track_id, last_ptr_)->landmarkId() == 0) n_new_tracks++;
}
}
@@ -216,7 +420,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
WOLF_DEBUG("vote_min_features: ",
vote_min_features,
" - Active feature tracks longer than ",
- filter_track_length_th_,
+ track_length_th_,
": ",
n_tracks,
" (should be equal or bigger than ",
@@ -256,38 +460,117 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
void ProcessorLandmarkExternal::establishFactors()
{
- if (origin_ptr_ == last_ptr_) return;
-
- // reset n_tracks_origin_
- lmks_ids_origin_.clear();
-
// will emplace a factor (and landmark if needed) for each known feature in last with long tracks
- // (filter_track_length_th_)
FactorBasePtrList fac_list;
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_frm = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
for (auto track_id : track_ids_last)
{
- // check track length
- if (track_matrix_.trackSize(track_id) < filter_track_length_th_) continue;
+ auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
+
+ WOLF_DEBUG("Feature ",
+ feature->id(),
+ ": ID: ",
+ feature->landmarkId(),
+ " EXTERNAL_ID: ",
+ feature->getExternalId(),
+ " TYPE: ",
+ feature->getExternalType());
+
+ // not enough long track
+ WOLF_DEBUG_COND(track_matrix_.trackSize(track_id) < track_length_th_,
+ "Track NOT long enough ",
+ track_matrix_.trackSize(track_id));
+ if (track_matrix_.trackSize(track_id) < track_length_th_) continue;
+
+ // Landmark match
+ LandmarkBasePtr lmk;
+ if (feature->landmarkId() == 0) // landmark id 0 never used
+ {
+ // LOOP CLOSURE
+ // By ID
+ WOLF_DEBUG("Searching Loop closure by ID...");
+ if (close_loops_by_id_ and feature->getExternalId() != -1)
+ {
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
- auto feature = track_matrix_.feature(track_id, last_ptr_);
+ for (auto lmk_ptr : lmk_list)
+ {
+ if (lmk_ptr and lmk_ptr->getExternalId() == feature->getExternalId() and
+ detectionDistance(feature, lmk_ptr, pose_frm, pose_sen) < match_dist_th_id_)
+ {
+ lmk = lmk_ptr;
+ WOLF_DEBUG("Found loop closure by EXTERNAL_ID with ", lmk->id());
+ break;
+ }
+ WOLF_DEBUG_COND(lmk_ptr and lmk_ptr->getExternalId() == feature->getExternalId(),
+ "Landmark with EXTERNAL_ID found but not matched due to distance: ",
+ detectionDistance(feature, lmk_ptr, pose_frm, pose_sen))
+ }
+ }
+ // By TYPE
+ WOLF_DEBUG("Searching Loop closure by TYPE...");
+ if (not lmk and close_loops_by_type_ and feature->getExternalType() != -1)
+ {
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
- // get landmark
- LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
+ for (auto lmk_ptr : lmk_list)
+ {
+ if (lmk_ptr and lmk_ptr->getExternalType() == feature->getExternalType() and
+ detectionDistance(feature, lmk_ptr, pose_frm, pose_sen) < match_dist_th_type_)
+ {
+ lmk = lmk_ptr;
+ WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id());
+ break;
+ }
+ WOLF_DEBUG_COND(lmk_ptr and lmk_ptr->getExternalType() == feature->getExternalType(),
+ "Landmark with TYPE found but not matched due to distance: ",
+ detectionDistance(feature, lmk_ptr, pose_frm, pose_sen))
+ }
- // emplace landmark
- if (not lmk) lmk = emplaceLandmark(feature);
+ // update Landmark EXTERNAL_ID (if available)
+ if (feature->getExternalId() != -1) lmk->setExternalId(feature->getExternalId());
+ }
- // modify landmark
- modifyLandmark(lmk, feature);
+ // Emplace new landmark (loop closure not found or not enabled)
+ if (not lmk)
+ {
+ lmk = emplaceLandmark(feature);
+ WOLF_DEBUG("Emplaced new landmark ", lmk->id());
+ }
- // emplace factor
- auto fac = emplaceFactor(feature, lmk);
+ // set LANDMARK_ID in all features of the track
+ for (auto feat_pair : track_matrix_.track(track_id))
+ {
+ feat_pair.second->setLandmarkId(lmk->id());
+ WOLF_DEBUG("Setting landmark id in feature ",
+ feat_pair.second->id(),
+ " landmark_id: ",
+ feat_pair.second->landmarkId());
+ }
+ }
+ // landmarkId already set
+ else
+ {
+ lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
+ if (not lmk)
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::establishFactors: Feature has LANDMARK_ID but there is not "
+ "any landmark with this id.");
+ }
- lmks_ids_origin_.insert(lmk->id());
+ // modify landmark (add missing state blocks if needed)
+ modifyLandmark(lmk, feature);
- if (fac) fac_list.push_back(fac);
+ // Emplace factors for all tracked features in keyframes
+ for (auto feat_pair : track_matrix_.trackAtKeyframes(track_id))
+ if (feat_pair.second->getFactorList().empty())
+ {
+ auto fac = emplaceFactor(feature, lmk);
+ if (fac) fac_list.push_back(fac);
+ }
}
WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
@@ -359,7 +642,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
return nullptr;
}
-LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
+LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -369,9 +652,11 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
assert(getProblem());
assert(getProblem()->getMap());
- if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
+ if (_feature->landmarkId() != 0)
throw std::runtime_error(
- "ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+ "ProcessorLandmarkExternal::emplaceLandmark: attempting to emplace a landmark for a feature that has "
+ "been "
+ "already matched with an existing one");
LandmarkBasePtr lmk;
@@ -383,10 +668,10 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
- Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
+ VectorXd lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(), std::make_shared<StatePoint2d>(lmk_p));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<Landmark2d>(
+ getProblem()->getMap(), VectorComposite({{'P', lmk_p}}), PriorComposite{});
}
// 2D - Relative Pose
else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
@@ -396,13 +681,12 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
- Vector2d lmk_p =
+ VectorXd lmk_p =
frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
- lmk = LandmarkBase::emplace<LandmarkPose2d>(
- getProblem()->getMap(), std::make_shared<StatePoint2d>(lmk_p), std::make_shared<StateAngle>(lmk_o));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<Landmark2d>(
+ getProblem()->getMap(), VectorComposite({{'P', lmk_p}, {'O', Vector1d(lmk_o)}}), PriorComposite{});
}
// 3D - Relative Position
else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
@@ -414,8 +698,8 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkPoint3d>(getProblem()->getMap(), std::make_shared<StatePoint3d>(lmk_p));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<Landmark3d>(
+ getProblem()->getMap(), VectorComposite({{'P', lmk_p}}), PriorComposite{});
}
// 3D - Relative Pose
else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
@@ -428,17 +712,20 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
- lmk = LandmarkBase::emplace<LandmarkPose3d>(
- getProblem()->getMap(), std::make_shared<StatePoint3d>(lmk_p), std::make_shared<StateQuaternion>(lmk_o));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<Landmark3d>(
+ getProblem()->getMap(), VectorComposite({{'P', lmk_p}, {'O', lmk_o.coeffs()}}), PriorComposite{});
}
else
throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
+ // Set EXTERNAL_ID and EXTERNAL_TYPE
+ lmk->setExternalId(_feature->getExternalId());
+ lmk->setExternalType(_feature->getExternalType());
+
return lmk;
}
-void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature)
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, FeatureLandmarkExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -466,7 +753,7 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
- _landmark->addStateBlock('O', std::make_shared<StateAngle>(lmk_o));
+ _landmark->emplaceStateBlock('O', "StateAngle", Eigen::Vector1d(lmk_o), false);
}
}
// 3D - Relative Position
@@ -486,7 +773,7 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
- _landmark->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o));
+ _landmark->emplaceStateBlock('O', "StateQuaternion", lmk_o.coeffs(), false);
}
}
else
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d681b6f94c653a27ebeeefc81e54c3172424df3a..8d73ce85c58cd069c14908851abba0b3ad11fe04 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -245,7 +245,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
// integrate motion data
- // Done at this place because setPrior() needs
+ // Done at this place because emplaceFirstFrame() needs
integrateOneStep();
// perform bootstrap steps (usually only IMU requires this)
@@ -299,10 +299,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = getState(timestamp_from_callback);
for (auto pair_key_vec : proc_state)
- if (!keyframe_from_callback->has(pair_key_vec.first))
- keyframe_from_callback->addStateBlock(
- pair_key_vec.first,
- FactoryStateBlock::create(std::string(1, pair_key_vec.first), pair_key_vec.second, false));
+ if (not keyframe_from_callback->has(pair_key_vec.first))
+ keyframe_from_callback->emplaceStateBlock(
+ pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
keyframe_from_callback->setState(proc_state);
// Find the capture acting as the buffer's origin
@@ -392,12 +391,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = this->getState(timestamp_from_callback);
- for (auto pair_key_vector : proc_state)
- if (!keyframe_from_callback->has(pair_key_vector.first))
- keyframe_from_callback->addStateBlock(
- pair_key_vector.first,
- FactoryStateBlock::create(
- std::string(1, pair_key_vector.first), pair_key_vector.second, false));
+ for (auto pair_key_vec : proc_state)
+ if (not keyframe_from_callback->has(pair_key_vec.first))
+ keyframe_from_callback->emplaceStateBlock(
+ pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
keyframe_from_callback->setState(proc_state);
auto& capture_existing = last_ptr_;
@@ -442,11 +439,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->setTimeStamp(ts);
last_ptr_->getFrame()->setTimeStamp(ts);
VectorComposite state_propa = getState(ts);
- for (auto& pair_key_vec : state_propa)
- if (!last_ptr_->getFrame()->has(pair_key_vec.first))
- last_ptr_->getFrame()->addStateBlock(
- pair_key_vec.first,
- FactoryStateBlock::create(std::string(1, pair_key_vec.first), pair_key_vec.second, false));
+ for (auto pair_key_vec : state_propa)
+ if (not last_ptr_->getFrame()->has(pair_key_vec.first))
+ last_ptr_->getFrame()->emplaceStateBlock(
+ pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
last_ptr_->getFrame()->setState(state_propa);
if (permittedKeyFrame() && voteForKeyFrame())
@@ -770,16 +766,15 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
assert(_origin_frame->getTrajectory() != nullptr &&
"ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
- // add states to origin_frame if missing
+ // add (zero) states to origin_frame if missing
if (not _origin_frame->has(getStateKeys()))
- {
- std::string missing_keys = "";
for (auto key : getStateKeys())
- if (not _origin_frame->has(key)) missing_keys.push_back(key);
-
- // add zero state unfixed for each missing key
- _origin_frame->emplaceStateBlocks(getStateTypes()(missing_keys), getProblem()->stateZero(missing_keys), false);
- }
+ if (not _origin_frame->has(key))
+ _origin_frame->emplaceStateBlock(
+ key,
+ getStateTypes().at(key),
+ getProblem()->stateZero(std::string(1, key)).vector(std::string(1, key)),
+ false);
TimeStamp origin_ts = _origin_frame->getTimeStamp();
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index dc36c2baa72985a9680229c83dab0e75f77ce4ae..2d9fb444fa8c13f50495b048f480acd71f4565b3 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -30,8 +30,15 @@ namespace wolf
{
unsigned int SensorBase::sensor_id_count_ = 0;
-SensorBase::SensorBase(const std::string& _type, const int& _dim_compatible, const YAML::Node& _params)
- : NodeStateBlocks("SENSOR", _type, SpecStateComposite(_params["states"])),
+SensorBase::SensorBase(const std::string& _type,
+ const int& _dim_compatible,
+ const TypeComposite& _state_types,
+ const YAML::Node& _params)
+ : NodeStateBlocks("SENSOR",
+ _type,
+ _state_types,
+ VectorComposite(_params["states"], "value", false, _state_types.getKeys()),
+ PriorComposite(_params["states"], "prior", true, _state_types.getKeys())),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
state_block_dynamic_(),
@@ -41,13 +48,24 @@ SensorBase::SensorBase(const std::string& _type, const int& _dim_compatible, con
{
setName(_params["name"].as<std::string>());
- // Drift covariance rates
- auto priors_sensor = SpecStateSensorComposite(_params["states"]);
+ // Dynamic states and drift covariance rates
+ auto dynamics = BoolComposite(_params["states"], "dynamic", true, _state_types.getKeys());
+ auto drifts = VectorComposite(_params["states"], "drift_std", false, _state_types.getKeys());
for (auto state_pair : getStateBlockMap())
{
- state_block_dynamic_.emplace(state_pair.first, priors_sensor.at(state_pair.first).isDynamic());
- if (priors_sensor.at(state_pair.first).isDynamic())
- setDriftStd(state_pair.first, priors_sensor.at(state_pair.first).getDriftStd());
+ state_block_dynamic_.emplace(state_pair.first, dynamics.at(state_pair.first));
+
+ if (dynamics.at(state_pair.first) and drifts.count(state_pair.first))
+ setDriftStd(state_pair.first, drifts.at(state_pair.first));
+
+ WOLF_WARN_COND(not dynamics.at(state_pair.first) and drifts.count(state_pair.first),
+ "Sensor of type ",
+ _type,
+ " named ",
+ this->getName(),
+ " was specified with a drift for the state '",
+ state_pair.first,
+ "' but this state was not specified as 'dynamic', ignoring drift.")
}
}
@@ -529,7 +547,7 @@ CheckLog SensorBase::localCheck(bool _verbose, std::ostream& _stream, std::strin
}
// check NodeStateBlocks
- auto node_log = checkStatesAndFactoredBy(_verbose, _stream, _tabs);
+ auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
log.compose(node_log);
return log;
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 7eb9c843c0bb3d8a8dd7df068cb241c84c72a6ff..05b5455d1278eb88cb3158f331d44f52bdadd933 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -25,7 +25,7 @@
namespace wolf
{
SensorDiffDrive::SensorDiffDrive(const YAML::Node& _params)
- : SensorBase("SensorDiffDrive", 2, _params),
+ : SensorBase("SensorDiffDrive", 2, {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'I', "StateParams3"}}, _params),
ticks_per_wheel_revolution_(_params["ticks_per_wheel_revolution"].as<double>()),
ticks_std_factor_(_params["ticks_std_factor"].as<double>())
{
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index 62238d5b71932cd7a88dc8987491f0c18df7d2f9..da0e6f2261687eb3c93e77ce17aec783c846aad8 100644
--- a/src/sensor/sensor_motion_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -22,7 +22,7 @@
namespace wolf
{
-SensorMotionModel::SensorMotionModel(const YAML::Node& _params) : SensorBase("SensorMotionModel", -1, _params) {}
+SensorMotionModel::SensorMotionModel(const YAML::Node& _params) : SensorBase("SensorMotionModel", -1, {}, _params) {}
SensorMotionModel::~SensorMotionModel() {}
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
index ef0058b0cc8721cd572fd082cdc95ae759f41b6f..7cbb8a8ad70f8a20acabb180c3020126e82644b7 100644
--- a/src/state_block/state_block_derived.cpp
+++ b/src/state_block/state_block_derived.cpp
@@ -38,5 +38,15 @@ WOLF_REGISTER_STATEBLOCK(StateParams7);
WOLF_REGISTER_STATEBLOCK(StateParams8);
WOLF_REGISTER_STATEBLOCK(StateParams9);
WOLF_REGISTER_STATEBLOCK(StateParams10);
+WOLF_REGISTER_STATEBLOCK(StateParams11);
+WOLF_REGISTER_STATEBLOCK(StateParams12);
+WOLF_REGISTER_STATEBLOCK(StateParams13);
+WOLF_REGISTER_STATEBLOCK(StateParams14);
+WOLF_REGISTER_STATEBLOCK(StateParams15);
+WOLF_REGISTER_STATEBLOCK(StateParams16);
+WOLF_REGISTER_STATEBLOCK(StateParams17);
+WOLF_REGISTER_STATEBLOCK(StateParams18);
+WOLF_REGISTER_STATEBLOCK(StateParams19);
+WOLF_REGISTER_STATEBLOCK(StateParams20);
} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index ee49b777b3bf3923ce4dd79b6ffa29863ab23bec..4dc478d94e55200f808ced328400fae242b62aa0 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -13,7 +13,7 @@ set(SRC_DUMMY
dummy/processor_motion_provider_dummy_pov.cpp
dummy/processor_tracker_feature_dummy.cpp
dummy/processor_tracker_landmark_dummy.cpp
- dummy/sensor_dummy.cpp
+ dummy/sensor_dummy_po.cpp
dummy/sensor_dummy_poia.cpp
dummy/solver_dummy.cpp
dummy/tree_manager_dummy.cpp
@@ -47,6 +47,9 @@ wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
+# Composite class test
+wolf_add_gtest(gtest_composite gtest_composite.cpp)
+
# FactorBase class test
wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
@@ -203,20 +206,17 @@ wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_lo
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-# Map yaml save/load test
-wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
-target_link_libraries(gtest_map_yaml PUBLIC dummy)
-
-# MatrixComposite class test
-wolf_add_gtest(gtest_matrix_composite gtest_matrix_composite.cpp)
-
-# ProcessorFixedWingModel class test
-wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+# Map save/load test
+wolf_add_gtest(gtest_map gtest_map.cpp)
+target_link_libraries(gtest_map PUBLIC dummy)
# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive PUBLIC dummy)
+# ProcessorFixedWingModel class test
+wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+
# ProcessorLandmarkExternal class test
wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
@@ -237,14 +237,14 @@ wolf_add_gtest(gtest_processor_pose_3d gtest_processor_pose_3d.cpp)
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC dummy)
-# ProcessorTrackerFeatureDummy 2 processors in one sensor test
-wolf_add_gtest(gtest_processor_tracker_two_processors_one_sensor gtest_processor_tracker_two_processors_one_sensor.cpp)
-target_link_libraries(gtest_processor_tracker_two_processors_one_sensor PUBLIC dummy)
-
# ProcessorTrackerLandmarkDummy class test
wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)
+# 2 processors in one sensor test
+wolf_add_gtest(gtest_processor_tracker_two_processors_one_sensor gtest_processor_tracker_two_processors_one_sensor.cpp)
+target_link_libraries(gtest_processor_tracker_two_processors_one_sensor PUBLIC dummy)
+
# Schema test
wolf_add_gtest(gtest_schema gtest_schema.cpp)
target_link_libraries(gtest_schema PUBLIC dummy)
@@ -266,12 +266,6 @@ IF (Ceres_FOUND)
wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
ENDIF(Ceres_FOUND)
-# SpecStateSensorComposite
-wolf_add_gtest(gtest_spec_state_sensor_composite gtest_spec_state_sensor_composite.cpp)
-
-# SpecStateComposite
-wolf_add_gtest(gtest_spec_state_composite gtest_spec_state_composite.cpp)
-
# TreeManagerSlidingWindow class test
wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
diff --git a/test/dummy/LandmarkDummy.schema b/test/dummy/LandmarkDummy.schema
index a5859d85ec7ecb923454040e9354205fd1b69304..eabd59efb67be6d9bfcb26f99a17cc9b737070fe 100644
--- a/test/dummy/LandmarkDummy.schema
+++ b/test/dummy/LandmarkDummy.schema
@@ -1 +1,15 @@
-follow: LandmarkPose2d.schema
\ No newline at end of file
+follow: LandmarkBase.schema
+
+states:
+ P:
+ _type: StateP2d
+ _mandatory: true
+ _doc: The specification of the position state.
+ O:
+ _type: StateO2d
+ _mandatory: true
+ _doc: The specification of the orientation state.
+ A:
+ _type: StateParams1
+ _mandatory: false
+ _doc: The state A specifications.
\ No newline at end of file
diff --git a/test/dummy/SensorDummy2d.schema b/test/dummy/SensorDummyPo2d.schema
similarity index 52%
rename from test/dummy/SensorDummy2d.schema
rename to test/dummy/SensorDummyPo2d.schema
index c733dc45ddd7e385c2fd7d24e2ba2a9dc5e7c4c7..3105f953826c5fce1dcaa106575be1b9faf19a74 100644
--- a/test/dummy/SensorDummy2d.schema
+++ b/test/dummy/SensorDummyPo2d.schema
@@ -1,14 +1,9 @@
follow: SensorBase.schema
states:
- keys:
- _mandatory: false
- _type: string
- _value: PO
- _doc: States' keys of sensor
P:
- follow: SpecStateSensorP2d.schema
+ follow: StateSensorP2d.schema
O:
- follow: SpecStateSensorO2d.schema
+ follow: StateSensorO2d.schema
noise_p_std:
_mandatory: true
_type: double
diff --git a/test/dummy/SensorDummy3d.schema b/test/dummy/SensorDummyPo3d.schema
similarity index 52%
rename from test/dummy/SensorDummy3d.schema
rename to test/dummy/SensorDummyPo3d.schema
index c656bcfcfce9272dc55347b82a5697ab6e994ce3..8efb470d965d41f03ad31b27a5c899507b6d7716 100644
--- a/test/dummy/SensorDummy3d.schema
+++ b/test/dummy/SensorDummyPo3d.schema
@@ -1,14 +1,9 @@
follow: SensorBase.schema
states:
- keys:
- _mandatory: false
- _type: string
- _value: PO
- _doc: States' keys of sensor
P:
- follow: SpecStateSensorP3d.schema
+ follow: StateSensorP3d.schema
O:
- follow: SpecStateSensorO3d.schema
+ follow: StateSensorO3d.schema
noise_p_std:
_mandatory: true
_type: double
diff --git a/test/dummy/SensorDummyPoia2d.schema b/test/dummy/SensorDummyPoia2d.schema
index 68bdaac1dfc5a706eda32d177348251d0ed512df..b73c2fdc107efea9a0e7fc068fc413ac3a219660 100644
--- a/test/dummy/SensorDummyPoia2d.schema
+++ b/test/dummy/SensorDummyPoia2d.schema
@@ -1,27 +1,34 @@
follow: SensorBase.schema
states:
- keys:
- _mandatory: false
- _type: string
- _value: POIA
- _doc: states' keys of the sensor
P:
- follow: SpecStateSensorP2d.schema
+ follow: StateSensorP2d.schema
O:
- follow: SpecStateSensorO2d.schema
+ follow: StateSensorO2d.schema
I:
- follow: SpecStateSensor.schema
- type:
- _type: string
- _mandatory: false
- _value: StateParams5
- _doc: The derived type of the state in 'I'
- state:
+ dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the state 'I' is dynamic, i.e. it changes along time.
+ value:
_type: Vector5d
_mandatory: true
- _doc: A vector containing the state 'I' values
+ _doc: A vector containing the state 'I' values.
+ prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+ factor_std:
+ _type: Vector5d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
+ drift_std:
+ _type: Vector5d
+ _mandatory: false
+ _doc: A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)].
A:
- follow: SpecStateSensorO3d.schema
+ follow: StateSensorO3d.schema
noise_p_std:
_mandatory: true
_type: double
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
index ee3e760498daee7ac226ac7a86c6055ef28285a7..80de9fbab37713097fd4af705127bf6395c0ac27 100644
--- a/test/dummy/SensorDummyPoia3d.schema
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -1,27 +1,34 @@
follow: SensorBase.schema
states:
- keys:
- _mandatory: false
- _type: string
- _value: POIA
- _doc: states' keys of the sensor
P:
- follow: SpecStateSensorP3d.schema
+ follow: StateSensorP3d.schema
O:
- follow: SpecStateSensorO3d.schema
+ follow: StateSensorO3d.schema
I:
- follow: SpecStateSensor.schema
- type:
- _type: string
- _mandatory: false
- _value: StateParams5
- _doc: The derived type of the state in 'I'
- state:
+ dynamic:
+ _type: bool
+ _mandatory: true
+ _doc: If the state 'I' is dynamic, i.e. it changes along time.
+ value:
_type: Vector5d
_mandatory: true
- _doc: A vector containing the state 'I' values
+ _doc: A vector containing the state 'I' values.
+ prior:
+ mode:
+ _type: string
+ _mandatory: true
+ _options: ["fix", "factor", "initial_guess"]
+ _doc: Can be 'factor' to add an absolute factor (with covariance defined in 'factor_std'), 'fix' to set the values constant or 'initial_guess' to just set the values.
+ factor_std:
+ _type: Vector5d
+ _mandatory: $mode == 'factor'
+ _doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m].
+ drift_std:
+ _type: Vector5d
+ _mandatory: false
+ _doc: A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)].
A:
- follow: SpecStateSensorO3d.schema
+ follow: StateSensorO3d.schema
noise_p_std:
_mandatory: true
_type: double
diff --git a/test/dummy/landmark_dummy.cpp b/test/dummy/landmark_dummy.cpp
index c1ca2670881dfd63f1bfb30da13c3df420426f52..c9c06e04c1a9da9f4906db397a3e72aad524e63b 100644
--- a/test/dummy/landmark_dummy.cpp
+++ b/test/dummy/landmark_dummy.cpp
@@ -22,18 +22,21 @@
namespace wolf
{
-LandmarkDummy::LandmarkDummy(StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- const int& _int_param,
- const double& _double_param)
- : LandmarkBase("LandmarkDummy", SpecStateComposite({})), int_param_(_int_param), double_param_(_double_param)
+LandmarkDummy::LandmarkDummy(const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors,
+ const int& _int_param,
+ const double& _double_param)
+ : LandmarkBase("LandmarkDummy",
+ {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'A', "StateParams1"}},
+ _state_vectors,
+ _state_priors),
+ int_param_(_int_param),
+ double_param_(_double_param)
{
- addStateBlock('P', _p_ptr);
- addStateBlock('O', _o_ptr);
}
LandmarkDummy::LandmarkDummy(const YAML::Node& _node)
- : LandmarkBase("LandmarkDummy", _node),
+ : LandmarkBase("LandmarkDummy", {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'A', "StateParams1"}}, _node),
int_param_(_node["int_param"].as<int>()),
double_param_(_node["double_param"].as<double>())
{
diff --git a/test/dummy/landmark_dummy.h b/test/dummy/landmark_dummy.h
index 38b906afbfc7331df1cc725d7dd25cef7f233a9a..eb0b2923a4f28cb25514e51307bf447d64c2313b 100644
--- a/test/dummy/landmark_dummy.h
+++ b/test/dummy/landmark_dummy.h
@@ -36,7 +36,10 @@ WOLF_PTR_TYPEDEFS(LandmarkDummy);
class LandmarkDummy : public LandmarkBase
{
public:
- LandmarkDummy(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const int& _int_param, const double& _double_param);
+ LandmarkDummy(const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors,
+ const int& _int_param,
+ const double& _double_param);
LandmarkDummy(const YAML::Node& _node);
WOLF_LANDMARK_CREATE(LandmarkDummy);
diff --git a/test/dummy/node_state_blocks_dummy.h b/test/dummy/node_state_blocks_dummy.h
index 1afdda4ea7696415bc57687452a24c65a31402db..ff186cea1d7af2c99068d49415b3bed409bf1c2a 100644
--- a/test/dummy/node_state_blocks_dummy.h
+++ b/test/dummy/node_state_blocks_dummy.h
@@ -31,9 +31,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(NodeStateBlocksDummy);
class NodeStateBlocksDummy : public NodeStateBlocks
{
public:
- NodeStateBlocksDummy() : NodeStateBlocks("DUMMY_CATEGORY", "NodeStateBlocksDummy"){};
- NodeStateBlocksDummy(const SpecStateComposite& _specs)
- : NodeStateBlocks("DUMMY_CATEGORY", "NodeStateBlocksDummy", _specs){};
+ NodeStateBlocksDummy() : NodeStateBlocks("DUMMY_CATEGORY", "NodeStateBlocksDummy", {}, {}, {}){};
+ NodeStateBlocksDummy(const TypeComposite& _state_types,
+ const VectorComposite& _state_vectors,
+ const PriorComposite& _state_priors)
+ : NodeStateBlocks("DUMMY_CATEGORY", "NodeStateBlocksDummy", _state_types, _state_vectors, _state_priors){};
unsigned int id() const override
{
diff --git a/test/dummy/processor_loop_closure_dummy.cpp b/test/dummy/processor_loop_closure_dummy.cpp
index e97544f4f6ff6ba74043bf690dc42d3b9e96847b..0a1a1016b1f5615c766fde65c9d38de05aac3bbb 100644
--- a/test/dummy/processor_loop_closure_dummy.cpp
+++ b/test/dummy/processor_loop_closure_dummy.cpp
@@ -20,6 +20,8 @@
#include "processor_loop_closure_dummy.h"
+using namespace Eigen;
+
namespace wolf
{
ProcessorLoopClosureDummy::ProcessorLoopClosureDummy(const YAML::Node& _params)
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 3139d8ec32f6e5aee6821db3823f2dd97ac320ec..9b526f454d38280b0f9123907e8a3d1d4fc0ae3d 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -19,7 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "processor_tracker_landmark_dummy.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "factor_landmark_dummy.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_block_derived.h"
@@ -101,7 +101,6 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int&
WOLF_INFO("feature ", ftr->id(), " detected!");
}
-
WOLF_INFO(_features_out.size(), " features detected!");
return _features_out.size();
@@ -110,8 +109,8 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int&
LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
// std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
- return LandmarkBase::emplace<LandmarkPose2d>(
- getProblem()->getMap(), std::make_shared<StatePoint2d>(Vector2d::Zero()), std::make_shared<StateAngle>(0));
+ return LandmarkBase::emplace<Landmark2d>(
+ getProblem()->getMap(), VectorComposite({{'P', Vector2d::Zero()}, {'O', Vector1d(0)}}), PriorComposite());
}
FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
diff --git a/test/dummy/sensor_dummy.cpp b/test/dummy/sensor_dummy_po.cpp
similarity index 90%
rename from test/dummy/sensor_dummy.cpp
rename to test/dummy/sensor_dummy_po.cpp
index 5403802c54e014917386c11349660143c8df6e89..d979b05a67432d0b2a8103b06143332eaa3e2e5c 100644
--- a/test/dummy/sensor_dummy.cpp
+++ b/test/dummy/sensor_dummy_po.cpp
@@ -18,11 +18,11 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "sensor_dummy.h"
+#include "sensor_dummy_po.h"
namespace wolf
{
-WOLF_REGISTER_SENSOR(SensorDummy2d);
-WOLF_REGISTER_SENSOR(SensorDummy3d);
+WOLF_REGISTER_SENSOR(SensorDummyPo2d);
+WOLF_REGISTER_SENSOR(SensorDummyPo3d);
} // namespace wolf
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy_po.h
similarity index 73%
rename from test/dummy/sensor_dummy.h
rename to test/dummy/sensor_dummy_po.h
index 3e81866a4925d0647e5713a95ac829cae68c8d7d..7b539457557639e188af86c30cdd54ed7be1eb76 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy_po.h
@@ -28,32 +28,36 @@
namespace wolf
{
template <unsigned int DIM>
-class SensorDummy : public SensorBase
+class SensorDummyPo : public SensorBase
{
private:
double noise_p_std_;
double noise_o_std_;
public:
- SensorDummy(const YAML::Node& _params)
- : SensorBase("SensorDummy" + toString(DIM) + "d", DIM, _params),
+ SensorDummyPo(const YAML::Node& _params)
+ : SensorBase(
+ "SensorDummyPo" + toString(DIM) + "d",
+ DIM,
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params),
noise_p_std_(_params["noise_p_std"].as<double>()),
noise_o_std_(_params["noise_o_std"].as<double>())
{
static_assert(DIM == 2 or DIM == 3);
}
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDummy, DIM);
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorDummyPo, DIM);
- virtual ~SensorDummy() = default;
+ virtual ~SensorDummyPo() = default;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override
{
return (Eigen::Vector2d() << noise_p_std_, noise_o_std_).finished().cwiseAbs2().asDiagonal();
}
};
-typedef SensorDummy<2> SensorDummy2d;
-typedef SensorDummy<3> SensorDummy3d;
-WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy2d);
-WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy3d);
+typedef SensorDummyPo<2> SensorDummyPo2d;
+typedef SensorDummyPo<3> SensorDummyPo3d;
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPo2d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPo3d);
} // namespace wolf
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
index 8c0e930960c5d8dba124bfbc8ef3dc4fda9f321f..3c528e4395d52dcf51555b46606ac1c7e8323a7a 100644
--- a/test/dummy/sensor_dummy_poia.h
+++ b/test/dummy/sensor_dummy_poia.h
@@ -36,7 +36,13 @@ class SensorDummyPoia : public SensorBase
public:
SensorDummyPoia(const YAML::Node& _params)
- : SensorBase("SensorDummyPoia" + toString(DIM) + "d", DIM, _params),
+ : SensorBase("SensorDummyPoia" + toString(DIM) + "d",
+ DIM,
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"},
+ {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"},
+ {'I', "StateParams5"},
+ {'A', "StateQuaternion"}},
+ _params),
noise_p_std_(_params["noise_p_std"].as<double>()),
noise_o_std_(_params["noise_o_std"].as<double>())
{
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 2294c819aec95c1f2c6bed5c0e7b1d6cc857c614..f15e97fc69307cd2b3c610f08b4f18c1373b5d85 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,73 +1,15 @@
-if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
- message("CMake version less than 3.11.0")
+include(FetchContent)
- # Enable ExternalProject CMake module
- include(ExternalProject)
+FetchContent_Declare(
+ googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main)
- set(GTEST_FORCE_SHARED_CRT ON)
- set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
-
- # Download GoogleTest
- ExternalProject_Add(googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG v1.8.x
- # TIMEOUT 1 # We'll try this
- CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
- -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
- -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
- -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
- -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
- -DBUILD_GTEST=ON
- PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
- # Disable install step
- INSTALL_COMMAND ""
- UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
- )
-
- # Get GTest source and binary directories from CMake project
-
- # Specify include dir
- ExternalProject_Get_Property(googletest source_dir)
- set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-
- # Specify MainTest's link libraries
- ExternalProject_Get_Property(googletest binary_dir)
- set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-
- # Create a libgtest target to be used as a dependency by test programs
- add_library(libgtest IMPORTED STATIC GLOBAL)
- add_dependencies(libgtest googletest)
-
- # Set libgtest properties
- set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
- )
-
-else()
-
- message("CMake version equal or greater than 3.11.0")
-
- include(FetchContent)
-
- FetchContent_Declare(
- googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG main)
-
- SET(INSTALL_GTEST OFF) # Disable installation of googletest
- FetchContent_MakeAvailable(googletest)
-
-endif()
-
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
+
function(wolf_add_gtest target)
add_executable(${target} ${ARGN})
- if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
- add_dependencies(${target} libgtest)
- target_link_libraries(${target} PUBLIC libgtest ${PLUGIN_NAME})
- target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
- else()
- target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
- endif()
+ target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
add_test(NAME ${target} COMMAND ${target})
endfunction()
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index aede3ab604fca2320958fe50c5fed4f9bc74f93c..f2580c9392fc0fa88cc42600f6a00b5b53d052fc 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -132,14 +132,15 @@ TEST(SE3, inverseComposite)
p.setRandom();
Vector4d qvec;
qvec.setRandom().normalized();
- VectorComposite pose_vc("PO", {p, qvec});
+ VectorComposite pose_vc({{'P', p}, {'O', qvec}});
Quaterniond q(qvec);
// ground truth
Vector3d pi_true = -(q.conjugate() * p);
Quaterniond qi_true = q.conjugate();
- VectorComposite pose_vc_out("PO", Vector7d::Zero(), {3, 4});
+ VectorComposite pose_vc_out(
+ {{'P', Vector3d::Zero()}, {'O', Vector4d::Zero()}}); // no need normalization, just initializing
inverse(pose_vc, pose_vc_out);
ASSERT_MATRIX_APPROX(pose_vc_out.at('P'), pi_true, 1e-8);
ASSERT_MATRIX_APPROX(pose_vc_out.at('O'), qi_true.coeffs(), 1e-8);
@@ -213,7 +214,8 @@ TEST(SE3, composeVectorComposite)
compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
- VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3, 4});
+ VectorComposite x1, x2,
+ xc({{'P', Vector3d::Zero()}, {'O', Vector4d::Zero()}}); // no need normalization, just initializing
x1.emplace('P', p1);
x1.emplace('O', q1.coeffs());
@@ -230,44 +232,44 @@ TEST(SE3, composeVectorComposite)
ASSERT_QUATERNION_VECTOR_APPROX(vc.at('O'), qc.coeffs(), 1e-8);
}
-TEST(SE3, composeVectorComposite_Jacobians)
-{
- Vector3d p1, p2, pc;
- Quaterniond q1, q2, qc;
- p1.setRandom();
- q1 = exp_q(Vector3d::Random() * 100);
- p2.setRandom();
- q2 = exp_q(Vector3d::Random() * 100);
+// TEST(SE3, composeVectorComposite_Jacobians)
+// {
+// Vector3d p1, p2, pc;
+// Quaterniond q1, q2, qc;
+// p1.setRandom();
+// q1 = exp_q(Vector3d::Random() * 100);
+// p2.setRandom();
+// q2 = exp_q(Vector3d::Random() * 100);
- Matrix3d R1 = q1.matrix();
- Matrix3d R2 = q2.matrix();
+// Matrix3d R1 = q1.matrix();
+// Matrix3d R2 = q2.matrix();
- compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+// compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
- VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3, 4});
- MatrixComposite J_xc_x1, J_xc_x2;
+// VectorComposite x1, x2, xc({{'P', Vector3d::Zero()}, {'P', Vector4d::Zero()}}); // no need normalization, just
+// initializing MatrixComposite J_xc_x1, J_xc_x2;
- x1.emplace('P', p1);
- x1.emplace('O', q1.coeffs());
- x2.emplace('P', p2);
- x2.emplace('O', q2.coeffs());
+// x1.emplace('P', p1);
+// x1.emplace('O', q1.coeffs());
+// x2.emplace('P', p2);
+// x2.emplace('O', q2.coeffs());
- // we'll do xc = x1 * x2 and obtain Jacobians
- compose(x1, x2, xc, J_xc_x1, J_xc_x2);
+// // we'll do xc = x1 * x2 and obtain Jacobians
+// compose(x1, x2, xc, J_xc_x1, J_xc_x2);
- ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
- ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+// ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
+// ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), -R1 * skew(p2), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose(), 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity(), 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), -R1 * skew(p2), 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'P'), Matrix3d::Zero(), 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x1.at('O', 'O'), R2.transpose(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1, 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero(), 1e-8);
- ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity(), 1e-8);
-}
+// ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'P'), R1, 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x2.at('P', 'O'), Matrix3d::Zero(), 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'P'), Matrix3d::Zero(), 1e-8);
+// ASSERT_MATRIX_APPROX(J_xc_x2.at('O', 'O'), Matrix3d::Identity(), 1e-8);
+// }
TEST(SE3, exp_0_Composite)
{
diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
index 9d09b405d31ad1148b035de15f321bec8382e811..2a62ac796a72dc168f61da7060a1ea12c0f9476a 100644
--- a/test/gtest_buffer_frame.cpp
+++ b/test/gtest_buffer_frame.cpp
@@ -42,10 +42,10 @@ class BufferFrameTest : public testing::Test
void SetUp(void) override
{
- f10 = std::make_shared<FrameBase>(TimeStamp(10), nullptr, nullptr, nullptr);
- f20 = std::make_shared<FrameBase>(TimeStamp(20), nullptr, nullptr, nullptr);
- f21 = std::make_shared<FrameBase>(TimeStamp(21), nullptr, nullptr, nullptr);
- f28 = std::make_shared<FrameBase>(TimeStamp(28), nullptr, nullptr, nullptr);
+ f10 = std::make_shared<FrameBase>(TimeStamp(10), TypeComposite{}, VectorComposite{});
+ f20 = std::make_shared<FrameBase>(TimeStamp(20), TypeComposite{}, VectorComposite{});
+ f21 = std::make_shared<FrameBase>(TimeStamp(21), TypeComposite{}, VectorComposite{});
+ f28 = std::make_shared<FrameBase>(TimeStamp(28), TypeComposite{}, VectorComposite{});
tt10 = 1.0;
tt20 = 1.0;
@@ -228,8 +228,7 @@ TEST_F(BufferFrameTest, removeUpTo)
// Specifically, only f28 should remain
buffer_kf.emplace(28, f28);
ASSERT_EQ(buffer_kf.size(), (unsigned int)2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22), nullptr, nullptr, nullptr);
- // PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22), TypeComposite(), VectorComposite());
buffer_kf.removeUpTo(f22->getTimeStamp());
ASSERT_EQ(buffer_kf.size(), (unsigned int)1);
ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(), tt) == nullptr);
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 39c949c506598e3cba756d14cc7504ffcd535f5d..ae2051b6597484e146658912142b307ec79127ff 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -61,21 +61,33 @@ TEST(CaptureBase, Static_sensor_params)
ASSERT_EQ(C->getSensorP(), S->getP());
ASSERT_EQ(C->getSensorO(), S->getO());
ASSERT_EQ(C->getSensorIntrinsic(), nullptr);
+
+ // create a capture with a static sensor state block should throw a runtime error
+ ASSERT_THROW(std::make_shared<CaptureBase>(
+ "DUMMY", 1.5, S, TypeComposite{{'P', "StatePoint2d"}}, VectorComposite{{'P', Vector2d::Zero()}}),
+ std::runtime_error);
}
TEST(CaptureBase, Dynamic_sensor_params)
{
SensorBasePtr S = FactorySensorFile::create(
"SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_dir});
- StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
- StateBlockPtr o(std::make_shared<StateAngle>(0));
- StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
- CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
+ CaptureBasePtr C(std::make_shared<CaptureBase>(
+ "DUMMY",
+ 1.5, // timestamp = 1.5
+ S,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'I', "StateParams3"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}, {'I', Vector3d::Zero()}}));
// query capture blocks
- ASSERT_EQ(C->getSensorP(), p);
- ASSERT_EQ(C->getSensorO(), o);
- ASSERT_EQ(C->getSensorIntrinsic(), i);
+ ASSERT_EQ(C->getSensorP(), C->getStateBlock('P'));
+ ASSERT_EQ(C->getSensorO(), C->getStateBlock('O'));
+ ASSERT_EQ(C->getSensorIntrinsic(), C->getStateBlock('I'));
+
+ // create a capture with a wrong type state block should throw a runtime error
+ ASSERT_THROW(std::make_shared<CaptureBase>(
+ "DUMMY", 1.5, S, TypeComposite{{'I', "StateParams2"}}, VectorComposite{{'I', Vector2d::Zero()}}),
+ std::runtime_error);
}
TEST(CaptureBase, addFeature)
@@ -114,7 +126,7 @@ TEST(CaptureBase, move_from_F_to_KF)
auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, TypeComposite(), VectorComposite()); // dummy F object
auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
@@ -134,7 +146,7 @@ TEST(CaptureBase, move_from_F_to_null)
FrameBasePtr F0 = nullptr;
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, TypeComposite(), VectorComposite()); // dummy F object
auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
@@ -168,7 +180,7 @@ TEST(CaptureBase, move_from_null_to_F)
{
ProblemPtr problem = Problem::create("PO", 2);
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, TypeComposite(), VectorComposite()); // dummy F object
auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
@@ -185,7 +197,7 @@ TEST(CaptureBase, move_from_KF_to_F)
auto KF = problem->emplaceFrame(0.0); // dummy F object
- auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+ auto F = std::make_shared<FrameBase>(0.0, TypeComposite(), VectorComposite()); // dummy F object
auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
diff --git a/test/gtest_composite.cpp b/test/gtest_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ccfea70b58d580a46387b8d9171df2322279bea0
--- /dev/null
+++ b/test/gtest_composite.cpp
@@ -0,0 +1,339 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "core/utils/utils_gtest.h"
+#include "core/common/wolf.h"
+#include "core/composite/composite.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+typedef Composite<int> DummyComposite;
+
+TEST(CompositeTest, ConstructorList)
+{
+ auto dc = DummyComposite{{'a', 1}, {'b', 2}, {'c', 3}, {'d', 4}, {'e', 5}, {'f', 6}, {'g', 7}};
+
+ // getKeys
+ ASSERT_EQ(dc.getKeys(), "abcdefg");
+
+ // has
+ ASSERT_TRUE(dc.has('a'));
+ ASSERT_TRUE(dc.has('b'));
+ ASSERT_TRUE(dc.has('c'));
+ ASSERT_TRUE(dc.has('d'));
+ ASSERT_TRUE(dc.has('e'));
+ ASSERT_TRUE(dc.has('f'));
+ ASSERT_TRUE(dc.has('g'));
+ ASSERT_FALSE(dc.has('A'));
+ ASSERT_FALSE(dc.has('B'));
+ ASSERT_FALSE(dc.has('C'));
+ ASSERT_FALSE(dc.has('D'));
+ ASSERT_FALSE(dc.has('E'));
+ ASSERT_FALSE(dc.has('F'));
+ ASSERT_FALSE(dc.has('G'));
+
+ // at
+ ASSERT_EQ(dc.at('a'), 1);
+ ASSERT_EQ(dc.at('b'), 2);
+ ASSERT_EQ(dc.at('c'), 3);
+ ASSERT_EQ(dc.at('d'), 4);
+ ASSERT_EQ(dc.at('e'), 5);
+ ASSERT_EQ(dc.at('f'), 6);
+ ASSERT_EQ(dc.at('g'), 7);
+
+ // operator()
+ ASSERT_EQ(dc("aceg").getKeys(), "aceg");
+ ASSERT_TRUE(dc("aceg").has('a'));
+ ASSERT_FALSE(dc("aceg").has('b'));
+ ASSERT_TRUE(dc("aceg").has('c'));
+ ASSERT_FALSE(dc("aceg").has('d'));
+ ASSERT_TRUE(dc("aceg").has('e'));
+ ASSERT_FALSE(dc("aceg").has('f'));
+ ASSERT_TRUE(dc("aceg").has('g'));
+
+ // operator <<
+ WOLF_INFO(dc);
+}
+
+TEST(CompositeTest, emplace)
+{
+ auto dc = DummyComposite();
+ dc.emplace('a', 1);
+ dc.emplace('b', 2);
+ dc.emplace('c', 3);
+ dc.emplace('d', 4);
+ dc.emplace('e', 5);
+ dc.emplace('f', 6);
+ dc.emplace('g', 7);
+
+ // getKeys
+ ASSERT_EQ(dc.getKeys(), "abcdefg");
+
+ // has
+ ASSERT_TRUE(dc.has('a'));
+ ASSERT_TRUE(dc.has('b'));
+ ASSERT_TRUE(dc.has('c'));
+ ASSERT_TRUE(dc.has('d'));
+ ASSERT_TRUE(dc.has('e'));
+ ASSERT_TRUE(dc.has('f'));
+ ASSERT_TRUE(dc.has('g'));
+ ASSERT_FALSE(dc.has('A'));
+ ASSERT_FALSE(dc.has('B'));
+ ASSERT_FALSE(dc.has('C'));
+ ASSERT_FALSE(dc.has('D'));
+ ASSERT_FALSE(dc.has('E'));
+ ASSERT_FALSE(dc.has('F'));
+ ASSERT_FALSE(dc.has('G'));
+
+ // at
+ ASSERT_EQ(dc.at('a'), 1);
+ ASSERT_EQ(dc.at('b'), 2);
+ ASSERT_EQ(dc.at('c'), 3);
+ ASSERT_EQ(dc.at('d'), 4);
+ ASSERT_EQ(dc.at('e'), 5);
+ ASSERT_EQ(dc.at('f'), 6);
+ ASSERT_EQ(dc.at('g'), 7);
+}
+
+TEST(CompositeTest, toYaml)
+{
+ auto dc = DummyComposite{{'a', 1}, {'b', 2}, {'c', 3}, {'d', 4}, {'e', 5}, {'f', 6}, {'g', 7}};
+
+ auto dc_yaml = dc.toYaml();
+
+ WOLF_INFO(dc);
+ WOLF_INFO(dc_yaml);
+
+ ASSERT_EQ(dc_yaml["a"].as<int>(), 1);
+ ASSERT_EQ(dc_yaml["b"].as<int>(), 2);
+ ASSERT_EQ(dc_yaml["c"].as<int>(), 3);
+ ASSERT_EQ(dc_yaml["d"].as<int>(), 4);
+ ASSERT_EQ(dc_yaml["e"].as<int>(), 5);
+ ASSERT_EQ(dc_yaml["f"].as<int>(), 6);
+ ASSERT_EQ(dc_yaml["g"].as<int>(), 7);
+}
+
+TEST(CompositeTest, ConstructorYamlPlain)
+{
+ YAML::Node dc_yaml;
+ dc_yaml["a"] = 1;
+ dc_yaml["b"] = 2;
+ dc_yaml["c"] = 3;
+ dc_yaml["d"] = 4;
+ dc_yaml["e"] = 5;
+ dc_yaml["f"] = 6;
+ dc_yaml["g"] = 7;
+
+ auto dc = DummyComposite{dc_yaml, "", true}; // no field
+
+ // getKeys
+ ASSERT_EQ(dc.getKeys(), "abcdefg");
+
+ // has
+ ASSERT_TRUE(dc.has('a'));
+ ASSERT_TRUE(dc.has('b'));
+ ASSERT_TRUE(dc.has('c'));
+ ASSERT_TRUE(dc.has('d'));
+ ASSERT_TRUE(dc.has('e'));
+ ASSERT_TRUE(dc.has('f'));
+ ASSERT_TRUE(dc.has('g'));
+
+ // at
+ ASSERT_EQ(dc.at('a'), 1);
+ ASSERT_EQ(dc.at('b'), 2);
+ ASSERT_EQ(dc.at('c'), 3);
+ ASSERT_EQ(dc.at('d'), 4);
+ ASSERT_EQ(dc.at('e'), 5);
+ ASSERT_EQ(dc.at('f'), 6);
+ ASSERT_EQ(dc.at('g'), 7);
+}
+
+TEST(CompositeTest, ConstructorYamlField)
+{
+ YAML::Node dc_yaml;
+ dc_yaml["a"]["dummy_field"] = 1;
+ dc_yaml["a"]["other_field"] = "whatever_1";
+ dc_yaml["b"]["dummy_field"] = 2;
+ dc_yaml["b"]["other_field"] = "whatever_2";
+ dc_yaml["c"]["dummy_field"] = 3;
+ dc_yaml["c"]["other_field"] = "whatever_3";
+ dc_yaml["d"]["dummy_field"] = 4;
+ dc_yaml["d"]["other_field"] = "whatever_4";
+ dc_yaml["e"]["dummy_field"] = 5;
+ dc_yaml["e"]["other_field"] = "whatever_5";
+ dc_yaml["f"]["dummy_field"] = 6;
+ dc_yaml["f"]["other_field"] = "whatever_6";
+ dc_yaml["g"]["dummy_field"] = 7;
+ dc_yaml["g"]["other_field"] = "whatever_7";
+
+ auto dc = DummyComposite(dc_yaml, "dummy_field", true);
+
+ // getKeys
+ ASSERT_EQ(dc.getKeys(), "abcdefg");
+
+ // has
+ ASSERT_TRUE(dc.has('a'));
+ ASSERT_TRUE(dc.has('b'));
+ ASSERT_TRUE(dc.has('c'));
+ ASSERT_TRUE(dc.has('d'));
+ ASSERT_TRUE(dc.has('e'));
+ ASSERT_TRUE(dc.has('f'));
+ ASSERT_TRUE(dc.has('g'));
+
+ // at
+ ASSERT_EQ(dc.at('a'), 1);
+ ASSERT_EQ(dc.at('b'), 2);
+ ASSERT_EQ(dc.at('c'), 3);
+ ASSERT_EQ(dc.at('d'), 4);
+ ASSERT_EQ(dc.at('e'), 5);
+ ASSERT_EQ(dc.at('f'), 6);
+ ASSERT_EQ(dc.at('g'), 7);
+}
+
+TEST(CompositeTest, ConstructorYamlFieldOptional)
+{
+ YAML::Node dc_yaml;
+ dc_yaml["a"]["dummy_field"] = 1;
+ dc_yaml["a"]["other_field"] = "whatever_1";
+ // dc_yaml["b"]["dummy_field"] = 2;
+ dc_yaml["b"]["other_field"] = "whatever_2";
+ dc_yaml["c"]["dummy_field"] = 3;
+ dc_yaml["c"]["other_field"] = "whatever_3";
+ dc_yaml["d"]["dummy_field"] = 4;
+ dc_yaml["d"]["other_field"] = "whatever_4";
+ dc_yaml["e"]["dummy_field"] = 5;
+ dc_yaml["e"]["other_field"] = "whatever_5";
+ dc_yaml["f"]["dummy_field"] = 6;
+ dc_yaml["f"]["other_field"] = "whatever_6";
+ dc_yaml["g"]["dummy_field"] = 7;
+ dc_yaml["g"]["other_field"] = "whatever_7";
+
+ auto dc = DummyComposite(dc_yaml, "dummy_field", false);
+
+ // getKeys
+ ASSERT_EQ(dc.getKeys(), "acdefg");
+
+ // has
+ ASSERT_TRUE(dc.has('a'));
+ ASSERT_FALSE(dc.has('b'));
+ ASSERT_TRUE(dc.has('c'));
+ ASSERT_TRUE(dc.has('d'));
+ ASSERT_TRUE(dc.has('e'));
+ ASSERT_TRUE(dc.has('f'));
+ ASSERT_TRUE(dc.has('g'));
+
+ // at
+ ASSERT_EQ(dc.at('a'), 1);
+ ASSERT_EQ(dc.at('c'), 3);
+ ASSERT_EQ(dc.at('d'), 4);
+ ASSERT_EQ(dc.at('e'), 5);
+ ASSERT_EQ(dc.at('f'), 6);
+ ASSERT_EQ(dc.at('g'), 7);
+}
+
+TEST(CompositeTest, ConstructorYaml_wrong)
+{
+ YAML::Node dc_yaml;
+ dc_yaml["a"] = 1;
+ dc_yaml["a"] = "whatever_1";
+ // dc_yaml["b"] = 2;
+ dc_yaml["b"] = "whatever_2";
+ dc_yaml["c"] = 3;
+ dc_yaml["c"] = "whatever_3";
+ dc_yaml["d"] = 4;
+ dc_yaml["d"] = "whatever_4";
+ dc_yaml["e"] = 5;
+ dc_yaml["e"] = "whatever_5";
+ dc_yaml["f"] = 6;
+ dc_yaml["f"] = "whatever_6";
+ dc_yaml["g"] = 7;
+ dc_yaml["g"] = "whatever_7";
+
+ ASSERT_THROW(DummyComposite(dc_yaml, "", true), std::runtime_error);
+}
+
+TEST(CompositeTest, ConstructorYamlField_wrong)
+{
+ YAML::Node dc_yaml;
+ dc_yaml["a"]["dummy_field"] = 1;
+ dc_yaml["a"]["other_field"] = "whatever_1";
+ // dc_yaml["b"]["dummy_field"] = 2;
+ dc_yaml["b"]["other_field"] = "whatever_2";
+ dc_yaml["c"]["dummy_field"] = 3;
+ dc_yaml["c"]["other_field"] = "whatever_3";
+ dc_yaml["d"]["dummy_field"] = 4;
+ dc_yaml["d"]["other_field"] = "whatever_4";
+ dc_yaml["e"]["dummy_field"] = 5;
+ dc_yaml["e"]["other_field"] = "whatever_5";
+ dc_yaml["f"]["dummy_field"] = 6;
+ dc_yaml["f"]["other_field"] = "whatever_6";
+ dc_yaml["g"]["dummy_field"] = 7;
+ dc_yaml["g"]["other_field"] = "whatever_7";
+
+ ASSERT_THROW(DummyComposite(dc_yaml, "dummy_field", true), std::runtime_error);
+}
+
+TEST(CompositeTest, ConstructorYamlFieldKeys)
+{
+ YAML::Node dc_yaml;
+ dc_yaml["a"]["dummy_field"] = 1;
+ dc_yaml["a"]["other_field"] = "whatever_1";
+ dc_yaml["b"]["dummy_field"] = 2;
+ dc_yaml["b"]["other_field"] = "whatever_2";
+ dc_yaml["c"]["dummy_field"] = 3;
+ dc_yaml["c"]["other_field"] = "whatever_3";
+ dc_yaml["d"]["dummy_field"] = 4;
+ dc_yaml["d"]["other_field"] = "whatever_4";
+ dc_yaml["e"]["dummy_field"] = 5;
+ dc_yaml["e"]["other_field"] = "whatever_5";
+ dc_yaml["f"]["dummy_field"] = 6;
+ dc_yaml["f"]["other_field"] = "whatever_6";
+ dc_yaml["g"]["dummy_field"] = 7;
+ dc_yaml["g"]["other_field"] = "whatever_7";
+
+ auto dc = DummyComposite{dc_yaml, "dummy_field", true, "aceg"};
+
+ // getKeys
+ ASSERT_EQ(dc.getKeys(), "aceg");
+
+ // has
+ ASSERT_TRUE(dc.has('a'));
+ ASSERT_FALSE(dc.has('b'));
+ ASSERT_TRUE(dc.has('c'));
+ ASSERT_FALSE(dc.has('d'));
+ ASSERT_TRUE(dc.has('e'));
+ ASSERT_FALSE(dc.has('f'));
+ ASSERT_TRUE(dc.has('g'));
+
+ // at
+ ASSERT_EQ(dc.at('a'), 1);
+ ASSERT_EQ(dc.at('c'), 3);
+ ASSERT_EQ(dc.at('e'), 5);
+ ASSERT_EQ(dc.at('g'), 7);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index e075351493d287f17c31eeff2062b78775406d17..2471162fe159902408ba8a919bc7c51e81006fa0 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -24,10 +24,10 @@
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
#include "core/problem/problem.h"
-#include "core/landmark/landmark_point.h"
+#include "core/landmark/landmark.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom.h"
-#include "dummy/sensor_dummy.h"
+#include "dummy/sensor_dummy_po.h"
#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_3d.h"
#include "core/processor/processor_odom_2d.h"
@@ -45,13 +45,15 @@ TEST(Emplace, Landmark)
{
ProblemPtr P = Problem::create("POV", 3);
- auto L = LandmarkBase::emplace<LandmarkBase>(P->getMap(), "Dummy", SpecStateComposite({}));
+ auto L = LandmarkBase::emplace<LandmarkBase>(
+ P->getMap(), "Dummy", TypeComposite(), VectorComposite(), PriorComposite());
ASSERT_EQ(P, L->getProblem());
ASSERT_EQ(P->getMap(), L->getMap());
ASSERT_EQ(P->getMap()->getLandmarkList().front(), L);
- auto L2 = LandmarkBase::emplace<LandmarkPoint3d>(P->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ auto L2 =
+ LandmarkBase::emplace<Landmark3d>(P->getMap(), VectorComposite{{'P', Vector3d::Zero()}}, PriorComposite());
ASSERT_EQ(P, L2->getProblem());
ASSERT_EQ(P->getMap(), L2->getMap());
@@ -62,11 +64,9 @@ TEST(Emplace, Sensor)
{
ProblemPtr P = Problem::create("POV", 3);
- auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml");
- params["plugin"] = "core";
- params["type"] = "SensorDummy3d";
+ auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml");
- auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(), params, {wolf_dir});
+ auto S = SensorBase::emplace<SensorDummyPo3d>(P->getHardware(), params, {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -78,10 +78,12 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(0),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
+ auto F =
+ FrameBase::emplace<FrameBase>(P->getTrajectory(),
+ TimeStamp(0),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{{'P', Prior("fix")}, {'O', Prior("fix")}});
ASSERT_EQ(P, F->getProblem());
ASSERT_EQ(P->getTrajectory(), F->getTrajectory());
@@ -94,9 +96,9 @@ TEST(Emplace, Processor)
auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml");
params["plugin"] = "core";
- params["type"] = "SensorDummy2d";
+ params["type"] = "SensorDummyPo2d";
- auto S = SensorBase::emplace<SensorDummy2d>(P->getHardware(), params, {wolf_dir});
+ auto S = SensorBase::emplace<SensorDummyPo2d>(P->getHardware(), params, {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -111,9 +113,9 @@ TEST(Emplace, Processor)
auto params2 = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml");
params2["plugin"] = "core";
- params2["type"] = "SensorDummy2d";
+ params2["type"] = "SensorDummyPo2d";
- SensorBasePtr sen2 = SensorBase::emplace<SensorDummy2d>(P->getHardware(), params2, {wolf_dir});
+ SensorBasePtr sen2 = SensorBase::emplace<SensorDummyPo2d>(P->getHardware(), params2, {wolf_dir});
ProcessorOdom2dPtr prc2 = ProcessorBase::emplace<ProcessorOdom2d>(
sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
@@ -128,16 +130,18 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(0),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
+ auto F =
+ FrameBase::emplace<FrameBase>(P->getTrajectory(),
+ TimeStamp(0),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{});
ASSERT_EQ(P, F->getProblem());
ASSERT_EQ(P->getTrajectory(), F->getTrajectory());
ASSERT_EQ(P->getTrajectory()->getFirstFrame(), F);
- auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(0), nullptr);
ASSERT_EQ(P, C->getProblem());
ASSERT_EQ(F->getCaptureList().front(), C);
@@ -149,16 +153,18 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(0),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
+ auto F =
+ FrameBase::emplace<FrameBase>(P->getTrajectory(),
+ TimeStamp(0),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{});
ASSERT_EQ(P, F->getProblem());
ASSERT_EQ(P->getTrajectory(), F->getTrajectory());
ASSERT_EQ(P->getTrajectory()->getFirstFrame(), F);
- auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(0), nullptr);
ASSERT_EQ(P, C->getProblem());
ASSERT_EQ(F->getCaptureList().front(), C);
@@ -175,26 +181,29 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
-
- auto F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(0),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
+ auto F0 =
+ FrameBase::emplace<FrameBase>(P->getTrajectory(),
+ TimeStamp(0),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{});
ASSERT_EQ(P, F0->getProblem());
ASSERT_EQ(P->getTrajectory(), F0->getTrajectory());
ASSERT_TRUE(P->getTrajectory()->hasFrame(F0));
- auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(1),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
+ auto F =
+ FrameBase::emplace<FrameBase>(P->getTrajectory(),
+ TimeStamp(1),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{});
ASSERT_EQ(P, F->getProblem());
ASSERT_EQ(P->getTrajectory(), F->getTrajectory());
ASSERT_TRUE(P->getTrajectory()->hasFrame(F));
- auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(1), nullptr, nullptr, nullptr, nullptr);
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(1), nullptr);
ASSERT_EQ(P, C->getProblem());
ASSERT_EQ(F->getCaptureList().front(), C);
@@ -227,8 +236,9 @@ TEST(Emplace, EmplaceDerived)
auto F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(),
TimeStamp(0),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Random()}},
+ PriorComposite{});
ASSERT_EQ(P, F0->getProblem());
ASSERT_EQ(P->getTrajectory(), F0->getTrajectory());
@@ -236,8 +246,9 @@ TEST(Emplace, EmplaceDerived)
auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(),
TimeStamp(1),
- std::make_shared<StatePoint2d>(Vector2d::Zero(), true),
- std::make_shared<StateAngle>(2, true));
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Random()}},
+ PriorComposite{});
ASSERT_EQ(P, F->getProblem());
ASSERT_EQ(P->getTrajectory(), F->getTrajectory());
@@ -272,17 +283,21 @@ TEST(Emplace, ReturnDerived)
ASSERT_NE(P->getTrajectory(), nullptr);
- auto F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(0),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
-
- auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(),
- TimeStamp(1),
- std::make_shared<StatePoint3d>(Vector3d::Zero(), true),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized(), true));
-
- auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(1), nullptr, nullptr, nullptr, nullptr);
+ auto F0 = FrameBase::emplace<FrameBase>(
+ P->getTrajectory(),
+ TimeStamp(0),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Random()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{});
+
+ auto F = FrameBase::emplace<FrameBase>(
+ P->getTrajectory(),
+ TimeStamp(1),
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Random()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite{});
+
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(1), nullptr);
FeatureOdom2dPtr f = FeatureBase::emplace<FeatureOdom2d>(C, VectorXd::Random(2), MatrixXd::Identity(2, 2));
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index b0a1e1e2112ddab220b91a0f4630ebb797d1a7f8..186dee51c0ed8692c75158cfc8f20f05cfbc049c 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -51,10 +51,12 @@ void randomSetup(int dim)
// random pose
if (dim == 2)
- pose = VectorComposite("POV", {Vector2d::Random() * 10, Vector1d::Random() * M_PI, Vector2d::Random() * 2});
+ pose = VectorComposite{
+ {'P', Vector2d::Random() * 10}, {'O', Vector1d::Random() * M_PI}, {'V', Vector2d::Random() * 2}};
else
- pose =
- VectorComposite("POV", {Vector3d::Random() * 10, Vector4d::Random().normalized(), Vector3d::Random() * 2});
+ pose = VectorComposite{
+ {'P', Vector3d::Random() * 10}, {'O', Vector4d::Random().normalized()}, {'V', Vector3d::Random() * 2}};
+ ;
// frame and capture
frm = problem->emplaceFrame(0, pose);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 269b033e0a16c69e8dbe4e013f7dbf451a241708..b0aef27ecdb669095ed0450438bd30b570a94ae0 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -63,8 +63,8 @@ void testFactorAutodiff15(FactorBasePtr _fac, StateBlockPtr _sb, const std::vect
TEST(FactorAutodiff, AutodiffDummy1)
{
- FrameBasePtr F = std::make_shared<FrameBase>(0, nullptr);
- StateBlockPtr sb = F->addStateBlock('a', std::make_shared<StateParams1>(Vector1d::Random()));
+ FrameBasePtr F = std::make_shared<FrameBase>(0, TypeComposite(), VectorComposite());
+ StateBlockPtr sb = F->emplaceStateBlock('a', "StateParams1", Vector1d::Random(), false);
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1, 2), Matrix2d::Identity());
auto fac = std::make_shared<FactorDummyZero1>(ftr, F, sb);
@@ -81,22 +81,22 @@ TEST(FactorAutodiff, AutodiffDummy1)
TEST(FactorAutodiff, AutodiffDummy15)
{
- FrameBasePtr F = std::make_shared<FrameBase>(0, nullptr);
- StateBlockPtr sb1 = F->addStateBlock('a', std::make_shared<StateParams1>(Vector1d::Random()));
- StateBlockPtr sb2 = F->addStateBlock('b', std::make_shared<StateParams2>(Vector2d::Random()));
- StateBlockPtr sb3 = F->addStateBlock('c', std::make_shared<StateParams3>(Vector3d::Random()));
- StateBlockPtr sb4 = F->addStateBlock('d', std::make_shared<StateParams4>(Vector4d::Random()));
- StateBlockPtr sb5 = F->addStateBlock('e', std::make_shared<StateParams5>(Vector5d::Random()));
- StateBlockPtr sb6 = F->addStateBlock('f', std::make_shared<StateParams6>(Vector6d::Random()));
- StateBlockPtr sb7 = F->addStateBlock('g', std::make_shared<StateParams7>(Vector7d::Random()));
- StateBlockPtr sb8 = F->addStateBlock('h', std::make_shared<StateParams8>(Vector8d::Random()));
- StateBlockPtr sb9 = F->addStateBlock('i', std::make_shared<StateParams9>(Vector9d::Random()));
- StateBlockPtr sb10 = F->addStateBlock('j', std::make_shared<StateParams10>(Vector10d::Random()));
- StateBlockPtr sb11 = F->addStateBlock('k', std::make_shared<StateParams<11>>(VectorXd::Random(11)));
- StateBlockPtr sb12 = F->addStateBlock('l', std::make_shared<StateParams<12>>(VectorXd::Random(12)));
- StateBlockPtr sb13 = F->addStateBlock('m', std::make_shared<StateParams<13>>(VectorXd::Random(13)));
- StateBlockPtr sb14 = F->addStateBlock('n', std::make_shared<StateParams<14>>(VectorXd::Random(14)));
- StateBlockPtr sb15 = F->addStateBlock('o', std::make_shared<StateParams<15>>(VectorXd::Random(15)));
+ FrameBasePtr F = std::make_shared<FrameBase>(0, TypeComposite(), VectorComposite());
+ StateBlockPtr sb1 = F->emplaceStateBlock('a', "StateParams1", Vector1d::Random(), false);
+ StateBlockPtr sb2 = F->emplaceStateBlock('b', "StateParams2", Vector2d::Random(), false);
+ StateBlockPtr sb3 = F->emplaceStateBlock('c', "StateParams3", Vector3d::Random(), false);
+ StateBlockPtr sb4 = F->emplaceStateBlock('d', "StateParams4", Vector4d::Random(), false);
+ StateBlockPtr sb5 = F->emplaceStateBlock('e', "StateParams5", Vector5d::Random(), false);
+ StateBlockPtr sb6 = F->emplaceStateBlock('f', "StateParams6", Vector6d::Random(), false);
+ StateBlockPtr sb7 = F->emplaceStateBlock('g', "StateParams7", Vector7d::Random(), false);
+ StateBlockPtr sb8 = F->emplaceStateBlock('h', "StateParams8", Vector8d::Random(), false);
+ StateBlockPtr sb9 = F->emplaceStateBlock('i', "StateParams9", Vector9d::Random(), false);
+ StateBlockPtr sb10 = F->emplaceStateBlock('j', "StateParams10", Vector10d::Random(), false);
+ StateBlockPtr sb11 = F->emplaceStateBlock('k', "StateParams11", VectorXd::Random(11), false);
+ StateBlockPtr sb12 = F->emplaceStateBlock('l', "StateParams12", VectorXd::Random(12), false);
+ StateBlockPtr sb13 = F->emplaceStateBlock('m', "StateParams13", VectorXd::Random(13), false);
+ StateBlockPtr sb14 = F->emplaceStateBlock('n', "StateParams14", VectorXd::Random(14), false);
+ StateBlockPtr sb15 = F->emplaceStateBlock('o', "StateParams15", VectorXd::Random(15), false);
std::vector<StateBlockPtr> state_blocks(
{sb1, sb2, sb3, sb4, sb5, sb6, sb7, sb8, sb9, sb10, sb11, sb12, sb13, sb14, sb15});
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 3509958bdb9cfcfcbecea6a4fe5ccc5cd508985e..fa6d2cba690531bb77827084ad7cc27ff4c60f95 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -20,8 +20,8 @@
#include "core/utils/utils_gtest.h"
#include "dummy/factor_dummy.h"
-#include "dummy/sensor_dummy.h"
-#include "core/landmark/landmark_pose.h"
+#include "dummy/sensor_dummy_po.h"
+#include "core/landmark/landmark.h"
using namespace wolf;
using namespace Eigen;
@@ -37,29 +37,43 @@ class FactorBaseTest : public testing::Test
void SetUp() override
{
YAML::Node rand_params;
- rand_params["name"] = "Sensor0";
- rand_params["states"]["P"]["type"] = "StatePoint2d";
- rand_params["states"]["P"]["state"] = Vector2d::Random();
- rand_params["states"]["P"]["mode"] = "initial_guess";
- rand_params["states"]["P"]["dynamic"] = false;
- rand_params["states"]["O"]["type"] = "StateAngle";
- rand_params["states"]["O"]["state"] = Vector1d::Random();
- rand_params["states"]["O"]["mode"] = "initial_guess";
- rand_params["states"]["O"]["dynamic"] = false;
- rand_params["noise_p_std"] = 0.1;
- rand_params["noise_o_std"] = 0.1;
-
- S0 = SensorDummy2d::create(rand_params, {});
+ rand_params["name"] = "Sensor0";
+ rand_params["states"]["P"]["value"] = Vector2d::Random();
+ rand_params["states"]["P"]["prior"]["mode"] = "initial_guess";
+ rand_params["states"]["P"]["dynamic"] = false;
+ rand_params["states"]["O"]["value"] = Vector1d::Random();
+ rand_params["states"]["O"]["prior"]["mode"] = "initial_guess";
+ rand_params["states"]["O"]["dynamic"] = false;
+ rand_params["noise_p_std"] = 0.1;
+ rand_params["noise_o_std"] = 0.1;
+
+ S0 = SensorDummyPo2d::create(rand_params, {});
rand_params["name"] = "Sensor1";
- S1 = SensorDummy2d::create(rand_params, {});
- F0 = std::make_shared<FrameBase>(0.0, SpecStateComposite(rand_params["states"]));
- F1 = std::make_shared<FrameBase>(1.0, SpecStateComposite(rand_params["states"]));
- C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
- C0->emplaceStateBlocks(SpecStateComposite(rand_params["states"]));
- C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
- C1->emplaceStateBlocks(SpecStateComposite(rand_params["states"]));
- L0 = std::make_shared<LandmarkPose2d>(SpecStateComposite(rand_params["states"]));
- L1 = std::make_shared<LandmarkPose2d>(SpecStateComposite(rand_params["states"]));
+ S1 = SensorDummyPo2d::create(rand_params, {});
+ F0 = std::make_shared<FrameBase>(0.0,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Random()}, {'O', Vector1d::Random()}},
+ PriorComposite{}); // P & O take default prior: "initial_guess"
+ F1 = std::make_shared<FrameBase>(1.0,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Random()}, {'O', Vector1d::Random()}},
+ PriorComposite{}); // P & O take default prior: "initial_guess"
+ C0 = std::make_shared<CaptureBase>("Capture",
+ 0.0,
+ nullptr,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Random()}, {'O', Vector1d::Random()}},
+ PriorComposite{}); // P & O take default prior: "initial_guess"
+ C1 = std::make_shared<CaptureBase>("Capture",
+ 1.0,
+ nullptr,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Random()}, {'O', Vector1d::Random()}},
+ PriorComposite{}); // P & O take default prior: "initial_guess"
+ rand_params["id"] = 1;
+ L0 = std::make_shared<Landmark2d>(rand_params);
+ rand_params["id"] = 2;
+ L1 = std::make_shared<Landmark2d>(rand_params);
}
};
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index b6f4731d312c033fa14e2b7018bdf11f419096fc..ad9332e50ff5456b521e934b4da12057c85aa39f 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -57,14 +57,13 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
- VectorComposite state_zero = problem_->stateZero();
- SpecStateComposite priors;
- priors.emplace('P', SpecState("StatePoint3d", state_zero.vector("P"), "fix"));
- priors.emplace('O', SpecState("StateQuaternion", state_zero.vector("O"), "fix"));
- priors.emplace('V', SpecState("StateVector3d", state_zero.vector("V"), "fix"));
- KF0_ = problem_->setPrior(priors, t0);
-
- KF1_ = problem_->emplaceFrame(t1, state_zero);
+ KF0_ =
+ problem_->emplaceFirstFrame(t0,
+ problem_->getFrameTypes(),
+ problem_->stateZero(),
+ PriorComposite{{'P', Prior("fix")}, {'O', Prior("fix")}, {'V', Prior("fix")}});
+
+ KF1_ = problem_->emplaceFrame(t1, problem_->stateZero());
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 4707451f5bbd9123202afb89db2f9c1c287b2342..c65e6cdb93e578a5a711793db9ee5c2de52b0933 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -71,8 +71,8 @@ class FactorDiffDriveTest : public testing::Test
{wolf_dir + "/test/dummy", wolf_dir + "/schema"}));
// frames
- F0 = problem->emplaceFrame(0.0, "PO", Vector3d::Zero());
- F1 = problem->emplaceFrame(1.0, "PO", Vector3d(1, 0, 0));
+ F0 = problem->emplaceFrame(0.0, problem->stateZero());
+ F1 = problem->emplaceFrame(1.0, VectorComposite{{'P', Vector2d(1, 0)}, {'O', Vector1d(0)}});
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0, 0.0, sensor, data0.Zero(), data_cov, nullptr);
@@ -337,10 +337,11 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d x2(3.0, -3.0, 0.0);
// set prior at t=0 and processor origin
- SpecStateComposite problem_prior;
- problem_prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.1)));
- problem_prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.1)));
- auto F0 = problem->setPrior(problem_prior, t);
+ auto F0 = problem->emplaceFirstFrame(t,
+ problem->getFrameTypes(),
+ problem->stateZero(),
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(0.1))},
+ {'O', Prior("factor", Vector1d::Constant(0.1))}});
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
@@ -458,10 +459,11 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector3d x2(3.0, -3.0, 0.0);
// set prior at t=0 and processor origin
- SpecStateComposite problem_prior;
- problem_prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1)));
- problem_prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1)));
- auto F0 = problem->setPrior(problem_prior, t);
+ auto F0 = problem->emplaceFirstFrame(t,
+ problem->getFrameTypes(),
+ problem->stateZero(),
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(0.1))},
+ {'O', Prior("factor", Vector1d::Constant(0.1))}});
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index c1d35394bd3118e7765b50770cb1c1392bbf330e..d198001c9bb29163f5d6d70da449957db0bb3486 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -40,7 +40,7 @@ auto problem_ptr = Problem::create("PO", 2);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, problem_ptr->stateZero());
// Capture for frm
auto cap = CaptureBase::emplace<CapturePose>(frm, 0, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp
index e97c07b3d7fa76ea47df2258698ab79c3a21ea8a..7b4c46bff1af42da03d4ffd011a126a416ce6793 100644
--- a/test/gtest_factor_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp
@@ -47,7 +47,7 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve
auto sensor_pose2d = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, problem_ptr->stateZero());
// Capture for frm
auto cap = CaptureBase::emplace<CapturePose>(frm, 0, sensor_pose2d, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index fa50c3a40ce5fb461aeaad69051360dd1c60df73..424a63136e43dc006379994b6483154aeb1566ea 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -47,7 +47,7 @@ auto problem_ptr = Problem::create("PO", 3);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, problem_ptr->stateZero());
// Capture for frm
auto cap = CaptureBase::emplace<CapturePose>(frm, 0, nullptr, Vector7d::Zero(), data_cov);
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp
index e6b4426035c9c819d525da6eb508be78ed75845d..abebef94873d7eff723cd6a8e869a26bd6f90da5 100644
--- a/test/gtest_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp
@@ -47,7 +47,7 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve
auto sensor_pose3d = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, problem_ptr->stateZero());
// Capture for frm
auto cap = CaptureBase::emplace<CapturePose>(frm, 0, sensor_pose3d, Vector7d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index e3f957e3e861a3b2707e9de00089df81f11b25c1..7a05fc649c81e5471c78ba38162c165b8ce826a9 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -23,7 +23,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
@@ -48,13 +48,13 @@ auto problem_ptr = Problem::create("PO", 2);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, "PO", Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, "PO", Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, problem_ptr->stateZero());
// Landmark
-LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkPose2d>(problem_ptr->getMap(),
- std::make_shared<StatePoint2d>(Vector2d::Zero()),
- std::make_shared<StateAngle>(Vector1d::Zero()));
+LandmarkBasePtr lmk =
+ LandmarkBase::emplace<Landmark2d>(problem_ptr->getMap(),
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_2d_autodiff.cpp b/test/gtest_factor_relative_pose_2d_autodiff.cpp
index ff74c6f5f43e2da8f95b74dffb4f7c907db66574..47ae192aa4e5aad5f9bc1f11a4f3e54352735b79 100644
--- a/test/gtest_factor_relative_pose_2d_autodiff.cpp
+++ b/test/gtest_factor_relative_pose_2d_autodiff.cpp
@@ -40,8 +40,8 @@ auto problem_ptr = Problem::create("PO", 2);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "PO", Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "PO", Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, problem_ptr->stateZero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index f21dd90d1b8eb08607bd7f6c8fc51fd8f588fbe2..3da1a2e5c856fbffee568fe8b0b3864d0f551495 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -24,7 +24,7 @@
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/capture/capture_odom_2d.h"
#include "core/sensor/sensor_odom.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "core/processor/processor_odom_2d.h"
#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
@@ -53,13 +53,13 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve
auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, problem_ptr->stateZero());
// Landmark
-LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkPose2d>(problem_ptr->getMap(),
- std::make_shared<StatePoint2d>(Vector2d::Zero()),
- std::make_shared<StateAngle>(Vector1d::Zero()));
+LandmarkBasePtr lmk =
+ LandmarkBase::emplace<Landmark2d>(problem_ptr->getMap(),
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
// Capture
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 670db2aca8479d0815fafc89a7990418aafd14c1..0e071b294e89234ffe614fa729fc1c25539b5e6d 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -23,7 +23,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_3d.h"
#include "core/capture/capture_odom_3d.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
@@ -48,14 +48,13 @@ auto problem_ptr = Problem::create("PO", 3);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), problem_ptr->stateZero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, problem_ptr->stateZero());
// Landmark
-LandmarkBasePtr lmk =
- LandmarkBase::emplace<LandmarkPose3d>(problem_ptr->getMap(),
- std::make_shared<StatePoint3d>(Vector3d::Zero()),
- std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+LandmarkBasePtr lmk = LandmarkBase::emplace<Landmark3d>(
+ problem_ptr->getMap(),
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}});
// Capture
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index af04092fa910790c01dfd8200886b680cd8a05a8..8920e50ad77fe2dbd8a29110bb19862c9c5c4af1 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -24,7 +24,7 @@
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
#include "core/capture/capture_odom_3d.h"
#include "core/sensor/sensor_odom.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
@@ -50,14 +50,13 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve
auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, problem_ptr->stateZero());
// Landmark
-LandmarkBasePtr lmk =
- LandmarkBase::emplace<LandmarkPose3d>(problem_ptr->getMap(),
- std::make_shared<StatePoint3d>(Vector3d::Zero()),
- std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+LandmarkBasePtr lmk = LandmarkBase::emplace<Landmark3d>(
+ problem_ptr->getMap(),
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}});
// Capture
auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor, Vector7d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
index f55b01e5f6ef5bfd6b469221e7cb9c9ecbd6c345..bf55ab422ca7a6e98268f02c5d21e35025389510 100644
--- a/test/gtest_factor_relative_position_2d.cpp
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -23,7 +23,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_position_2d.h"
#include "core/capture/capture_void.h"
-#include "core/landmark/landmark_point.h"
+#include "core/landmark/landmark.h"
#include "core/sensor/sensor_odom.h"
#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
@@ -50,12 +50,12 @@ auto problem_ptr = Problem::create("PO", 2);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, problem_ptr->stateZero());
// Landmark
LandmarkBasePtr lmk =
- LandmarkBase::emplace<LandmarkPoint2d>(problem_ptr->getMap(), std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ LandmarkBase::emplace<Landmark2d>(problem_ptr->getMap(), VectorComposite{{'P', Vector2d::Zero()}});
// Capture
auto cap = CaptureBase::emplace<CaptureVoid>(frm1, 1, nullptr);
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
index 1ac48284a4157ec7ba7955a56906f2b7d443564c..8c10f8444057637b68dec599f9feb2781f53a88f 100644
--- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -23,7 +23,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
#include "core/capture/capture_void.h"
-#include "core/landmark/landmark_point.h"
+#include "core/landmark/landmark.h"
#include "core/sensor/sensor_odom.h"
#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
@@ -53,11 +53,11 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve
auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Frame
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
// Landmark
LandmarkBasePtr lmk =
- LandmarkBase::emplace<LandmarkPoint2d>(problem_ptr->getMap(), std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ LandmarkBase::emplace<Landmark2d>(problem_ptr->getMap(), VectorComposite{{'P', Vector2d::Zero()}});
// Capture
auto cap = CaptureBase::emplace<CaptureVoid>(frm, 1, sensor);
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
index 23c4fd42486c2cff1c9b0da4676e154ccbd89a64..8e43dd7bb5494287777fd50d55882dc620e95683 100644
--- a/test/gtest_factor_relative_position_3d.cpp
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -22,7 +22,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_position_3d.h"
-#include "core/landmark/landmark_point.h"
+#include "core/landmark/landmark.h"
#include "core/capture/capture_odom_3d.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
@@ -48,20 +48,20 @@ auto problem_ptr = Problem::create("PO", 3);
auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
-FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
-FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, problem_ptr->stateZero());
+FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, problem_ptr->stateZero());
+FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, problem_ptr->stateZero());
// Landmarks
LandmarkBasePtr lmk1 =
- LandmarkBase::emplace<LandmarkPoint3d>(problem_ptr->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ LandmarkBase::emplace<Landmark3d>(problem_ptr->getMap(), VectorComposite{{'P', Vector3d::Zero()}});
LandmarkBasePtr lmk2 =
- LandmarkBase::emplace<LandmarkPoint3d>(problem_ptr->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ LandmarkBase::emplace<Landmark3d>(problem_ptr->getMap(), VectorComposite{{'P', Vector3d::Zero()}});
LandmarkBasePtr lmk3 =
- LandmarkBase::emplace<LandmarkPoint3d>(problem_ptr->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ LandmarkBase::emplace<Landmark3d>(problem_ptr->getMap(), VectorComposite{{'P', Vector3d::Zero()}});
// Captures
CaptureBasePtr cap1, cap2, cap3;
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
index 34e87214e13d86cade19b8350ce0343414dfb31a..68b39536c395756c700efcfdc13e947684662e11 100644
--- a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -23,7 +23,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
#include "core/capture/capture_odom_3d.h"
-#include "core/landmark/landmark_point.h"
+#include "core/landmark/landmark.h"
#include "core/sensor/sensor_odom.h"
#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
@@ -53,17 +53,17 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve
auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
// Frame
-FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
+FrameBasePtr frm = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero());
// Landmarks
LandmarkBasePtr lmk1 =
- LandmarkBase::emplace<LandmarkPoint3d>(problem_ptr->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ LandmarkBase::emplace<Landmark3d>(problem_ptr->getMap(), VectorComposite{{'P', Vector3d::Zero()}});
LandmarkBasePtr lmk2 =
- LandmarkBase::emplace<LandmarkPoint3d>(problem_ptr->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ LandmarkBase::emplace<Landmark3d>(problem_ptr->getMap(), VectorComposite{{'P', Vector3d::Zero()}});
LandmarkBasePtr lmk3 =
- LandmarkBase::emplace<LandmarkPoint3d>(problem_ptr->getMap(), std::make_shared<StatePoint3d>(Vector3d::Zero()));
+ LandmarkBase::emplace<Landmark3d>(problem_ptr->getMap(), VectorComposite{{'P', Vector3d::Zero()}});
// Capture
auto cap = CaptureBase::emplace<CaptureOdom3d>(frm, 1, sensor, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factory.cpp b/test/gtest_factory.cpp
index 2c00b1c632873e2f5ee1695e74b523b047f7f21a..3c4e7bd8ae4c528b0dfc84f0c5267aecb7d05ca8 100644
--- a/test/gtest_factory.cpp
+++ b/test/gtest_factory.cpp
@@ -21,6 +21,7 @@
#include "core/utils/utils_gtest.h"
#include "dummy/factory_dummy_object.h"
#include "dummy/dummy_object_derived.h"
+#include "dummy/dummy_object_derived_derived.h"
#include "yaml-cpp/yaml.h"
using namespace wolf;
@@ -60,6 +61,16 @@ TEST(TestFactory, getRegisteredKeys)
registered_keys.end());
}
+TEST(TestFactory, duplicatedCreator)
+{
+ // trying to register again the same key and creator should just skip this second registering
+ ASSERT_FALSE(wolf::FactoryDummyObject::registerCreator("DummyObjectDerived", DummyObjectDerived::create));
+
+ // trying to register a different creator with a key registered should give an error
+ ASSERT_THROW(wolf::FactoryDummyObject::registerCreator("DummyObjectDerived", DummyObjectDerivedDerived::create),
+ std::runtime_error);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 8b4ddec1dc214b6b1aa6c52856a7f8086d634920..dc6f265da2480fa8f30a5d899ffbe2e5ad688e06 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -32,15 +32,16 @@
#include <iostream>
using namespace Eigen;
-using namespace std;
using namespace wolf;
std::string wolf_dir = _WOLF_CODE_DIR;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F =
- make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ FrameBasePtr F = std::make_shared<FrameBase>(1,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
// getters
ASSERT_EQ(F->id(), (unsigned int)1);
@@ -53,9 +54,10 @@ TEST(FrameBase, GettersAndSetters)
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F =
- make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
-
+ FrameBasePtr F = std::make_shared<FrameBase>(1,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
ASSERT_EQ(F->getStateBlockMap().size(), (unsigned int)2);
ASSERT_EQ(F->getP()->getState().size(), (unsigned int)2);
ASSERT_EQ(F->getO()->getState().size(), (unsigned int)1);
@@ -64,9 +66,10 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F =
- make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
-
+ FrameBasePtr F = std::make_shared<FrameBase>(1,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
auto S = FactorySensorFile::create("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
@@ -81,10 +84,17 @@ TEST(FrameBase, LinksToTree)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
auto S = P->installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
- auto F1 =
- FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F2 =
- FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+
+ auto F1 = FrameBase::emplace<FrameBase>(T,
+ 1,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F2 = FrameBase::emplace<FrameBase>(T,
+ 2,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
auto p = P->installProcessor(S, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double, 1, 1>::Identity() * .01);
@@ -124,29 +134,59 @@ TEST(FrameBase, LinksToTree)
TEST(FrameBase, Frames)
{
// Problem with 10 frames
- ProblemPtr P = Problem::create("PO", 2);
- TrajectoryBasePtr T = P->getTrajectory();
- auto S = P->installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
- auto F0 =
- FrameBase::emplace<FrameBase>(T, 0, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F1 =
- FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F2 =
- FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F3 =
- FrameBase::emplace<FrameBase>(T, 3, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F4 =
- FrameBase::emplace<FrameBase>(T, 4, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F5 =
- FrameBase::emplace<FrameBase>(T, 5, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F6 =
- FrameBase::emplace<FrameBase>(T, 6, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F7 =
- FrameBase::emplace<FrameBase>(T, 7, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F8 =
- FrameBase::emplace<FrameBase>(T, 8, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
- auto F9 =
- FrameBase::emplace<FrameBase>(T, 9, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ ProblemPtr P = Problem::create("PO", 2);
+ TrajectoryBasePtr T = P->getTrajectory();
+ auto S = P->installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto F0 = FrameBase::emplace<FrameBase>(T,
+ 0,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F1 = FrameBase::emplace<FrameBase>(T,
+ 1,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F2 = FrameBase::emplace<FrameBase>(T,
+ 2,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F3 = FrameBase::emplace<FrameBase>(T,
+ 3,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F4 = FrameBase::emplace<FrameBase>(T,
+ 4,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F5 = FrameBase::emplace<FrameBase>(T,
+ 5,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F6 = FrameBase::emplace<FrameBase>(T,
+ 6,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F7 = FrameBase::emplace<FrameBase>(T,
+ 7,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F8 = FrameBase::emplace<FrameBase>(T,
+ 8,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
+ auto F9 = FrameBase::emplace<FrameBase>(T,
+ 9,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite());
// tree is now consistent
ASSERT_TRUE(P->check(0));
@@ -393,13 +433,12 @@ TEST(FrameBase, Frames)
#include "core/state_block/state_quaternion.h"
TEST(FrameBase, GetSetState)
{
- // Create PQV_3d state blocks
- StateBlockPtr sbp = make_shared<StatePoint3d>(Vector3d::Zero());
- StateBlockPtr sbv = make_shared<StateVector3d>(Vector3d::Zero());
- StateQuaternionPtr sbq = make_shared<StateQuaternion>();
-
- // Create frame
- FrameBase F(1, sbp, sbq, sbv);
+ // Create PQV_3d frame
+ FrameBase F(
+ 1,
+ TypeComposite{{'P', "StatePoint3d"}, {'V', "StateVector3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'V', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}},
+ PriorComposite());
// Give values to vectors and vector blocks
VectorXd x(10), x1(10), x2(10);
@@ -426,7 +465,8 @@ TEST(FrameBase, Constructor_composite)
FrameBase F =
FrameBase(0.0,
{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'V', "StateVector3d"}},
- VectorComposite("POV", {Vector3d(1, 2, 3), Vector4d(1, 2, 3, 4).normalized(), Vector3d(1, 2, 3)}));
+ {{'P', Vector3d(1, 2, 3)}, {'O', Vector4d(1, 2, 3, 4).normalized()}, {'V', Vector3d(1, 2, 3)}},
+ {});
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1, 2, 3), 1e-15);
ASSERT_TRUE(F.getO()->hasLocalParametrization());
diff --git a/test/gtest_map.cpp b/test/gtest_map.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..30a09c2b12a268f1d5b676a2d98d01370d096bf7
--- /dev/null
+++ b/test/gtest_map.cpp
@@ -0,0 +1,279 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include "core/utils/utils_gtest.h"
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "core/map/map_base.h"
+#include "core/landmark/landmark.h"
+#include "dummy/landmark_dummy.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+
+#include <iostream>
+using namespace wolf;
+using namespace Eigen;
+
+VectorXd p1_2d = VectorXd::Random(2);
+VectorXd p2_2d = VectorXd::Random(2);
+VectorXd o2_2d = VectorXd::Random(1);
+VectorXd pdummy = VectorXd::Random(2);
+VectorXd odummy = VectorXd::Random(1);
+VectorXd p1_3d = VectorXd::Random(3);
+VectorXd p2_3d = VectorXd::Random(3);
+VectorXd o2_3d = VectorXd::Random(4).normalized();
+
+int int_param = (int)(Vector1d::Random()(0) * 1e4);
+double double_param = Vector1d::Random()(0) * 1e4;
+
+std::string wolf_dir = _WOLF_CODE_DIR;
+std::string map_path = wolf_dir + "/test/yaml/maps";
+
+TEST(MapYaml, save_2d)
+{
+ WOLF_INFO("p1_2d: ", p1_2d.transpose());
+ WOLF_INFO("p2_2d: ", p2_2d.transpose());
+ WOLF_INFO("o2_2d: ", o2_2d.transpose());
+ WOLF_INFO("pdummy: ", pdummy.transpose());
+ WOLF_INFO("odummy: ", odummy.transpose());
+ WOLF_INFO("p1_3d: ", p1_3d.transpose());
+ WOLF_INFO("p2_3d: ", p2_3d.transpose());
+ WOLF_INFO("o2_3d: ", o2_3d.transpose());
+
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto lmk1 = LandmarkBase::emplace<Landmark2d>(problem->getMap(), //
+ VectorComposite{{'P', p1_2d}},
+ PriorComposite{{'P', Prior("initial_guess")}},
+ 1,
+ 11);
+ auto lmk2 = LandmarkBase::emplace<Landmark2d>(problem->getMap(),
+ VectorComposite{{'P', p2_2d}, {'O', o2_2d}},
+ PriorComposite{{'P', Prior("fix")}, {'O', Prior("factor", o2_2d)}},
+ 2,
+ 12);
+ auto lmk3 = LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), //
+ VectorComposite{{'P', pdummy}, {'O', odummy}},
+ PriorComposite{},
+ int_param,
+ double_param);
+ auto lmk4 = LandmarkBase::emplace<LandmarkDummy>(
+ problem->getMap(),
+ VectorComposite{{'P', pdummy}, {'O', odummy}, {'A', Vector1d(double_param)}},
+ PriorComposite{{'P', Prior("initial_guess")}, //
+ {'O', Prior("fix")},
+ {'A', Prior("factor", o2_2d.cwiseAbs())}},
+ int_param,
+ double_param);
+ lmk1->setTrackId(1);
+ lmk2->setTrackId(2);
+ lmk3->setTrackId(3);
+ lmk4->setTrackId(4);
+
+ // Check Problem functions
+ std::string filename = map_path + "/map_2d_problem.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ ASSERT_TRUE(problem->saveMap(filename, "2d map saved from Problem::saveMap()"));
+
+ // Check Map functions
+ filename = map_path + "/map_2d_map.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ ASSERT_TRUE(problem->getMap()->save(filename, "2d map saved from Map::save()"));
+}
+
+TEST(MapYaml, save_3d)
+{
+ WOLF_INFO("p1_2d: ", p1_2d.transpose());
+ WOLF_INFO("p2_2d: ", p2_2d.transpose());
+ WOLF_INFO("o2_2d: ", o2_2d.transpose());
+ WOLF_INFO("pdummy: ", pdummy.transpose());
+ WOLF_INFO("odummy: ", odummy.transpose());
+ WOLF_INFO("p1_3d: ", p1_3d.transpose());
+ WOLF_INFO("p2_3d: ", p2_3d.transpose());
+ WOLF_INFO("o2_3d: ", o2_3d.transpose());
+
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ auto lmk1 = LandmarkBase::emplace<Landmark3d>(problem->getMap(), //
+ VectorComposite{{'P', p1_3d}},
+ PriorComposite{{'P', Prior("initial_guess")}},
+ 1,
+ 11);
+ auto lmk2 = LandmarkBase::emplace<Landmark3d>(problem->getMap(),
+ VectorComposite{{'P', p2_3d}, {'O', o2_3d}},
+ PriorComposite{{'P', Prior("fix")}, //
+ {'O', Prior("factor", p2_3d.cwiseAbs())}},
+ 2,
+ 12);
+ auto lmk3 = LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), //
+ VectorComposite{{'P', pdummy}, {'O', odummy}},
+ PriorComposite{},
+ int_param,
+ double_param);
+ auto lmk4 = LandmarkBase::emplace<LandmarkDummy>(problem->getMap(),
+ VectorComposite{{'P', pdummy}, //
+ {'O', odummy},
+ {'A', Vector1d(double_param)}},
+ PriorComposite{{'P', Prior("initial_guess")}, //
+ {'O', Prior("fix")},
+ {'A', Prior("factor", o2_2d.cwiseAbs())}},
+ int_param,
+ double_param);
+ lmk1->setTrackId(1);
+ lmk2->setTrackId(2);
+ lmk3->setTrackId(3);
+ lmk4->setTrackId(4);
+
+ // Check Problem functions
+ std::string filename = map_path + "/map_3d_problem.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ ASSERT_TRUE(problem->saveMap(filename, "3d map saved from Problem::saveMap()"));
+
+ // Check Map functions
+ filename = map_path + "/map_3d_map.yaml";
+ std::cout << "Saving map to file: " << filename << std::endl;
+ ASSERT_TRUE(problem->getMap()->save(filename, "3d map saved from Map::save()"));
+}
+
+TEST(MapYaml, load)
+{
+ WOLF_INFO("p1_2d: ", p1_2d.transpose());
+ WOLF_INFO("p2_2d: ", p2_2d.transpose());
+ WOLF_INFO("o2_2d: ", o2_2d.transpose());
+ WOLF_INFO("pdummy: ", pdummy.transpose());
+ WOLF_INFO("odummy: ", odummy.transpose());
+ WOLF_INFO("p1_3d: ", p1_3d.transpose());
+ WOLF_INFO("p2_3d: ", p2_3d.transpose());
+ WOLF_INFO("o2_3d: ", o2_3d.transpose());
+
+ std::list<std::string> yamls{wolf_dir + "/test/yaml/problem_map_2d_1.yaml",
+ wolf_dir + "/test/yaml/problem_map_2d_2.yaml",
+ wolf_dir + "/test/yaml/problem_map_3d_1.yaml",
+ wolf_dir + "/test/yaml/problem_map_3d_2.yaml"};
+
+ for (auto problem_yaml : yamls)
+ {
+ ProblemPtr problem = Problem::autoSetup(problem_yaml, {wolf_dir});
+ ASSERT_TRUE(problem);
+ bool is2d = problem->getDim() == 2;
+
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk->trackId() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), is2d ? p1_2d : p1_3d, 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getPriorFactor('P') == nullptr);
+ ASSERT_EQ(lmk->getType(), is2d ? "Landmark2d" : "Landmark3d");
+ ASSERT_EQ(lmk->getExternalId(), 1);
+ ASSERT_EQ(lmk->getExternalType(), 11);
+ }
+ else if (lmk->trackId() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), is2d ? p2_2d : p2_3d, 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), is2d ? o2_2d : o2_3d, 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getPriorFactor('P') == nullptr);
+ ASSERT_TRUE(lmk->getPriorFactor('O') != nullptr);
+ WOLF_INFO_COND(is2d, "o2_2d.cwiseAbs() = ", o2_2d.cwiseAbs().transpose());
+ WOLF_INFO_COND(not is2d, "p2_3d.cwiseAbs() = ", p2_3d.cwiseAbs().transpose());
+ WOLF_INFO("lmk->getPriorFactor('O')->getMeasurementSquareRootInformationUpper():\n",
+ lmk->getPriorFactor('O')->getMeasurementSquareRootInformationUpper());
+ ASSERT_MATRIX_APPROX(
+ lmk->getPriorFactor('O')->getMeasurementSquareRootInformationUpper().diagonal(),
+ is2d ? o2_2d.cwiseAbs2().cwiseInverse().cwiseSqrt() : p2_3d.cwiseAbs2().cwiseInverse().cwiseSqrt(),
+ 1e-12);
+ ASSERT_EQ(lmk->getType(), is2d ? "Landmark2d" : "Landmark3d");
+ ASSERT_EQ(lmk->getExternalId(), 2);
+ ASSERT_EQ(lmk->getExternalType(), 12);
+ }
+ else if (lmk->trackId() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_TRUE(lmk->getStateBlock('A') == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), pdummy, 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), odummy, 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getPriorFactor('P') == nullptr);
+ ASSERT_TRUE(lmk->getPriorFactor('O') == nullptr);
+ ASSERT_EQ(lmk->getType(), "LandmarkDummy");
+
+ auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk);
+ ASSERT_TRUE(lmk_dummy != nullptr);
+ ASSERT_EQ(lmk_dummy->getIntParam(), int_param);
+ ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12);
+ ASSERT_EQ(lmk->getExternalId(), -1);
+ ASSERT_EQ(lmk->getExternalType(), -1);
+ }
+ else if (lmk->trackId() == 4)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_TRUE(lmk->getStateBlock('A') != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), pdummy, 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), odummy, 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getStateBlock('A')->getState(), Vector1d(double_param), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ ASSERT_FALSE(lmk->getStateBlock('A')->isFixed());
+ ASSERT_TRUE(lmk->getPriorFactor('P') == nullptr);
+ ASSERT_TRUE(lmk->getPriorFactor('O') == nullptr);
+ ASSERT_TRUE(lmk->getPriorFactor('A') != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getPriorFactor('A')->getMeasurementSquareRootInformationUpper().diagonal(),
+ o2_2d.cwiseAbs2().cwiseInverse().cwiseSqrt(),
+ 1e-12);
+ ASSERT_EQ(lmk->getType(), "LandmarkDummy");
+
+ auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk);
+ ASSERT_TRUE(lmk_dummy != nullptr);
+ ASSERT_EQ(lmk_dummy->getIntParam(), int_param);
+ ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12);
+ ASSERT_EQ(lmk->getExternalId(), -1);
+ ASSERT_EQ(lmk->getExternalType(), -1);
+ }
+ else
+ ASSERT_FALSE(true);
+ }
+
+ // empty the map
+ {
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list) lmk->remove();
+ }
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
deleted file mode 100644
index 9ddf6ece9e8865684c38ca7ecacf8fad879e6fdf..0000000000000000000000000000000000000000
--- a/test/gtest_map_yaml.cpp
+++ /dev/null
@@ -1,171 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/utils/utils_gtest.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_point.h"
-#include "core/landmark/landmark_pose.h"
-#include "dummy/landmark_dummy.h"
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_angle.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/state_block/local_parametrization_quaternion.h"
-
-#include <iostream>
-using namespace wolf;
-using namespace Eigen;
-
-VectorXd p1_2d = VectorXd::Random(2);
-VectorXd p2_2d = VectorXd::Random(2);
-VectorXd o2_2d = VectorXd::Random(1);
-
-VectorXd pdummy = VectorXd::Random(2);
-VectorXd odummy = VectorXd::Random(1);
-
-VectorXd p1_3d = VectorXd::Random(3);
-VectorXd p2_3d = VectorXd::Random(3);
-VectorXd o2_3d = VectorXd::Random(4).normalized();
-
-int int_param = (int)(Vector1d::Random()(0) * 1e4);
-double double_param = Vector1d::Random()(0) * 1e4;
-
-std::string wolf_dir = _WOLF_CODE_DIR;
-std::string map_path = wolf_dir + "/test/yaml/maps";
-
-TEST(MapYaml, save_2d)
-{
- ProblemPtr problem = Problem::create("PO", 2);
-
- StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1_2d);
- StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2_2d);
- StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2_2d(0));
- StateBlockPtr pd_sb = std::make_shared<StatePoint2d>(pdummy, true);
- StateBlockPtr od_sb = std::make_shared<StateAngle>(odummy, true);
-
- LandmarkBase::emplace<LandmarkPoint2d>(problem->getMap(), p1_sb);
- LandmarkBase::emplace<LandmarkPose2d>(problem->getMap(), p2_sb, o2_sb);
- LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), pd_sb, od_sb, int_param, double_param);
-
- // Check Problem functions
- std::string filename = map_path + "/map_2d_problem.yaml";
- std::cout << "Saving map to file: " << filename << std::endl;
- ASSERT_TRUE(problem->saveMap(filename, "2d map saved from Problem::saveMap()"));
-
- // Check Map functions
- filename = map_path + "/map_2d_map.yaml";
- std::cout << "Saving map to file: " << filename << std::endl;
- ASSERT_TRUE(problem->getMap()->save(filename, "2d map saved from Map::save()"));
-}
-
-TEST(MapYaml, save_3d)
-{
- ProblemPtr problem = Problem::create("PO", 3);
-
- auto p1_sb = std::make_shared<StatePoint3d>(p1_3d);
- auto p2_sb = std::make_shared<StatePoint3d>(p2_3d);
- auto o2_sb = std::make_shared<StateQuaternion>(o2_3d);
- auto pd_sb = std::make_shared<StatePoint2d>(pdummy, true);
- auto od_sb = std::make_shared<StateAngle>(odummy, true);
-
- LandmarkBase::emplace<LandmarkPoint3d>(problem->getMap(), p1_sb);
- LandmarkBase::emplace<LandmarkPose3d>(problem->getMap(), p2_sb, o2_sb);
- LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), pd_sb, od_sb, int_param, double_param);
-
- // Check Problem functions
- std::string filename = map_path + "/map_3d_problem.yaml";
- std::cout << "Saving map to file: " << filename << std::endl;
- ASSERT_TRUE(problem->saveMap(filename, "3d map saved from Problem::saveMap()"));
-
- // Check Map functions
- filename = map_path + "/map_3d_map.yaml";
- std::cout << "Saving map to file: " << filename << std::endl;
- ASSERT_TRUE(problem->getMap()->save(filename, "3d map saved from Map::save()"));
-}
-
-TEST(MapYaml, load)
-{
- std::list<std::string> yamls{wolf_dir + "/test/yaml/problem_map_2d_1.yaml",
- wolf_dir + "/test/yaml/problem_map_2d_2.yaml",
- wolf_dir + "/test/yaml/problem_map_3d_1.yaml",
- wolf_dir + "/test/yaml/problem_map_3d_2.yaml"};
-
- for (auto problem_yaml : yamls)
- {
- ProblemPtr problem = Problem::autoSetup(problem_yaml, {wolf_dir});
- ASSERT_TRUE(problem);
- bool is2d = problem->getDim() == 2;
-
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- WOLF_INFO(lmk->getP()->getState().transpose())
- WOLF_INFO(p1_3d)
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), is2d ? p1_2d : p1_3d, 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_EQ(lmk->getType(), is2d ? "LandmarkPoint2d" : "LandmarkPoint3d");
- }
- else if (lmk->id() == 2)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), is2d ? p2_2d : p2_3d, 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), is2d ? o2_2d : o2_3d, 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- ASSERT_EQ(lmk->getType(), is2d ? "LandmarkPose2d" : "LandmarkPose3d");
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), pdummy, 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), odummy, 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- ASSERT_EQ(lmk->getType(), "LandmarkDummy");
-
- auto lmk_dummy = std::dynamic_pointer_cast<LandmarkDummy>(lmk);
- ASSERT_TRUE(lmk_dummy != nullptr);
- ASSERT_EQ(lmk_dummy->getIntParam(), int_param);
- ASSERT_NEAR(lmk_dummy->getDoubleParam(), double_param, 1e-12);
- }
- }
-
- // empty the map
- {
- auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list) lmk->remove();
- }
- ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_matrix_composite.cpp b/test/gtest_matrix_composite_OLD.cpp
similarity index 99%
rename from test/gtest_matrix_composite.cpp
rename to test/gtest_matrix_composite_OLD.cpp
index 2946ace5b27790af4bbaa9f3362c2664bb14e807..1e0e1627193a8430b65b08f6c83e9c06b7422425 100644
--- a/test/gtest_matrix_composite.cpp
+++ b/test/gtest_matrix_composite_OLD.cpp
@@ -25,6 +25,7 @@
#include "core/utils/utils_gtest.h"
using namespace wolf;
+using namespace Eigen;
using namespace std;
TEST(MatrixComposite, Constructor_empty)
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index dbfe70eca706e2d7a5c85d8b8d418fd244a56709..1b9b59667abf6ea9940c3d501f7ba576064dae28 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -117,8 +117,8 @@ TEST_F(MotionProviderTest, install)
TEST_F(MotionProviderTest, odometry)
{
- VectorComposite odom_p("P", {Vector2d::Zero()});
- VectorComposite odom_pov("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
+ VectorComposite odom_p({{'P', Vector2d::Zero()}});
+ VectorComposite odom_pov({{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}, {'V', Vector2d::Zero()}});
// Error: required PO keys to be added
ASSERT_DEATH({ mp1->setOdometry(odom_p); }, "");
@@ -127,7 +127,7 @@ TEST_F(MotionProviderTest, odometry)
mp3->setOdometry(odom_pov);
// mp1 ->set odom = 0, 0, 0
- VectorComposite odom1("PO", {Vector2d::Zero(), Vector1d::Zero()});
+ VectorComposite odom1({{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
mp1->setOdometry(odom1);
auto odom1_get = mp1->getOdometry();
EXPECT_TRUE(odom1_get.count('P') == 1);
@@ -136,7 +136,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
// mp1 ->set odom = 1, 1, 1
- VectorComposite odom2("PO", {Vector2d::Ones(), Vector1d::Ones()});
+ VectorComposite odom2({{'P', Vector2d::Ones()}, {'O', Vector1d::Ones()}});
mp2->setOdometry(odom2);
auto odom2_get = mp2->getOdometry();
EXPECT_TRUE(odom2_get.count('P') == 1);
@@ -145,7 +145,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
// mp1 ->set odom = 2, 2, 2, 2, 2
- VectorComposite odom3("POV", {2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()});
+ VectorComposite odom3({{'P', 2 * Vector2d::Ones()}, {'O', 2 * Vector1d::Ones()}, {'V', 2 * Vector2d::Ones()}});
mp3->setOdometry(odom3);
auto odom3_get = mp3->getOdometry();
EXPECT_TRUE(odom3_get.count('P') == 1);
diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp
index 8bda6dbdd05f94305e7332d3a725ec8078ad3508..35c627f242131c7eb62f29df96d55c023d77c2c4 100644
--- a/test/gtest_node_state_blocks.cpp
+++ b/test/gtest_node_state_blocks.cpp
@@ -31,6 +31,7 @@
#include "core/state_block/state_angle.h"
using namespace wolf;
+using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
@@ -95,14 +96,17 @@ TEST(NodeStateBlocksTest, constructor_empty)
ASSERT_FALSE(N->isSensor());
}
-// category, type and SpecStateComposite (NodeStateBlocksDummy specs)
-TEST(NodeStateBlocksTest, constructor_specs)
+// types, vectors and priors
+TEST(NodeStateBlocksTest, constructor_type_vector_prior)
{
- auto specs = SpecStateComposite({{'P', SpecState("StatePoint3d", p_state, "fix")},
- {'O', SpecState("StateQuaternion", o_state, "factor", o_std)},
- {'I', SpecState("StateParams5", i_state, "initial_guess")},
- {'A', SpecState("StateAngle", a_state, "factor", a_std)}});
- auto N = std::make_shared<NodeStateBlocksDummy>(specs);
+ auto types =
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'I', "StateParams5"}, {'A', "StateAngle"}};
+ auto vectors = VectorComposite{{'P', p_state}, {'O', o_state}, {'I', i_state}, {'A', a_state}};
+ auto priors = PriorComposite{{'P', Prior("fix")},
+ {'O', Prior("factor", o_std)},
+ {'I', Prior("initial_guess")},
+ {'A', Prior("factor", a_std)}};
+ auto N = std::make_shared<NodeStateBlocksDummy>(types, vectors, priors);
ASSERT_TRUE(N->has("POIA"));
ASSERT_EQ(N->getKeys().size(), 4);
@@ -123,8 +127,8 @@ TEST(NodeStateBlocksTest, constructor_specs)
ASSERT_FALSE(N->isLandmark());
ASSERT_FALSE(N->isSensor());
- // apply priors
- N->emplacePriors(specs);
+ // apply priors (fix and initial guess priors wouldn't be necessary to put here)
+ N->emplacePriors();
checkNode(N, 'P', p_state, true, MatrixXd(0, 0));
checkNode(N, 'O', o_state, false, o_cov);
@@ -136,83 +140,42 @@ TEST(NodeStateBlocksTest, constructor_specs)
ASSERT_EQ(N->getPriorFactorMap().size(), 2);
}
-// category, type and TypeComposite and VectorComposite (NodeStateBlocksDummy types and vectors)
-TEST(NodeStateBlocksTest, constructor_types_vectors)
-{
- auto types =
- TypeComposite({{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'I', "StateParams5"}, {'A', "StateAngle"}});
- auto vectors = VectorComposite({{'P', p_state}, {'O', o_state}, {'I', i_state}, {'A', a_state}});
-
- auto N = std::make_shared<NodeStateBlocksDummy>(SpecStateComposite(types, vectors, false));
-
- ASSERT_TRUE(N->has("POIA"));
- ASSERT_EQ(N->getKeys().size(), 4);
- ASSERT_EQ(N->getStateBlockMap().size(), 4);
- ASSERT_EQ(N->getStateBlockVec().size(), 4);
-
- checkNode(N, 'P', p_state, false, MatrixXd(0, 0));
- checkNode(N, 'O', o_state, false, MatrixXd(0, 0));
- checkNode(N, 'I', i_state, false, MatrixXd(0, 0));
- checkNode(N, 'A', a_state, false, MatrixXd(0, 0));
-
- ASSERT_TRUE(N->getFactoredBySet().empty());
- ASSERT_TRUE(N->getSensoryFactorSet().empty());
- ASSERT_TRUE(N->getPriorFactorMap().empty());
-
- ASSERT_FALSE(N->isCapture());
- ASSERT_FALSE(N->isFrame());
- ASSERT_FALSE(N->isLandmark());
- ASSERT_FALSE(N->isSensor());
-
- // apply priors
- vectors = VectorComposite({{'P', Vector3d::Zero()},
- {'O', Quaterniond::Identity().coeffs()},
- {'I', Vector5d::Zero()},
- {'A', Vector1d::Zero()}});
- N->emplacePriors(SpecStateComposite(types, vectors, true)); // changes states and fix
-
- checkNode(N, 'P', Vector3d::Zero(), true, MatrixXd(0, 0));
- checkNode(N, 'O', Quaterniond::Identity().coeffs(), true, MatrixXd(0, 0));
- checkNode(N, 'I', Vector5d::Zero(), true, MatrixXd(0, 0));
- checkNode(N, 'A', Vector1d::Zero(), true, MatrixXd(0, 0));
-
- ASSERT_TRUE(N->getFactoredBySet().empty());
- ASSERT_TRUE(N->getSensoryFactorSet().empty());
- ASSERT_TRUE(N->getPriorFactorMap().empty());
-}
-
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// OTHER CLASS TESTS ////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
class NodeStateBlocksDerivedTest : public testing::Test
{
public:
- ProblemPtr problem;
- SensorBasePtr S0, S1;
- FrameBasePtr F0, F1;
- CaptureBasePtr C0, C1;
- LandmarkBasePtr L0, L1;
- StateBlockPtr sbp0, sbv0, sbp1, sbv1;
- StateQuaternionPtr sbo0, sbo1;
+ ProblemPtr problem;
+ SensorBasePtr S0, S1;
+ FrameBasePtr F0, F1;
+ StateBlockPtr sbp0, sbo0, sbv0, sbp1, sbv1;
+ FactorBasePtr f0p;
void SetUp() override
{
problem = Problem::create("POV", 3);
- sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
- sbo0 = std::make_shared<StateQuaternion>();
- sbv0 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
- sbp1 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
- sbo1 = std::make_shared<StateQuaternion>();
- sbv1 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
-
- F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
- F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+ // Two frames not linked to problem
+ F0 = FrameBase::emplace<FrameBase>(
+ nullptr,
+ 0.0,
+ TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}},
+ VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d(Quaterniond::Identity().coeffs())}},
+ PriorComposite{{'P', Prior("factor", Vector3d::Ones())}});
+ F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, TypeComposite(), VectorComposite(), PriorComposite());
+ f0p = F0->getPriorFactor('P');
+ sbp0 = F0->getStateBlock('P');
+ sbo0 = F0->getStateBlock('O');
+
+ ASSERT_TRUE(f0p);
+ ASSERT_TRUE(sbp0);
+ ASSERT_TRUE(sbo0);
}
void TearDown() override {}
};
-TEST_F(NodeStateBlocksDerivedTest, Notifications_add_makeKF)
+TEST_F(NodeStateBlocksDerivedTest, Notifications_emplaceSB_makeKF)
{
Notification n;
@@ -220,7 +183,7 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_add_makeKF)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->addStateBlock('V', sbv0);
+ sbv0 = F0->emplaceStateBlock('V', "StateVector3d", Vector3d::Zero(), false);
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
@@ -233,7 +196,7 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_add_makeKF)
ASSERT_EQ(n, ADD);
}
-TEST_F(NodeStateBlocksDerivedTest, Notifications_makeKF_add)
+TEST_F(NodeStateBlocksDerivedTest, Notifications_makeKF_emplaceSB)
{
Notification n;
@@ -241,22 +204,19 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_makeKF_add)
F1->link(problem);
- F1->addStateBlock('P', sbp1);
+ sbp1 = F1->emplaceStateBlock('P', "StatePoint3d", Vector3d::Zero(), false);
ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
ASSERT_EQ(n, ADD);
// add another SB
-
- ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
-
- F1->addStateBlock('V', sbv1);
+ sbv1 = F1->emplaceStateBlock('V', "StateVector3d", Vector3d::Zero(), false);
ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
ASSERT_EQ(n, ADD);
}
-TEST_F(NodeStateBlocksDerivedTest, Add_solve_notify_solve_add)
+TEST_F(NodeStateBlocksDerivedTest, emplace_solve_notify_solve_emplace)
{
auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
@@ -266,7 +226,7 @@ TEST_F(NodeStateBlocksDerivedTest, Add_solve_notify_solve_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->addStateBlock('V', sbv0);
+ sbv0 = F0->emplaceStateBlock('V', "StateVector3d", Vector3d::Zero(), false);
F0->link(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
@@ -293,7 +253,30 @@ TEST_F(NodeStateBlocksDerivedTest, Add_solve_notify_solve_add)
// Add again the same SB. This should crash
- ASSERT_DEATH(F0->addStateBlock('V', sbv0), "");
+ ASSERT_THROW(F0->emplaceStateBlock('V', "StateVector3d", Vector3d::Zero(), false), std::runtime_error);
+}
+
+TEST_F(NodeStateBlocksDerivedTest, notify_prior_factors)
+{
+ /* check that priors emplaced in constructor or afterwards are notified and sent to the solver*/
+ F0->link(problem);
+ Notification n;
+
+ // prior factor notifications
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getFactorNotification(f0p, n));
+
+ // create solver
+ SolverManagerPtr solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+ // update will consume notifications
+ solver->update();
+
+ // prior factor notifications
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_FALSE(problem->getFactorNotification(f0p, n));
+
+ ASSERT_TRUE(solver->isFactorRegistered(f0p));
}
TEST_F(NodeStateBlocksDerivedTest, hasStateBlock)
@@ -314,7 +297,7 @@ TEST_F(NodeStateBlocksDerivedTest, stateBlockKey)
TEST_F(NodeStateBlocksDerivedTest, getState_structure)
{
- F0->addStateBlock('V', sbv0); // now KF0 is POV
+ sbv0 = F0->emplaceStateBlock('V', "StateVector3d", Vector3d::Zero(), false); // now KF0 is POV
WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getKeys());
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 78a21375ff5545e5e54ccc7218f8dfabf4d9c2e0..84f7db992717ce6d9c452e8f1de74a70e6d543d0 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -23,7 +23,7 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom.h"
-#include "dummy/sensor_dummy.h"
+#include "dummy/sensor_dummy_po.h"
#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
@@ -36,8 +36,8 @@
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
-#include "core/landmark/landmark_point.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
+#include "core/landmark/landmark.h"
#include "core/math/SE3.h"
#include <iostream>
@@ -90,14 +90,14 @@ TEST(Problem, Installers)
ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getMotionProviderMap().begin()->second), pm);
}
-TEST(Problem, SetOrigin_PO_2d)
+TEST(Problem, emplaceFirstFrame_PO_2d)
{
- ProblemPtr P = Problem::create("PO", 2);
- TimeStamp t0(0);
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d(1, 2), "factor", Vector2d(sqrt(0.1), sqrt(0.1))));
- prior.emplace('O', SpecState("StateAngle", Vector1d(3), "factor", Vector1d(sqrt(0.1))));
- P->setPrior(prior, t0);
+ ProblemPtr P = Problem::create("PO", 2);
+ TimeStamp t0(0);
+ VectorComposite first_frame_values{{'P', Vector2d(1, 2)}, {'O', Vector1d(3)}};
+ PriorComposite first_frame_priors{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))},
+ {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}};
+ P->emplaceFirstFrame(t0, {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, first_frame_values, first_frame_priors);
WOLF_INFO("printing...");
P->print(4, 1, 1, 1);
@@ -106,7 +106,7 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd)0);
// check that the state is correct
- ASSERT_POSE2d_APPROX(prior.getVectorComposite().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL);
+ ASSERT_POSE2d_APPROX(first_frame_values.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL);
// check that the tree has one frame without captures
TrajectoryBasePtr T = P->getTrajectory();
@@ -130,12 +130,12 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(fac->getLandmarksFactored().size(), 0);
ASSERT_EQ(fac->getCapturesFactored().size(), 0);
}
- MatrixXd sqrt_info_p = prior.at('P').getNoiseStd().cwiseInverse().asDiagonal();
- MatrixXd sqrt_info_o = prior.at('O').getNoiseStd().cwiseInverse().asDiagonal();
+ MatrixXd sqrt_info_p = first_frame_priors.at('P').getFactorStd().cwiseInverse().asDiagonal();
+ MatrixXd sqrt_info_o = first_frame_priors.at('O').getFactorStd().cwiseInverse().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(prior.at('P').getState(), F->getPriorFactor('P')->getMeasurement(), Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(prior.at('O').getState(), F->getPriorFactor('O')->getMeasurement(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(first_frame_values.at('P'), F->getPriorFactor('P')->getMeasurement(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(first_frame_values.at('O'), F->getPriorFactor('O')->getMeasurement(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(
sqrt_info_p, F->getPriorFactor('P')->getMeasurementSquareRootInformationUpper(), Constants::EPS);
ASSERT_MATRIX_APPROX(
@@ -144,15 +144,15 @@ TEST(Problem, SetOrigin_PO_2d)
// P->print(4,1,1,1);
}
-TEST(Problem, SetOrigin_PO_3d)
+TEST(Problem, emplaceFirstFrame_PO_3d)
{
- ProblemPtr P = Problem::create("PO", 3);
- TimeStamp t0(0);
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint3d", Vector3d(1, 2, 3), "factor", Vector3d::Constant(sqrt(0.1))));
- prior.emplace(
- 'O', SpecState("StateQuaternion", Vector4d(4, 5, 6, 7).normalized(), "factor", Vector3d::Constant(sqrt(0.1))));
- P->setPrior(prior, t0);
+ ProblemPtr P = Problem::create("PO", 3);
+ TimeStamp t0(0);
+ VectorComposite first_frame_values{{'P', Vector3d(1, 2, 3)}, {'O', Vector4d(4, 5, 6, 7).normalized()}};
+ PriorComposite first_frame_priors{{'P', Prior("factor", Vector3d::Constant(sqrt(0.1)))},
+ {'O', Prior("factor", Vector3d::Constant(sqrt(0.1)))}};
+ P->emplaceFirstFrame(
+ t0, {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, first_frame_values, first_frame_priors);
WOLF_INFO("printing...");
P->print(4, 1, 1, 1);
@@ -161,7 +161,7 @@ TEST(Problem, SetOrigin_PO_3d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd)0);
// check that the state is correct
- ASSERT_POSE3d_APPROX(prior.getVectorComposite().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL);
+ ASSERT_POSE3d_APPROX(first_frame_values.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL);
// check that the tree has one frame without captures
TrajectoryBasePtr T = P->getTrajectory();
@@ -185,12 +185,12 @@ TEST(Problem, SetOrigin_PO_3d)
ASSERT_EQ(fac->getLandmarksFactored().size(), 0);
ASSERT_EQ(fac->getCapturesFactored().size(), 0);
}
- MatrixXd sqrt_info_p = prior.at('P').getNoiseStd().cwiseInverse().asDiagonal();
- MatrixXd sqrt_info_o = prior.at('O').getNoiseStd().cwiseInverse().asDiagonal();
+ MatrixXd sqrt_info_p = first_frame_priors.at('P').getFactorStd().cwiseInverse().asDiagonal();
+ MatrixXd sqrt_info_o = first_frame_priors.at('O').getFactorStd().cwiseInverse().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(prior.at('P').getState(), F->getPriorFactor('P')->getMeasurement(), Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(prior.at('O').getState(), F->getPriorFactor('O')->getMeasurement(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(first_frame_values.at('P'), F->getPriorFactor('P')->getMeasurement(), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(first_frame_values.at('O'), F->getPriorFactor('O')->getMeasurement(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(
sqrt_info_p, F->getPriorFactor('P')->getMeasurementSquareRootInformationUpper(), Constants::EPS);
ASSERT_MATRIX_APPROX(
@@ -258,7 +258,6 @@ TEST(Problem, StateBlocks)
ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7d xs3d;
xs3d << 1, 2, 3, 0, 0, 0, 1; // required normalized quaternion (solver manager checks this)
- Eigen::Vector3d xs2d;
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
@@ -350,6 +349,8 @@ TEST(Problem, perturb)
// make a sensor first
auto sensor = problem->installSensor(wolf_dir + "/test/yaml/sensor_diff_drive_gtest_problem.yaml", {wolf_dir});
+ ASSERT_TRUE(sensor->isStateBlockDynamic('I'));
+
Vector3d pose;
pose << 0, 0, 0;
@@ -361,17 +362,20 @@ TEST(Problem, perturb)
for (int j = 0; j < 2; j++)
{
- auto sb = std::make_shared<StatePoint3d>(Vector3d(1, 1, 1));
- auto cap = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
+ CaptureBase::emplace<CaptureBase>(F,
+ "CaptureBase",
+ t,
+ sensor,
+ TypeComposite{{'I', "StateParams3"}},
+ VectorComposite{{'I', Vector3d(1, 1, 1)}});
}
i++;
}
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1, 2));
- auto sb2 = std::make_shared<StateAngle>(3);
- auto L = LandmarkBase::emplace<LandmarkPose2d>(problem->getMap(), sb1, sb2);
+ auto L = LandmarkBase::emplace<Landmark2d>(problem->getMap(),
+ VectorComposite{{'P', Vector2d(1, 2)}, {'O', Vector1d(3)}});
if (l == 0) L->fix();
}
@@ -434,9 +438,8 @@ TEST(Problem, check)
// landmarks
for (int l = 0; l < 5; l++)
{
- auto sb1 = std::make_shared<StatePoint2d>(Vector2d::Random());
- auto sb2 = std::make_shared<StateAngle>(Vector1d::Random());
- auto L = LandmarkBase::emplace<LandmarkPose2d>(problem->getMap(), sb1, sb2);
+ auto L = LandmarkBase::emplace<Landmark2d>(problem->getMap(),
+ VectorComposite{{'P', Vector2d(1, 2)}, {'O', Vector1d(3)}});
if (l == 0) L->fix();
}
@@ -449,9 +452,8 @@ TEST(Problem, check)
auto F = problem->emplaceFrame(t, "PO", pose);
if (i == 0) F->fix();
- auto sb = std::make_shared<StatePoint3d>(Vector3d(1, 1, 1));
auto cap = CaptureBase::emplace<CaptureDiffDrive>(
- F, t, sensor, Vector2d(1, 2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
+ F, t, sensor, Vector2d(1, 2), Matrix2d::Identity(), nullptr, VectorComposite{{'I', Vector3d::Ones()}});
if (prev_cap)
{
@@ -670,7 +672,8 @@ TEST(Problem, transform)
// Add dynamic sensor params
Vector3d param_sensor = Vector3d(3, 2, 1);
- S->addStateBlock('b', std::make_shared<StateParams3>(param_sensor, false), true); // Estimated ; Dynamic
+ S->emplaceStateBlock('b', "StateParams3", param_sensor, false);
+ S->setStateBlockDynamic('b', true);
Vector6d data;
data << 0, 0.2, 0, 0, 0, 0; // advance on Y
@@ -689,16 +692,15 @@ TEST(Problem, transform)
{
for (auto c : t_f.second->getCapturesOf(S))
{
- c->addStateBlock('b', std::make_shared<StateParams3>(S->getStateBlock('b')->getState()));
+ c->emplaceStateBlock('b', "StateParams3", S->getStateBlock('b')->getState(), false);
}
}
// Add a couple of lmks
- LandmarkBase::emplace<LandmarkPoint3d>(P->getMap(), //
- std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)));
- LandmarkBase::emplace<LandmarkPose3d>(P->getMap(),
- std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)),
- std::make_shared<StateQuaternion>(Vector4d(0, 0, 0, 1)));
+ LandmarkBase::emplace<Landmark3d>(P->getMap(), //
+ VectorComposite{{'P', Vector3d(1, 2, 3)}});
+ LandmarkBase::emplace<Landmark3d>(P->getMap(),
+ VectorComposite{{'P', Vector3d(1, 2, 3)}, {'O', Vector4d(0, 0, 0, 1)}});
// P->print(2, 0, 1, 1); // reference
@@ -706,16 +708,16 @@ TEST(Problem, transform)
// Define transformations
// translation
- Quaterniond q_w1_w0 = Quaterniond::Identity(); // no rotation
- VectorComposite pose_w1_w0("PO", {Vector3d(1, 0, 0), q_w1_w0.coeffs()}); // translation X: 1m
+ Quaterniond q_w1_w0 = Quaterniond::Identity(); // no rotation
+ VectorComposite pose_w1_w0({{'P', Vector3d(1, 0, 0)}, {'O', q_w1_w0.coeffs()}}); // translation X: 1m
// rotation
- Quaterniond q_w2_w1(AngleAxisd(M_PI_2, Vector3d(1, 0, 0))); // rotation X: 90deg
- VectorComposite pose_w2_w1("PO", {Vector3d(0, 0, 0), q_w2_w1.coeffs()}); // no translation,
+ Quaterniond q_w2_w1(AngleAxisd(M_PI_2, Vector3d(1, 0, 0))); // rotation X: 90deg
+ VectorComposite pose_w2_w1({{'P', Vector3d(0, 0, 0)}, {'O', q_w2_w1.coeffs()}}); // no translation,
// rotation + translation
- Quaterniond q_w3_w2(AngleAxisd(-M_PI_2, Vector3d(1, 0, 0))); // rotation X: -90deg
- VectorComposite pose_w3_w2("PO", {Vector3d(1, 0, 0), q_w3_w2.coeffs()}); // translation X: 1m
+ Quaterniond q_w3_w2(AngleAxisd(-M_PI_2, Vector3d(1, 0, 0))); // rotation X: -90deg
+ VectorComposite pose_w3_w2({{'P', Vector3d(1, 0, 0)}, {'O', q_w3_w2.coeffs()}}); // translation X: 1m
// State of the problem in the initial frame "w0"
VectorComposite pose_w0_r0 = P->getTrajectory()->getFirstFrame()->getState();
@@ -779,6 +781,6 @@ TEST(Problem, transform)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // testing::GTEST_FLAG(filter) = "Problem.check";
+ // testing::GTEST_FLAG(filter) = "Problem.perturb";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 8c920152703fdf99979b77edd35b9dbbdef6fcee..647bf7e15d226723c6fb5625ad0b8188f1831a93 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -23,7 +23,7 @@
#include "core/processor/processor_odom_2d.h"
#include "core/sensor/sensor_odom.h"
-#include "dummy/sensor_dummy.h"
+#include "dummy/sensor_dummy_po.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
@@ -83,11 +83,12 @@ TEST(ProcessorBase, KeyFrameCallback)
// Sequence to test Key Frame creations (callback calls)
// initialize
- TimeStamp t(0.0);
- SpecStateComposite prior{
- {'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(sqrt(0.1), sqrt(0.1)))},
- {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(sqrt(0.1)))}};
- problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ problem->emplaceFirstFrame(t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))},
+ {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}});
CaptureOdom2dPtr capt_odo = std::make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5, 0));
@@ -115,10 +116,7 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "5\n";
// keyframe creation
- if (ii == 5)
- problem->emplaceFrame(t,
- SpecStateComposite{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "initial_guess")},
- {'O', SpecState("StateAngle", Vector1d(0), "initial_guess")}});
+ if (ii == 5) problem->emplaceFrame(t, VectorComposite{{'P', Vector2d(0, 0)}, {'O', Vector1d(0)}});
problem->print(4, 1, 1, 0);
std::cout << "6\n";
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 506d4a7a57120ec05774a4885f994108d0fd3849..81340bccde34531f3652aad8c877b9e5b2eda2a8 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -160,7 +160,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
Vector3d calib(1, 1, 1);
double dt = 1.0;
VectorXd delta(3);
- VectorComposite x1("PO", Vector3d::Zero(), {2, 1}), x2("PO", Vector3d::Zero(), {2, 1});
+ VectorComposite x1(problem->stateZero("PO")), x2(problem->stateZero("PO"));
MatrixXd delta_cov(3, 3), J_delta_calib(3, 3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
@@ -168,7 +168,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
data(1) = 0.50 * sensor->getTicksPerWheelRevolution() / 3;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- x1.setZero();
+ x1 = problem->stateZero("PO");
processor->statePlusDelta(x1, delta, dt, x2); // 30
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 60
@@ -182,7 +182,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
data(1) = 0.25 * sensor->getTicksPerWheelRevolution() / 4;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- x1.setZero();
+ x1 = problem->stateZero("PO");
processor->statePlusDelta(x1, delta, dt, x2); // 22.5
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 45
@@ -202,10 +202,13 @@ TEST_F(ProcessorDiffDriveTest, process)
TimeStamp t = 0.0;
double dt = 1.0;
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1)));
- prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1)));
- auto F0 = problem->setPrior(prior, t);
+ auto F0 = problem->emplaceFirstFrame(t,
+ TypeComposite{{'P', "StatePoint2d"}, //
+ {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, //
+ {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(1))}, //
+ {'O', Prior("factor", Vector1d::Constant(1))}});
processor->setOrigin(F0);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
@@ -233,10 +236,11 @@ TEST_F(ProcessorDiffDriveTest, linear)
data_cov.setIdentity();
TimeStamp t = 0.0;
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1)));
- prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1)));
- auto F0 = problem->setPrior(prior, t);
+ auto F0 = problem->emplaceFirstFrame(t,
+ problem->getFrameTypes(),
+ problem->stateZero("PO"),
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(1))}, //
+ {'O', Prior("factor", Vector1d::Constant(1))}});
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
@@ -263,10 +267,11 @@ TEST_F(ProcessorDiffDriveTest, angular)
data_cov.setIdentity();
TimeStamp t = 0.0;
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1)));
- prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1)));
- auto F0 = problem->setPrior(prior, t);
+ auto F0 = problem->emplaceFirstFrame(t,
+ problem->getFrameTypes(),
+ problem->stateZero("PO"),
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(1))}, //
+ {'O', Prior("factor", Vector1d::Constant(1))}});
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index fa9c60f16f4ca5f2da902fe9574b2b5a38368972..0c977002356ef6648bcfef7794533d2ea6b5f1c5 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -21,8 +21,8 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/landmark/landmark_point.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
+#include "core/landmark/landmark.h"
#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/processor/processor_odom_3d.h"
@@ -47,6 +47,7 @@ class ProcessorLandmarkExternalTest : public testing::Test
protected:
int dim;
bool orientation;
+ int mode; // 0: external ID, 1: external TYPE, 2: nothing, 3: all mixed
TimeStamp t;
double dt;
@@ -61,43 +62,51 @@ class ProcessorLandmarkExternalTest : public testing::Test
VectorComposite state_robot, state_sensor;
std::vector<VectorComposite> state_landmarks;
- virtual void SetUp()
- {
- dt = 0.1;
- };
+ virtual void SetUp(){};
void initProblem(int _dim,
bool _orientation,
+ int _mode,
double _quality_th,
double _dist_th,
unsigned int _track_length_th,
double _time_span,
- bool _add_landmarks);
+ bool _init_landmarks);
void randomStep();
CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
- void testConfiguration(int dim,
- bool orientation,
- double quality_th,
- double dist_th,
- int track_length,
- double time_span,
- bool add_landmarks);
+ void testConfiguration(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ int _track_length,
+ double _time_span,
+ bool _init_landmarks);
void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
};
void ProcessorLandmarkExternalTest::initProblem(int _dim,
bool _orientation,
+ int _mode,
double _quality_th,
double _dist_th,
unsigned int _track_length_th,
double _time_span,
- bool _add_landmarks)
+ bool _init_landmarks)
{
+ // INCOMPATIBLE OPTIONS
+ if (_init_landmarks and _mode == 2)
+ throw std::runtime_error("Landmarks initialized with mode 2 (no id no type), impossible to close loops");
+ if (_init_landmarks and _mode == 4)
+ throw std::runtime_error("Landmarks initialized with mode 4 (changing), impossible to close loops");
+
dim = _dim;
orientation = _orientation;
+ mode = _mode;
t = TimeStamp(0);
+ dt = 0.1;
problem = Problem::create("PO", dim);
- solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+ solver = FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensors
sensor = problem->installSensor(wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml", {wolf_dir});
@@ -128,15 +137,18 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
YAML::Node params = YAML::LoadFile(wolf_dir + "/test/yaml/processor_landmark_external.yaml");
params["time_tolerance"] = dt / 2;
params["use_orientation"] = orientation;
- params["filter"]["quality_th"] = _quality_th;
- params["match_dist_th"] = _dist_th;
- params["filter"]["track_length_th"] = _track_length_th;
+ params["quality_th"] = _quality_th;
+ params["match_dist_th_id"] = _dist_th;
+ params["match_dist_th_type"] = _dist_th;
+ params["match_dist_th_unknown"] = _dist_th;
+ params["track_length_th"] = _track_length_th;
params["keyframe_vote"]["time_span"] = _time_span;
+ params["close_loops_by_id"] = _init_landmarks;
+ params["close_loops_by_type"] = _init_landmarks;
processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params, {wolf_dir});
// Emplace frame
- auto F0 = problem->emplaceFrame(
- t, "PO", (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0, 0, 0, 0, 0, 0, 1).finished()));
+ auto F0 = problem->emplaceFrame(t, problem->stateZero("PO"));
F0->fix();
processor->keyFrameCallback(F0);
processor_motion->setOrigin(F0);
@@ -144,29 +156,26 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Emplace 3 random landmarks
for (auto i = 0; i < 3; i++)
{
+ bool init_landmark = _init_landmarks and not(mode == 3 and i % 3 == 2);
LandmarkBasePtr lmk;
if (dim == 2)
{
- if (orientation)
- lmk = LandmarkBase::emplace<LandmarkPose2d>(_add_landmarks ? problem->getMap() : nullptr,
- std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
- std::make_shared<StateAngle>(Vector1d::Random() * M_PI));
- else
- lmk = LandmarkBase::emplace<LandmarkPoint2d>(_add_landmarks ? problem->getMap() : nullptr,
- std::make_shared<StatePoint2d>(Vector2d::Random() * 10));
+ lmk = LandmarkBase::emplace<Landmark2d>(
+ init_landmark ? problem->getMap() : nullptr,
+ orientation ? VectorComposite{{'P', Vector2d::Random() * 10}, {'O', Vector1d::Random() * M_PI}}
+ : VectorComposite{{'P', Vector2d::Random() * 10}},
+ PriorComposite{});
}
else
{
- if (orientation)
- lmk = LandmarkBase::emplace<LandmarkPose3d>(
- _add_landmarks ? problem->getMap() : nullptr,
- std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized()));
- else
- lmk = LandmarkBase::emplace<LandmarkPoint3d>(_add_landmarks ? problem->getMap() : nullptr,
- std::make_shared<StatePoint3d>(Vector3d::Random() * 10));
+ lmk = LandmarkBase::emplace<Landmark3d>(
+ init_landmark ? problem->getMap() : nullptr,
+ orientation ? VectorComposite{{'P', Vector3d::Random() * 10}, {'O', Vector4d::Random().normalized()}}
+ : VectorComposite{{'P', Vector3d::Random() * 10}},
+ PriorComposite{});
}
- lmk->setId(i);
+ lmk->setExternalId(i + 1);
+ lmk->setExternalType(3 * i + 10);
landmarks.push_back(lmk);
state_landmarks.push_back(lmk->getState());
}
@@ -232,34 +241,80 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
.coeffs();
}
// cov
- MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
- if (orientation and dim != 2) cov = MatrixXd::Identity(6, 6);
+ MatrixXd cov = 0.01 * MatrixXd::Identity(meas.size(), meas.size());
+ if (orientation and dim != 2) cov = 0.01 * MatrixXd::Identity(6, 6);
// quality
double quality = (double)i / (double)(landmarks.size() - 1); // increasing from 0 to 1
// add detection
- cap->addDetection(landmarks.at(i)->id(), meas, cov, quality);
+ auto step = (int)round(t.get() / dt);
+ // with ID
+ if (mode == 0 or (mode == 3 and i % 3 == 0) or (mode == 4 and step % 3 == 0))
+ cap->addDetection(landmarks.at(i)->getExternalId(), -1, meas, cov, quality);
+ // with TYPE
+ else if (mode == 1 or (mode == 3 and i % 3 == 1) or (mode == 4 and step % 3 == 1))
+ cap->addDetection(-1, landmarks.at(i)->getExternalType(), meas, cov, quality);
+ // with nothing
+ else if (mode == 2 or (mode == 3 and i % 3 == 2) or (mode == 4 and step % 3 == 2))
+ cap->addDetection(-1, -1, meas, cov, quality);
}
return cap;
}
-void ProcessorLandmarkExternalTest::testConfiguration(int dim,
- bool orientation,
- double quality_th,
- double dist_th,
- int track_length,
- double time_span,
- bool add_landmarks)
+void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ int _track_length,
+ double _time_span,
+ bool _init_landmarks)
{
- initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);
+ initProblem(_dim, _orientation, _mode, _quality_th, _dist_th, _track_length, _time_span, _init_landmarks);
+
+ FactorBasePtrList fac_list;
ASSERT_TRUE(problem->check());
for (auto i = 0; i < 10; i++)
{
WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
+ WOLF_INFO("robot state: ", state_robot);
+ WOLF_INFO("sensor state: ", state_sensor);
+
+ // store previous number of kf, lmks, and factors
+ auto n_prev_kf = problem->getTrajectory()->size();
+ auto n_prev_lmk = problem->getMap()->getLandmarkList().size();
+ fac_list.clear();
+ problem->getTrajectory()->getFactorList(fac_list);
+ auto n_prev_fac = fac_list.size();
+
+ // Things to check
+ bool any_active_track = _quality_th <= 1 and i >= _track_length;
+ bool should_emplace_KF =
+ (t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt > _time_span and
+ any_active_track);
+ bool should_emplace_factors = (i == 1 or should_emplace_KF) and any_active_track;
+
+ WOLF_INFO("should emplace factors: ",
+ should_emplace_factors,
+ " _quality_th = ",
+ _quality_th,
+ " track_length-1 = ",
+ _track_length - 1);
+
+ WOLF_INFO("should emplace KF: ",
+ should_emplace_KF,
+ " i = ",
+ i,
+ " t-last_frame_stamp-dt = ",
+ t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt,
+ " time_span = ",
+ _time_span,
+ " track_length-1 = ",
+ _track_length - 1);
// detection of landmarks
auto cap = computeCaptureLandmarks();
@@ -268,30 +323,25 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
// process detections
cap->process();
ASSERT_TRUE(problem->check());
- // problem->print(4,1,1,1);
+ // problem->print(4, 1, 1, 1);
// CHECKS
- FactorBasePtrList fac_list;
+ fac_list.clear();
problem->getTrajectory()->getFactorList(fac_list);
- // should emplace KF in last?
- bool any_active_track = quality_th <= 1 and i >= track_length - 1;
- bool should_emplace_KF = t - dt > time_span and any_active_track;
- WOLF_INFO("should emplace KF: ",
- should_emplace_KF,
- " t-dt = ",
- t - dt,
- " time_span = ",
- time_span,
- " track_length-1 = ",
- track_length - 1);
-
+ // Check voted&emplaced a keyframe
if (should_emplace_KF)
{
- // voted for keyframe
- ASSERT_TRUE(problem->getTrajectory()->size() > 1);
+ ASSERT_EQ(problem->getTrajectory()->size(), n_prev_kf + 1);
+ }
+ else
+ {
+ ASSERT_EQ(problem->getTrajectory()->size(), n_prev_kf);
+ }
- // emplaced factors
- ASSERT_FALSE(fac_list.empty());
+ // Check emplaced factors and landmarks
+ if (should_emplace_factors)
+ {
+ ASSERT_GT(fac_list.size(), n_prev_fac);
// factors' type
for (auto fac : fac_list)
@@ -305,38 +355,36 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
}
// landmarks created by processor
- if (not add_landmarks)
+ if (not _init_landmarks)
{
auto landmarks_map = problem->getMap()->getLandmarkList();
- // amount of landmarks due to quality filtering of detections
- auto n_landmarks = (quality_th <= 0 ? 3 : (quality_th <= 0.5 ? 2 : (quality_th <= 1 ? 1 : 0)));
+ // amount of landmarks according to quality filtering of detections
+ auto n_landmarks = (_quality_th <= 0 ? 3 : (_quality_th <= 0.5 ? 2 : (_quality_th <= 1 ? 1 : 0)));
ASSERT_EQ(landmarks_map.size(), n_landmarks);
// check state correctly initialized
for (auto lmk_map : landmarks_map)
{
- ASSERT_TRUE(lmk_map->id() < landmarks.size());
- ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ if (lmk_map->getExternalId() != -1)
+ {
+ ASSERT_EQ(lmk_map->getExternalId(),
+ landmarks.at(lmk_map->getExternalId() - 1)->getExternalId());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->getExternalId() - 1));
+ }
}
}
}
else
{
- // didn't vote for keyframe
- ASSERT_FALSE(problem->getTrajectory()->size() > 1);
// no factors emplaced
- ASSERT_TRUE(fac_list.empty());
+ ASSERT_EQ(fac_list.size(), n_prev_fac);
+
// no landmarks created yet (if not added by the test)
- ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), n_prev_lmk);
}
- // step with random movement
- t += dt;
- randomStep();
-
// solve
- if (should_emplace_KF)
+ if (should_emplace_factors)
{
// perturb landmarks
for (auto lmk : problem->getMap()->getLandmarkList()) lmk->perturb();
@@ -352,9 +400,14 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
// check values
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ if (lmk_map->getExternalId() != -1)
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->getExternalId() - 1));
}
}
+
+ // step with random movement
+ t += dt;
+ randomStep();
}
}
@@ -379,137 +432,634 @@ void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite&
throw std::runtime_error("wrong vector composite");
}
-// / TESTS //////////////////////////////////////////
-TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks)
+/// TESTS //////////////////////////////////////////
+
+//-------------------- Position in 2D --------------------
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_id)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_type)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_mixed)
{
testConfiguration(2, // int dim
false, // bool orientation
+ 3, // int mode
0, // double quality_th
- 1e6, // double dist_th
+ 1e-2, // double dist_th
6, // int track_length
4.5 * dt, // double time_span
- true); // bool add_landmarks
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_id)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_type)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed)
{
testConfiguration(2, // int dim
false, // bool orientation
+ 3, // int mode
0, // double quality_th
- 1e6, // double dist_th
+ 1e-2, // double dist_th
2, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_changing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_id)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_type)
{
testConfiguration(2, // int dim
false, // bool orientation
+ 1, // int mode
0.3, // double quality_th
- 1e6, // double dist_th
+ 1e-2, // double dist_th
6, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_nothing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_mixed)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_changing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position & Orientation in 2D --------------------
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_type)
{
testConfiguration(2, // int dim
true, // bool orientation
+ 1, // int mode
0, // double quality_th
- 1e6, // double dist_th
- 3, // int track_length
+ 1e-2, // double dist_th
+ 6, // int track_length
4.5 * dt, // double time_span
- true); // bool add_landmarks
+ true); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_mixed)
{
testConfiguration(2, // int dim
true, // bool orientation
+ 3, // int mode
0, // double quality_th
- 1e6, // double dist_th
- 1, // int track_length
+ 1e-2, // double dist_th
+ 6, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ true); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_id)
{
testConfiguration(2, // int dim
true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_nothing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_changing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
0.3, // double quality_th
- 1e6, // double dist_th
- 0, // int track_length
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_nothing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_changing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position in 3D --------------------
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_mixed)
{
testConfiguration(3, // int dim
false, // bool orientation
+ 3, // int mode
0, // double quality_th
- 1e6, // double dist_th
- 7, // int track_length
+ 1e-2, // double dist_th
+ 6, // int track_length
4.5 * dt, // double time_span
- true); // bool add_landmarks
+ true); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_3d_id)
{
testConfiguration(3, // int dim
false, // bool orientation
+ 0, // int mode
0, // double quality_th
- 1e6, // double dist_th
- 53, // int track_length
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_nothing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_changing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_nothing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_changing)
{
testConfiguration(3, // int dim
false, // bool orientation
+ 4, // int mode
0.3, // double quality_th
- 1e6, // double dist_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position & Orientation in 3D --------------------
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
2, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_type)
{
testConfiguration(3, // int dim
true, // bool orientation
+ 1, // int mode
0, // double quality_th
- 1e6, // double dist_th
- 1, // int track_length
+ 1e-2, // double dist_th
+ 2, // int track_length
4.5 * dt, // double time_span
- true); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks)
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_nothing)
{
testConfiguration(3, // int dim
true, // bool orientation
+ 2, // int mode
0, // double quality_th
- 1e6, // double dist_th
- 4, // int track_length
+ 1e-2, // double dist_th
+ 2, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality)
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_mixed)
{
testConfiguration(3, // int dim
true, // bool orientation
- 0.2, // double quality_th
- 1e6, // double dist_th
- 5, // int track_length
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_changing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_nothing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_changing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
4.5 * dt, // double time_span
- false); // bool add_landmarks
+ false); // bool init_landmarks & loop closure
}
int main(int argc, char** argv)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 5bd817a81f49d043fc3036d315e591d04b042e34..887a8f7a8496e49634006e556ec0738594b4878c 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -186,11 +186,12 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(1, 1)));
- prior.emplace('O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(1)));
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ auto KF_0 = problem->emplaceFirstFrame(
+ t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("factor", Vector2d(1, 1))}, {'O', Prior("factor", Vector1d(1))}});
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -203,6 +204,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
+
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
@@ -212,10 +214,11 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "fix")},
- {'O', SpecState("StateAngle", Vector1d(0), "fix")}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ auto KF_0 = problem->emplaceFirstFrame(t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("fix")}, {'O', Prior("fix")}});
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -262,10 +265,12 @@ TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(1, 1))},
- {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(1))}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ auto KF_0 = problem->emplaceFirstFrame(
+ t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("factor", Vector2d(1, 1))}, {'O', Prior("factor", Vector1d(1))}});
processor->setOrigin(KF_0);
data << 1, 2 * M_PI / 10; // advance in circle
@@ -287,11 +292,11 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d(0, 0), "fix"));
- prior.emplace('O', SpecState("StateAngle", Vector1d(0), "fix"));
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ auto KF_0 = problem->emplaceFirstFrame(t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("fix")}, {'O', Prior("fix")}});
processor->setOrigin(KF_0);
data << 1, 2 * M_PI / 10; // advance in circle
@@ -421,10 +426,12 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(1, 1))},
- {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(1))}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ auto KF_0 = problem->emplaceFirstFrame(
+ t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("factor", Vector2d(1, 1))}, {'O', Prior("factor", Vector1d(1))}});
processor->setOrigin(KF_0);
data << 1, 2 * M_PI / 10; // advance in circle
@@ -457,10 +464,11 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
TEST_F(ProcessorMotion_test, splitBufferFixPrior)
{
// Prior
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "fix")},
- {'O', SpecState("StateAngle", Vector1d(0), "fix")}};
- auto KF_0 = problem->setPrior(prior, t);
+ TimeStamp t(0.0);
+ auto KF_0 = problem->emplaceFirstFrame(t,
+ TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}},
+ VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}},
+ PriorComposite{{'P', Prior("fix")}, {'O', Prior("fix")}});
processor->setOrigin(KF_0);
data << 1, 2 * M_PI / 10; // advance in circle
diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp
index 45d20f90c88bc1df17eaf1fb4e8f61e3f3c2e227..5c62eff0c749bd55f76c9f1282956ac1ca282e64 100644
--- a/test/gtest_processor_odom_2d.cpp
+++ b/test/gtest_processor_odom_2d.cpp
@@ -108,13 +108,21 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Origin Key Frame
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d(sqrt(0.1), sqrt(0.1))));
- prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d(sqrt(0.1))));
- FrameBasePtr KF = problem->setPrior(prior, t0);
+ FrameBasePtr KF =
+ problem->emplaceFirstFrame(t0,
+ problem->getFrameTypes(),
+ problem->stateZero("PO"),
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))},
+ {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}});
processor_odom2d->setOrigin(KF);
- solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ ASSERT_TRUE(KF->getProblem());
+
+ problem->print(4, 1, 1, 1);
+ problem->check(4);
+
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
@@ -189,7 +197,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
}
// Solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(4, 1, 1, 1);
@@ -240,10 +248,12 @@ TEST(ProcessorOdom2d, KF_callback)
auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Origin Key Frame
- SpecStateComposite prior;
- prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d(sqrt(0.1), sqrt(0.1))));
- prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d(sqrt(0.1))));
- FrameBasePtr keyframe_0 = problem->setPrior(prior, t0);
+ FrameBasePtr keyframe_0 =
+ problem->emplaceFirstFrame(t0,
+ problem->getFrameTypes(),
+ problem->stateZero("PO"),
+ PriorComposite{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))},
+ {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}});
processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
@@ -305,8 +315,8 @@ TEST(ProcessorOdom2d, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
- Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, "PO", x_split);
+ VectorComposite x_split = processor_odom2d->getState(t_split, "PO");
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->size(), 2);
@@ -342,8 +352,8 @@ TEST(ProcessorOdom2d, KF_callback)
problem->print(4, 1, 1, 1);
- x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, "PO", x_split);
+ x_split = processor_odom2d->getState(t_split, "PO");
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1);
diff --git a/test/gtest_processor_pose_3d.cpp b/test/gtest_processor_pose_3d.cpp
index 781945d758dd3d82c596851aaf6b16482621f35e..4e2bb8735a57e04814232489990d282b06ae35fa 100644
--- a/test/gtest_processor_pose_3d.cpp
+++ b/test/gtest_processor_pose_3d.cpp
@@ -116,8 +116,8 @@ class ProcessorPose3d_Base_Test : public testing::Test
// pose sensor (load yaml and modify to put random extrinsics)
auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_pose_3d_initial_guess.yaml");
- params["states"]["P"]["state"] = b_p_bm_;
- params["states"]["O"]["state"] = b_q_m_.coeffs();
+ params["states"]["P"]["value"] = b_p_bm_;
+ params["states"]["O"]["value"] = b_q_m_.coeffs();
sensor_pose_ = problem_->installSensor(params, {wolf_dir});
WOLF_INFO("sensor created");
auto proc_pose =
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 4925188ea5068ef6d3e93c895833ac2fab048233..ce980fcf7c9023b0f618ae31b83274beb5adab12 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -27,6 +27,7 @@
#include "core/state_block/state_block_derived.h"
using namespace wolf;
+using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
@@ -165,13 +166,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
{
- FrameBasePtr frm = FrameBase::emplace<FrameBase>(nullptr, 0, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBasePtr frm = FrameBase::emplace<FrameBase>(nullptr, 0, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr cap = CaptureBase::emplace<CaptureVoid>(frm, 0, sensor);
FeatureBasePtr ftr =
FeatureBase::emplace<FeatureBase>(cap, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
FrameBasePtr frm_other =
- FrameBase::emplace<FrameBase>(nullptr, 1, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBase::emplace<FrameBase>(nullptr, 1, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr cap_other = CaptureBase::emplace<CaptureVoid>(frm_other, 1, sensor);
FeatureBasePtr ftr_other = FeatureBase::emplace<FeatureBase>(
cap_other, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
@@ -194,12 +195,13 @@ TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
{
// Put a capture on last_ptr_
FrameBasePtr last_frm =
- FrameBase::emplace<FrameBase>(nullptr, 0, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBase::emplace<FrameBase>(nullptr, 0, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr last_cap = CaptureBase::emplace<CaptureVoid>(last_frm, 0, sensor);
processor->setLast(last_cap);
// Put a capture on incoming_ptr_
- FrameBasePtr inc_frm = FrameBase::emplace<FrameBase>(nullptr, 1, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBasePtr inc_frm =
+ FrameBase::emplace<FrameBase>(nullptr, 1, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor);
processor->setInc(inc_cap);
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 84b33054251202569251e39dacd0e40b25518a3b..bc65782ddabb99f53bf56be0869a714c69b3b504 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -23,11 +23,12 @@
#include "core/sensor/factory_sensor.h"
#include "dummy/processor_tracker_landmark_dummy.h"
#include "core/capture/capture_void.h"
-#include "core/landmark/landmark_pose.h"
+#include "core/landmark/landmark.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
using namespace wolf;
+using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
@@ -210,12 +211,12 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
{
- FrameBasePtr frm = FrameBase::emplace<FrameBase>(nullptr, 0, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBasePtr frm = FrameBase::emplace<FrameBase>(nullptr, 0, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr cap = CaptureBase::emplace<CaptureVoid>(frm, 0, sensor);
FeatureBasePtr ftr =
FeatureBase::emplace<FeatureBase>(cap, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
- LandmarkBasePtr lmk(std::make_shared<LandmarkPose2d>(std::make_shared<StatePoint2d>(Vector2d::Zero()),
- std::make_shared<StateAngle>(0)));
+ LandmarkBasePtr lmk =
+ LandmarkBase::emplace<Landmark2d>(nullptr, VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
FactorBasePtr fac = processor->callEmplaceFactor(ftr, lmk);
ASSERT_EQ(fac->getFeature(), ftr);
@@ -234,12 +235,13 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
{
// Put a capture on last_ptr_
FrameBasePtr last_frm =
- FrameBase::emplace<FrameBase>(nullptr, 0, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBase::emplace<FrameBase>(nullptr, 0, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr last_cap = CaptureBase::emplace<CaptureVoid>(last_frm, 0, sensor);
processor->setLast(last_cap);
// Put a capture on incoming_ptr_
- FrameBasePtr inc_frm = FrameBase::emplace<FrameBase>(nullptr, 1, std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ FrameBasePtr inc_frm =
+ FrameBase::emplace<FrameBase>(nullptr, 1, problem->getFrameTypes(), problem->stateZero("P"));
CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor);
processor->setInc(inc_cap);
diff --git a/test/gtest_processor_tracker_two_processors_one_sensor.cpp b/test/gtest_processor_tracker_two_processors_one_sensor.cpp
index 080527e46b42f604284ffdf9a512f0ab93eaa17f..884adb34d2d8c8d0d2c6dab63111dd61b93800c9 100644
--- a/test/gtest_processor_tracker_two_processors_one_sensor.cpp
+++ b/test/gtest_processor_tracker_two_processors_one_sensor.cpp
@@ -25,6 +25,7 @@
#include "core/capture/capture_void.h"
using namespace wolf;
+using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
index 62d8b5f7c267500fa166492bc3d5d09eb712f7aa..f097930a1a7de1651b21e005d03d0a3cf6bce982 100644
--- a/test/gtest_schema.cpp
+++ b/test/gtest_schema.cpp
@@ -32,6 +32,7 @@
using namespace wolf;
using namespace yaml_schema_cpp;
+using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index f3bfb436939a5fcfd6cbb1bd361d2a4b02b24ce5..a9000ffb451b04cd88f2c9299cff1a24a953dbad 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -21,7 +21,7 @@
#include "core/internal/config.h"
#include "core/sensor/sensor_base.h"
#include "core/utils/utils_gtest.h"
-#include "dummy/sensor_dummy.h"
+#include "dummy/sensor_dummy_po.h"
#include "dummy/sensor_dummy_poia.h"
#include "yaml-schema-cpp/yaml_server.hpp"
@@ -119,24 +119,25 @@ TEST(SensorBase, sensor_dummy)
WOLF_INFO("---- Sensor ", name, "...");
YAML::Node params;
- params["type"] = "SensorDummy" + toString(dim) + "d";
- params["name"] = name;
- params["plugin"] = "core";
- params["noise_p_std"] = noise_p_std;
- params["noise_o_std"] = noise_o_std;
- params["states"]["keys"] = "PO";
-
- params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
- params["states"]["P"]["state"] = dim == 2 ? p_state_2D : p_state_3D;
- params["states"]["P"]["mode"] = mode;
- if (mode == "factor") params["states"]["P"]["noise_std"] = (dim == 2 ? p_std_2D : p_std_3D);
+ params["type"] = "SensorDummyPo" + toString(dim) + "d";
+ params["name"] = name;
+ params["plugin"] = "core";
+ params["noise_p_std"] = noise_p_std;
+ params["noise_o_std"] = noise_o_std;
+
+ params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
+ params["states"]["P"]["value"] = dim == 2 ? p_state_2D : p_state_3D;
+ params["states"]["P"]["prior"]["mode"] = mode;
+ if (mode == "factor")
+ params["states"]["P"]["prior"]["factor_std"] = (dim == 2 ? p_std_2D : p_std_3D);
params["states"]["P"]["dynamic"] = dynamic;
if (drift) params["states"]["P"]["drift_std"] = (dim == 2 ? p_std_2D : p_std_3D);
- params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
- params["states"]["O"]["state"] = dim == 2 ? o_state_2D : o_state_3D;
- params["states"]["O"]["mode"] = mode;
- if (mode == "factor") params["states"]["O"]["noise_std"] = dim == 2 ? o_std_2D : o_std_3D;
+ params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
+ params["states"]["O"]["value"] = dim == 2 ? o_state_2D : o_state_3D;
+ params["states"]["O"]["prior"]["mode"] = mode;
+ if (mode == "factor")
+ params["states"]["O"]["prior"]["factor_std"] = dim == 2 ? o_std_2D : o_std_3D;
params["states"]["O"]["dynamic"] = dynamic;
if (drift) params["states"]["O"]["drift_std"] = dim == 2 ? o_std_2D : o_std_3D;
@@ -144,8 +145,8 @@ TEST(SensorBase, sensor_dummy)
// create from node ------------------------------------------------------------------------
auto S = dim == 2
- ? std::static_pointer_cast<SensorBase>(SensorDummy2d::create(params, {wolf_dir}))
- : std::static_pointer_cast<SensorBase>(SensorDummy3d::create(params, {wolf_dir}));
+ ? std::static_pointer_cast<SensorBase>(SensorDummyPo2d::create(params, {wolf_dir}))
+ : std::static_pointer_cast<SensorBase>(SensorDummyPo3d::create(params, {wolf_dir}));
ASSERT_TRUE(S);
// noise
@@ -168,9 +169,9 @@ TEST(SensorBase, sensor_dummy)
drift ? dim == 2 ? o_std_2D : o_std_3D : vector0);
// emplace ------------------------------------------------------------------------
- auto S2 = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorBase::emplace<SensorDummy2d>(
+ auto S2 = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorBase::emplace<SensorDummyPo2d>(
problem_2D->getHardware(), params, {wolf_dir}))
- : std::static_pointer_cast<SensorBase>(SensorBase::emplace<SensorDummy3d>(
+ : std::static_pointer_cast<SensorBase>(SensorBase::emplace<SensorDummyPo3d>(
problem_3D->getHardware(), params, {wolf_dir}));
ASSERT_TRUE(S2);
@@ -207,7 +208,7 @@ TEST(SensorBase, sensor_dummy)
auto params_node = YAML::LoadFile(yaml_file);
auto S3 = FactorySensorNode::create(
- "SensorDummy" + toString(dim) + "d", params_node, {wolf_dir});
+ "SensorDummyPo" + toString(dim) + "d", params_node, {wolf_dir});
if (S3)
{
@@ -247,8 +248,8 @@ TEST(SensorBase, sensor_dummy)
// factory file ------------------------------------------------------------------------
try
{
- auto S4 =
- FactorySensorFile::create("SensorDummy" + toString(dim) + "d", yaml_file, {wolf_dir});
+ auto S4 = FactorySensorFile::create(
+ "SensorDummyPo" + toString(dim) + "d", yaml_file, {wolf_dir});
if (S4)
{
@@ -291,36 +292,35 @@ TEST(SensorBase, sensor_dummy)
TEST(SensorBase, sensor_dummy_poia)
{
YAML::Node params;
- params["type"] = "SensorDummyPoia3d";
- params["name"] = "cool sensor";
- params["plugin"] = "core";
- params["noise_p_std"] = noise_p_std;
- params["noise_o_std"] = noise_o_std;
- params["states"]["keys"] = "POIA";
-
- params["states"]["P"]["type"] = "StatePoint3d";
- params["states"]["P"]["state"] = p_state_3D_yaml;
- params["states"]["P"]["mode"] = "factor";
- params["states"]["P"]["noise_std"] = p_std_3D_yaml;
- params["states"]["P"]["dynamic"] = true;
-
- params["states"]["O"]["type"] = "StateQuaternion";
- params["states"]["O"]["state"] = o_state_3D_yaml;
- params["states"]["O"]["mode"] = "fix";
- params["states"]["O"]["dynamic"] = false;
-
- params["states"]["I"]["type"] = "StateParams5";
- params["states"]["I"]["state"] = i_state_yaml;
- params["states"]["I"]["mode"] = "initial_guess";
- params["states"]["I"]["dynamic"] = true;
- params["states"]["I"]["drift_std"] = i_std_yaml;
-
- params["states"]["A"]["type"] = "StateQuaternion";
- params["states"]["A"]["state"] = a_state_yaml;
- params["states"]["A"]["mode"] = "factor";
- params["states"]["A"]["noise_std"] = a_std_yaml;
- params["states"]["A"]["dynamic"] = true;
- params["states"]["A"]["drift_std"] = a_std_yaml;
+ params["type"] = "SensorDummyPoia3d";
+ params["name"] = "cool sensor";
+ params["plugin"] = "core";
+ params["noise_p_std"] = noise_p_std;
+ params["noise_o_std"] = noise_o_std;
+
+ params["states"]["P"]["type"] = "StatePoint3d";
+ params["states"]["P"]["value"] = p_state_3D_yaml;
+ params["states"]["P"]["prior"]["mode"] = "factor";
+ params["states"]["P"]["prior"]["factor_std"] = p_std_3D_yaml;
+ params["states"]["P"]["dynamic"] = true;
+
+ params["states"]["O"]["type"] = "StateQuaternion";
+ params["states"]["O"]["value"] = o_state_3D_yaml;
+ params["states"]["O"]["prior"]["mode"] = "fix";
+ params["states"]["O"]["dynamic"] = false;
+
+ params["states"]["I"]["type"] = "StateParams5";
+ params["states"]["I"]["value"] = i_state_yaml;
+ params["states"]["I"]["prior"]["mode"] = "initial_guess";
+ params["states"]["I"]["dynamic"] = true;
+ params["states"]["I"]["drift_std"] = i_std_yaml;
+
+ params["states"]["A"]["type"] = "StateQuaternion";
+ params["states"]["A"]["value"] = a_state_yaml;
+ params["states"]["A"]["prior"]["mode"] = "factor";
+ params["states"]["A"]["prior"]["factor_std"] = a_std_yaml;
+ params["states"]["A"]["dynamic"] = true;
+ params["states"]["A"]["drift_std"] = a_std_yaml;
// create from node ------------------------------------------------------------------------
auto S = SensorDummyPoia3d::create(params, {wolf_dir});
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index d76217135cf272dd9ab0af08c42f1a90c3d3a8fa..bdbf1860f447900c187194cef453e8a7330ecb7d 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -37,18 +37,18 @@ TEST(SensorDiffDrive, create)
{
YAML::Node param;
- param["states"]["P"]["type"] = "StatePoint2d";
- param["states"]["P"]["state"] = p_state;
- param["states"]["P"]["mode"] = "fix";
- param["states"]["P"]["dynamic"] = false;
- param["states"]["O"]["type"] = "StateAngle";
- param["states"]["O"]["state"] = o_state;
- param["states"]["O"]["mode"] = "fix";
- param["states"]["O"]["dynamic"] = false;
- param["states"]["I"]["type"] = "StateParams3";
- param["states"]["I"]["state"] = i_state;
- param["states"]["I"]["mode"] = "fix";
- param["states"]["I"]["dynamic"] = false;
+ param["states"]["P"]["type"] = "StatePoint2d";
+ param["states"]["P"]["value"] = p_state;
+ param["states"]["P"]["prior"]["mode"] = "fix";
+ param["states"]["P"]["dynamic"] = false;
+ param["states"]["O"]["type"] = "StateAngle";
+ param["states"]["O"]["value"] = o_state;
+ param["states"]["O"]["prior"]["mode"] = "fix";
+ param["states"]["O"]["dynamic"] = false;
+ param["states"]["I"]["type"] = "StateParams3";
+ param["states"]["I"]["value"] = i_state;
+ param["states"]["I"]["prior"]["mode"] = "fix";
+ param["states"]["I"]["dynamic"] = false;
param["name"] = "just a sensor";
param["type"] = "SensorDiffDrive";
@@ -111,7 +111,10 @@ TEST(SensorDiffDrive, getParams)
auto sen = std::static_pointer_cast<SensorDiffDrive>(SensorDiffDrive::create(param, {wolf_dir}));
- ASSERT_NE(sen->getParams(), nullptr);
+ ASSERT_TRUE(sen->getParams());
+
+ WOLF_INFO(param);
+ WOLF_INFO(sen->getParams());
ASSERT_NEAR(sen->getTicksPerWheelRevolution(), 400, Constants::EPS);
ASSERT_NEAR(sen->getRadiansPerTick(), 2 * M_PI / 400, Constants::EPS);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 09605925698465e569fcc3ac211e83aa98995d44..7ece71ee3356b7474aef0b4d40494c934a55cae3 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -102,17 +102,16 @@ TEST(SensorOdom, creators_and_factories)
params["k_rot_to_rot"] = k_rot_to_rot;
params["min_disp_var"] = min_disp_var;
params["min_rot_var"] = min_rot_var;
- params["states"]["keys"] = "PO";
- params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
- params["states"]["P"]["state"] = dim == 2 ? p_state_2D : p_state_3D;
- params["states"]["P"]["mode"] = "fix";
- params["states"]["P"]["dynamic"] = false;
+ params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
+ params["states"]["P"]["value"] = dim == 2 ? p_state_2D : p_state_3D;
+ params["states"]["P"]["prior"]["mode"] = "fix";
+ params["states"]["P"]["dynamic"] = false;
- params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
- params["states"]["O"]["state"] = dim == 2 ? o_state_2D : o_state_3D;
- params["states"]["O"]["mode"] = "fix";
- params["states"]["O"]["dynamic"] = false;
+ params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
+ params["states"]["O"]["value"] = dim == 2 ? o_state_2D : o_state_3D;
+ params["states"]["O"]["prior"]["mode"] = "fix";
+ params["states"]["O"]["dynamic"] = false;
// create from node ------------------------------------------------------------------------
auto S = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorOdom2d::create(params, {wolf_dir}))
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index b66dbdf9c4f14e87a2123f9fffcbb1a73bbd1c88..48ed403b94da708dff69558c7a12726fb0e2eb9f 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -87,21 +87,20 @@ TEST(SensorPose, creators_and_factories)
std::string name = "sensor_pose_" + toString(dim) + "d";
YAML::Node params;
- params["type"] = "SensorPose" + toString(dim) + "d";
- params["name"] = name;
- params["plugin"] = "core";
- params["std_noise"] = dim == 2 ? std_noise_2D : std_noise_3D;
- params["states"]["keys"] = "PO";
-
- params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
- params["states"]["P"]["state"] = dim == 2 ? p_state_2D : p_state_3D;
- params["states"]["P"]["mode"] = "fix";
- params["states"]["P"]["dynamic"] = false;
-
- params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
- params["states"]["O"]["state"] = dim == 2 ? o_state_2D : o_state_3D;
- params["states"]["O"]["mode"] = "fix";
- params["states"]["O"]["dynamic"] = false;
+ params["type"] = "SensorPose" + toString(dim) + "d";
+ params["name"] = name;
+ params["plugin"] = "core";
+ params["std_noise"] = dim == 2 ? std_noise_2D : std_noise_3D;
+
+ params["states"]["P"]["type"] = dim == 2 ? "StatePoint2d" : "StatePoint3d";
+ params["states"]["P"]["value"] = dim == 2 ? p_state_2D : p_state_3D;
+ params["states"]["P"]["prior"]["mode"] = "fix";
+ params["states"]["P"]["dynamic"] = false;
+
+ params["states"]["O"]["type"] = dim == 2 ? "StateAngle" : "StateQuaternion";
+ params["states"]["O"]["value"] = dim == 2 ? o_state_2D : o_state_3D;
+ params["states"]["O"]["prior"]["mode"] = "fix";
+ params["states"]["O"]["dynamic"] = false;
// create from node ------------------------------------------------------------------------
auto S = dim == 2 ? std::static_pointer_cast<SensorBase>(SensorPose2d::create(params, {wolf_dir}))
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index d00df5cfa36fda4f6630430835971b772149a458..ddbde0b85d14116da560d2d2e33bc5c6c534b7b8 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -65,11 +65,11 @@ class SolverCeresTest : public testing::Test
}
};
-NodeStateBlocksPtr createStateBlock()
+NodeStateBlocksPtr createNodeStateBlock()
{
auto node = std::make_shared<NodeStateBlocksDummy>();
- node->addStateBlock('I', std::make_shared<StateParams4>((Vector4d() << 1, 0, 0, 0).finished()));
+ node->emplaceStateBlock('I', "StateParams4", (Vector4d() << 1, 0, 0, 0).finished(), false);
return node;
}
@@ -131,7 +131,7 @@ TEST(SolverCeresTestFactories, FactoryFile)
TEST_F(SolverCeresTest, FloatingStateBlock_Add)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -159,7 +159,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_Add)
TEST_F(SolverCeresTest, FloatingStateBlock_DoubleAdd)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -202,7 +202,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_DoubleAdd)
TEST_F(SolverCeresTest, FloatingStateBlock_AddFix)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -269,7 +269,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddFix)
TEST_F(SolverCeresTest, FloatingStateBlock_AddFixed)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -316,7 +316,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddFixed)
TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdateLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -374,7 +374,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdateLocalParam)
TEST_F(SolverCeresTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Local param
@@ -435,7 +435,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
TEST_F(SolverCeresTest, FloatingStateBlock_Remove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -477,7 +477,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_Remove)
TEST_F(SolverCeresTest, FloatingStateBlock_AddRemove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -511,7 +511,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddRemove)
TEST_F(SolverCeresTest, FloatingStateBlock_AddRemoveAdd)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -553,7 +553,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddRemoveAdd)
TEST_F(SolverCeresTest, FloatingStateBlock_DoubleRemove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -591,7 +591,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_DoubleRemove)
TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdated)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Fix
@@ -673,7 +673,7 @@ TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdated)
TEST_F(SolverCeresTest, StateBlockAndFactor_Add)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -720,7 +720,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_Add)
TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleAdd)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -776,7 +776,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleAdd)
TEST_F(SolverCeresTest, StateBlockAndFactor_Fix)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -848,7 +848,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_Fix)
TEST_F(SolverCeresTest, StateBlockAndFactor_AddFixed)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -901,7 +901,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_AddFixed)
TEST_F(SolverCeresTest, StateBlockAndFactor_UpdateLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -959,7 +959,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_UpdateLocalParam)
TEST_F(SolverCeresTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1025,7 +1025,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1076,7 +1076,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveStateBlock)
TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveFactor)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1127,7 +1127,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveFactor)
TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1172,7 +1172,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveStateBlock)
TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveFactor)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1216,7 +1216,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveFactor)
TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1277,7 +1277,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveStateBlock)
TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveFactor)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1332,7 +1332,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveFactor)
TEST_F(SolverCeresTest, StateBlockAndFactor_AddUpdatedStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1417,7 +1417,7 @@ TEST_F(SolverCeresTest, StateBlockAndFactor_AddUpdatedStateBlock)
TEST_F(SolverCeresTest, OnlyFactor_Add)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1449,7 +1449,7 @@ TEST_F(SolverCeresTest, OnlyFactor_Add)
TEST_F(SolverCeresTest, OnlyFactor_Remove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1495,7 +1495,7 @@ TEST_F(SolverCeresTest, OnlyFactor_Remove)
TEST_F(SolverCeresTest, OnlyFactor_AddRemove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 2e60d5fa4029a10e38774c5ed21d8a1947b9e136..3a162365b3230ef2ad68258cc545b52fb92ab401 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -65,11 +65,11 @@ class SolverManagerTest : public testing::Test
}
};
-NodeStateBlocksPtr createStateBlock()
+NodeStateBlocksPtr createNodeStateBlock()
{
auto node = std::make_shared<NodeStateBlocksDummy>();
- node->addStateBlock('I', std::make_shared<StateParams4>((Vector4d() << 1, 0, 0, 0).finished()));
+ node->emplaceStateBlock('I', "StateParams4", (Vector4d() << 1, 0, 0, 0).finished(), false);
return node;
}
@@ -118,7 +118,7 @@ TEST(SolverManagerFactories, FactoryFile)
TEST_F(SolverManagerTest, FloatingStateBlock_Add)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -147,7 +147,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_Add)
TEST_F(SolverManagerTest, FloatingStateBlock_DoubleAdd)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -190,7 +190,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_DoubleAdd)
TEST_F(SolverManagerTest, FloatingStateBlock_AddFix)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -257,7 +257,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddFix)
TEST_F(SolverManagerTest, FloatingStateBlock_AddFixed)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -304,7 +304,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddFixed)
TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdateLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -362,7 +362,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdateLocalParam)
TEST_F(SolverManagerTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Local param
@@ -423,7 +423,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
TEST_F(SolverManagerTest, FloatingStateBlock_Remove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -465,7 +465,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_Remove)
TEST_F(SolverManagerTest, FloatingStateBlock_AddRemove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -499,7 +499,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddRemove)
TEST_F(SolverManagerTest, FloatingStateBlock_AddRemoveAdd)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -541,7 +541,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddRemoveAdd)
TEST_F(SolverManagerTest, FloatingStateBlock_DoubleRemove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
@@ -579,7 +579,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_DoubleRemove)
TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdated)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Fix
@@ -661,7 +661,7 @@ TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdated)
TEST_F(SolverManagerTest, StateBlockAndFactor_Add)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -708,7 +708,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_Add)
TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleAdd)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -764,7 +764,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleAdd)
TEST_F(SolverManagerTest, StateBlockAndFactor_Fix)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -836,7 +836,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_Fix)
TEST_F(SolverManagerTest, StateBlockAndFactor_AddFixed)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -889,7 +889,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_AddFixed)
TEST_F(SolverManagerTest, StateBlockAndFactor_UpdateLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -947,7 +947,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_UpdateLocalParam)
TEST_F(SolverManagerTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1013,7 +1013,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1064,7 +1064,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveStateBlock)
TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveFactor)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1115,7 +1115,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveFactor)
TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1160,7 +1160,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveStateBlock)
TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveFactor)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1204,7 +1204,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveFactor)
TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1265,7 +1265,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveStateBlock)
TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveFactor)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1320,7 +1320,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveFactor)
TEST_F(SolverManagerTest, StateBlockAndFactor_AddUpdatedStateBlock)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1405,7 +1405,7 @@ TEST_F(SolverManagerTest, StateBlockAndFactor_AddUpdatedStateBlock)
TEST_F(SolverManagerTest, OnlyFactor_Add)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1437,7 +1437,7 @@ TEST_F(SolverManagerTest, OnlyFactor_Add)
TEST_F(SolverManagerTest, OnlyFactor_Remove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
@@ -1483,7 +1483,7 @@ TEST_F(SolverManagerTest, OnlyFactor_Remove)
TEST_F(SolverManagerTest, OnlyFactor_AddRemove)
{
// Create State block
- auto node_ptr = createStateBlock();
+ auto node_ptr = createNodeStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// Create Factor
diff --git a/test/gtest_spec_state_composite.cpp b/test/gtest_spec_state_composite.cpp
deleted file mode 100644
index 0efced83baa926fea39fc76a89f914c96edb58f7..0000000000000000000000000000000000000000
--- a/test/gtest_spec_state_composite.cpp
+++ /dev/null
@@ -1,295 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/utils/utils_gtest.h"
-#include "core/common/wolf.h"
-#include "core/composite/spec_state_composite.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-std::string wolf_dir = _WOLF_CODE_DIR;
-
-VectorXd vector0 = VectorXd(0);
-VectorXd vector1 = VectorXd::Random(1);
-VectorXd vector2 = VectorXd::Random(2);
-VectorXd vector3 = VectorXd::Random(3);
-VectorXd vector4 = VectorXd::Random(4);
-VectorXd vectorq = VectorXd::Random(4).normalized();
-
-struct SpecAsStruct
-{
- std::string type;
- std::string mode;
- VectorXd state;
- VectorXd noise_std;
-};
-
-void testSpecSensor(const SpecState& P, const SpecAsStruct& pstruct)
-{
- ASSERT_EQ(pstruct.type, P.getType());
- ASSERT_EQ(pstruct.mode, P.getMode());
- ASSERT_MATRIX_APPROX(pstruct.state, P.getState(), 1e-8);
-
- if (pstruct.mode == "factor")
- {
- ASSERT_MATRIX_APPROX(pstruct.noise_std, P.getNoiseStd(), 1e-8);
- }
- else
- {
- ASSERT_EQ(0, P.getNoiseStd().size());
- }
-};
-
-void testSpecSensorVector(const std::vector<SpecAsStruct>& setups, bool should_work)
-{
- WOLF_INFO("Testing setups that should ", (should_work ? "" : "NOT"), " work...");
- for (auto setup : setups)
- {
- WOLF_INFO("spec setup: ",
- "\n\ttype: ",
- setup.type,
- "\n\tmode: ",
- setup.mode,
- "\n\tstate: ",
- setup.state.transpose(),
- "\n\tnoise_std: ",
- setup.noise_std.transpose());
- if (should_work)
- testSpecSensor(SpecState(setup.type, setup.state, setup.mode, setup.noise_std), setup);
- else
- {
- ASSERT_THROW(testSpecSensor(SpecState(setup.type, setup.state, setup.mode, setup.noise_std), setup),
- std::runtime_error);
- }
- }
-}
-
-TEST(SpecStateSensor, getVectorComposite_and_getTypeComposite)
-{
- SpecStateComposite specs({{'P', SpecState("StatePoint2d", vector2, "initial_guess", vector0)},
- {'O', SpecState("StateAngle", vector1, "initial_guess", vector0)},
- {'I', SpecState("StateParams4", vector4, "factor", vector4)}});
-
- auto types = specs.getTypeComposite();
- WOLF_INFO(types);
- ASSERT_EQ(types.size(), specs.size());
- ASSERT_EQ(types.at('P'), "StatePoint2d");
- ASSERT_EQ(types.at('O'), "StateAngle");
- ASSERT_EQ(types.at('I'), "StateParams4");
-
- auto vectors = specs.getVectorComposite();
- WOLF_INFO(vectors);
- ASSERT_EQ(vectors.size(), specs.size());
- ASSERT_MATRIX_APPROX(vectors.at('P'), vector2, Constants::EPS);
- ASSERT_MATRIX_APPROX(vectors.at('O'), vector1, Constants::EPS);
- ASSERT_MATRIX_APPROX(vectors.at('I'), vector4, Constants::EPS);
-}
-
-TEST(SpecStateSensor, P)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Initial guess
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "initial_guess", vector2, vector0}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "initial_guess", vector3, vector0}));
- setups_death.push_back(SpecAsStruct({"StatePoint3d", "initial_guess", vector4, vector0})); // wrong state size
-
- // Fix
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "fix", vector2, vector0}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "fix", vector3, vector0}));
- setups_death.push_back(SpecAsStruct({"StatePoint2d", "fix", vector4, vector0})); // wrong size
-
- // Factor
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "factor", vector2, vector2}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "factor", vector3, vector3}));
- setups_death.push_back(SpecAsStruct({"StatePoint3d", "factor", vector4, vector4})); // wrong state size
- setups_death.push_back(SpecAsStruct({"StatePoint2d", "factor", vector2, vector3})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-
-TEST(SpecStateSensor, StateBlock)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Unknown type -> StateBlock
- setups_death.push_back(SpecAsStruct({"K", "initial_guess", vector1, vector0})); // unknown K
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "initial_guess", vector2, vector0}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "initial_guess", vector3, vector0}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "initial_guess", vector4, vector0}));
- setups_death.push_back(SpecAsStruct({"StateParams3", "initial_guess", vector4, vector0})); // wrong size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "fix", vector2, vector0}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "fix", vector3, vector0}));
- setups_death.push_back(SpecAsStruct({"StateParams3", "fix", vector4, vector0})); // wrong size
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "factor", vector2, vector2}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "factor", vector3, vector3}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "factor", vector4, vector4}));
- setups_death.push_back(SpecAsStruct({"StateParams2", "factor", vector2, vector3})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-TEST(SpecStateSensor, StateAngle)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "initial_guess", vector1, vector0}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "initial_guess", vector3, vector0})); // wrong size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "fix", vector1, vector0}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "fix", vector3, vector0})); // wrong size
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "factor", vector1, vector1}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "factor", vector3, vector3})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateAngle", "factor", vector1, vector4})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-
-TEST(SpecStateSensor, StateQuaternion)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vectorq, vector0}));
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vector4, vector0})); // not normalized
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vector3, vector0})); // wrong size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "fix", vectorq, vector0}));
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "fix", vector3, vector0})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "fix", vector4, vector0})); // not normalized
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "factor", vectorq, vector3}));
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "factor", vector4, vector3})); // not normalized
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "factor", vector3, vector3})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "factor", vectorq, vector4})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-
-TEST(SpecStateSensor, YamlNode)
-{
- YAML::Node input_node = YAML::LoadFile(wolf_dir + "/test/yaml/spec_state.yaml");
-
- std::vector<std::string> keys({"P", "O"});
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"initial_guess", "fix", "factor"});
-
- VectorXd p_state(4), o_state(4), po_std(4);
- p_state << 1, 2, 3, 4;
- o_state << 1, 0, 0, 0;
- po_std << 0.1, 0.2, 0.3, 0.4;
-
- // P & O
- for (auto key : keys)
- for (auto dim : dims)
- for (auto mode : modes)
- {
- std::string prefix = key + "_" + toString(dim) + "D_" + mode;
-
- WOLF_INFO("Creating spec from prefix ", prefix);
- auto prior = input_node[prefix].as<SpecState>();
-
- // Checks
- ASSERT_EQ(prior.getMode(), mode);
-
- VectorXd state = p_state.head(dim);
- VectorXd noise_std = po_std.head(dim);
- if (key == "O")
- {
- state = o_state.head(dim == 2 ? 1 : 4);
- noise_std = po_std.head(dim == 2 ? 1 : 3);
- }
-
- ASSERT_MATRIX_APPROX(prior.getState(), state, wolf::Constants::EPS);
-
- if (mode == "factor")
- {
- ASSERT_MATRIX_APPROX(prior.getNoiseStd(), noise_std, wolf::Constants::EPS);
- }
- }
-
- // I
- for (auto mode : modes)
- {
- std::string prefix = "I_" + mode;
- WOLF_INFO("Creating spec from prefix ", prefix);
- auto prior = input_node[prefix].as<SpecState>();
- ASSERT_EQ(prior.getMode(), mode);
- ASSERT_MATRIX_APPROX(prior.getState(), p_state, wolf::Constants::EPS);
- if (mode == "factor")
- {
- ASSERT_MATRIX_APPROX(prior.getNoiseStd(), po_std, wolf::Constants::EPS);
- }
- }
-}
-
-TEST(SpecStateSensor, ParamsServerWrong)
-{
- YAML::Node input_node = YAML::LoadFile(wolf_dir + "/test/yaml/spec_state_wrong.yaml");
-
- std::vector<std::string> keys({"P", "O"});
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"initial_guess", "fix", "factor"});
-
- // P & O
- for (auto key : keys)
- for (auto dim : dims)
- for (auto mode : modes)
- {
- std::string prefix = key + "_" + toString(dim) + "D_" + mode;
-
- WOLF_INFO("Creating spec from prefix ", prefix);
- ASSERT_THROW(auto spec = input_node[prefix].as<SpecState>(), std::runtime_error);
- }
-
- // I
- for (auto mode : modes)
- {
- std::string prefix = "I_" + mode;
- WOLF_INFO("Creating spec from prefix ", prefix);
- ASSERT_THROW(auto spec = input_node[prefix].as<SpecState>(), std::runtime_error);
- }
-}
-
-int main(int argc, char** argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_spec_state_sensor_composite.cpp b/test/gtest_spec_state_sensor_composite.cpp
deleted file mode 100644
index dc53ee0fc80d1d48a82e3a0e4d1097f8e19546f4..0000000000000000000000000000000000000000
--- a/test/gtest_spec_state_sensor_composite.cpp
+++ /dev/null
@@ -1,486 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023,2024
-// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/wolf
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include "core/utils/utils_gtest.h"
-#include "core/common/wolf.h"
-#include "core/composite/spec_state_sensor_composite.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-std::string wolf_dir = _WOLF_CODE_DIR;
-
-VectorXd vector0 = VectorXd(0);
-VectorXd vector1 = VectorXd::Random(1);
-VectorXd vector2 = VectorXd::Random(2);
-VectorXd vector3 = VectorXd::Random(3);
-VectorXd vector4 = VectorXd::Random(4);
-VectorXd vectorq = VectorXd::Random(4).normalized();
-
-struct SpecAsStruct
-{
- std::string type;
- std::string mode;
- VectorXd state;
- VectorXd noise_std;
- bool dynamic;
- VectorXd drift_std;
-};
-
-void testSpecSensor(const SpecStateSensor& P, const SpecAsStruct& pstruct)
-{
- ASSERT_EQ(pstruct.type, P.getType());
- ASSERT_EQ(pstruct.mode, P.getMode());
- ASSERT_MATRIX_APPROX(pstruct.state, P.getState(), 1e-8);
-
- if (pstruct.mode == "factor")
- {
- ASSERT_MATRIX_APPROX(pstruct.noise_std, P.getNoiseStd(), 1e-8);
- }
- else
- {
- ASSERT_EQ(0, P.getNoiseStd().size());
- }
-
- ASSERT_EQ(pstruct.dynamic, P.isDynamic());
-
- if (pstruct.drift_std.size() != 0)
- {
- ASSERT_MATRIX_APPROX(pstruct.drift_std, P.getDriftStd(), 1e-8);
- }
-};
-
-void testSpecSensorVector(const std::vector<SpecAsStruct>& setups, bool should_work)
-{
- WOLF_INFO("Testing setups that should ", (should_work ? "" : "NOT"), " work...");
- for (auto setup : setups)
- {
- WOLF_INFO("spec setup: ",
- "\n\ttype: ",
- setup.type,
- "\n\tmode: ",
- setup.mode,
- "\n\tstate: ",
- setup.state.transpose(),
- "\n\tnoise_std: ",
- setup.noise_std.transpose(),
- "\n\tdynamic: ",
- setup.dynamic,
- "\n\tdrift_std: ",
- setup.drift_std.transpose());
- if (should_work)
- testSpecSensor(
- SpecStateSensor(setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std),
- setup);
- else
- {
- ASSERT_THROW(testSpecSensor(
- SpecStateSensor(
- setup.type, setup.state, setup.mode, setup.noise_std, setup.dynamic, setup.drift_std),
- setup),
- std::runtime_error);
- }
- }
-}
-
-TEST(SpecStateSensor, cast)
-{
- SpecStateSensorComposite specs({{'P', SpecStateSensor("StatePoint2d", vector2, "initial_guess", vector0, false)},
- {'O', SpecStateSensor("StateAngle", vector1, "fix", vector0, false)},
- {'I', SpecStateSensor("StateParams4", vector4, "factor", vector4, true)}});
-
- auto specs_casted = specs.cast<SpecStateComposite>();
-
- ASSERT_EQ(specs_casted.size(), specs.size());
- ASSERT_EQ(specs_casted.getKeys(), specs.getKeys());
- ASSERT_EQ(specs_casted.at('P').getType(), "StatePoint2d");
- ASSERT_EQ(specs_casted.at('O').getType(), "StateAngle");
- ASSERT_EQ(specs_casted.at('I').getType(), "StateParams4");
- ASSERT_MATRIX_APPROX(specs_casted.at('P').getState(), vector2, Constants::EPS);
- ASSERT_MATRIX_APPROX(specs_casted.at('O').getState(), vector1, Constants::EPS);
- ASSERT_MATRIX_APPROX(specs_casted.at('I').getState(), vector4, Constants::EPS);
- ASSERT_EQ(specs_casted.at('P').getMode(), "initial_guess");
- ASSERT_EQ(specs_casted.at('O').getMode(), "fix");
- ASSERT_EQ(specs_casted.at('I').getMode(), "factor");
- ASSERT_MATRIX_APPROX(specs_casted.at('P').getNoiseStd(), vector0, Constants::EPS);
- ASSERT_MATRIX_APPROX(specs_casted.at('O').getNoiseStd(), vector0, Constants::EPS);
- ASSERT_MATRIX_APPROX(specs_casted.at('I').getNoiseStd(), vector4, Constants::EPS);
-}
-
-TEST(SpecStateSensor, getVectorComposite_and_getTypeComposite)
-{
- SpecStateSensorComposite specs({{'P', SpecStateSensor("StatePoint2d", vector2, "initial_guess", vector0, false)},
- {'O', SpecStateSensor("StateAngle", vector1, "initial_guess", vector0, false)},
- {'I', SpecStateSensor("StateParams4", vector4, "factor", vector4, true)}});
-
- auto types = specs.cast<SpecStateComposite>().getTypeComposite();
- WOLF_INFO(types);
- ASSERT_EQ(types.size(), specs.size());
- ASSERT_EQ(types.at('P'), "StatePoint2d");
- ASSERT_EQ(types.at('O'), "StateAngle");
- ASSERT_EQ(types.at('I'), "StateParams4");
-
- auto vectors = specs.cast<SpecStateComposite>().getVectorComposite();
- WOLF_INFO(vectors);
- ASSERT_EQ(vectors.size(), specs.size());
- ASSERT_MATRIX_APPROX(vectors.at('P'), vector2, Constants::EPS);
- ASSERT_MATRIX_APPROX(vectors.at('O'), vector1, Constants::EPS);
- ASSERT_MATRIX_APPROX(vectors.at('I'), vector4, Constants::EPS);
-}
-
-TEST(SpecStateSensor, P)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "initial_guess", vector2, vector0, false}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "initial_guess", vector3, vector0, false}));
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "initial_guess", vector4, vector0, false})); // wrong state size
-
- // Initial guess - dynamic
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "initial_guess", vector2, vector0, true}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "initial_guess", vector3, vector0, true, vector0}));
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "initial_guess", vector2, vector0, true, vector2}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "initial_guess", vector3, vector0, true, vector3}));
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "initial_guess", vector4, vector0, true, vector4})); // wrong state size
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "initial_guess", vector3, vector0, true, vector4})); // inconsistent size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "fix", vector2, vector0, false}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "fix", vector3, vector0, false}));
- setups_death.push_back(SpecAsStruct({"StatePoint2d", "fix", vector4, vector0, false})); // wrong size
-
- // Fix - dynamic
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "fix", vector2, vector0, true, vector0}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "fix", vector3, vector0, true}));
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "fix", vector2, vector0, true, vector2}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "fix", vector3, vector0, true, vector3}));
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "fix", vector4, vector0, true, vector4})); // wrong state size
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "fix", vector3, vector0, true, vector4})); // inconsistent size
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "factor", vector2, vector2, false}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "factor", vector3, vector3, false}));
- setups_death.push_back(SpecAsStruct({"StatePoint3d", "factor", vector4, vector4, false})); // wrong state size
- setups_death.push_back(SpecAsStruct({"StatePoint2d", "factor", vector2, vector3, false})); // inconsistent size
-
- // Factor - dynamic
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "factor", vector2, vector2, true}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "factor", vector3, vector3, true}));
- setups_ok.push_back(SpecAsStruct({"StatePoint2d", "factor", vector2, vector2, true, vector2}));
- setups_ok.push_back(SpecAsStruct({"StatePoint3d", "factor", vector3, vector3, true, vector3}));
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "factor", vector4, vector4, true, vector4})); // wrong state size
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "factor", vector3, vector3, true, vector4})); // inconsistent size
- setups_death.push_back(
- SpecAsStruct({"StatePoint3d", "factor", vector3, vector4, true, vector3})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-
-TEST(SpecStateSensor, StateBlock)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Unknown type -> StateBlock
- setups_death.push_back(SpecAsStruct({"K", "initial_guess", vector1, vector0, false})); // unknown K
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "initial_guess", vector2, vector0, false}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "initial_guess", vector3, vector0, false}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "initial_guess", vector4, vector0, false}));
-
- // Initial guess - dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "initial_guess", vector2, vector0, true}));
- setups_ok.push_back(SpecAsStruct({"StateParams2", "initial_guess", vector2, vector0, true, vector2}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "initial_guess", vector3, vector0, true, vector3}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "initial_guess", vector4, vector0, true, vector4}));
- setups_death.push_back(
- SpecAsStruct({"StateParams3", "initial_guess", vector3, vector0, true, vector4})); // inconsistent size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "fix", vector2, vector0, false}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "fix", vector3, vector0, false}));
- setups_death.push_back(SpecAsStruct({"StateParams3", "fix", vector4, vector0, false})); // wrong size
-
- // Fix - dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "fix", vector2, vector0, true, vector0}));
- setups_ok.push_back(SpecAsStruct({"StateParams2", "fix", vector2, vector0, true, vector2}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "fix", vector3, vector0, true, vector3}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "fix", vector4, vector0, true, vector4}));
- setups_death.push_back(
- SpecAsStruct({"StateParams3", "fix", vector3, vector0, true, vector4})); // inconsistent size
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "factor", vector2, vector2, false}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "factor", vector3, vector3, false}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "factor", vector4, vector4, false}));
- setups_death.push_back(SpecAsStruct({"StateParams2", "factor", vector2, vector3, false})); // inconsistent size
-
- // Factor - dynamic
- setups_ok.push_back(SpecAsStruct({"StateParams2", "factor", vector2, vector2, true}));
- setups_ok.push_back(SpecAsStruct({"StateParams2", "factor", vector2, vector2, true, vector2}));
- setups_ok.push_back(SpecAsStruct({"StateParams3", "factor", vector3, vector3, true, vector3}));
- setups_ok.push_back(SpecAsStruct({"StateParams4", "factor", vector4, vector4, true, vector4}));
- setups_death.push_back(
- SpecAsStruct({"StateParams3", "factor", vector3, vector3, true, vector4})); // inconsistent size
- setups_death.push_back(
- SpecAsStruct({"StateParams3", "factor", vector3, vector4, true, vector3})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-TEST(SpecStateSensor, StateAngle)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "initial_guess", vector1, vector0, false}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "initial_guess", vector3, vector0, false})); // wrong size
-
- // Initial guess - dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "initial_guess", vector1, vector0, true}));
- setups_ok.push_back(SpecAsStruct({"StateAngle", "initial_guess", vector1, vector0, true, vector1}));
- setups_death.push_back(
- SpecAsStruct({"StateAngle", "initial_guess", vector3, vector0, true, vector3})); // wrong size
- setups_death.push_back(
- SpecAsStruct({"StateAngle", "initial_guess", vector1, vector0, true, vector4})); // inconsistent size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "fix", vector1, vector0, false}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "fix", vector3, vector0, false})); // wrong size
-
- // Fix - dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "fix", vector1, vector0, true, vector0}));
- setups_ok.push_back(SpecAsStruct({"StateAngle", "fix", vector1, vector0, true, vector1}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "fix", vector3, vector0, true, vector3})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateAngle", "fix", vector1, vector0, true, vector4})); // inconsistent size
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "factor", vector1, vector1, false}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "factor", vector3, vector3, false})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateAngle", "factor", vector1, vector4, false})); // inconsistent size
-
- // Factor - dynamic
- setups_ok.push_back(SpecAsStruct({"StateAngle", "factor", vector1, vector1, true}));
- setups_ok.push_back(SpecAsStruct({"StateAngle", "factor", vector1, vector1, true, vector1}));
- setups_death.push_back(SpecAsStruct({"StateAngle", "factor", vector3, vector3, true, vector3})); // wrong size
- setups_death.push_back(
- SpecAsStruct({"StateAngle", "factor", vector1, vector3, true, vector1})); // inconsistent size
- setups_death.push_back(
- SpecAsStruct({"StateAngle", "factor", vector1, vector1, true, vector3})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-
-TEST(SpecStateSensor, StateQuaternion)
-{
- std::vector<SpecAsStruct> setups_ok, setups_death;
-
- // Initial guess - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vectorq, vector0, false}));
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "initial_guess", vector4, vector0, false})); // not normalized
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vector3, vector0, false})); // wrong size
-
- // Initial guess - dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vectorq, vector0, true}));
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "initial_guess", vectorq, vector0, true, vector3}));
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "initial_guess", vector4, vector0, true, vector3})); // not normalized
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "initial_guess", vector3, vector0, true, vector3})); // wrong size
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "initial_guess", vectorq, vector0, true, vector4})); // inconsistent size
-
- // Fix - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "fix", vectorq, vector0, false}));
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "fix", vector3, vector0, false})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "fix", vector4, vector0, false})); // not normalized
-
- // Fix - dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "fix", vectorq, vector0, true, vector0}));
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "fix", vectorq, vector0, true, vector3}));
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "fix", vector4, vector0, true, vector3})); // not normalized
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "fix", vector3, vector0, true, vector3})); // wrong size
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "fix", vectorq, vector0, true, vector4})); // inconsistent size
-
- // Factor - not dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "factor", vectorq, vector3, false}));
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "factor", vector4, vector3, false})); // not normalized
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "factor", vector3, vector3, false})); // wrong size
- setups_death.push_back(SpecAsStruct({"StateQuaternion", "factor", vectorq, vector4, false})); // inconsistent size
-
- // Factor - dynamic
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "factor", vectorq, vector3, true}));
- setups_ok.push_back(SpecAsStruct({"StateQuaternion", "factor", vectorq, vector3, true, vector3}));
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "factor", vector4, vector3, true, vector3})); // not normalized
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "factor", vector3, vector3, true, vector3})); // wrong size
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "factor", vectorq, vector3, true, vector4})); // inconsistent size
- setups_death.push_back(
- SpecAsStruct({"StateQuaternion", "factor", vectorq, vector4, true, vector3})); // inconsistent size
-
- // TEST SETUPS
- testSpecSensorVector(setups_ok, true);
- testSpecSensorVector(setups_death, false);
-}
-
-TEST(SpecStateSensor, YamlNode)
-{
- YAML::Node input_node = YAML::LoadFile(wolf_dir + "/test/yaml/spec_state_sensor.yaml");
-
- std::vector<std::string> keys({"P", "O"});
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"initial_guess", "fix", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
-
- VectorXd p_state(4), o_state(4), po_std(4);
- p_state << 1, 2, 3, 4;
- o_state << 1, 0, 0, 0;
- po_std << 0.1, 0.2, 0.3, 0.4;
-
- // P & O
- for (auto key : keys)
- for (auto dim : dims)
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "");
-
- WOLF_INFO("Creating spec from prefix ", prefix);
- auto P = SpecStateSensor(input_node[prefix]);
-
- // Checks
- ASSERT_EQ(P.getMode(), mode);
- ASSERT_EQ(P.isDynamic(), dynamic);
-
- VectorXd state = p_state.head(dim);
- VectorXd noise_std = po_std.head(dim);
- if (key == "O")
- {
- state = o_state.head(dim == 2 ? 1 : 4);
- noise_std = po_std.head(dim == 2 ? 1 : 3);
- }
-
- ASSERT_MATRIX_APPROX(P.getState(), state, wolf::Constants::EPS);
-
- if (drift)
- {
- ASSERT_MATRIX_APPROX(P.getDriftStd(), noise_std, wolf::Constants::EPS);
- }
- if (mode == "factor")
- {
- ASSERT_MATRIX_APPROX(P.getNoiseStd(), noise_std, wolf::Constants::EPS);
- }
- }
-
- // I
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
- WOLF_INFO("Creating spec from prefix ", prefix);
- auto P = SpecStateSensor(input_node[prefix]);
- ASSERT_EQ(P.getMode(), mode);
- ASSERT_EQ(P.isDynamic(), dynamic);
- ASSERT_MATRIX_APPROX(P.getState(), p_state, wolf::Constants::EPS);
- if (drift)
- {
- ASSERT_MATRIX_APPROX(P.getDriftStd(), po_std, wolf::Constants::EPS);
- }
- if (mode == "factor")
- {
- ASSERT_MATRIX_APPROX(P.getNoiseStd(), po_std, wolf::Constants::EPS);
- }
- }
-}
-
-TEST(SpecStateSensor, ParamsServerWrong)
-{
- YAML::Node input_node = YAML::LoadFile(wolf_dir + "/test/yaml/spec_state_sensor_wrong.yaml");
-
- std::vector<std::string> keys({"P", "O"});
- std::vector<int> dims({2, 3});
- std::vector<std::string> modes({"initial_guess", "fix", "factor"});
- std::vector<bool> dynamics({false, true});
- std::vector<bool> drifts({false, true});
-
- // P & O
- for (auto key : keys)
- for (auto dim : dims)
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") +
- (drift ? "_drift" : "");
-
- WOLF_INFO("Creating spec from prefix ", prefix);
- ASSERT_THROW(auto spec = SpecStateSensor(input_node[prefix]), std::runtime_error);
- }
-
- // I
- for (auto mode : modes)
- for (auto dynamic : dynamics)
- for (auto drift : drifts)
- {
- // nonsense combination
- if (not dynamic and drift) continue;
-
- std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
- WOLF_INFO("Creating spec from prefix ", prefix);
- ASSERT_THROW(auto spec = SpecStateSensor(input_node[prefix]), std::runtime_error);
- }
-}
-
-int main(int argc, char** argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index 02483741f6fba75c7272ca4617542672e771edec..b3990eaa86a986053f8a24657a509272da35062c 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -93,9 +93,7 @@ TEST(StatePoint3d, transform_identity)
Vector3d x0(1, 2, 3);
StatePoint3d sb(x0);
- Vector7d xtrf;
- xtrf << 0, 0, 0, 0, 0, 0, 1;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}});
sb.transform(trf);
ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15);
@@ -106,9 +104,7 @@ TEST(StatePoint3d, transform_translation_4x5y6z)
Vector3d x0(1, 2, 3);
StatePoint3d sb(x0);
- Vector7d xtrf;
- xtrf << 4, 5, 6, 0, 0, 0, 1;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d(4, 5, 6)}, {'O', Quaterniond::Identity().coeffs()}});
Vector3d x(5, 7, 9); // x0 translated (4,5,6)
@@ -121,9 +117,7 @@ TEST(StatePoint3d, transform_rotation_90x)
Vector3d x0(1, 2, 3);
StatePoint3d sb(x0);
- Vector7d xtrf;
- xtrf << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d::Zero()}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
Vector3d x(1, -3, 2); // x0 rotated (90,0,0)
@@ -136,9 +130,7 @@ TEST(StatePoint3d, transform_translation_1y_rotation_90x)
Vector3d x0(1, 2, 3);
StatePoint3d sb(x0);
- Vector7d xtrf;
- xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d(0, 1, 0)}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
Vector3d x(1, -2, 2); // x0 translated (0,1,0) and rotated (90,0,0)
@@ -151,9 +143,7 @@ TEST(StatePoint3d, non_transformable_translation_rotation)
Vector3d x0(1, 2, 3);
StatePoint3d sb(x0, false, false); // non transformable
- Vector7d xtrf;
- xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d(0, 1, 0)}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
Vector3d x(1, 2, 3); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
@@ -166,9 +156,7 @@ TEST(StateVector3d, transform_translation_1y_rotation_90x)
Vector3d x0(1, 2, 3);
StateVector3d sb(x0);
- Vector7d xtrf;
- xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d(0, 1, 0)}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
Vector3d x(1, -3, 2); // x0 NOT translated (0,1,0) and rotated (90,0,0)
@@ -181,9 +169,7 @@ TEST(StatePoint2d, transform_translation_1y_rotation_90)
Vector2d x0(1, 2);
StatePoint2d sb(x0);
- Vector3d xtrf;
- xtrf << 0, 1, M_PI / 2;
- VectorComposite trf("PO", xtrf, {2, 1});
+ VectorComposite trf({{'P', Vector2d(0, 1)}, {'O', Vector1d(M_PI / 2)}});
Vector2d x(-2, 2); // x0 translated (0,1) and rotated 90 deg
@@ -196,9 +182,7 @@ TEST(StateVector2d, transform_translation_1y_rotation_90)
Vector2d x0(1, 2);
StateVector2d sb(x0);
- Vector3d xtrf;
- xtrf << 0, 1, M_PI / 2;
- VectorComposite trf("PO", xtrf, {2, 1});
+ VectorComposite trf({{'P', Vector2d(0, 1)}, {'O', Vector1d(M_PI / 2)}});
Vector2d x(-2, 1); // x0 NON translated (0,1) and rotated 90 deg
@@ -211,9 +195,7 @@ TEST(StateParams4, transform_translation_1y_rotation_90x)
Vector4d x0(1, 2, 3, 4);
StateParams<4> sb(x0);
- Vector7d xtrf;
- xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
- VectorComposite trf("PO", xtrf, {3, 4});
+ VectorComposite trf({{'P', Vector3d(0, 1, 0)}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
Vector4d x(1, 2, 3, 4); // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 03e29ba2ba6e8ab1fd9bf0936e63fff7efdb1075..ed0d5bbc3e8f547dd9e9a0ac0f7c1018b29bbd5f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -22,6 +22,7 @@
#include "core/processor/track_matrix.h"
+using namespace Eigen;
using namespace wolf;
class TrackMatrixTest : public testing::Test
@@ -29,8 +30,8 @@ class TrackMatrixTest : public testing::Test
public:
TrackMatrix track_matrix;
- Eigen::Vector2d m;
- Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity() * 0.01;
+ Vector2d m;
+ Matrix2d m_cov = Matrix2d::Identity() * 0.01;
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
@@ -50,11 +51,11 @@ class TrackMatrixTest : public testing::Test
// unlinked frames
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- F0 = std::make_shared<FrameBase>(0.0, nullptr);
- F1 = std::make_shared<FrameBase>(1.0, nullptr);
- F2 = std::make_shared<FrameBase>(2.0, nullptr);
- F3 = std::make_shared<FrameBase>(3.0, nullptr);
- F4 = std::make_shared<FrameBase>(4.0, nullptr);
+ F0 = FrameBase::emplace<FrameBase>(nullptr, 0.0, TypeComposite(), VectorComposite());
+ F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, TypeComposite(), VectorComposite());
+ F2 = FrameBase::emplace<FrameBase>(nullptr, 2.0, TypeComposite(), VectorComposite());
+ F3 = FrameBase::emplace<FrameBase>(nullptr, 3.0, TypeComposite(), VectorComposite());
+ F4 = FrameBase::emplace<FrameBase>(nullptr, 4.0, TypeComposite(), VectorComposite());
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 56faa8b7c24cc072134691958fab6a7a6579375c..997992f2c33c5ec637e959116a15345566702fee 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -29,6 +29,7 @@
#include <iostream>
using namespace wolf;
+using namespace Eigen;
/// Set to true if you want debug info
bool debug = false;
@@ -45,12 +46,12 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero());
- FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero());
+ FrameBasePtr F1 = P->emplaceFrame(1, "PO", Vector3d::Zero());
+ FrameBasePtr F2 = P->emplaceFrame(2, "PO", Vector3d::Zero());
FrameBasePtr F3 = std::make_shared<FrameBase>(
- 3, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
+ 3, P->getFrameTypes(), VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
FrameBasePtr F4 = std::make_shared<FrameBase>(
- 4, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
+ 4, P->getFrameTypes(), VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
FrameBasePtr KF; // closest key-frame queried
@@ -90,14 +91,14 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceFrame(1, "PO", Eigen::Vector3d::Zero()); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceFrame(1, "PO", Vector3d::Zero()); // 2 non-fixed
if (debug) P->print(2, 0, 0, 0);
ASSERT_EQ(T->getFrameMap().size(), (SizeStd)1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceFrame(2, "PO", Eigen::Vector3d::Zero()); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceFrame(2, "PO", Vector3d::Zero()); // 1 fixed, 1 not
if (debug) P->print(2, 0, 0, 0);
ASSERT_EQ(T->getFrameMap().size(), (SizeStd)2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)4);
@@ -105,7 +106,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// add F3
FrameBasePtr F3 = std::make_shared<FrameBase>(
- 3, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
+ 3, P->getFrameTypes(), VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}});
if (debug) P->print(2, 0, 0, 0);
ASSERT_EQ(T->getFrameMap().size(), (SizeStd)2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)4);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 2b62de000c57984bb860f314f6dbee1df8e44a1b..85db80ff20e405843df6738c395d04fd0035dc31 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -97,7 +97,7 @@ TEST(TreeManager, FactoryYaml)
TEST(TreeManager, autoConf)
{
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_tree_manager1.yaml", {wolf_dir});
- P->applyPriorOptions(0);
+ P->applyFirstFrameOptions(0);
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
@@ -106,7 +106,7 @@ TEST(TreeManager, autoConf)
TEST(TreeManager, autoConfNone)
{
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_tree_manager2.yaml", {wolf_dir});
- P->applyPriorOptions(0);
+ P->applyFirstFrameOptions(0);
ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 7a06e126f7f11cc1bb7be7e20c2df131eeba39f7..5f7963888bc04eb818dcee286e8b2323c18296e8 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -111,7 +111,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// window size: 3
// first 2 frames fixed
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window1.yaml", {wolf_dir});
- P->applyPriorOptions(0);
+ P->applyFirstFrameOptions(0);
// FRAME 1 ----------------------------------------------------------
auto F1 = P->getTrajectory()->getLastFrame();
@@ -249,7 +249,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
{
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window2.yaml", {wolf_dir});
- P->applyPriorOptions(0);
+ P->applyFirstFrameOptions(0);
// FRAME 1 (prior) ----------------------------------------------------------
auto F1 = P->getTrajectory()->getLastFrame();
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index 23af0bcc2648d807e9b71e3c3363d98e494b9733..cd9583f4f5307ec9f8bc5d0200272b4a7cb94a2d 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -119,7 +119,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*/
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window_dual_rate1.yaml", {wolf_dir});
- P->applyPriorOptions(0);
+ P->applyFirstFrameOptions(0);
/* FRAME 1 ----------------------------------------------------------
*
@@ -636,7 +636,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* rate_old_frames: 2
*/
ProblemPtr P = Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window_dual_rate2.yaml", {wolf_dir});
- P->applyPriorOptions(0);
+ P->applyFirstFrameOptions(0);
/* FRAME 1 ----------------------------------------------------------
*
diff --git a/test/gtest_vector_composite.cpp b/test/gtest_vector_composite.cpp
index 48aa61e94df477b9723ed0db1abbabdf604824a7..8866142c6d8b962b3f4e79a62a5ddb6ea57e374b 100644
--- a/test/gtest_vector_composite.cpp
+++ b/test/gtest_vector_composite.cpp
@@ -23,6 +23,7 @@
#include "core/utils/utils_gtest.h"
using namespace wolf;
+using namespace Eigen;
using namespace std;
TEST(VectorComposite, constructor_empty)
@@ -31,6 +32,15 @@ TEST(VectorComposite, constructor_empty)
ASSERT_TRUE(v.empty());
}
+TEST(VectorComposite, constructor_list)
+{
+ VectorComposite u{{'a', Vector2d(1, 2)}, {'b', Vector3d(3, 4, 5)}}; // std::map API
+
+ ASSERT_EQ(u.getKeys(), "ab");
+ ASSERT_MATRIX_APPROX(u['a'], Vector2d(1, 2), 1e-20);
+ ASSERT_MATRIX_APPROX(u['b'], Vector3d(3, 4, 5), 1e-20);
+}
+
TEST(VectorComposite, constructor_copy)
{
VectorComposite u;
@@ -39,32 +49,11 @@ TEST(VectorComposite, constructor_copy)
VectorComposite v(u);
- ASSERT_FALSE(v.empty());
-
+ ASSERT_EQ(u.getKeys(), v.getKeys());
ASSERT_MATRIX_APPROX(u['a'], v['a'], 1e-20);
ASSERT_MATRIX_APPROX(u['b'], v['b'], 1e-20);
}
-TEST(VectorComposite, constructor_from_vector_and_list)
-{
- VectorComposite v("ab", Vector4d(1, 2, 3, 4), {3, 1});
-
- ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1, 2, 3), 1e-20);
- ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
-
- ASSERT_THROW(VectorComposite("ab", Vector4d(1, 2, 3, 4), {3, 2}), std::runtime_error);
-}
-
-TEST(VectorComposite, constructor_from_list_of_vectors)
-{
- VectorComposite v("ab", {Vector3d(1, 2, 3), Vector1d(4)});
-
- ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1, 2, 3), 1e-20);
- ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
-
- ASSERT_THROW(VectorComposite("ab", {Vector3d(1, 2, 3), Vector1d(4), Vector2d(5, 6)}), std::runtime_error);
-}
-
TEST(VectorComposite, constructor_yaml)
{
VectorComposite u;
@@ -75,38 +64,15 @@ TEST(VectorComposite, constructor_yaml)
WOLF_INFO(node);
- VectorComposite v(node);
+ VectorComposite v(node, "", false);
- ASSERT_FALSE(v.empty());
+ WOLF_INFO(v);
+ ASSERT_EQ(u.getKeys(), v.getKeys());
ASSERT_MATRIX_APPROX(u['a'], v['a'], 1e-20);
ASSERT_MATRIX_APPROX(u['b'], v['b'], 1e-20);
}
-TEST(VectorComposite, set_vector_and_list)
-{
- VectorComposite v;
-
- v.set("ab", Vector4d(1, 2, 3, 4), {3, 1});
-
- ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1, 2, 3), 1e-20);
- ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
-
- ASSERT_THROW(v.set("cd", Vector4d(1, 2, 3, 4), {3, 2}), std::runtime_error);
-}
-
-TEST(VectorComposite, set_list_of_vectors)
-{
- VectorComposite v;
-
- v.set("ab", {Vector3d(1, 2, 3), Vector1d(4)});
-
- ASSERT_MATRIX_APPROX(v.at('a'), Vector3d(1, 2, 3), 1e-20);
- ASSERT_MATRIX_APPROX(v.at('b'), Vector1d(4), 1e-20);
-
- ASSERT_THROW(v.set("cd", {Vector3d(1, 2, 3)}), std::runtime_error);
-}
-
TEST(VectorComposite, operatorStream)
{
using namespace Eigen;
@@ -119,7 +85,7 @@ TEST(VectorComposite, operatorStream)
WOLF_DEBUG("X = ", x);
}
-TEST(VectorComposite, stateVector)
+TEST(VectorComposite, vector)
{
VectorComposite x;
@@ -144,9 +110,6 @@ int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // restrict to a group of tests only
- // ::testing::GTEST_FLAG(filter) = "VectorComposite.*";
-
// restrict to this test only
// ::testing::GTEST_FLAG(filter) = "MatrixComposite.propagate";
// ::testing::GTEST_FLAG(filter) = "MatrixComposite.*";
diff --git a/test/yaml/problem_autosetup.yaml b/test/yaml/problem_autosetup.yaml
index c853b11f2e68ddb0236313df0dffe1ccca69f1dc..bfbe8a7bd90d7467e98522d4602d274b67cf7f73 100644
--- a/test/yaml/problem_autosetup.yaml
+++ b/test/yaml/problem_autosetup.yaml
@@ -2,13 +2,15 @@ problem:
dimension: 3
first_frame:
P:
- state: [0,0,0]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
O:
- state: [0,0,0,1]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
tree_manager:
type: "None"
map:
@@ -26,12 +28,14 @@ sensors:
min_rot_var: 0.1
states:
P:
- state: [1,2,3]
- mode: fix
+ value: [1,2,3]
+ prior:
+ mode: fix
dynamic: false
O:
- state: [0,0,0,1]
- mode: "fix"
+ value: [0,0,0,1]
+ prior:
+ mode: "fix"
dynamic: false
processors:
-
diff --git a/test/yaml/problem_autosetup_no_map.yaml b/test/yaml/problem_autosetup_no_map.yaml
index 012c02ebc3dbf6a83a3fc53ba545e359fd50ced1..928c72aa172b7e980fa1b08818275be978acabce 100644
--- a/test/yaml/problem_autosetup_no_map.yaml
+++ b/test/yaml/problem_autosetup_no_map.yaml
@@ -2,13 +2,15 @@ problem:
dimension: 3
first_frame:
P:
- state: [0,0,0]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
O:
- state: [0,0,0,1]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
tree_manager:
type: "None"
sensors:
@@ -23,12 +25,14 @@ sensors:
min_rot_var: 0.1
states:
P:
- state: [1,2,3]
- mode: fix
+ value: [1,2,3]
+ prior:
+ mode: fix
dynamic: false
O:
- state: [0,0,0,1]
- mode: "fix"
+ value: [0,0,0,1]
+ prior:
+ mode: "fix"
dynamic: false
processors:
-
diff --git a/test/yaml/problem_map_2d_1.yaml b/test/yaml/problem_map_2d_1.yaml
index 6f16395757e50dd82ebbad9b09f0e48c674d73c2..b4a52ed041a806ec756c9ef4fc83291296e01709 100644
--- a/test/yaml/problem_map_2d_1.yaml
+++ b/test/yaml/problem_map_2d_1.yaml
@@ -2,11 +2,13 @@ problem:
dimension: 2
first_frame:
P:
- state: [0,0]
- mode: "fix"
+ value: [0,0]
+ prior:
+ mode: "fix"
O:
- state: [0]
- mode: "fix"
+ value: [0]
+ prior:
+ mode: "fix"
tree_manager:
type: "None"
sensors: []
diff --git a/test/yaml/problem_map_2d_2.yaml b/test/yaml/problem_map_2d_2.yaml
index 0865d31efb594dc50fcd2133c9173c206ce1d88a..549e8135f2de10541040b78f6692f0f6f19008c6 100644
--- a/test/yaml/problem_map_2d_2.yaml
+++ b/test/yaml/problem_map_2d_2.yaml
@@ -2,11 +2,13 @@ problem:
dimension: 2
first_frame:
P:
- state: [0,0]
- mode: "fix"
+ value: [0,0]
+ prior:
+ mode: "fix"
O:
- state: [0]
- mode: "fix"
+ value: [0]
+ prior:
+ mode: "fix"
tree_manager:
type: "None"
sensors: []
diff --git a/test/yaml/problem_map_3d_1.yaml b/test/yaml/problem_map_3d_1.yaml
index e7090dc3cf51136d2dcd43b1bdc3b3bf66ac7933..01864e6e382e63ab73a153f3cca1bab3055f78a3 100644
--- a/test/yaml/problem_map_3d_1.yaml
+++ b/test/yaml/problem_map_3d_1.yaml
@@ -2,11 +2,13 @@ problem:
dimension: 3
first_frame:
P:
- state: [0,0,0]
- mode: "fix"
+ value: [0,0,0]
+ prior:
+ mode: "fix"
O:
- state: [0,0,0,1]
- mode: "fix"
+ value: [0,0,0,1]
+ prior:
+ mode: "fix"
tree_manager:
type: "None"
sensors: []
diff --git a/test/yaml/problem_map_3d_2.yaml b/test/yaml/problem_map_3d_2.yaml
index 5e71123f0a4951fc977c22eaf6c78e3063bde130..d1ff2e8dde8ac2efcc8c508edd5cb3e97a667ced 100644
--- a/test/yaml/problem_map_3d_2.yaml
+++ b/test/yaml/problem_map_3d_2.yaml
@@ -2,11 +2,13 @@ problem:
dimension: 3
first_frame:
P:
- state: [0,0,0]
- mode: "fix"
+ value: [0,0,0]
+ prior:
+ mode: "fix"
O:
- state: [0,0,0,1]
- mode: "fix"
+ value: [0,0,0,1]
+ prior:
+ mode: "fix"
tree_manager:
type: "None"
sensors: []
diff --git a/test/yaml/problem_sliding_window1.yaml b/test/yaml/problem_sliding_window1.yaml
index 661e71c8822a7e0af4f56af2f611484bac3bcdab..a6dae57f5f6f5e45a81363557961cae91fb0b112 100644
--- a/test/yaml/problem_sliding_window1.yaml
+++ b/test/yaml/problem_sliding_window1.yaml
@@ -2,13 +2,15 @@ problem:
dimension: 3
first_frame:
P:
- mode: factor
- state: [0,0,0]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
O:
- mode: factor
- state: [0,0,0,1]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
tree_manager:
follow: "tree_manager_sliding_window1.yaml"
@@ -19,12 +21,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_sliding_window2.yaml b/test/yaml/problem_sliding_window2.yaml
index 0a82235fdde9715e2192c6c8f1e1963b5747dccf..4555bcf09a65515a6cd180b9b0624448a95170f4 100644
--- a/test/yaml/problem_sliding_window2.yaml
+++ b/test/yaml/problem_sliding_window2.yaml
@@ -2,13 +2,15 @@ problem:
dimension: 3
first_frame:
P:
- mode: factor
- state: [0,0,0]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
O:
- mode: factor
- state: [0,0,0,1]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
tree_manager:
follow: "tree_manager_sliding_window2.yaml"
@@ -19,12 +21,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_sliding_window_dual_rate1.yaml b/test/yaml/problem_sliding_window_dual_rate1.yaml
index 630c904970a24aab20fdffb111db74604b22c07c..69f708db9921d3b76bf1901083fcbc80ca2286d8 100644
--- a/test/yaml/problem_sliding_window_dual_rate1.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate1.yaml
@@ -2,13 +2,15 @@ problem:
dimension: 3
first_frame:
P:
- mode: factor
- state: [0,0,0]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
O:
- mode: factor
- state: [0,0,0,1]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
tree_manager:
follow: "tree_manager_sliding_window_dual_rate1.yaml"
@@ -19,12 +21,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_sliding_window_dual_rate2.yaml b/test/yaml/problem_sliding_window_dual_rate2.yaml
index 4fe6afe2150b4c8ae9b4292470e247edd83a4ab2..c6123bb126d15755a620490e067bb7f2efd16671 100644
--- a/test/yaml/problem_sliding_window_dual_rate2.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate2.yaml
@@ -2,13 +2,15 @@ problem:
dimension: 3
first_frame:
P:
- mode: factor
- state: [0,0,0]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
O:
- mode: factor
- state: [0,0,0,1]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
plugin: "core"
@@ -25,12 +27,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_sliding_window_dual_rate3.yaml b/test/yaml/problem_sliding_window_dual_rate3.yaml
index 3a2ea2bdd9248f0eb5932150ac26d046798350a5..9ccf2e0898c3ec2e5db9c498c44e9495eb7a521a 100644
--- a/test/yaml/problem_sliding_window_dual_rate3.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate3.yaml
@@ -5,13 +5,15 @@ problem:
dimension: 3
first_frame:
P:
- mode: factor
- state: [0,0,0]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
O:
- mode: factor
- state: [0,0,0,1]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
plugin: "core"
@@ -28,12 +30,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_sliding_window_dual_rate_baseline.yaml b/test/yaml/problem_sliding_window_dual_rate_baseline.yaml
index d5d4d6820a54122d36f45df380b78f33a08b1d7f..7f7f2c71b2fa689ef0910479d157f78fb806460e 100644
--- a/test/yaml/problem_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/problem_sliding_window_dual_rate_baseline.yaml
@@ -5,13 +5,15 @@ problem:
dimension: 3
first_frame:
P:
- mode: factor
- state: [0,0,0]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
O:
- mode: factor
- state: [0,0,0,1]
- noise_std: [0.31, 0.31, 0.31]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
tree_manager:
type: "None"
@@ -22,12 +24,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_tree_manager1.yaml b/test/yaml/problem_tree_manager1.yaml
index 7cd75e82515b754e4ffaef4ac50f100c1811600d..6027dada1825c7b627357f3f5c3991cada174502 100644
--- a/test/yaml/problem_tree_manager1.yaml
+++ b/test/yaml/problem_tree_manager1.yaml
@@ -2,17 +2,20 @@ problem:
dimension: 3
first_frame:
P:
- state: [0,0,0]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
O:
- state: [0,0,0,1]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
+ prior:
+ mode: factor
+ factor_std: [0.31, 0.31, 0.31]
V:
- state: [0,0,0]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
type: StateVector3d
tree_manager:
follow: "tree_manager_dummy.yaml"
@@ -24,12 +27,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/problem_tree_manager2.yaml b/test/yaml/problem_tree_manager2.yaml
index 36b41078613b3da0bceb18d2a0e31db3282f69d4..e5b30f4e4aae70ac9370bfdb907a0f8c09fd7552 100644
--- a/test/yaml/problem_tree_manager2.yaml
+++ b/test/yaml/problem_tree_manager2.yaml
@@ -2,17 +2,20 @@ problem:
dimension: 3
first_frame:
P:
- state: [0,0,0]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
O:
- state: [0,0,0,1]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0,1]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
V:
- state: [0,0,0]
- mode: "factor"
- noise_std: [0.31, 0.31, 0.31]
+ value: [0,0,0]
+ prior:
+ mode: "factor"
+ factor_std: [0.31, 0.31, 0.31]
type: StateVector3d
tree_manager:
type: "None"
@@ -24,12 +27,14 @@ sensors:
plugin: "core"
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0, 0, 0, 1]
+ prior:
+ mode: fix
+ value: [0, 0, 0, 1]
dynamic: false
k_disp_to_disp: 0.1
k_disp_to_rot: 0.1
diff --git a/test/yaml/processor_landmark_external.yaml b/test/yaml/processor_landmark_external.yaml
index db4cd0149fc6bd310016206843cae5a85ba9e299..c97236f5a495ec6cfaf8f88494bf351538045548 100644
--- a/test/yaml/processor_landmark_external.yaml
+++ b/test/yaml/processor_landmark_external.yaml
@@ -4,18 +4,21 @@ plugin: core
keyframe_vote:
voting_active: true
- min_features_for_keyframe: 1
time_span: 10
+ min_features_for_keyframe: 1
new_features_for_keyframe: 1000
time_tolerance: 0.05
-max_new_features: -1
-voting_active: true
apply_loss_function: false
+max_new_features: -1
+
use_orientation: true
-match_dist_th: 0.5
-min_features_for_keyframe: 1
+match_dist_th_id: 0.5
+match_dist_th_type: 0.5
+match_dist_th_unknown: 0.5
+
+track_length_th: 10
+quality_th: 0.5
-filter:
- track_length_th: 10
- quality_th: 0.5
\ No newline at end of file
+close_loops_by_id: true
+close_loops_by_type: true
\ No newline at end of file
diff --git a/test/yaml/sensor_diff_drive.yaml b/test/yaml/sensor_diff_drive.yaml
index ab6d5923687f288023723aa6bf67b6b1eba8f351..2a2f41870befccd2c22e13002fc51daac5077d39 100644
--- a/test/yaml/sensor_diff_drive.yaml
+++ b/test/yaml/sensor_diff_drive.yaml
@@ -4,16 +4,19 @@ type: SensorDiffDrive
plugin: core
states:
P:
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: false
O:
- mode: fix
- state: [3]
+ prior:
+ mode: fix
+ value: [3]
dynamic: false
I:
- mode: fix
- state: [0.1, 0.2, 0.3]
+ prior:
+ mode: fix
+ value: [0.1, 0.2, 0.3]
dynamic: false
ticks_per_wheel_revolution: 4
diff --git a/test/yaml/sensor_diff_drive_dynamic.yaml b/test/yaml/sensor_diff_drive_dynamic.yaml
index 65bb57822633437496ed81ea7da752ed833ff69b..d185e701c3ddc840fede78d9061b07e24aa0953b 100644
--- a/test/yaml/sensor_diff_drive_dynamic.yaml
+++ b/test/yaml/sensor_diff_drive_dynamic.yaml
@@ -3,16 +3,19 @@ type: SensorDiffDrive
plugin: core
states:
P:
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: true
O:
- mode: fix
- state: [3]
+ prior:
+ mode: fix
+ value: [3]
dynamic: true
I:
- mode: fix
- state: [0.1, 0.2, 0.3]
+ prior:
+ mode: fix
+ value: [0.1, 0.2, 0.3]
dynamic: true
ticks_per_wheel_revolution: 4
diff --git a/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml b/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml
index ca47c67a16afd1e1af4265e11ebcdd9ca30af8cd..94d5cd77f1ffefdc6bdf7a8287219eeadaa09fc3 100644
--- a/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml
+++ b/test/yaml/sensor_diff_drive_gtest_factor_diff_drive.yaml
@@ -4,16 +4,19 @@ type: SensorDiffDrive
plugin: core
states:
P:
- mode: fix
- state: [0, 0]
+ prior:
+ mode: fix
+ value: [0, 0]
dynamic: false
O:
- mode: fix
- state: [0]
+ prior:
+ mode: fix
+ value: [0]
dynamic: false
I:
- mode: initial_guess
- state: [1, 1, 1]
+ prior:
+ mode: initial_guess
+ value: [1, 1, 1]
dynamic: false
ticks_per_wheel_revolution: 100
diff --git a/test/yaml/sensor_diff_drive_gtest_problem.yaml b/test/yaml/sensor_diff_drive_gtest_problem.yaml
index 74107a1d9a66b1707b25740cbf2883297bac7291..b08147ab43a7d0325f623b5f3919836ea6bb3a92 100644
--- a/test/yaml/sensor_diff_drive_gtest_problem.yaml
+++ b/test/yaml/sensor_diff_drive_gtest_problem.yaml
@@ -4,16 +4,19 @@ type: SensorDiffDrive
plugin: core
states:
P:
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: false
O:
- mode: fix
- state: [3]
+ prior:
+ mode: fix
+ value: [3]
dynamic: false
I:
- mode: initial_guess
- state: [0.1, 0.2, 0.3]
+ prior:
+ mode: initial_guess
+ value: [0.1, 0.2, 0.3]
dynamic: true
ticks_per_wheel_revolution: 4
diff --git a/test/yaml/sensor_dummy_2d.yaml b/test/yaml/sensor_dummy_2d.yaml
index 8bd7fcfa66e246047706e9b47cee5b69e5892de2..91fb661280740db834f503b613ae020b2b9c0316 100644
--- a/test/yaml/sensor_dummy_2d.yaml
+++ b/test/yaml/sensor_dummy_2d.yaml
@@ -1,4 +1,4 @@
# DO NOT MODIFY THIS FILE, USED BY gtest_processor_base, gtest_problem
follow: sensor_tests/sensor_PO_2D_fix.yaml
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
\ No newline at end of file
diff --git a/test/yaml/sensor_dummy_3d.yaml b/test/yaml/sensor_dummy_3d.yaml
index 38f009802e242c2af8e3ccb3d558e4cf74cf5e82..57c425adb7de50c958d2a1df566e4b8aeb761a54 100644
--- a/test/yaml/sensor_dummy_3d.yaml
+++ b/test/yaml/sensor_dummy_3d.yaml
@@ -1,4 +1,4 @@
# DO NOT MODIFY THIS FILE, USED BY gtest_problem
follow: sensor_tests/sensor_PO_3D_fix.yaml
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
index 9507b9c4edec0e041fbb36e920250e0a0e138acf..926b769005972af716ff917a0fa89d4e47aefa71 100644
--- a/test/yaml/sensor_odom_2d.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -4,12 +4,14 @@ type: SensorOdom2d
plugin: core
states:
P:
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: false
O:
- mode: fix
- state: [3]
+ prior:
+ mode: fix
+ value: [3]
dynamic: false
k_disp_to_disp: 0.1
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
index 2a703a9680b5454dcf7f2e17c125c22ab0087db6..90cee85aa93d42b5928f0f6dd488057bb3879ac9 100644
--- a/test/yaml/sensor_odom_3d.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -4,12 +4,14 @@ type: SensorOdom3d
plugin: core
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
dynamic: false
k_disp_to_disp: 0.1 # m^2 / m
diff --git a/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml b/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml
index 9eaf70d6b192d68639d583ec8b5e2d3ca09d2dc6..58ab3eae66f254a19931af9dce474897d5de6479 100644
--- a/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml
+++ b/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml
@@ -4,12 +4,14 @@ type: SensorOdom3d
plugin: core
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
dynamic: false
O:
- mode: initial_guess
- state: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0, 0]
dynamic: false
k_disp_to_disp: 0.02
diff --git a/test/yaml/sensor_pose_2d.yaml b/test/yaml/sensor_pose_2d.yaml
index 09c0fd3c6fa89be07c7666629175e66ace61f602..efed1135011e967b4ad1d56d6626b8868ff4c8aa 100644
--- a/test/yaml/sensor_pose_2d.yaml
+++ b/test/yaml/sensor_pose_2d.yaml
@@ -3,12 +3,14 @@ type: SensorPose2d
plugin: core
states:
P:
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: false
O:
- mode: fix
- state: [3]
+ prior:
+ mode: fix
+ value: [3]
dynamic: false
std_noise: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_pose_3d.yaml b/test/yaml/sensor_pose_3d.yaml
index 21213669d432a5eb63380e64a41b39e053b8b19d..c264fece797d36a6d3d4485fb79ca734b599fcc5 100644
--- a/test/yaml/sensor_pose_3d.yaml
+++ b/test/yaml/sensor_pose_3d.yaml
@@ -3,12 +3,14 @@ type: SensorPose3d
plugin: core
states:
P:
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
O:
- mode: fix
- state: [0.5, 0.5, 0.5, 0.5]
+ prior:
+ mode: fix
+ value: [0.5, 0.5, 0.5, 0.5]
dynamic: false
std_noise: [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
diff --git a/test/yaml/sensor_pose_3d_initial_guess.yaml b/test/yaml/sensor_pose_3d_initial_guess.yaml
index a43f0872fe092173b3757db66c90549a61a67bfe..6d509fe5a09ddadf2975bf41afabce243f73c785 100644
--- a/test/yaml/sensor_pose_3d_initial_guess.yaml
+++ b/test/yaml/sensor_pose_3d_initial_guess.yaml
@@ -4,12 +4,14 @@ type: SensorPose3d
plugin: core
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
dynamic: false
O:
- mode: initial_guess
- state: [0.5, 0.5, 0.5, 0.5]
+ prior:
+ mode: initial_guess
+ value: [0.5, 0.5, 0.5, 0.5]
dynamic: false
std_noise: [1, 1, 1, 1, 1, 1]
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D.yaml b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
index a83753768b06d0bf2f931e9a357b722a7b77853e..ebc7273ed6836a72b43459334186ad1b29e730a8 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
@@ -5,23 +5,27 @@ plugin: core
# sensor_POIA_3D:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: false
I:
- mode: initial_guess
- state: [1, 2, 3, 4, 5]
+ value: [1, 2, 3, 4, 5]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4, 0.5]
A:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
index 7d0bec3688282d8fb3c3313f4f57eb34658fe4a1..1a123a78cb6ceaa1e036fae590062364085f9f59 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
@@ -5,23 +5,28 @@ plugin: core
# sensor_POIA_3D_wrong:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: false
I:
- mode: initial_guess
- state: [1, 2, 3, 4, 5]
+ value: [1, 2, 3, 4, 5]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4, 0.5]
+
# A:
- # mode: factor
- # state: [1, 0, 0, 0]
- # noise_std: [0.1, 0.2, 0.3]
+ # value: [1, 0, 0, 0]
+ # prior:
+ # mode: factor
+ # factor_std: [0.1, 0.2, 0.3]
# dynamic: true
# drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
index d73aee465e260001c608ac0d94d0468abde1f72b..0b0738800f09c371d9fb55a1e0a1841a8bc7e728 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_factor:
states:
P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: false
O:
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
index 742ff14de44764dc0368ea2da992bbbc9d8b036d..ee4d897b0459974e2a4565093a6e35343447bfa0 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_factor_dynamic:
states:
P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: true
O:
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
index f4924a2ad8589bc121d22aecf3ba30857f90eed8..727603e41be34bb9cc7bd6eafdbe5c1d804b51b0 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
@@ -1,19 +1,21 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_factor_dynamic_drift:
states:
P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: true
drift_std: [0.1, 0.2]
O:
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: true
drift_std: [0.1]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
index 8dc1324d9ab98fadf19fb7c172f717040de953c7..5c86243f0807abe0909364a1ce352a778a64a9ee 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
@@ -1,19 +1,21 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_factor_dynamic_drift_wrong:
states:
P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: true
drift_std: [0.1] #wrong size
O:
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: true
drift_std: [0.1]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
index 4b213e8ceeea2808bc97d4e7a994afd0e32fd32a..c3965321e4e64b3529b42513c960bfed6904be90 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_factor_dynamic_wrong:
states:
P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] #wrong size
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] #wrong size
dynamic: true
O:
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
index 507a3d3aabba3e20519153ee4864682928dedd0a..5bf6795887887e336381bd34713f6980fe76420a 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_factor_wrong:
states:
P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: false
O:
- mode: factor
- #state: [1] #missing
- noise_std: [0.1]
+ #value: [1] #missing
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
index 6dfa031fc5e172a322f2af09b78a1c7c1120d768..f67a51f42e56186475b22bf0ac089872e427bb66 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_fix:
states:
P:
- mode: fix
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: fix
dynamic: false
O:
- mode: fix
- state: [1]
+ value: [1]
+ prior:
+ mode: fix
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
index 5e1ae6891c5505ce22ae6b53d94fc1e80562dda1..71bedae44066256ba3ddfac83b399ba8df62841c 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_fix_dynamic:
states:
P:
- mode: fix
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: fix
dynamic: true
O:
- mode: fix
- state: [1]
+ value: [1]
+ prior:
+ mode: fix
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
index 3e5d903a6a145e851e9319beebba328c4b4f144f..8bf8e62d590dda6be00cc8b119e89cdb9eb49dfa 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_fix_dynamic_drift:
states:
P:
- mode: fix
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1, 0.2]
O:
- mode: fix
- state: [1]
+ value: [1]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
index 3b9c521d59db6c75dbb58a03bc7c514c93decb6c..6c469baaa46ce7a37335603fe3b5aac2874ff791 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_fix_dynamic_drift_wrong:
states:
P:
- mode: fix
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1, 0.2, 0.3] #wrong size
O:
- mode: fix
- state: [1]
+ value: [1]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
index 7bb1eac026ee35e724b1023426fd8d4ca879ec9f..2996004fa1e6bed6bda47082208a9054e98c8749 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_fix_dynamic_wrong:
states:
P:
- mode: fix
- state: [1, 2, 3] # wrong size
+ value: [1, 2, 3] # wrong size
+ prior:
+ mode: fix
dynamic: true
O:
- mode: fix
- state: [1]
+ value: [1]
+ prior:
+ mode: fix
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
index cfff96ed3fad7df917633fce68760c12eb2c1660..6353af311e551e49b6d9c88db4acecdacab25f4e 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_fix_wrong:
states:
P:
- mode: fix
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: fix
#dynamic: false #missing
O:
- mode: fix
- state: [1]
+ value: [1]
+ prior:
+ mode: fix
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
index 1de932db25e8fe6ccccd924b0a934a9538c44c4d..0676a2e527251ffc5e45c70097d36a6cdb3732ed 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_initial_guess:
states:
P:
- mode: initial_guess
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: initial_guess
dynamic: false
O:
- mode: initial_guess
- state: [1]
+ value: [1]
+ prior:
+ mode: initial_guess
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
index 180222310fec191b959b7391f53f84321f1451e0..a3ebd61307613ba9a946cc8472be60659888dd9d 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_initial_guess_dynamic:
states:
P:
- mode: initial_guess
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: initial_guess
dynamic: true
O:
- mode: initial_guess
- state: [1]
+ value: [1]
+ prior:
+ mode: initial_guess
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
index 813af78c07dc1c84cd90f1f5bb3c1c1a773f5c67..2d2bf7ff6160cec9ff69075216613079caeeef14 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_initial_guess_dynamic_drift:
states:
P:
- mode: initial_guess
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2]
O:
- mode: initial_guess
- state: [1]
+ value: [1]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
index 2138d6028d4b93e9facf158ae6c3318339472eee..b0e7a59339f6b99b2a7304a5056198a46be958b9 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_initial_guess_dynamic_drift_wrong:
states:
P:
- mode: initial_guess
- state: [1] #wrong size
+ value: [1] #wrong size
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2]
O:
- mode: initial_guess
- state: [1]
+ value: [1]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1]
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
index 9404a436c9f8c0b128212cab6ef23b0c83898a40..c919e824328af91437678dd605a210715827a298 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
@@ -1,16 +1,17 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_initial_guess_dynamic_wrong:
states:
# P: #missing
+ # value: [1, 2]
# mode: initial_guess
- # state: [1, 2]
# dynamic: true
O:
- mode: initial_guess
- state: [1]
+ value: [1]
+ prior:
+ mode: initial_guess
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
index 8c720c52bcea16206d106df90c61d91d7a6325c0..3ce46566aa503081e7a380008f381bd173c4f046 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
@@ -1,16 +1,17 @@
name: sensor_1
-type: SensorDummy2d
+type: SensorDummyPo2d
plugin: core
# sensor_PO_2D_initial_guess_wrong:
states:
P:
- mode: initial_guess
- state: [1, 2]
+ value: [1, 2]
+ prior:
+ mode: initial_guess
dynamic: false
O:
+ value: [1]
#mode: initial_guess #missing
- state: [1]
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
index c23e6c8c8db3b4367b699b6802e1675a6f26fdb8..d4f7535676d91292a6cb5d553f3c565a5b52b72a 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_factor:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: false
O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
index 5ba32eda99ea58d25e6a3d7c83fee37dd6f3e6c9..6b1a14d67432fb01ad9b317616a146636776a220 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_factor_dynamic:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
index 02004a58dba1d173979b31847267df723c5e5548..c3d93ebd5931ed8936af74aa2304a4bf47dd2dc9 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
@@ -1,19 +1,21 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_factor_dynamic_drift:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
index 7503806786a2bcc360d370bbae37fcbf9d0128d0..a5a30fb447050c06fb9c9fdfaa92031075380408 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
@@ -1,19 +1,21 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_factor_dynamic_drift_wrong:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4] #wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
index cc0b0c00d8de81de69b6cc504e64459b09adafc0..9d1d8c7b83275e929928ddb1c924cd8a801bf6c1 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_factor_dynamic_wrong:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4] # wrong size
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
index 5a8349fed0ce860ef7c3090d37d343e879701ab0..f1a2572bbdccc201137b621f9b9a96ac013fad98 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
@@ -1,18 +1,20 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
#plugin: core #missing
# sensor_PO_3D_factor_wrong:
states:
P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: false
O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
index 42ad3870d3edc92121cd99506a9ea967813b2462..2b6c54a5343f81294b60a65f849aa43e1f07faa4 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_fix:
states:
P:
- mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: fix
dynamic: false
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
index 23ef0b27e6ac2fe83244c08c85606f5714aa4588..267ff1150d280bfc30c3a58860cdfd8c9e2ebe07 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_fix_dynamic:
states:
P:
- mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: fix
dynamic: true
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
index b7428cac6f4eb59375a2dbe52cda3a5e4298e4d8..7fcf97397560e8282be9ac4311104cfdc4788475 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_fix_dynamic_drift:
states:
P:
- mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
index f323e9ad23c898d44946d2b21e2112b616c503ed..32ee7f8589e74d6f3f894b1d1c49e461b664fc21 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-#type: SensorDummy3d #missing
+#type: SensorDummyPo3d #missing
plugin: core
# sensor_PO_3D_fix_dynamic_drift_wrong:
states:
P:
- mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
index bcec87c68fa1bc2a04d69e800acd5056da791d00..59f0da776c97fa32f4d41389c11513d3841a0a79 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_fix_dynamic_wrong:
states:
P:
- mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: fix
dynamic: true
O:
- mode: fix
- state: [1, 0, 0, 1] # not normalized
+ value: [1, 0, 0, 1] # not normalized
+ prior:
+ mode: fix
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
index 1339c6908b632517f0f79b1f6d957c4578862174..fc36a57d6f4e10af60e3fd9bf79cc3d60248bc30 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_fix_wrong:
states:
P:
- mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: fix
dynamic: false
O:
- mode: fix
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: fix
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
index 6e05a257d62fb7811f9301996d50ae49f0b59c97..2a3689a3b9c03f7c59b26219fcfdf30d9150a138 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_initial_guess:
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: initial_guess
dynamic: false
O:
- mode: initial_guess
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
index 32cc1869dd1ea41e49ee3f11395625bc59d6cfa2..22e59d643a2a76cad2dd9583359b57cdc40bafee 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_initial_guess_dynamic:
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: initial_guess
dynamic: true
O:
- mode: initial_guess
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
index 8977dd9f5aa2a20ca5e1e8a342a7acbd115980aa..d8656ffa6466d5cb1582d0b6367ce8e3d9942b09 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
@@ -1,17 +1,19 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_initial_guess_dynamic_drift:
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O:
- mode: initial_guess
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
index 8518582da18420363d253d214eeb1d8f9825fb00..fec6833ae720aa595547d1648d4fb7ce7b49cf8d 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
@@ -1,17 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_initial_guess_dynamic_drift_wrong:
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: initial_guess
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O:
+ value: [1, 0, 0, 0]
#mode: initial_guess #missing
- state: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
index 999c693a12cc0d8af25a27f90bc4a45ed7b18602..9d177d7faa6ca0d307f7aca573b76243697357ee 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
@@ -1,16 +1,18 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_initial_guess_dynamic_wrong:
states:
P:
- mode: initial_guess
- state: [1, 2] # wrong size
+ value: [1, 2] # wrong size
+ prior:
+ mode: initial_guess
dynamic: true
O:
- mode: initial_guess
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
dynamic: true
# used in gtest_sensor_base
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
index 33d1bf439e22c09dc247b7b0b814a59e35963429..f5552f33ccb4015cba4c5b601200f6339cde0257 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
@@ -1,16 +1,17 @@
name: sensor_1
-type: SensorDummy3d
+type: SensorDummyPo3d
plugin: core
# sensor_PO_3D_initial_guess_wrong:
states:
P:
- mode: initial_guess
- state: [1, 2, 3]
+ value: [1, 2, 3]
+ prior:
+ mode: initial_guess
dynamic: false
# O: #missing
+ # value: [1, 0, 0, 0]
# mode: initial_guess
- # state: [1, 0, 0, 0]
# dynamic: false
# used in gtest_sensor_base
diff --git a/test/yaml/spec_state.yaml b/test/yaml/spec_state.yaml
index 2589f71ed2ccccf51332e20dedd89c31aef389b2..421343ff445cb4d95b56d9e9b8672d1c5e560109 100644
--- a/test/yaml/spec_state.yaml
+++ b/test/yaml/spec_state.yaml
@@ -1,84 +1,99 @@
# P in 2D
P_2D_initial_guess:
type: StatePoint2d
- mode: initial_guess
- state: [1, 2]
+ prior:
+ mode: initial_guess
+ value: [1, 2]
P_2D_fix:
type: StatePoint2d
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
P_2D_factor:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
# P in 3D
P_3D_initial_guess:
type: StatePoint3d
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
P_3D_fix:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
P_3D_factor:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
# O in 2D
O_2D_initial_guess:
type: StateAngle
- mode: initial_guess
- state: [1]
+ prior:
+ mode: initial_guess
+ value: [1]
O_2D_fix:
type: StateAngle
- mode: fix
- state: [1]
+ prior:
+ mode: fix
+ value: [1]
O_2D_factor:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
# O in 3D
O_3D_initial_guess:
type: StateQuaternion
- mode: initial_guess
- state: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0, 0]
O_3D_fix:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
O_3D_factor:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
# I
I_initial_guess:
type: StateParams4
- mode: initial_guess
- state: [1, 2, 3, 4]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3, 4]
I_fix:
type: StateParams4
- mode: fix
- state: [1, 2, 3, 4]
+ prior:
+ mode: fix
+ value: [1, 2, 3, 4]
I_factor:
type: StateParams4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file
diff --git a/test/yaml/spec_state_sensor.yaml b/test/yaml/spec_state_sensor.yaml
index e6cea5317bc1f90ba67a948c465b31c31f1a78e1..88148dd2566632e358d6e850a84a43aa7028d555 100644
--- a/test/yaml/spec_state_sensor.yaml
+++ b/test/yaml/spec_state_sensor.yaml
@@ -1,304 +1,349 @@
# P in 2D
P_2D_initial_guess:
type: StatePoint2d
- mode: initial_guess
- state: [1, 2]
+ prior:
+ mode: initial_guess
+ value: [1, 2]
dynamic: false
P_2D_fix:
type: StatePoint2d
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: false
P_2D_factor:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: false
P_2D_initial_guess_dynamic_drift:
type: StatePoint2d
- mode: initial_guess
- state: [1, 2]
+ prior:
+ mode: initial_guess
+ value: [1, 2]
dynamic: true
drift_std: [0.1, 0.2]
P_2D_fix_dynamic_drift:
type: StatePoint2d
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: true
drift_std: [0.1, 0.2]
P_2D_factor_dynamic_drift:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: true
drift_std: [0.1, 0.2]
P_2D_initial_guess_dynamic:
type: StatePoint2d
- mode: initial_guess
- state: [1, 2]
+ prior:
+ mode: initial_guess
+ value: [1, 2]
dynamic: true
P_2D_fix_dynamic:
type: StatePoint2d
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: true
P_2D_factor_dynamic:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2]
dynamic: true
# P in 3D
P_3D_initial_guess:
type: StatePoint3d
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
dynamic: false
P_3D_fix:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: false
P_3D_factor:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: false
P_3D_initial_guess_dynamic_drift:
type: StatePoint3d
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
P_3D_fix_dynamic_drift:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
P_3D_factor_dynamic_drift:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
P_3D_initial_guess_dynamic:
type: StatePoint3d
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
dynamic: true
P_3D_fix_dynamic:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
dynamic: true
P_3D_factor_dynamic:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
# O in 2D
O_2D_initial_guess:
type: StateAngle
- mode: initial_guess
- state: [1]
+ prior:
+ mode: initial_guess
+ value: [1]
dynamic: false
O_2D_fix:
type: StateAngle
- mode: fix
- state: [1]
+ prior:
+ mode: fix
+ value: [1]
dynamic: false
O_2D_factor:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: false
O_2D_initial_guess_dynamic_drift:
type: StateAngle
- mode: initial_guess
- state: [1]
+ prior:
+ mode: initial_guess
+ value: [1]
dynamic: true
drift_std: [0.1]
O_2D_fix_dynamic_drift:
type: StateAngle
- mode: fix
- state: [1]
+ prior:
+ mode: fix
+ value: [1]
dynamic: true
drift_std: [0.1]
O_2D_factor_dynamic_drift:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: true
drift_std: [0.1]
O_2D_initial_guess_dynamic:
type: StateAngle
- mode: initial_guess
- state: [1]
+ prior:
+ mode: initial_guess
+ value: [1]
dynamic: true
O_2D_fix_dynamic:
type: StateAngle
- mode: fix
- state: [1]
+ prior:
+ mode: fix
+ value: [1]
dynamic: true
O_2D_factor_dynamic:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1]
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1]
dynamic: true
# O in 3D
O_3D_initial_guess:
type: StateQuaternion
- mode: initial_guess
- state: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0, 0]
dynamic: false
O_3D_fix:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
dynamic: false
O_3D_factor:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: false
O_3D_initial_guess_dynamic_drift:
type: StateQuaternion
- mode: initial_guess
- state: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O_3D_fix_dynamic_drift:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O_3D_factor_dynamic_drift:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O_3D_initial_guess_dynamic:
type: StateQuaternion
- mode: initial_guess
- state: [1, 0, 0, 0]
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0, 0]
dynamic: true
O_3D_fix_dynamic:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
dynamic: true
O_3D_factor_dynamic:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
# I
I_initial_guess:
type: StateParams4
- mode: initial_guess
- state: [1, 2, 3, 4]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3, 4]
dynamic: false
I_fix:
type: StateParams4
- mode: fix
- state: [1, 2, 3, 4]
+ prior:
+ mode: fix
+ value: [1, 2, 3, 4]
dynamic: false
I_factor:
type: StateParams4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4]
dynamic: false
I_initial_guess_dynamic_drift:
type: StateParams4
- mode: initial_guess
- state: [1, 2, 3, 4]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3, 4]
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4]
I_fix_dynamic_drift:
type: StateParams4
- mode: fix
- state: [1, 2, 3, 4]
+ prior:
+ mode: fix
+ value: [1, 2, 3, 4]
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4]
I_factor_dynamic_drift:
type: StateParams4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4]
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4]
I_initial_guess_dynamic:
type: StateParams4
- mode: initial_guess
- state: [1, 2, 3, 4]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3, 4]
dynamic: true
I_fix_dynamic:
type: StateParams4
- mode: fix
- state: [1, 2, 3, 4]
+ prior:
+ mode: fix
+ value: [1, 2, 3, 4]
dynamic: true
I_factor_dynamic:
type: StateParams4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4]
dynamic: true
\ No newline at end of file
diff --git a/test/yaml/spec_state_sensor_wrong.yaml b/test/yaml/spec_state_sensor_wrong.yaml
index 41ad00608ffd70aaeaaea4d447d6d22f91e7cf05..7e4b707f69f6cd01d640aa129ac73fa13f9a1479 100644
--- a/test/yaml/spec_state_sensor_wrong.yaml
+++ b/test/yaml/spec_state_sensor_wrong.yaml
@@ -1,245 +1,280 @@
# P in _2D_
P_2D_initial_guess:
type: StatePoint2d
- mode: initial_guess
- #state: [1, 2] # missing
+ prior:
+ mode: initial_guess
+ #value: [1, 2] # missing
dynamic: false
P_2D_fix:
type: StatePoint2d
- mode: fix
- #state: [1, 2] # missing
+ prior:
+ mode: fix
+ #value: [1, 2] # missing
dynamic: false
P_2D_factor:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] # wrong size
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] # wrong size
dynamic: false
P_2D_initial_guess_dynamic:
type: StatePoint2d
- mode: initial_guess
- state: [1, 2]
+ prior:
+ mode: initial_guess
+ value: [1, 2]
#dynamic: true #missing
P_2D_fix_dynamic:
type: StatePoint2d
- mode: fix
- state: [1] # wrong size
+ prior:
+ mode: fix
+ value: [1] # wrong size
dynamic: true
P_2D_factor_dynamic:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- #noise_std: [0.1, 0.2] # missing
+ value: [1, 2]
+ prior:
+ mode: factor
+ # factor_std: [0.1, 0.2] # missing
dynamic: true
P_2D_initial_guess_dynamic_drift:
type: StatePoint2d
- mode: initial_guess
- state: [1, 2]
+ prior:
+ mode: initial_guess
+ value: [1, 2]
dynamic: true
drift_std: [0.1, 0.2, 0.3] # wrong size
P_2D_fix_dynamic_drift:
type: StatePoint2d
- mode: fix
- state: [1, 2]
+ prior:
+ mode: fix
+ value: [1, 2]
dynamic: true
drift_std: [0.1] # wrong size
P_2D_factor_dynamic_drift:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- #noise_std: [0.1, 0.2] # missing
+ value: [1, 2]
+ prior:
+ mode: factor
+ # factor_std: [0.1, 0.2] # missing
dynamic: true
drift_std: [0.1, 0.2]
# P in _3D_
P_3D_initial_guess:
type: StatePoint3d
- #mode: initial_guess # missing
- state: [1, 2, 3]
+ #prior:
+ # mode: initial_guess # missing
+ value: [1, 2, 3]
dynamic: false
P_3D_fix:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3, 4] # wrong size
+ prior:
+ mode: fix
+ value: [1, 2, 3, 4] # wrong size
dynamic: false
P_3D_factor:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
dynamic: false
P_3D_initial_guess_dynamic:
type: StatePoint3d
- mode: initial_guess
- state: [1, 2, 3, 4] # wrong size
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3, 4] # wrong size
dynamic: true
P_3D_fix_dynamic:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3]
+ prior:
+ mode: fix
+ value: [1, 2, 3]
# dynamic: true # missing
P_3D_factor_dynamic:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3]
P_3D_initial_guess_dynamic_drift:
type: StatePoint3d
- mode: initial_guess
- state: [1, 2, 3]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3]
dynamic: true
drift_std: [0.1, 0.2] # wrong size
P_3D_fix_dynamic_drift:
type: StatePoint3d
#mode: fix
- state: [1, 2, 3]
+ value: [1, 2, 3]
dynamic: true
drift_std: [0.1, 0.2]
P_3D_factor_dynamic_drift:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3]
# O in _2D_
O_2D_initial_guess:
type: StateAngle
- mode: initial_guess
- state: [1, 2] # wrong size
+ prior:
+ mode: initial_guess
+ value: [1, 2] # wrong size
dynamic: false
O_2D_fix:
type: StateAngle
- mode: fix
- state: [1 2] # wrong size
+ prior:
+ mode: fix
+ value: [1 2] # wrong size
dynamic: false
O_2D_factor:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
dynamic: false
O_2D_initial_guess_dynamic:
type: StateAngle
- mode: initial_guess
- state: [1, 2] #wrong size
+ prior:
+ mode: initial_guess
+ value: [1, 2] #wrong size
dynamic: true
O_2D_fix_dynamic:
type: StateAngle
- mode: fix
- state: [1]
+ prior:
+ mode: fix
+ value: [1]
# dynamic: true # missing
O_2D_factor_dynamic:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1 0.2] # wrong size
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1 0.2] # wrong size
dynamic: true
O_2D_initial_guess_dynamic_drift:
type: StateAngle
- mode: initial_guess
- state: [1]
+ prior:
+ mode: initial_guess
+ value: [1]
dynamic: true
drift_std: [0.1, 0.2] # wrong size
O_2D_fix_dynamic_drift:
type: StateAngle
- mode: fix
- state: [1 2] # wrong size
+ prior:
+ mode: fix
+ value: [1 2] # wrong size
dynamic: true
drift_std: [0.1]
O_2D_factor_dynamic_drift:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1 0.2] # wrong size
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1 0.2] # wrong size
dynamic: true
drift_std: [0.1]
# O in _3D_
O_3D_initial_guess:
type: StateQuaternion
- mode: initial_guess
- #state: [1, 0, 0, 0] # missing
+ prior:
+ mode: initial_guess
+ #value: [1, 0, 0, 0] # missing
dynamic: false
O_3D_fix:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
#dynamic: false # missing
O_3D_factor:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
dynamic: false
O_3D_initial_guess_dynamic:
type: StateQuaternion
- mode: initial_guess
- state: [1, 0, 0] # wrong size
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0] # wrong size
dynamic: true
O_3D_fix_dynamic:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
#dynamic: true
O_3D_factor_dynamic:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3, 0.4] # wrong size
dynamic: true
O_3D_initial_guess_dynamic_drift:
type: StateQuaternion
- mode: initial_guess
- state: [1, 0, 0] # wrong size
+ prior:
+ mode: initial_guess
+ value: [1, 0, 0] # wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3]
O_3D_fix_dynamic_drift:
type: StateQuaternion
- mode: fix
- state: [1, 0, 0, 0]
+ prior:
+ mode: fix
+ value: [1, 0, 0, 0]
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4] #wrong size
O_3D_factor_dynamic_drift:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3]
dynamic: true
drift_std: [0.1, 0.2] # wrong size
@@ -247,59 +282,66 @@ O_3D_factor_dynamic_drift:
I_initial_guess:
type: StateParam4
#mode: initial_guess # missing
- state: [1, 2, 3, 4]
+ value: [1, 2, 3, 4]
dynamic: false
I_fix:
type: StateParam4
- mode: fix
- #state: [1, 2, 3, 4] # missing
+ prior:
+ mode: fix
+ #value: [1, 2, 3, 4] # missing
dynamic: false
I_factor:
type: StateParam4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] # wrong size
dynamic: false
I_initial_guess_dynamic:
type: StateParam4
#mode: initial_guess # missing
- state: [1, 2, 3, 4]
+ value: [1, 2, 3, 4]
dynamic: true
I_fix_dynamic:
type: StateParam4
- mode: fix
- state: [] # wrong size
+ prior:
+ mode: fix
+ value: [] # wrong size
dynamic: true
I_factor_dynamic:
type: StateParam4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] # wrong size
dynamic: true
I_initial_guess_dynamic_drift:
type: StateParam4
- mode: initial_guess
- state: [1, 2, 3, 4]
+ prior:
+ mode: initial_guess
+ value: [1, 2, 3, 4]
dynamic: true
drift_std: [0.1, 0.2, 0.3] # wrong size
I_fix_dynamic_drift:
type: StateParam4
- mode: fix
- state: [1, 2, 3] # wrong size
+ prior:
+ mode: fix
+ value: [1, 2, 3] # wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4]
I_factor_dynamic_drift:
type: StateParam4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] # wrong size
dynamic: true
drift_std: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file
diff --git a/test/yaml/spec_state_wrong.yaml b/test/yaml/spec_state_wrong.yaml
index b7eb6c7340c0c853fc580850dab912552be145f5..fa97949c1a311d97bd6b82cf7fad9a49ae846010 100644
--- a/test/yaml/spec_state_wrong.yaml
+++ b/test/yaml/spec_state_wrong.yaml
@@ -1,84 +1,96 @@
# P in _2D_
P_2D_initial_guess:
type: StatePoint2d
- mode: initial_guess
- #state: [1, 2] # missing
+ prior:
+ mode: initial_guess
+ #value: [1, 2] # missing
P_2D_fix:
type: StatePoint2d
- mode: fix
- #state: [1, 2] # missing
+ prior:
+ mode: fix
+ #value: [1, 2] # missing
P_2D_factor:
type: StatePoint2d
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] # wrong size
+ value: [1, 2]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] # wrong size
# P in _3D_
P_3D_initial_guess:
type: StatePoint3d
#mode: initial_guess # missing
- state: [1, 2, 3]
+ value: [1, 2, 3]
P_3D_fix:
type: StatePoint3d
- mode: fix
- state: [1, 2, 3, 4] # wrong size
+ prior:
+ mode: fix
+ value: [1, 2, 3, 4] # wrong size
P_3D_factor:
type: StatePoint3d
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1, 2, 3]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
# O in _2D_
O_2D_initial_guess:
type: StateAngle
- mode: initial_guess
- state: [1, 2] # wrong size
+ prior:
+ mode: initial_guess
+ value: [1, 2] # wrong size
O_2D_fix:
type: StateAngle
- mode: fix
- state: [1 2] # wrong size
+ prior:
+ mode: fix
+ value: [1 2] # wrong size
O_2D_factor:
type: StateAngle
- mode: factor
- state: [1]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
# O in _3D_
O_3D_initial_guess:
type: StateQuaternion
- mode: initial_guess
- #state: [1, 0, 0, 0] # missing
+ prior:
+ mode: initial_guess
+ #value: [1, 0, 0, 0] # missing
O_3D_fix:
type: StateQuaternion
#mode: fix #missing
- state: [1, 0, 0, 0]
+ value: [1, 0, 0, 0]
O_3D_factor:
type: StateQuaternion
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2] # wrong size
+ value: [1, 0, 0, 0]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2] # wrong size
# I
I_initial_guess:
type: StateParam4
#mode: initial_guess # missing
- state: [1, 2, 3, 4]
+ value: [1, 2, 3, 4]
I_fix:
type: StateParam4
- mode: fix
- #state: [1, 2, 3, 4] # missing
+ prior:
+ mode: fix
+ #value: [1, 2, 3, 4] # missing
I_factor:
type: StateParam4
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
\ No newline at end of file
+ value: [1, 2, 3, 4]
+ prior:
+ mode: factor
+ factor_std: [0.1, 0.2, 0.3] # wrong size
\ No newline at end of file
diff --git a/yaml_templates/TypeAndPlugin.yaml b/yaml_templates/TypeAndPlugin.yaml
index 42bdc3ec4abeff58189fb6ce5bb6b4bdab6f6225..6ddc5b493043f5669a9c2c18cbbfac094322c405 100644
--- a/yaml_templates/TypeAndPlugin.yaml
+++ b/yaml_templates/TypeAndPlugin.yaml
@@ -1,2 +1,2 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
\ No newline at end of file
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/landmark/Landmark2d.yaml b/yaml_templates/landmark/Landmark2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..42bda996fe1a7c134c60ba45285a035e06400879
--- /dev/null
+++ b/yaml_templates/landmark/Landmark2d.yaml
@@ -0,0 +1,16 @@
+type: "whatever" # DOC Type of the landmark - TYPE string
+plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
+id: 0 # DOC Unique id of the landmark. - TYPE int
+external_id: 0 # OPTIONAL - DOC Id given by an external tracker via capture. Missing or -1 means untracked. - TYPE int
+external_type: 0 # OPTIONAL - DOC Type given by an external identifier via capture (for example: 1: chair, 2: person...). Missing or -1 means uknown. - TYPE int
+states:
+ P:
+ value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
+ O:
+ value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+ prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/landmark/Landmark3d.yaml b/yaml_templates/landmark/Landmark3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..40a6bbf107dc7077a78db61fc30fd37a3ed7c1c4
--- /dev/null
+++ b/yaml_templates/landmark/Landmark3d.yaml
@@ -0,0 +1,16 @@
+type: "whatever" # DOC Type of the landmark - TYPE string
+plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
+id: 0 # DOC Unique id of the landmark. - TYPE int
+external_id: 0 # OPTIONAL - DOC Id given by an external tracker via capture. Missing or -1 means untracked. - TYPE int
+external_type: 0 # OPTIONAL - DOC Type given by an external identifier via capture (for example: 1: chair, 2: person...). Missing or -1 means uknown. - TYPE int
+states:
+ P:
+ value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
+ O:
+ value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/landmark/LandmarkBase.yaml b/yaml_templates/landmark/LandmarkBase.yaml
index bb5df3eb479879cc051a02eab00095dab6c4b762..85fb4f84ca9500b3544250cd3284d9c6ebc5256c 100644
--- a/yaml_templates/landmark/LandmarkBase.yaml
+++ b/yaml_templates/landmark/LandmarkBase.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC Type of the landmark - TYPE string
plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
id: 0 # DOC Unique id of the landmark. - TYPE int
-states:
- keys: "whatever" # DOC The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas. - TYPE string
\ No newline at end of file
+external_id: 0 # OPTIONAL - DOC Id given by an external tracker via capture. Missing or -1 means untracked. - TYPE int
+external_type: 0 # OPTIONAL - DOC Type given by an external identifier via capture (for example: 1: chair, 2: person...). Missing or -1 means uknown. - TYPE int
\ No newline at end of file
diff --git a/yaml_templates/landmark/LandmarkPoint2d.yaml b/yaml_templates/landmark/LandmarkPoint2d.yaml
deleted file mode 100644
index 446dd04f72fa04c5e78b254eb8c3687d2c2235fa..0000000000000000000000000000000000000000
--- a/yaml_templates/landmark/LandmarkPoint2d.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-type: "whatever" # DOC Type of the landmark - TYPE string
-plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
-id: 0 # DOC Unique id of the landmark. - TYPE int
-states:
- P:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/landmark/LandmarkPoint3d.yaml b/yaml_templates/landmark/LandmarkPoint3d.yaml
deleted file mode 100644
index cb27e477ec12fd3af9daa53b0b83aa232bc248d4..0000000000000000000000000000000000000000
--- a/yaml_templates/landmark/LandmarkPoint3d.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-type: "whatever" # DOC Type of the landmark - TYPE string
-plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
-id: 0 # DOC Unique id of the landmark. - TYPE int
-states:
- P:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/landmark/LandmarkPose2d.yaml b/yaml_templates/landmark/LandmarkPose2d.yaml
deleted file mode 100644
index 147580f84600b5c139ee10ebb2ac697ed86c4bd9..0000000000000000000000000000000000000000
--- a/yaml_templates/landmark/LandmarkPose2d.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-type: "whatever" # DOC Type of the landmark - TYPE string
-plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
-id: 0 # DOC Unique id of the landmark. - TYPE int
-states:
- P:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
- O:
- state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/landmark/LandmarkPose3d.yaml b/yaml_templates/landmark/LandmarkPose3d.yaml
deleted file mode 100644
index 449197bec8479869cd21e5cee61f2fad681b5fde..0000000000000000000000000000000000000000
--- a/yaml_templates/landmark/LandmarkPose3d.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-type: "whatever" # DOC Type of the landmark - TYPE string
-plugin: "core" # OPTIONAL - DOC Name of the wolf plugin where the landmark type is implemented. - TYPE string
-id: 0 # DOC Unique id of the landmark. - TYPE int
-states:
- P:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- O:
- state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/problem/Problem2d.yaml b/yaml_templates/problem/Problem2d.yaml
index 9f86253675b7dacba6eea7bacba41db6a5c04127..3f0486b66c882ab8cb266f49db3e8a4f3130065a 100644
--- a/yaml_templates/problem/Problem2d.yaml
+++ b/yaml_templates/problem/Problem2d.yaml
@@ -5,17 +5,20 @@ problem:
dimension: 2 # DOC Dimension of the problem: "2" for 2D - TYPE int - OPTIONS [2]
first_frame:
P:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
O:
- state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+ value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+ prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
V:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ value: [0.0, 0.0] # DOC A vector containing the linear velocity components (x, y) [m]. - TYPE Vector2d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
map:
type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
follow: some/path/to/derived/type/parameters.yaml
diff --git a/yaml_templates/problem/Problem3d.yaml b/yaml_templates/problem/Problem3d.yaml
index 35037f64a20bcd526ce9a01b9542bbcfa5155982..459edd0581dd6660d048789e1c19a2d93b3a8a66 100644
--- a/yaml_templates/problem/Problem3d.yaml
+++ b/yaml_templates/problem/Problem3d.yaml
@@ -5,17 +5,20 @@ problem:
dimension: 3 # DOC Dimension of the problem: "3" for 3D - TYPE int - OPTIONS [3]
first_frame:
P:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
O:
- state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
V:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the state "V" values - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ value: [0.0, 0.0, 0.0] # DOC A vector containing the linear velocity components (x, y, z) [m]. - TYPE Vector3d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
map:
type: "DerivedType" # DOC String corresponding to the name of the object class (and its schema file).
follow: some/path/to/derived/type/parameters.yaml
diff --git a/yaml_templates/problem/StateO2d.yaml b/yaml_templates/problem/StateO2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..57f6c739d307fe87d73b6959c79b64c93aea4beb
--- /dev/null
+++ b/yaml_templates/problem/StateO2d.yaml
@@ -0,0 +1,4 @@
+value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/problem/StateO3d.yaml b/yaml_templates/problem/StateO3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7d2d34cb4dc1921600837f0a20d7aeb1208395b1
--- /dev/null
+++ b/yaml_templates/problem/StateO3d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/problem/StateP2d.yaml b/yaml_templates/problem/StateP2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9dad0aa18f48cfc73b18c38c33365613f85f7a3e
--- /dev/null
+++ b/yaml_templates/problem/StateP2d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/problem/StateP3d.yaml b/yaml_templates/problem/StateP3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0b4566aa2e1dd20190665968d62e555ecbbe74b8
--- /dev/null
+++ b/yaml_templates/problem/StateP3d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/problem/StateV2d.yaml b/yaml_templates/problem/StateV2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f8a55ddc39a6a218f70ef453faea3a06749795f7
--- /dev/null
+++ b/yaml_templates/problem/StateV2d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0] # DOC A vector containing the linear velocity components (x, y) [m]. - TYPE Vector2d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/problem/StateV3d.yaml b/yaml_templates/problem/StateV3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9d8ef07dd90fe722ad7bb3d5604d26df4fe5bb21
--- /dev/null
+++ b/yaml_templates/problem/StateV3d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0] # DOC A vector containing the linear velocity components (x, y, z) [m]. - TYPE Vector3d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorBase.yaml b/yaml_templates/processor/ProcessorBase.yaml
index 72d710cbd033b2d4ed61c4e7192708df0d9972f1..d74b27af85c43b3bcf91f7f643f7277d51bd9a29 100644
--- a/yaml_templates/processor/ProcessorBase.yaml
+++ b/yaml_templates/processor/ProcessorBase.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorBaseWithSensor.yaml b/yaml_templates/processor/ProcessorBaseWithSensor.yaml
index 60328b0cab7677eba31fb42d5364446440250abd..f11771b82539f4618b9174970ac82ad5a0ffc315 100644
--- a/yaml_templates/processor/ProcessorBaseWithSensor.yaml
+++ b/yaml_templates/processor/ProcessorBaseWithSensor.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorDiffDrive.yaml b/yaml_templates/processor/ProcessorDiffDrive.yaml
index c38d91af1a863e01aa76df6c23f780ed7a0b09e1..12587124f674a75826fff74f963ef845377fbe7d 100644
--- a/yaml_templates/processor/ProcessorDiffDrive.yaml
+++ b/yaml_templates/processor/ProcessorDiffDrive.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorFixedWingModel.yaml b/yaml_templates/processor/ProcessorFixedWingModel.yaml
index b0b3689d01f7b3427cd2d1740b480fe9ce1cbc63..c70230c2bbf0bf124a1fb3a2e8568dc49e2a86ef 100644
--- a/yaml_templates/processor/ProcessorFixedWingModel.yaml
+++ b/yaml_templates/processor/ProcessorFixedWingModel.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorLandmarkExternal.yaml b/yaml_templates/processor/ProcessorLandmarkExternal.yaml
index 04ae5d1beaf06fb0fcdaca3f788e5af97ace52e8..5bcc168cda6d8947e857e55dc8e53f7e0030f668 100644
--- a/yaml_templates/processor/ProcessorLandmarkExternal.yaml
+++ b/yaml_templates/processor/ProcessorLandmarkExternal.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
@@ -9,8 +9,11 @@ keyframe_vote:
new_features_for_keyframe: 0 # DOC the amount of new features sufficient to create a new keyframe (sufficient condition) - TYPE unsigned int
apply_loss_function: false # DOC If the factors created by the processor have loss function. - TYPE bool
max_new_features: 0 # DOC Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.) - TYPE int
-filter:
- quality_th: 0.0 # DOC The minimum quality to consider the detection - TYPE double
- track_length_th: 0 # DOC The minimum track length to consider the detection - TYPE unsigned int
+quality_th: 0.0 # DOC The minimum quality to consider the detection - TYPE double
+track_length_th: 0 # DOC Minimum track length to emplace factors (necessary condition) - TYPE unsigned int
use_orientation: false # DOC If the orientation measurement is considered when emplacing factors - TYPE bool
-match_dist_th: 0.0 # DOC the threshold in distance for considering a match between landmarks and detections - TYPE double
\ No newline at end of file
+match_dist_th_id: 0.0 # DOC Matching distance threshold to previous detection considering motion (necessary condition) when ID matches - TYPE double
+match_dist_th_type: 0.0 # DOC Matching distance threshold to previous detection considering motion (necessary condition) when TYPE matches - TYPE double
+match_dist_th_unknown: 0.0 # DOC Matching distance threshold to previous detection considering motion (necessary condition) when no ID/TYPE information - TYPE double
+close_loops_by_id: false # DOC Close loop if ID matches - TYPE bool
+close_loops_by_type: false # DOC Close loop if TYPE matches if distance check holds (match_dist_th_type) - TYPE bool
\ No newline at end of file
diff --git a/yaml_templates/processor/ProcessorLoopClosure.yaml b/yaml_templates/processor/ProcessorLoopClosure.yaml
index c30f5771ff8afd2c7b34fa91ff5741f42bba14ab..34b901a8962d35838d32ac2748782717168ccfe7 100644
--- a/yaml_templates/processor/ProcessorLoopClosure.yaml
+++ b/yaml_templates/processor/ProcessorLoopClosure.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorMotion.yaml b/yaml_templates/processor/ProcessorMotion.yaml
index 37409fad2b4cf5ca065cf08eb20a8e0b24b187cb..6c0c9333f20cc119c7c958d0d472fe0c9357e1cd 100644
--- a/yaml_templates/processor/ProcessorMotion.yaml
+++ b/yaml_templates/processor/ProcessorMotion.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorOdom2d.yaml b/yaml_templates/processor/ProcessorOdom2d.yaml
index c38d91af1a863e01aa76df6c23f780ed7a0b09e1..12587124f674a75826fff74f963ef845377fbe7d 100644
--- a/yaml_templates/processor/ProcessorOdom2d.yaml
+++ b/yaml_templates/processor/ProcessorOdom2d.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorOdom3d.yaml b/yaml_templates/processor/ProcessorOdom3d.yaml
index 37409fad2b4cf5ca065cf08eb20a8e0b24b187cb..6c0c9333f20cc119c7c958d0d472fe0c9357e1cd 100644
--- a/yaml_templates/processor/ProcessorOdom3d.yaml
+++ b/yaml_templates/processor/ProcessorOdom3d.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorPose2d.yaml b/yaml_templates/processor/ProcessorPose2d.yaml
index 72d710cbd033b2d4ed61c4e7192708df0d9972f1..d74b27af85c43b3bcf91f7f643f7277d51bd9a29 100644
--- a/yaml_templates/processor/ProcessorPose2d.yaml
+++ b/yaml_templates/processor/ProcessorPose2d.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorPose3d.yaml b/yaml_templates/processor/ProcessorPose3d.yaml
index 72d710cbd033b2d4ed61c4e7192708df0d9972f1..d74b27af85c43b3bcf91f7f643f7277d51bd9a29 100644
--- a/yaml_templates/processor/ProcessorPose3d.yaml
+++ b/yaml_templates/processor/ProcessorPose3d.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorTracker.yaml b/yaml_templates/processor/ProcessorTracker.yaml
index abace2bc177412ce9867bd71c8c43ea880eb0572..b98f3042a02306eda3d27020487f7f1c5b9e5387 100644
--- a/yaml_templates/processor/ProcessorTracker.yaml
+++ b/yaml_templates/processor/ProcessorTracker.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorTrackerFeature.yaml b/yaml_templates/processor/ProcessorTrackerFeature.yaml
index abace2bc177412ce9867bd71c8c43ea880eb0572..b98f3042a02306eda3d27020487f7f1c5b9e5387 100644
--- a/yaml_templates/processor/ProcessorTrackerFeature.yaml
+++ b/yaml_templates/processor/ProcessorTrackerFeature.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/processor/ProcessorTrackerLandmark.yaml b/yaml_templates/processor/ProcessorTrackerLandmark.yaml
index abace2bc177412ce9867bd71c8c43ea880eb0572..b98f3042a02306eda3d27020487f7f1c5b9e5387 100644
--- a/yaml_templates/processor/ProcessorTrackerLandmark.yaml
+++ b/yaml_templates/processor/ProcessorTrackerLandmark.yaml
@@ -1,5 +1,5 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The processor"s name. It has to be unique. - TYPE string
time_tolerance: 0.0 # DOC Maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. [s]. - TYPE double
keyframe_vote:
diff --git a/yaml_templates/sensor/SensorBase.yaml b/yaml_templates/sensor/SensorBase.yaml
index c1663906e4264a1c79170952a150ede4229d076f..dd2208d3483fc2b8586f73b586d0d0ba010aada4 100644
--- a/yaml_templates/sensor/SensorBase.yaml
+++ b/yaml_templates/sensor/SensorBase.yaml
@@ -1,5 +1,3 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
-name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
-states:
- keys: "whatever" # DOC The keys corresponding to the states of the sensor, to be filled with _value in the derived schemas. - TYPE string
\ No newline at end of file
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorDiffDrive.yaml b/yaml_templates/sensor/SensorDiffDrive.yaml
index d4e4c6f46fc23c38111f808ae94c6a78ab782852..b693923be78098bd4431993f1337fc5ba81f68a2 100644
--- a/yaml_templates/sensor/SensorDiffDrive.yaml
+++ b/yaml_templates/sensor/SensorDiffDrive.yaml
@@ -1,24 +1,27 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+ticks_per_wheel_revolution: 0.0 # DOC Amount of sensor ticks of a whole wheel revolution (if measurement is directly radiants, put 2*PI). - TYPE double
+ticks_std_factor: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
states:
P:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
+ drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector2d
O:
- state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
+ dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+ prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
+ drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) [rad/sqrt(s)]. - TYPE Vector1d
I:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the intrinsic state values. 0=left wheel radius (m), 1=right wheel radius (m), 2=wheel separation (m) - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
-ticks_per_wheel_revolution: 0.0 # DOC ratio of displacement variance to displacement, for odometry noise calculation. - TYPE double
-ticks_std_factor: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
\ No newline at end of file
+ dynamic: false # DOC If the intrinsic state is dynamic, i.e. it changes along time (0: left wheel radius [m], 1: right wheel radius [m], 2: wheel separation [m]). - TYPE bool
+ value: [0.0, 0.0, 0.0] # DOC A vector containing the intrinsic state values (0: left wheel radius [m], 1: right wheel radius [m], 2: wheel separation [m]). - TYPE Vector3d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorMotionModel.yaml b/yaml_templates/sensor/SensorMotionModel.yaml
index c9395cfbc6a8d9e96165b1b3a21229bcf216bfcf..dd2208d3483fc2b8586f73b586d0d0ba010aada4 100644
--- a/yaml_templates/sensor/SensorMotionModel.yaml
+++ b/yaml_templates/sensor/SensorMotionModel.yaml
@@ -1,5 +1,3 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
-name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
-states:
- {}
\ No newline at end of file
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
+name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorOdom2d.yaml b/yaml_templates/sensor/SensorOdom2d.yaml
index 09787d36581ad7a929cb11ea8e09e30238f46d52..6c2a637cc5d4231cb772d4873d14e11598b88239 100644
--- a/yaml_templates/sensor/SensorOdom2d.yaml
+++ b/yaml_templates/sensor/SensorOdom2d.yaml
@@ -1,21 +1,23 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
-states:
- P:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
- O:
- state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
k_disp_to_disp: 0.0 # DOC ratio of displacement variance to displacement, for odometry noise calculation. - TYPE double
k_disp_to_rot: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
k_rot_to_rot: 0.0 # DOC ratio of rotation variance to rotation, for odometry noise calculation. - TYPE double
min_disp_var: 0.0 # DOC minimum displacement variance, for odometry noise calculation. - TYPE double
-min_rot_var: 0.0 # DOC minimum rotation variance, for odometry noise calculation. - TYPE double
\ No newline at end of file
+min_rot_var: 0.0 # DOC minimum rotation variance, for odometry noise calculation. - TYPE double
+states:
+ P:
+ dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
+ drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector2d
+ O:
+ dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+ prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
+ drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) [rad/sqrt(s)]. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorOdom3d.yaml b/yaml_templates/sensor/SensorOdom3d.yaml
index 9d898b41ecbcaa449caa3917d0a043fa7b74e986..b44ab8491aecabf63536b9989fb631f4705aaf77 100644
--- a/yaml_templates/sensor/SensorOdom3d.yaml
+++ b/yaml_templates/sensor/SensorOdom3d.yaml
@@ -1,21 +1,23 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
-states:
- P:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- O:
- state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
k_disp_to_disp: 0.0 # DOC ratio of displacement variance to displacement, for odometry noise calculation. - TYPE double
k_disp_to_rot: 0.0 # DOC ratio of displacement variance to rotation, for odometry noise calculation. - TYPE double
k_rot_to_rot: 0.0 # DOC ratio of rotation variance to rotation, for odometry noise calculation. - TYPE double
min_disp_var: 0.0 # DOC minimum displacement variance, for odometry noise calculation. - TYPE double
-min_rot_var: 0.0 # DOC minimum rotation variance, for odometry noise calculation. - TYPE double
\ No newline at end of file
+min_rot_var: 0.0 # DOC minimum rotation variance, for odometry noise calculation. - TYPE double
+states:
+ P:
+ dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector3d
+ O:
+ dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix [rad/sqrt(s)]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorPose2d.yaml b/yaml_templates/sensor/SensorPose2d.yaml
index e42f8605eab661e95729ebdbcd384b4f0ad915d2..619768c15088eefe2bd54b0dd302baaf7fe42d0e 100644
--- a/yaml_templates/sensor/SensorPose2d.yaml
+++ b/yaml_templates/sensor/SensorPose2d.yaml
@@ -1,17 +1,19 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+std_noise: [0.0, 0.0, 0.0] # DOC Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal). - TYPE Eigen::Vector3d
states:
P:
- state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
+ dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
+ drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector2d
O:
- state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
-std_noise: [0.0, 0.0, 0.0] # DOC Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal). - TYPE Eigen::Vector3d
\ No newline at end of file
+ dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+ prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
+ drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) [rad/sqrt(s)]. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SensorPose3d.yaml b/yaml_templates/sensor/SensorPose3d.yaml
index 38460e921a84f060afe795fb92ec0f910962bc4e..48c5b3bbe9778dd546f0beaf3d85e62228c7c890 100644
--- a/yaml_templates/sensor/SensorPose3d.yaml
+++ b/yaml_templates/sensor/SensorPose3d.yaml
@@ -1,17 +1,19 @@
type: "whatever" # DOC The class name - TYPE string
-plugin: "whatever" # DOC plugin where it is implemented the class - TYPE string
+plugin: "whatever" # DOC plugin where the class is implemented - TYPE string
name: "whatever" # DOC The sensor"s name. It has to be unique. - TYPE string
+std_noise: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal). - TYPE Eigen::Vector6d
states:
P:
- state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
+ dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector3d
O:
- state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
- mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
- noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
- dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
- drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
-std_noise: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC Vector containing the standard deviation of the position and orientation measurements (square root of the covariance matrix diagonal). - TYPE Eigen::Vector6d
\ No newline at end of file
+ dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+ value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+ prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
+ drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix [rad/sqrt(s)]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensor.yaml b/yaml_templates/sensor/SpecStateSensor.yaml
deleted file mode 100644
index a836a91f3f76bf4437deab175d58f1f7f09645ac..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensor.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-type: "whatever" # DOC The derived type of the StateBlock - TYPE string
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE VectorXd
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE VectorXd
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE VectorXd
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorO2d.yaml b/yaml_templates/sensor/SpecStateSensorO2d.yaml
deleted file mode 100644
index 44aab0d675c7eaa4ef9ec77e00de1cb03b6c74e7..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorO2d.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorO3d.yaml b/yaml_templates/sensor/SpecStateSensorO3d.yaml
deleted file mode 100644
index 9e55c6cf169588f6bbea29b8378ca713e4346174..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorO3d.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorP2d.yaml b/yaml_templates/sensor/SpecStateSensorP2d.yaml
deleted file mode 100644
index 125c7e9ba70aba065189f067b39dd71dc32d8be6..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorP2d.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorP3d.yaml b/yaml_templates/sensor/SpecStateSensorP3d.yaml
deleted file mode 100644
index cb22ff89a36c67414d7f185dda91bb8f5d695b7b..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorP3d.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams1.yaml b/yaml_templates/sensor/SpecStateSensorParams1.yaml
deleted file mode 100644
index 44aab0d675c7eaa4ef9ec77e00de1cb03b6c74e7..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams1.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams10.yaml b/yaml_templates/sensor/SpecStateSensorParams10.yaml
deleted file mode 100644
index af0d4f8d082af4c36476d7e38e8a9a2c0f736b83..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams10.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector10d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector10d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector10d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams2.yaml b/yaml_templates/sensor/SpecStateSensorParams2.yaml
deleted file mode 100644
index 125c7e9ba70aba065189f067b39dd71dc32d8be6..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams2.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams3.yaml b/yaml_templates/sensor/SpecStateSensorParams3.yaml
deleted file mode 100644
index de0000c68abd95182161de0a4d93a83c1d00068d..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams3.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector3d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams4.yaml b/yaml_templates/sensor/SpecStateSensorParams4.yaml
deleted file mode 100644
index 239cb8d24d28a9cd7bddba348d03f23294a16894..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams4.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector4d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector4d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector4d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams5.yaml b/yaml_templates/sensor/SpecStateSensorParams5.yaml
deleted file mode 100644
index 31fb0e4ca71f0bb1801cc0d9d89b7eec956fbf12..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams5.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector5d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector5d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector5d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams6.yaml b/yaml_templates/sensor/SpecStateSensorParams6.yaml
deleted file mode 100644
index 84d6a1f5b242e914e5feed63e09a7b5e1dd8cdc8..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams6.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector6d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector6d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector6d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams7.yaml b/yaml_templates/sensor/SpecStateSensorParams7.yaml
deleted file mode 100644
index c61970f601568f457c9f4f151cd41cb04949d5f1..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams7.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector7d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector7d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector7d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams8.yaml b/yaml_templates/sensor/SpecStateSensorParams8.yaml
deleted file mode 100644
index 5b3041f971aa6b73c72d2c7106f8d7ca1b681ab4..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams8.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector8d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector8d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector8d
\ No newline at end of file
diff --git a/yaml_templates/sensor/SpecStateSensorParams9.yaml b/yaml_templates/sensor/SpecStateSensorParams9.yaml
deleted file mode 100644
index e1ad86d9c356aaf954a390b2a7025c34f63a6d8e..0000000000000000000000000000000000000000
--- a/yaml_templates/sensor/SpecStateSensorParams9.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector9d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector9d
-dynamic: false # DOC If the state is dynamic, i.e. it changes along time. - TYPE bool
-drift_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector9d
\ No newline at end of file
diff --git a/yaml_templates/sensor/StateSensorO2d.yaml b/yaml_templates/sensor/StateSensorO2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fb3c52cffc60f483f3f3622fee5e42ba7565202b
--- /dev/null
+++ b/yaml_templates/sensor/StateSensorO2d.yaml
@@ -0,0 +1,6 @@
+dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
+drift_std: [0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) [rad/sqrt(s)]. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/sensor/StateSensorO3d.yaml b/yaml_templates/sensor/StateSensorO3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2f721f3226318b86c1072d30f595c8fbbf33ede8
--- /dev/null
+++ b/yaml_templates/sensor/StateSensorO3d.yaml
@@ -0,0 +1,6 @@
+dynamic: false # DOC If the orientation is dynamic, i.e. it changes along time. - TYPE bool
+value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
+drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix [rad/sqrt(s)]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/sensor/StateSensorP2d.yaml b/yaml_templates/sensor/StateSensorP2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..86be3ccc0d2646a47f5ad9257d55c33b0d68ca45
--- /dev/null
+++ b/yaml_templates/sensor/StateSensorP2d.yaml
@@ -0,0 +1,6 @@
+dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
+drift_std: [0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/sensor/StateSensorP3d.yaml b/yaml_templates/sensor/StateSensorP3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a2e9c0cb4ebc7b070685db0e1779fd709d4cdd45
--- /dev/null
+++ b/yaml_templates/sensor/StateSensorP3d.yaml
@@ -0,0 +1,6 @@
+dynamic: false # DOC If the position is dynamic, i.e. it changes along time. - TYPE bool
+value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
+drift_std: [0.0, 0.0, 0.0] # OPTIONAL - DOC A vector containing the stdev values of the noise of the drift factor per second (only if dynamic==true) i.e. the sqrt of the diagonal elements of the covariance matrix [m/sqrt(s)]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecState.yaml b/yaml_templates/state/SpecState.yaml
deleted file mode 100644
index d5544cc069c545497a6decce58cb8055d6623c13..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecState.yaml
+++ /dev/null
@@ -1,4 +0,0 @@
-type: "whatever" # DOC The derived type of the StateBlock - TYPE string
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE VectorXd
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE VectorXd
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateO2d.yaml b/yaml_templates/state/SpecStateO2d.yaml
deleted file mode 100644
index 8716ad1064682fad5487c0ef61bcc296a4dd28ba..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateO2d.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateO3d.yaml b/yaml_templates/state/SpecStateO3d.yaml
deleted file mode 100644
index d759a324b167ec0465936ae2c3ae82cd8235eaad..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateO3d.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. It should be a quaternion (i.e. four values and normalized) - TYPE Vector4d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateP2d.yaml b/yaml_templates/state/SpecStateP2d.yaml
deleted file mode 100644
index c27c76bd94a4f1e1fe2bda3a9163f27f7a467668..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateP2d.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateP3d.yaml b/yaml_templates/state/SpecStateP3d.yaml
deleted file mode 100644
index e4780f145fb67f9668f0771941998828b6652633..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateP3d.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state "P" values - TYPE Vector3d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams1.yaml b/yaml_templates/state/SpecStateParams1.yaml
deleted file mode 100644
index 8716ad1064682fad5487c0ef61bcc296a4dd28ba..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams1.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0] # DOC A vector containing the state values - TYPE Vector1d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams10.yaml b/yaml_templates/state/SpecStateParams10.yaml
deleted file mode 100644
index 124b1fb9f8e3fa7c130f0dcb51a12ef4e3efc468..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams10.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector10d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector10d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams2.yaml b/yaml_templates/state/SpecStateParams2.yaml
deleted file mode 100644
index c27c76bd94a4f1e1fe2bda3a9163f27f7a467668..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams2.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams3.yaml b/yaml_templates/state/SpecStateParams3.yaml
deleted file mode 100644
index d0d70e870ee14702c08b343e7f735bba4530253a..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams3.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector3d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams4.yaml b/yaml_templates/state/SpecStateParams4.yaml
deleted file mode 100644
index 2b30c558f2fe4d57e4d98c66389d76591e0b0a4c..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams4.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector4d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector4d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams5.yaml b/yaml_templates/state/SpecStateParams5.yaml
deleted file mode 100644
index 16b59863162e70c4e3d7b9a7b8af34cede8afb42..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams5.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector5d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector5d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams6.yaml b/yaml_templates/state/SpecStateParams6.yaml
deleted file mode 100644
index 1680a92b2e4f9adbe708af2bec4de1deea3e8c09..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams6.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector6d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector6d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams7.yaml b/yaml_templates/state/SpecStateParams7.yaml
deleted file mode 100644
index 0a346a990927a0a0f03bb38e229081f0e449ae9e..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams7.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector7d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector7d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams8.yaml b/yaml_templates/state/SpecStateParams8.yaml
deleted file mode 100644
index d5f214d897364a3dc6250491a337de1da1b5b1bc..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams8.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector8d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector8d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateParams9.yaml b/yaml_templates/state/SpecStateParams9.yaml
deleted file mode 100644
index b18b8f6e4d4519de1eb57019ffd8b6fc0070bf70..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateParams9.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values - TYPE Vector9d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector9d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateV2d.yaml b/yaml_templates/state/SpecStateV2d.yaml
deleted file mode 100644
index c27c76bd94a4f1e1fe2bda3a9163f27f7a467668..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateV2d.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0] # DOC A vector containing the state values - TYPE Vector2d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/SpecStateV3d.yaml b/yaml_templates/state/SpecStateV3d.yaml
deleted file mode 100644
index 616cce89b4f042861a186a648f7b29f24cfc7783..0000000000000000000000000000000000000000
--- a/yaml_templates/state/SpecStateV3d.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-state: [0.0, 0.0, 0.0] # DOC A vector containing the state "V" values - TYPE Vector3d
-mode: "fix" # DOC The prior mode can be "factor" to add an absolute factor (requires "noise_std"), "fix" to set the values constant or "initial_guess" to just set the values - TYPE string - OPTIONS [fix, factor, initial_guess]
-noise_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/StateO2d.yaml b/yaml_templates/state/StateO2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..57f6c739d307fe87d73b6959c79b64c93aea4beb
--- /dev/null
+++ b/yaml_templates/state/StateO2d.yaml
@@ -0,0 +1,4 @@
+value: [0.0] # DOC A vector containing the orientation, yaw [rad]. - TYPE Vector1d
+prior:
+ mode: "fix" # DOC It can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/state/StateO3d.yaml b/yaml_templates/state/StateO3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7d2d34cb4dc1921600837f0a20d7aeb1208395b1
--- /dev/null
+++ b/yaml_templates/state/StateO3d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the quaternion values (x, y, z, w) - TYPE Vector4d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [rad]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/StateP2d.yaml b/yaml_templates/state/StateP2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9dad0aa18f48cfc73b18c38c33365613f85f7a3e
--- /dev/null
+++ b/yaml_templates/state/StateP2d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0] # DOC A vector containing the position (x, y) [m]. - TYPE Vector2d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/StateP3d.yaml b/yaml_templates/state/StateP3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0b4566aa2e1dd20190665968d62e555ecbbe74b8
--- /dev/null
+++ b/yaml_templates/state/StateP3d.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0] # DOC A vector containing the position (x, y, z) [m]. - TYPE Vector3d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix [m]. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams1.yaml b/yaml_templates/state/StateParams1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..af418f56d1d3a46701d863763c8b0345293cadaf
--- /dev/null
+++ b/yaml_templates/state/StateParams1.yaml
@@ -0,0 +1,4 @@
+value: [0.0] # DOC A vector containing the state value. - TYPE Vector1d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector1d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams10.yaml b/yaml_templates/state/StateParams10.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8b6900e393c10634891a9e49fe01945960829fb7
--- /dev/null
+++ b/yaml_templates/state/StateParams10.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector10d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector10d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams2.yaml b/yaml_templates/state/StateParams2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..92a906ab78f30edbb648ad2d4dfc72dee4c46ce6
--- /dev/null
+++ b/yaml_templates/state/StateParams2.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector2d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector2d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams3.yaml b/yaml_templates/state/StateParams3.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..230086b4351d9a02b98899c0047e45ac04175865
--- /dev/null
+++ b/yaml_templates/state/StateParams3.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector3d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector3d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams4.yaml b/yaml_templates/state/StateParams4.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2473eda2789e32828574990a99719f6c9f453317
--- /dev/null
+++ b/yaml_templates/state/StateParams4.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector4d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector4d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams5.yaml b/yaml_templates/state/StateParams5.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ece391fc74b1d20d7d1479e769823dd52803a398
--- /dev/null
+++ b/yaml_templates/state/StateParams5.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector5d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector5d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams6.yaml b/yaml_templates/state/StateParams6.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ffaebabd16dd2f71523138f2d175db91ba62f3f6
--- /dev/null
+++ b/yaml_templates/state/StateParams6.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector6d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector6d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams7.yaml b/yaml_templates/state/StateParams7.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9ee2323fdfe3c2612f30daf706b8d563524236b6
--- /dev/null
+++ b/yaml_templates/state/StateParams7.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector7d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector7d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams8.yaml b/yaml_templates/state/StateParams8.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..92017c2ab7f04eddb17ec3035accfd0831d18e1a
--- /dev/null
+++ b/yaml_templates/state/StateParams8.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector8d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector8d
\ No newline at end of file
diff --git a/yaml_templates/state/StateParams9.yaml b/yaml_templates/state/StateParams9.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8655bcc2d296b4b95792a0b58af35344a14a76f0
--- /dev/null
+++ b/yaml_templates/state/StateParams9.yaml
@@ -0,0 +1,4 @@
+value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # DOC A vector containing the state values. - TYPE Vector9d
+prior:
+ mode: "fix" # DOC Can be "factor" to add an absolute factor (with covariance defined in "factor_std"), "fix" to set the values constant or "initial_guess" to just set the values. - TYPE string - OPTIONS [fix, factor, initial_guess]
+ factor_std: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # MANDATORY if $mode == "factor" - DOC A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. - TYPE Vector9d
\ No newline at end of file