diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3ae0e7b8e8a34092bf79dfa86bb8fa68cce81904..fc21bf0533b0ce73d9bdf7950b6cd4968ad02bd5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -247,6 +247,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_pose_3d.h
include/core/factor/factor_quaternion_absolute.h
include/core/factor/factor_relative_2d_analytic.h
+ include/core/factor/factor_relative_pose_2d_with_extrinsics.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
@@ -409,8 +410,6 @@ ELSE(Ceres_FOUND)
SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
-#SUBDIRECTORIES
-add_subdirectory(hello_wolf)
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e6b2bb11cbe6b95dceba4095af5c57b2c76f16c0
--- /dev/null
+++ b/demos/CMakeLists.txt
@@ -0,0 +1,2 @@
+#Add hello_wolf demo
+add_subdirectory(hello_wolf)
diff --git a/demos/demo_map_yaml.cpp b/demos/demo_map_yaml.cpp
deleted file mode 100644
index 87023e52856c9e3ba937882cd5311e05260b507b..0000000000000000000000000000000000000000
--- a/demos/demo_map_yaml.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/**
- * \file test_map_yaml.cpp
- *
- * Created on: Jul 27, 2016
- * \author: jsola
- */
-
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_polyline_2d.h"
-#include "core/landmark/landmark_AHP.h"
-#include "core/state_block/state_block.h"
-#include "core/yaml/yaml_conversion.h"
-
-#include <iostream>
-using namespace wolf;
-
-void print(MapBase& _map)
-{
- for (auto lmk_ptr : _map.getLandmarkList())
- {
- std::cout << "Lmk ID: " << lmk_ptr->id();
- std::cout << "\nLmk type: " << lmk_ptr->getType();
- if (lmk_ptr->getType() == "POLYLINE 2d")
- {
- LandmarkPolyline2dPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2d>(lmk_ptr);
- std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
- std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
- std::cout << "\nn points: " << poly_ptr->getNPoints();
- std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
- std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
- std::cout << "\nLast def: " << poly_ptr->isLastDefined();
- for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
- std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
- break;
- }
- else if (lmk_ptr->getType() == "AHP")
- {
- LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
- std::cout << "\npos: " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
- std::cout << "\ndescript: " << ahp_ptr->getCvDescriptor().t();
- break;
- }
- else
- break;
-
- std::cout << std::endl;
- }
-}
-
-int main()
-{
- using namespace Eigen;
-
- std::cout << "\nTesting Lmk creator and node saving..." << std::endl;
- Vector4d v;
- v << 1, 2, 3, 4;
- cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
- LandmarkAHP lmk_1(v, nullptr, nullptr, d);
- std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
- std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
-
- YAML::Node n = lmk_1.saveToYaml();
- std::cout << "Pos n = " << n["position"].as<VectorXd>().transpose() << std::endl;
- std::cout << "Des n = " << n["descriptor"].as<VectorXd>().transpose() << std::endl;
-
- LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
- std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
- std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
-
- std::string filename;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string wolf_config = wolf_root + "/src/examples";
- std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
-
- ProblemPtr problem = Problem::create("PO", 2);
- filename = wolf_config + "/map_polyline_example.yaml";
- std::cout << "Reading map from file: " << filename << std::endl;
- problem->loadMap(filename);
-
- std::cout << "Printing map..." << std::endl;
- print(*(problem->getMap()));
-
- filename = wolf_config + "/map_polyline_example_write.yaml";
- std::cout << "Writing map to file: " << filename << std::endl;
- std::string thisfile = __FILE__;
- problem->getMap()->save(filename, "Example generated by test file " + thisfile);
-
- std::cout << "Clearing map... " << std::endl;
- problem->getMap()->getLandmarkList().clear();
-
- std::cout << "Re-reading map from file: " << filename << std::endl;
- problem->loadMap(filename);
-
- std::cout << "Printing map..." << std::endl;
- print(*(problem->getMap()));
-
- return 0;
-}
diff --git a/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt
similarity index 100%
rename from hello_wolf/CMakeLists.txt
rename to demos/hello_wolf/CMakeLists.txt
diff --git a/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp
similarity index 89%
rename from hello_wolf/capture_range_bearing.cpp
rename to demos/hello_wolf/capture_range_bearing.cpp
index a2d44a101085ea0ad3aa4ae7d68af68370ea5024..9ead1c183b8b08a64f709faea5236d5dbb056231 100644
--- a/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
- CaptureBase("RANGE BEARING", _ts, _scanner),
+ CaptureBase("CaptureRangeBearing", _ts, _scanner),
ids_(_ids),
ranges_(_ranges),
bearings_(_bearings)
diff --git a/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h
similarity index 100%
rename from hello_wolf/capture_range_bearing.h
rename to demos/hello_wolf/capture_range_bearing.h
diff --git a/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
similarity index 100%
rename from hello_wolf/factor_bearing.h
rename to demos/hello_wolf/factor_bearing.h
diff --git a/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
similarity index 100%
rename from hello_wolf/factor_range_bearing.h
rename to demos/hello_wolf/factor_range_bearing.h
diff --git a/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp
similarity index 82%
rename from hello_wolf/feature_range_bearing.cpp
rename to demos/hello_wolf/feature_range_bearing.cpp
index 4d47cc5539f5fcfe2cc011ea4c4e6e11d2685f90..ea584a419aa21c1d7e462824d6872f7d57bd818c 100644
--- a/hello_wolf/feature_range_bearing.cpp
+++ b/demos/hello_wolf/feature_range_bearing.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
- FeatureBase("RANGE BEARING", _measurement, _meas_covariance)
+ FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
{
//
}
diff --git a/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h
similarity index 100%
rename from hello_wolf/feature_range_bearing.h
rename to demos/hello_wolf/feature_range_bearing.h
diff --git a/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
similarity index 100%
rename from hello_wolf/hello_wolf.cpp
rename to demos/hello_wolf/hello_wolf.cpp
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
similarity index 99%
rename from hello_wolf/hello_wolf_autoconf.cpp
rename to demos/hello_wolf/hello_wolf_autoconf.cpp
index a387adfe9656523c37d025bb3a74f0f198281b3d..5ab1ad1dd20a6b03411ea5cc458bde99b2b1fca8 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -98,7 +98,7 @@ int main()
WOLF_TRACE("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
- std::string config_file = "hello_wolf/yaml/hello_wolf_config.yaml";
+ std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
std::string wolf_path = std::string(_WOLF_ROOT_DIR);
// parse file into params server: each param will be retrievable from this params server:
diff --git a/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
similarity index 80%
rename from hello_wolf/landmark_point_2d.cpp
rename to demos/hello_wolf/landmark_point_2d.cpp
index 835c4fb04e4f7b1e5695958ca9db3cbb810c5ec5..8b2ad4c0e8a9b576f67e9d8d56e41d932abedef8 100644
--- a/hello_wolf/landmark_point_2d.cpp
+++ b/demos/hello_wolf/landmark_point_2d.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
- LandmarkBase("POINT 2d", std::make_shared<StateBlock>(_xy))
+ LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
{
setId(_id);
}
diff --git a/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h
similarity index 100%
rename from hello_wolf/landmark_point_2d.h
rename to demos/hello_wolf/landmark_point_2d.h
diff --git a/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
similarity index 100%
rename from hello_wolf/processor_range_bearing.cpp
rename to demos/hello_wolf/processor_range_bearing.cpp
diff --git a/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
similarity index 100%
rename from hello_wolf/processor_range_bearing.h
rename to demos/hello_wolf/processor_range_bearing.h
diff --git a/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
similarity index 100%
rename from hello_wolf/sensor_range_bearing.cpp
rename to demos/hello_wolf/sensor_range_bearing.cpp
diff --git a/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
similarity index 100%
rename from hello_wolf/sensor_range_bearing.h
rename to demos/hello_wolf/sensor_range_bearing.h
diff --git a/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
similarity index 69%
rename from hello_wolf/yaml/hello_wolf_config.yaml
rename to demos/hello_wolf/yaml/hello_wolf_config.yaml
index b84d7776622264f96a4597ade146d64bcec86438..1077fe00cb442147b03a36ffddba5b3b37016e8d 100644
--- a/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -2,6 +2,11 @@ config:
problem:
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_key_frames: -1
+ fix_first_key_frame: false
+ viral_remove_empty_parent: true
frame_structure: "PO" # keyframes have position and orientation
dimension: 2 # space is 2d
prior:
@@ -17,7 +22,7 @@ config:
name: "sen odom"
extrinsic:
pose: [0,0, 0]
- follow: "hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
+ follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
@@ -25,7 +30,7 @@ config:
extrinsic:
pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
- follow: hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
+ follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
processors:
@@ -33,10 +38,10 @@ config:
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
- follow: hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
+ follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
- type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
- follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
+ follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
diff --git a/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
similarity index 92%
rename from hello_wolf/yaml/processor_odom_2d.yaml
rename to demos/hello_wolf/yaml/processor_odom_2d.yaml
index bee82ff9ad905a72dc6c1ffd46ac6d7267ae8870..7e345ef211f52897b17267065e28feec0bf4b143 100644
--- a/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -10,3 +10,4 @@ keyframe_vote:
angle_turned: 999
max_buff_length: 999
cov_det: 999
+apply_loss_function: true
diff --git a/hello_wolf/yaml/processor_range_bearing.yaml b/demos/hello_wolf/yaml/processor_range_bearing.yaml
similarity index 84%
rename from hello_wolf/yaml/processor_range_bearing.yaml
rename to demos/hello_wolf/yaml/processor_range_bearing.yaml
index 61efbf8e31ee32ab9a348133a67037c5fbbdafee..3c257678c9a73eeb3b415e976b55eeddb3a79edd 100644
--- a/hello_wolf/yaml/processor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/processor_range_bearing.yaml
@@ -5,3 +5,5 @@ time_tolerance: 0.1
keyframe_vote:
voting_active: true
voting_aux_active: false
+
+apply_loss_function: true
diff --git a/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
similarity index 87%
rename from hello_wolf/yaml/sensor_odom_2d.yaml
rename to demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 3ad7204855cae5c1e8e00cfdc011ff27d2725692..0408497bf4d33520cfbe17619b744fad23f25933 100644
--- a/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -2,3 +2,4 @@ type: "SensorOdom2d" # This must match the KEY used in the SensorFa
k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad
+apply_loss_function: true
diff --git a/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
similarity index 58%
rename from hello_wolf/yaml/sensor_range_bearing.yaml
rename to demos/hello_wolf/yaml/sensor_range_bearing.yaml
index 40a2387a041f8a34a41c5669218dd4a4eb00c0bf..d1625337dfc0b0d323d662a727c1ac298117a455 100644
--- a/hello_wolf/yaml/sensor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -1,4 +1,5 @@
type: "SensorRangeBearing"
noise_range_metres_std: 0.1
-noise_bearing_degrees_std: 0.5
+noise_bearing_degrees_std: 0.5
+apply_loss_function: true
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 4488ab30ec274eeb84e49b7dcd600de9a93acf5e..2a6120408718df85ebb2945c9800d71b0060b6fd 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -27,8 +27,8 @@ namespace wolf {
* - "LANDMARK"
*
* - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow:
- * - "SesorCamera"
- * - "SesorLaser2d"
+ * - "SensorCamera"
+ * - "SensorLaser2d"
* - "LandmarkPoint3d"
* - "ProcessorLaser2d"
*
diff --git a/include/core/factor/factor_autodiff_distance_3d.h b/include/core/factor/factor_autodiff_distance_3d.h
index 8dc182d6f92ec60d7ba310ed1e7f94e27a9281fc..1ef90ba493d3d3922685f7f42ff2b14402d7a546 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_autodiff_distance_3d.h
@@ -23,7 +23,7 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff("DISTANCE 3d",
+ FactorAutodiff("FactorAutodiffDistance3d",
_frm_other,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index d936a99d6c1d64f29fda2c5aeca1806e0065c4d1..7b189e4424b419b655833020ff9a94c71952748e 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -14,6 +14,7 @@
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/feature/feature_motion.h"
+#include "core/math/rotations.h"
namespace
{
@@ -126,10 +127,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
residuals = delta_corrected - delta_predicted;
// angle remapping
- while (residuals(2) > T(M_PI))
- residuals(2) = residuals(2) - T(2. * M_PI);
- while (residuals(2) <= T(-M_PI))
- residuals(2) = residuals(2) + T(2. * M_PI);
+ pi2pi(residuals(2));
// weighted residual
residuals = getMeasurementSquareRootInformationUpper() * residuals;
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index e20f0f50f158b7ed04e2d09054fafee9300a6f29..1ce2a375c131cea5234a5e603cdf2808764bc4cc 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -72,10 +73,7 @@ inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, co
// Error
er = expected_measurement - getMeasurement();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2) = pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper() * er;
diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_odom_2d_analytic.h
index c7eec32a1778b9f1ad69da6c3c0087e50509a904..52ea04ccd817f2a6c914d771737c284bca4ce664 100644
--- a/include/core/factor/factor_odom_2d_analytic.h
+++ b/include/core/factor/factor_odom_2d_analytic.h
@@ -18,7 +18,7 @@ class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2dAnalytic("ODOM_2d",
+ FactorRelative2dAnalytic("FactorOdom2dAnalytic",
_ftr_ptr,
_frame_ptr,
_processor_ptr,
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index e7d35aefa48b549174d060668cbee540edb6e9e2..85a1785440a03a15a899117b619d3d07aaa3eb39 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -71,10 +72,7 @@ inline bool FactorOdom2dCloseloop::operator ()(const T* const _p1, const T* cons
// Error
er = expected_measurement - getMeasurement().cast<T>();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2)=pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index ce63c806017117e67d18c2e79c34468e6b16a61c..f11b674e7775c78c0f0f306be69c85c832e328de 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -54,11 +54,7 @@ inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _
Eigen::Matrix<T,3,1> er;
er(0) = meas(0) - _p[0];
er(1) = meas(1) - _p[1];
- er(2) = meas(2) - _o[0];
- while (er[2] > T(M_PI))
- er(2) = er(2) - T(2*M_PI);
- while (er(2) <= T(-M_PI))
- er(2) = er(2) + T(2*M_PI);
+ er(2) = pi2pi(meas(2) - _o[0]);
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 6533bd414d54a9eb45da8c92336ba3d1f0e07739..402615a5e5c4f11d1f306d57119cb7668031c459 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -27,7 +27,7 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
+ FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
index 5a67da1ff6c0e17614902ebc3195dd8124ec0ed6..2dba90bc6d4ead00634f601bf232d4471860c418 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_2d_analytic.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_analytic.h"
#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
#include <Eigen/StdVector>
namespace wolf {
@@ -19,11 +20,11 @@ class FactorRelative2dAnalytic : public FactorAnalytic
**/
FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
+ const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ FactorAnalytic(_tp, _frame_other_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_other_ptr->getP(), _frame_other_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
@@ -45,11 +46,11 @@ class FactorRelative2dAnalytic : public FactorAnalytic
**/
FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
+ FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_other_ptr->getP(), _landmark_other_ptr->getO())
{
//
}
@@ -113,10 +114,7 @@ inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
// Residual
residual = expected_measurement - getMeasurement();
- while (residual(2) > M_PI)
- residual(2) = residual(2) - 2 * M_PI;
- while (residual(2) <= -M_PI)
- residual(2) = residual(2) + 2 * M_PI;
+ residual(2) = pi2pi(residual(2));
residual = getMeasurementSquareRootInformationUpper() * residual;
return residual;
}
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..be13135c9c7e3e0899ac2bb9043f4dd1b7b60ef2
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -0,0 +1,119 @@
+#ifndef FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
+
+//class
+class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
+{
+ public:
+ FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+ {
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+ }
+
+ virtual ~FactorRelativePose2dWithExtrinsics() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+ template<typename T>
+ bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
+ T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+ const T* const _o2, const T* const _ps, const T* const _os, T* _residuals) const
+{
+
+ // MAPS
+ Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
+ T o1 = _o1[0];
+ T o2 = _o2[0];
+ T os = _os[0];
+
+ Eigen::Matrix<T, 3, 1> expected_measurement;
+ Eigen::Matrix<T, 3, 1> er; // error
+
+ // Expected measurement
+ // r1_p_r1_s1 = ps
+ // r2_p_r2_s2 = ps
+ // r1_R_s1 = R(os)
+ // r2_R_s2 = R(os)
+ // w_R_r1 = R(o1)
+ // w_R_r2 = R(o2)
+ // ----------------------------------------
+
+ // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
+ // s1_R_r1*r1_R_w*w_p_r1_w +
+ // s1_R_r1*r1_R_w*w_p_w_r2 +
+ // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
+
+ // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
+ expected_measurement(2) = o2 - o1;
+
+ // Error
+ er = expected_measurement - getMeasurement().cast<T>();
+ er(2) = pi2pi(er(2));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXs J(3,6);
+// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
+// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorRelativePose2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePose2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePose2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 7c2ede927a796adfd35e7dd9240e0128f1192c11..a0a86c6ee4c3ef095725a2df2230c2fdb8de751f 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -27,7 +27,7 @@ pi2pi(const T _angle)
T angle = _angle;
while (angle > T( M_PI ))
angle -= T( 2 * M_PI );
- while (angle < T( -M_PI ))
+ while (angle <= T( -M_PI ))
angle += T( 2 * M_PI );
return angle;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index e449ce1604fef9dbdafb1ed45d7dfd982f35845e..8578e59491805e64af66a198cd3be9c6b2126a3e 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -12,7 +12,7 @@ namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(StateStructure()),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
@@ -34,7 +34,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
}
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(StateStructure()),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
@@ -56,7 +56,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
}
FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
- NodeBase("FRAME", "Base"),
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index adda7984748ebfb2fa6e40517f0d1bedc331efd2..8995fc7830b1b540c055ee1bc6aa4ca2c01c33ed 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -4,7 +4,7 @@
namespace wolf {
HardwareBase::HardwareBase() :
- NodeBase("HARDWARE", "Base")
+ NodeBase("HARDWARE", "HardwareBase")
{
// std::cout << "constructed H" << std::endl;
}
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 428faf954d89ffc8f0d7a88b182cd04a92850c4a..7620c9fa29132561cb0324d60399da97355f7f67 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -4,7 +4,7 @@
namespace wolf {
TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
- NodeBase("TRAJECTORY", "Base"),
+ NodeBase("TRAJECTORY", "TrajectoryBase"),
frame_structure_(_frame_structure),
last_key_frame_ptr_(nullptr),
last_key_or_aux_frame_ptr_(nullptr)
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 710cd3566cd477e2dd8f9c02c0a71070c20e07d5..7e59cd114f691e24ea04c1a7dc218501c65d31ca 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -22,11 +22,11 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
// remove first KF if too many KF
if (n_kf > params_sw_->n_key_frames)
{
- WOLF_INFO("TreeManagerSlidingWindow removing first frame");
+ WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
first_KF->remove(params_sw_->viral_remove_empty_parent);
if (params_sw_->fix_first_key_frame)
{
- WOLF_INFO("TreeManagerSlidingWindow fixing new first frame");
+ WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
second_KF->fix();
}
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2558c2452ea038d9b622c06a863c754424ed988a..8a86ad15960d3978fe87f6034ff3c29cf622b845 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -183,6 +183,10 @@ target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME})
+# FactorOdom2d class test
+wolf_add_gtest(gtest_factor_odom_2d gtest_factor_odom_2d.cpp)
+target_link_libraries(gtest_factor_odom_2d ${PROJECT_NAME})
+
# FactorOdom3d class test
wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME})
@@ -195,6 +199,14 @@ target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME})
+# FactorRelativePose2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PROJECT_NAME})
+
+# FactorRelative2dAnalytic class test
+wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp)
+target_link_libraries(gtest_factor_relative_2d_analytic ${PROJECT_NAME})
+
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 3a30e98af0e409fb0fc8604231d7bee16a0630e6..dfa057a0c8f676b7a456845c9d1d988bb62bc0ec 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -58,6 +58,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new
FeatureBasePtrList& _features_out)
{
unsigned int max_features = _max_new_features;
+ WOLF_INFO("Detecting " , _max_new_features , " new features..." );
if (max_features == -1)
{
@@ -70,7 +71,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new
for (unsigned int i = 0; i < max_features; i++)
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
- "DUMMY FEATURE",
+ "FeatureDummy",
Eigen::Vector1d::Ones(),
Eigen::MatrixXd::Ones(1, 1));
_features_out.push_back(ftr);
@@ -92,26 +93,11 @@ FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _featur
return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this());
}
-ProcessorBasePtr ProcessorTrackerFeatureDummy::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params)
-{
- auto params = std::static_pointer_cast<ProcessorParamsTrackerFeatureDummy>(_params);
-
- // if cast failed use default value
- if (params == nullptr)
- params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
-
- auto prc_ptr = std::make_shared<ProcessorTrackerFeatureDummy>(params);
-
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
-}
-
} // namespace wolf
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("ProcessorTrackerFeatureDummy", ProcessorTrackerFeatureDummy)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorTrackerFeatureDummy", ProcessorTrackerFeatureDummy)
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 3328edf08c0234d6b587680b9b215bc91a7ae43f..0dd47af310ff8fc3b9eb967eea15c320a33d5e16 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -39,6 +39,10 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
public:
ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy();
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ProcessorParamsTrackerFeatureDummy);
+
virtual void configure(SensorBasePtr _sensor) { };
protected:
@@ -101,12 +105,6 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* This function emplaces a factor of the appropriate type for the derived processor.
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
- public:
-
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params);
-
};
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 72fc32f9baa1ce71216e6457d63185e057bcdc40..9195eb5597abf5d7f7b2562ce6f11931ec4d7726 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -112,4 +112,12 @@ FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _featu
return FactorBase::emplace<FactorLandmarkDummy>(_feature_ptr, _feature_ptr, _landmark_ptr, shared_from_this());
}
-} //namespace wolf
+} // namespace wolf
+
+// Register in the ProcessorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("ProcessorTrackerLandmarkDummy", ProcessorTrackerLandmarkDummy)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorTrackerLandmarkDummy", ProcessorTrackerLandmarkDummy)
+} // namespace wolf
+
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index a9531c8db7a0372636c1d0cb605c21c38aec8446..63beb8dac0f803c4afb6df89a9f8309a619ca1ec 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -34,6 +34,10 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
public:
ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
virtual ~ProcessorTrackerLandmarkDummy();
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ProcessorParamsTrackerLandmarkDummy);
+
virtual void configure(SensorBasePtr _sensor) { };
protected:
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 1e06c7a80377b99197a721e85473319f9bf34862..f661fb37fd65f4f99ecd7015d4e8474688521a18 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -66,6 +66,15 @@ class FixtureFactorBlockDifference : public testing::Test
virtual void TearDown() override {}
};
+TEST_F(FixtureFactorBlockDifference, CheckFactorType)
+{
+ // Feat_->setMeasurement()
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ Feat_, KF0_->getP(), KF1_->getP()
+ );
+ ASSERT_EQ(Fac->getType(), "FactorBlockDifference");
+}
+
TEST_F(FixtureFactorBlockDifference, EqualP)
{
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76d559a767f962ef7e82c41bffdded012dd4d04e
--- /dev/null
+++ b/test/gtest_factor_odom_2d.cpp
@@ -0,0 +1,109 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); // create and add
+
+TEST(FactorOdom2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorOdom2d, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ }
+}
+
+TEST(FactorOdom2d, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd8e2296c887aeb7ce6a07579eec939964d29a92
--- /dev/null
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -0,0 +1,109 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
+
+TEST(FactorRelative2dAnalytic, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelative2dAnalytic, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ }
+}
+
+TEST(FactorRelative2dAnalytic, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..547b4c9b7402a9b17eb695ea0e555869fb748a58
--- /dev/null
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -0,0 +1,219 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Sensor
+auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ProcessorParamsOdom2d>());
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->unfix();
+ sensor_odom2d->getO()->fix();
+ sensor_odom2d->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ //std::cout << "Sensor O: " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->unfix();
+ sensor_odom2d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index c4a1c4b0470c92bf09df1a950a1c19e57d35642d..5fbf82d7902281c3a78d0aab0bfe5d6b652cab34 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -23,14 +23,6 @@
using namespace wolf;
using namespace Eigen;
-
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
-} // namespace wolf
-
-
TEST(ProcessorBase, IsMotion)
{
using namespace wolf;
@@ -84,11 +76,13 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorOdom2d",
+ "SensorTrackerDummy",
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
- auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
+ auto proc_trk_params = make_shared<ProcessorParamsTrackerFeatureDummy>();
+ proc_trk_params->time_tolerance = dt/2;
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
// Install odometer (sensor and processor)
SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");