diff --git a/.gitignore b/.gitignore
index b690efc9b285d63727f435a9db3ee8b2039aee82..77906100d4ae252720e456622103b13d8c6371e5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,3 +34,5 @@ build_release/
.clangd
wolfcore.found
/wolf.found
+.ccls*
+compile_commands.json
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index dbf139b13a33b4b14ec3125cca2621f6c9e47381..1adcb74ceb72d69e5108c5fb639a45c7d2a0e1ee 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -67,7 +67,10 @@ wolf_build_and_test:
- mkdir -pv build
- cd build
- ls # we can check whether the directory was already full
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
- make -j$(nproc)
- ctest -j$(nproc)
+ # run demos
+ - ../bin/hello_wolf
+ - ../bin/hello_wolf_autoconf
- make install
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 246dffbc6f04c9054d91d8f09fb0daadf5052d98..72e9ec382ee49ea65b893c323e2a2f154ec649ad 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -216,19 +216,18 @@ SET(HDRS_CAPTURE
SET(HDRS_FACTOR
include/core/factor/factor_analytic.h
include/core/factor/factor_autodiff.h
- include/core/factor/factor_autodiff_distance_3d.h
include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h
include/core/factor/factor_block_difference.h
include/core/factor/factor_diff_drive.h
+ include/core/factor/factor_distance_3d.h
+ include/core/factor/factor_loopclosure_2d.h
include/core/factor/factor_odom_2d.h
- include/core/factor/factor_odom_2d_closeloop.h
- include/core/factor/factor_odom_2d_analytic.h
include/core/factor/factor_odom_3d.h
include/core/factor/factor_pose_2d.h
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.h
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
)
SET(HDRS_FEATURE
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index c0afa76e96229181122659244a20fd1fab433354..386d803138f33b89bee401aceda047fe2199f104 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -130,8 +130,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/demos/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt
index 81291a479cd3e524ba77b3676982ed0e140d965b..eff58ca207ecc5d2603e1839d59c3a69098d88fe 100644
--- a/demos/hello_wolf/CMakeLists.txt
+++ b/demos/hello_wolf/CMakeLists.txt
@@ -28,11 +28,4 @@ ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
TARGET_LINK_LIBRARIES(hello_wolf ${PLUGIN_NAME} hellowolf)
ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
-#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME})
TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME} hellowolf)
-
-#add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2d.cpp ../src/processor/processor_odom_2d.cpp)
-#TARGET_LINK_LIBRARIES(sensor_odom ${PLUGIN_NAME} hellowolf)
-#
-#add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
-#TARGET_LINK_LIBRARIES(range_bearing ${PLUGIN_NAME} hellowolf)
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index ef4f173494fbb602b2042088b065f45bf28ae43b..2b2d03d43f1a7d315efb0ae07a734ec022f9e4c6 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -233,12 +233,11 @@ int main()
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto& kf : *problem->getTrajectory())
- if (kf->isKey())
- {
- Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
- }
+ {
+ Eigen::MatrixXd cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index e975bb30fa55bb3cb39026a4377f7f4e62b2e58b..edeb6b1f04f1f673ad3af9b94493b4e67e46ab9f 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -219,12 +219,11 @@ int main()
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto& kf : *problem->getTrajectory())
- if (kf->isKey())
- {
- Eigen::MatrixXd cov;
- kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
- }
+ {
+ Eigen::MatrixXd cov;
+ kf->getCovariance(cov);
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ }
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 99f31be90ac5f4302127c0213c768e41ac9e9b80..4be898bc1a22d0af902f53e1280856cc76a67d74 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -16,10 +16,7 @@ config:
time_tolerance: 0.1
tree_manager:
- type: "TreeManagerSlidingWindow"
- n_frames: -1
- fix_first_frame: false
- viral_remove_empty_parent: true
+ type: "none"
solver:
max_num_iterations: 100
diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
index 328e7783982f12fe4991fbee9270af3878d1ff30..04d0ff166981259edc9ca51dc3e39260fd427a62 100644
--- a/demos/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -4,7 +4,6 @@ time_tolerance: 0.1
unmeasured_perturbation_std: 0.001
keyframe_vote:
voting_active: true
- voting_aux_active: false
max_time_span: 999
dist_traveled: 0.95
angle_turned: 999
diff --git a/demos/hello_wolf/yaml/processor_range_bearing.yaml b/demos/hello_wolf/yaml/processor_range_bearing.yaml
index 49a55d90dbb31b7fb18023eba9e71f7c1c03f633..c5a5608a42d47e02cfe1504aec59b99fea7e56c7 100644
--- a/demos/hello_wolf/yaml/processor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/processor_range_bearing.yaml
@@ -4,6 +4,5 @@ time_tolerance: 0.1
keyframe_vote:
voting_active: true
- voting_aux_active: false
apply_loss_function: true
diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml
index 2526e5dde7b256431a3f2fa5735b398a51faadb7..db3a961a1c3619ff44daab6b963d576a8595e880 100644
--- a/demos/processor_odom_3d.yaml
+++ b/demos/processor_odom_3d.yaml
@@ -5,7 +5,6 @@ unmeasured_perturbation_std: 0.001
keyframe_vote:
voting_active: false
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 40d3603da035205b145df7d1b7d43541a52b6a44..18a09ac3e24f36c3cef6b11c87dfc861e17d14be 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -82,7 +82,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
// State blocks
- const StateStructure& getStructure() const;
StateBlockPtr getStateBlock(const char& _key) const;
StateBlockPtr getSensorP() const;
StateBlockPtr getSensorO() const;
diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
index 203c38ef635cf02d6ded7aa0a0491169ef67a04d..2504d0b10bf669ececd7ce33e6ab9b15928eb26b 100644
--- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -12,7 +12,6 @@
#include "ceres/numeric_diff_cost_function.h"
// Factors
-#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_base.h"
namespace wolf {
diff --git a/include/core/factor/factor_autodiff_distance_3d.h b/include/core/factor/factor_distance_3d.h
similarity index 84%
rename from include/core/factor/factor_autodiff_distance_3d.h
rename to include/core/factor/factor_distance_3d.h
index bd0808978214570e80c02dae7e19a4ec5b96129c..d3f4ef7099e2f16def682de4abe7119dab4b688a 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_distance_3d.h
@@ -5,20 +5,20 @@
* \author: jsola
*/
-#ifndef FACTOR_AUTODIFF_DISTANCE_3d_H_
-#define FACTOR_AUTODIFF_DISTANCE_3d_H_
+#ifndef FACTOR_DISTANCE_3d_H_
+#define FACTOR_DISTANCE_3d_H_
#include "core/factor/factor_autodiff.h"
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3d);
+WOLF_PTR_TYPEDEFS(FactorDistance3d);
-class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d, 1, 3, 3>
+class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
{
public:
- FactorAutodiffDistance3d(const FeatureBasePtr& _feat,
+ FactorDistance3d(const FeatureBasePtr& _feat,
const FrameBasePtr& _frm_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
@@ -37,7 +37,7 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
{
}
- ~FactorAutodiffDistance3d() override { /* nothing */ }
+ ~FactorDistance3d() override { /* nothing */ }
std::string getTopology() const override
{
@@ -74,4 +74,4 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
} /* namespace wolf */
-#endif /* FACTOR_AUTODIFF_DISTANCE_3d_H_ */
+#endif /* FACTOR_DISTANCE_3d_H_ */
diff --git a/include/core/factor/factor_loopclosure_2d.h b/include/core/factor/factor_loopclosure_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a4c31a521e5ff95d81508f1ae53ece6e0fcca2d
--- /dev/null
+++ b/include/core/factor/factor_loopclosure_2d.h
@@ -0,0 +1,42 @@
+#ifndef FACTOR_LOOPCLOSURE_2d_H_
+#define FACTOR_LOOPCLOSURE_2d_H_
+
+//Wolf includes
+#include "core/factor/factor_relative_pose_2d.h"
+#include <Eigen/StdVector>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorLoopclosure2d);
+
+//class
+class FactorLoopclosure2d : public FactorRelativePose2d
+{
+ public:
+ FactorLoopclosure2d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorRelativePose2d("FactorLoopclosure2d",
+ _ftr_ptr,
+ _frame_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
+ {
+ //
+ }
+
+ ~FactorLoopclosure2d() override = default;
+
+ std::string getTopology() const override
+ {
+ return std::string("LOOP");
+ }
+
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index 298cc35840163d92a353a450ed118360e2acf3d5..b59cc723c7f8218ee39b18e0a160ece3b6eda74b 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -1,35 +1,29 @@
-#ifndef FACTOR_ODOM_2d_THETA_H_
-#define FACTOR_ODOM_2d_THETA_H_
+#ifndef FACTOR_ODOM_2d_H_
+#define FACTOR_ODOM_2d_H_
//Wolf includes
-#include "core/factor/factor_autodiff.h"
-#include "core/frame/frame_base.h"
-#include "core/math/rotations.h"
-
-//#include "ceres/jet.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include <Eigen/StdVector>
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorOdom2d);
-
+
//class
-class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
+class FactorOdom2d : public FactorRelativePose2d
{
public:
FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d",
- _ftr_ptr,
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorRelativePose2d("FactorOdom2d",
+ _ftr_ptr,
+ _frame_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
@@ -41,65 +35,8 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
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,
- T* _residuals) const;
-
-// public:
-// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
-// {
-// return std::make_shared<FactorOdom2d>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-// }
-
};
-template<typename T>
-inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
- const T* const _o2, 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);
- T o1 = _o1[0];
- T o2 = _o2[0];
- Eigen::Matrix<T, 3, 1> expected_measurement;
- Eigen::Matrix<T, 3, 1> er; // error
-
- // Expected measurement
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
- expected_measurement(2) = o2 - o1;
-
- // Error
- er = expected_measurement - getMeasurement();
- er(2) = pi2pi(er(2));
-
- // Residuals
- res = getMeasurementSquareRootInformationUpper() * er;
-
- ////////////////////////////////////////////////////////
- // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
-// using ceres::Jet;
-// Eigen::MatrixXd J(3,6);
-// J.row(0) = ((Jet<double, 6>)(er(0))).v;
-// J.row(1) = ((Jet<double, 6>)(er(1))).v;
-// J.row(2) = ((Jet<double, 6>)(er(2))).v;
-// J.row(0) = ((Jet<double, 6>)(res(0))).v;
-// J.row(1) = ((Jet<double, 6>)(res(1))).v;
-// J.row(2) = ((Jet<double, 6>)(res(2))).v;
-// if (sizeof(er(0)) != sizeof(double))
-// {
-// std::cout << "FactorOdom2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2d::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_odom_2d_analytic.h
deleted file mode 100644
index dc1437e4fe441c8c9a6e9406097ac757f56d8c03..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_odom_2d_analytic.h
+++ /dev/null
@@ -1,51 +0,0 @@
-#ifndef FACTOR_ODOM_2d_ANALYTIC_H_
-#define FACTOR_ODOM_2d_ANALYTIC_H_
-
-//Wolf includes
-#include "core/factor/factor_relative_2d_analytic.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorOdom2dAnalytic);
-
-//class
-class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
-{
- public:
- FactorOdom2dAnalytic(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2dAnalytic("FactorOdom2dAnalytic",
- _ftr_ptr,
- _frame_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status)
- {
- //
- }
-
- ~FactorOdom2dAnalytic() override = default;
-
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
- public:
-// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
-// const NodeBasePtr& _correspondant_ptr,
-// const ProcessorBasePtr& _processor_ptr = nullptr)
-// {
-// return std::make_shared<FactorOdom2dAnalytic>(_feature_ptr,
-// std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-// }
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
deleted file mode 100644
index 16f1d677024f4dd97968dbfda45c0e3aaca82723..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ /dev/null
@@ -1,104 +0,0 @@
-#ifndef FACTOR_ODOM_2d_CLOSELOOP_H_
-#define FACTOR_ODOM_2d_CLOSELOOP_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(FactorOdom2dCloseloop);
-
-//class
-class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>
-{
- public:
- FactorOdom2dCloseloop(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>("FactorOdom2dCloseloop",
- _ftr_ptr,
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
- {
- //
- }
-
- virtual ~FactorOdom2dCloseloop() = default;
-
- std::string getTopology() const override
- {
- return std::string("LOOP");
- }
-
-
- template<typename T>
- bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
- T* _residuals) const;
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
- {
- return std::make_shared<FactorOdom2dCloseloop>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
- }
-
-};
-
-template<typename T>
-inline bool FactorOdom2dCloseloop::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
- const T* const _o2, 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);
- T o1 = _o1[0];
- T o2 = _o2[0];
- Eigen::Matrix<T, 3, 1> expected_measurement;
- Eigen::Matrix<T, 3, 1> er; // error
-
- // Expected measurement
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
- 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::MatrixXd J(3,6);
-// J.row(0) = ((Jet<double, 6>)(er(0))).v;
-// J.row(1) = ((Jet<double, 6>)(er(1))).v;
-// J.row(2) = ((Jet<double, 6>)(er(2))).v;
-// J.row(0) = ((Jet<double, 6>)(res(0))).v;
-// J.row(1) = ((Jet<double, 6>)(res(1))).v;
-// J.row(2) = ((Jet<double, 6>)(res(2))).v;
-// if (sizeof(er(0)) != sizeof(double))
-// {
-// std::cout << "FactorOdom2dCloseloop::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2dCloseloop::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2dCloseloop::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 6e966fbc69d2eabb5f809ed4f8d3450634771a20..a897c97bf3c977d775e37ae876c0bb669dd55614 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -72,7 +72,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
*
* In rotations.h, we have
* minus(q1,q2) = minus_right(q1,q2) = log_q(q1.conjugate() * q2); --> this is a local 'minus'
- * minus_left(q1,q2) = log_q(q2.conjugate() * q1); --> this is a global 'minus'
+ * minus_left(q1,q2) = log_q(q2 * q1.conjugate()); --> this is a global 'minus'
*/
Eigen::Matrix<T, 3, 1> er;
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_pose_2d.h
similarity index 93%
rename from include/core/factor/factor_relative_2d_analytic.h
rename to include/core/factor/factor_relative_pose_2d.h
index dfb06c429b10e205ffe93146269f32a05d664a7d..72e77ea03eb9989058c597bc02a13a32ac9d9cc6 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_RELATIVE_2d_ANALYTIC_H_
-#define FACTOR_RELATIVE_2d_ANALYTIC_H_
+#ifndef FACTOR_RELATIVE_POSE_2d_H_
+#define FACTOR_RELATIVE_POSE_2d_H_
//Wolf includes
#include "core/factor/factor_analytic.h"
@@ -9,16 +9,16 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorRelative2dAnalytic);
+WOLF_PTR_TYPEDEFS(FactorRelativePose2d);
//class
-class FactorRelative2dAnalytic : public FactorAnalytic
+class FactorRelativePose2d : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelativePose2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -43,7 +43,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelativePose2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -68,7 +68,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelativePose2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -96,7 +96,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
return std::string("GEOM");
}
- ~FactorRelative2dAnalytic() override = default;
+ ~FactorRelativePose2d() override = default;
/** \brief Returns the factor residual size
**/
@@ -139,7 +139,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/// IMPLEMENTATION ///
-inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
+inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
Eigen::VectorXd residual(3);
@@ -155,7 +155,7 @@ inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
return residual;
}
-inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -191,7 +191,7 @@ inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -227,7 +227,7 @@ inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2dAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+inline void FactorRelativePose2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 4);
diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h
index cb65fa06b01d72cb32d1209752b44d6826f0d76b..071976faf1419ea48fd00c37707d95bf344ec855 100644
--- a/include/core/feature/feature_odom_2d.h
+++ b/include/core/feature/feature_odom_2d.h
@@ -2,9 +2,8 @@
#define FEATURE_ODOM_2d_H_
//Wolf includes
+#include <core/factor/factor_odom_2d.h>
#include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_2d.h"
-#include "core/factor/factor_odom_2d_analytic.h"
//std includes
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 1b368bf02df6a89567d90455edba72cf02f54bf0..2f790d6335dadc8d4c96d13d20948a8009da517d 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -8,15 +8,6 @@ namespace wolf{
class TrajectoryBase;
class CaptureBase;
class StateBlock;
-
-/** \brief Enumeration of frame types
- */
-typedef enum
-{
- KEY = 1, ///< key frame. It plays at optimizations (estimated).
- // AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
- NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
-} FrameType;
}
//Wolf includes
@@ -45,7 +36,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
protected:
unsigned int frame_id_;
- FrameType type_; ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h)
TimeStamp time_stamp_; ///< frame time stamp
public:
@@ -58,36 +48,34 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
* \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
**/
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+ FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _v_ptr = nullptr);
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
+ FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
const VectorComposite& _state);
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
+ FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
const std::list<VectorXd>& _vectors);
~FrameBase() override;
+
+ // Add and remove from WOLF tree ---------------------------------
+ template<typename classType, typename... T>
+ static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+
+ void link(TrajectoryBasePtr);
+ void link(ProblemPtr _prb);
+
virtual void remove(bool viral_remove_empty_parent=false);
// Frame properties -----------------------------------------------
public:
unsigned int id() const;
- // get type
- bool isKey() const;
-
- // set type
- void setNonEstimated();
- void setKey(ProblemPtr _prb);
-
// Frame values ------------------------------------------------
public:
void setTimeStamp(const TimeStamp& _ts);
@@ -96,8 +84,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// State blocks ----------------------------------------------------
public:
- using HasStateBlocks::addStateBlock;
- StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb);
StateBlockPtr getV() const;
protected:
@@ -122,17 +108,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
-
void getFactorList(FactorBasePtrList& _fac_list) const;
+
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
bool isConstrainedBy(const FactorBasePtr& _factor) const;
- void link(TrajectoryBasePtr);
- template<typename classType, typename... T>
- static std::shared_ptr<classType> emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all);
- template<typename classType, typename... T>
- static std::shared_ptr<classType> createNonKeyFrame(T&&... all);
+
+ // Debug and info -------------------------------------------------------
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
@@ -172,30 +155,18 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
namespace wolf {
template<typename classType, typename... T>
-std::shared_ptr<classType> FrameBase::emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all)
+std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
{
- std::shared_ptr<classType> frm = std::make_shared<classType>(FrameType::KEY, std::forward<T>(all)...);
+ std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
frm->link(_ptr);
return frm;
}
-template<typename classType, typename... T>
-std::shared_ptr<classType> FrameBase::createNonKeyFrame(T&&... all)
-{
- std::shared_ptr<classType> frm = std::make_shared<classType>(FrameType::NON_ESTIMATED, std::forward<T>(all)...);
- return frm;
-}
-
inline unsigned int FrameBase::id() const
{
return frame_id_;
}
-inline bool FrameBase::isKey() const
-{
- return (type_ == KEY);
-}
-
inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
{
_ts = time_stamp_;
@@ -231,17 +202,6 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
return constrained_by_list_;
}
-inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type,
- const StateBlockPtr& _sb)
-{
- if (isKey())
- HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
- else
- HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
-
- return _sb;
-}
-
inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index a051a63e3989f4f0363fc96a8ed462fd1b407cea..ffd8ac8725e49ea921b32140042eb0936cf5d467 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -36,6 +36,12 @@ inline Eigen::Matrix<T, 2, 2> exp(const T theta)
namespace SE2{
+template<class T>
+inline Eigen::Matrix<T, 2, 2> skew(const T& t)
+{
+ return (Eigen::Matrix<T, 2, 2>() << (T) 0, -t, t, (T) 0).finished();
+}
+
/** \brief Compute [1]_x * t = (-t.y ; t.x)
*/
template<class D>
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 002015f83efac35a081aa24873f9c684cb1b36b7..dc810f2764b895923f7a0516f6e7a6e14e2ebc5d 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -69,7 +69,6 @@ class Problem : public std::enable_shared_from_this<Problem>
PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure); // USE create() below !!
Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
void setup();
@@ -206,7 +205,6 @@ class Problem : public std::enable_shared_from_this<Problem>
const double &_time_tol);
/** \brief Emplace frame from string frame_structure, dimension and vector
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
@@ -217,13 +215,12 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim, //
const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame from string frame_structure and state
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _frame_state State vector; must match the size and format of the chosen frame structure
@@ -235,12 +232,11 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const VectorComposite& _frame_state);
/** \brief Emplace frame from state
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_state State; must be part of the problem's frame structure
*
@@ -252,11 +248,10 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const VectorComposite& _frame_state);
/** \brief Emplace frame from string frame_structure and dimension
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
@@ -269,12 +264,11 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim);
/** \brief Emplace frame from state vector
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
* \param _frame_state State vector; must match the size and format of the chosen frame structure
*
@@ -286,11 +280,10 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame, guess all values
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
*
* - The structure is taken from Problem
@@ -302,11 +295,10 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
// Frame getters
FrameBasePtr getLastFrame( ) const;
- FrameBasePtr getLastKeyOrAuxFrame( ) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
/** \brief Give the permission to a processor to create a new key Frame
@@ -325,22 +317,6 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorBasePtr _processor_ptr, //
const double& _time_tolerance);
- /** \brief Give the permission to a processor to create a new auxiliary Frame
- *
- * This should implement a black list of processors that have forbidden auxiliary frame creation.
- * - This decision is made at problem level, not at processor configuration level.
- * - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
- */
- bool permitAuxFrame(ProcessorBasePtr _processor_ptr) const;
-
- /** \brief New auxiliary frame callback
- *
- * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion.
- */
- void auxFrameCallback(FrameBasePtr _frame_ptr, //
- ProcessorBasePtr _processor_ptr, //
- const double& _time_tolerance);
-
// State getters
TimeStamp getTimeStamp ( ) const;
VectorComposite getState ( const StateStructure& _structure = "" ) const;
@@ -373,7 +349,6 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
- bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a7d3049a54aa576ccc2c0422f9e20e7744c08d27..c8945c8036ff54dc2e0fe2243b3ec0919c66eba4 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -227,7 +227,6 @@ struct ParamsProcessorBase : public ParamsBase
{
time_tolerance = _server.getParam<double>(prefix + _unique_name + "/time_tolerance");
voting_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_active");
- voting_aux_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_aux_active");
apply_loss_function = _server.getParam<bool>(prefix + _unique_name + "/apply_loss_function");
}
@@ -239,13 +238,11 @@ struct ParamsProcessorBase : public ParamsBase
*/
double time_tolerance;
bool voting_active; ///< Whether this processor is allowed to vote for a Key Frame or not
- bool voting_aux_active; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
bool apply_loss_function; ///< Whether this processor emplaces factors with loss function or not
std::string print() const override
{
return "voting_active: " + std::to_string(voting_active) + "\n"
- + "voting_aux_active: " + std::to_string(voting_aux_active) + "\n"
+ "time_tolerance: " + std::to_string(time_tolerance) + "\n"
+ "apply_loss_function: " + std::to_string(apply_loss_function) + "\n";
}
@@ -337,19 +334,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
virtual bool voteForKeyFrame() const = 0;
- /** \brief Vote for Auxiliary Frame generation
- *
- * If a Auxiliary Frame criterion is validated, this function returns true,
- * meaning that it wants to create a Auxiliary Frame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create Auxiliary Frames!
- */
- virtual bool voteForAuxFrame() const {return false;};
-
virtual bool permittedKeyFrame() final;
- virtual bool permittedAuxFrame() final;
-
void setProblem(ProblemPtr) override;
public:
@@ -377,8 +363,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool isVotingActive() const;
- bool isVotingAuxActive() const;
-
void setVotingActive(bool _voting_active = true);
int getDim() const;
@@ -388,7 +372,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void link(SensorBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
- void setVotingAuxActive(bool _voting_active = true);
virtual void printHeader(int depth, //
bool constr_by, //
@@ -439,21 +422,11 @@ inline bool ProcessorBase::isVotingActive() const
return params_->voting_active;
}
-inline bool ProcessorBase::isVotingAuxActive() const
-{
- return params_->voting_aux_active;
-}
-
inline void ProcessorBase::setVotingActive(bool _voting_active)
{
params_->voting_active = _voting_active;
}
-inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
-{
- params_->voting_aux_active = _voting_active;
-}
-
inline bool ProcessorBase::isMotion() const
{
// check if this inherits from IsMotion
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 427e74dc7c259d3abd3147fe5cf13608740d3472..7203ff3fd48833c5be92de7445dc174193e6a3e1 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -60,7 +60,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -71,7 +71,10 @@ class ProcessorDiffDrive : public ProcessorOdom2d
inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
{
- return _capture->getStateBlock('I')->getState();
+ if (_capture)
+ return _capture->getStateBlock('I')->getState();
+ else
+ return getSensor()->getStateBlockDynamic('I')->getState();
}
inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index c621f61c3ac0c09943a7f58789763c4f14b8e115..75053ab0f35d1c338092c723573d5cfbf85755b6 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -463,7 +463,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
public:
- virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
bool hasCalibration() const {return calib_size_ > 0;}
//getters
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 2ad5b846b59e3c40d98351a0f8b30f75b3f71635..254eded5ccc644faf5b6c20e128f55da77c92b95 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -10,7 +10,6 @@
#include "core/processor/processor_motion.h"
#include "core/capture/capture_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
#include "core/utils/params_server.h"
#include "core/math/SE2.h"
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 676cb6cd05d51240cad7a5008ca301ccb5af67e3..438dcc33b0e02f62c241b6852cd5d4fd29c4184e 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -114,13 +114,6 @@ class ProcessorOdom3d : public ProcessorMotion
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
- // noise parameters (stolen from owner SensorOdom3d)
- double k_disp_to_disp_; // displacement variance growth per meter of linear motion
- double k_disp_to_rot_; // orientation variance growth per meter of linear motion
- double k_rot_to_rot_; // orientation variance growth per radian of rotational motion
- double min_disp_var_; // floor displacement variance when no motion
- double min_rot_var_; // floor orientation variance when no motion
-
};
inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 4c27ecb6e45bcd2ff44ea5a20d22705d6c024b6d..f4cae62c31f2c8389107388bdebedbec72a68b5b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -90,7 +90,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
private:
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
- SizeEigen calib_size_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
@@ -234,9 +233,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
unsigned int _start_idx = 0,
int _size = -1);
- SizeEigen getCalibSize() const;
- Eigen::VectorXd getCalibration() const;
-
void setNoiseStd(const Eigen::VectorXd & _noise_std);
void setNoiseCov(const Eigen::MatrixXd & _noise_std);
Eigen::VectorXd getNoiseStd() const;
@@ -261,12 +257,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
-
- protected:
- SizeEigen computeCalibSize() const;
-
- private:
- void updateCalibSize();
};
}
@@ -356,16 +346,6 @@ inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eige
addPriorParameter('I', _x, _cov);
}
-inline SizeEigen SensorBase::getCalibSize() const
-{
- return calib_size_;
-}
-
-inline void SensorBase::updateCalibSize()
-{
- calib_size_ = computeCalibSize();
-}
-
} // namespace wolf
#endif
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index 5f2fc1a717b7c58f985ede8845ea2de01dc17fcb..ca06c7c010bc153756650f35c55e01114fdf4e34 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -62,6 +62,25 @@ class SensorOdom2d : public SensorBase
**/
double getRotVarToRotNoiseFactor() const;
+ /**
+ * Compute covariance of odometry given the motion data.
+ *
+ * NOTE: This is a helper function for the user, not called automatically anywhere.
+ *
+ * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+ * - a linear dependence with total displacement
+ * - a linear dependence with total rotation
+ *
+ * The formula for the variances is as follows:
+ *
+ * - disp_var = k_disp_to_disp * displacement
+ * - rot_var = k_rot_to_rot * rotation
+ *
+ * See implementation for details.
+ */
+ Matrix3d computeCovFromMotion (const VectorXd& _data) const;
+
+
};
} // namespace wolf
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index ab0e8b46872c21015b02ffcb9d521dbd77e2a606..d5f5b417eb36a0628a3425822d03b3fef494fb36 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -72,6 +72,26 @@ class SensorOdom3d : public SensorBase
double getMinDispVar() const;
double getMinRotVar() const;
+ /**
+ * Compute covariance of odometry given the motion data.
+ *
+ * NOTE: This is a helper function for the user, not called automatically anywhere.
+ *
+ * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
+ * - a minimal value for displacement
+ * - a minimal value for rotation
+ * - a linear dependence with total displacement
+ * - a linear dependence with total rotation
+ *
+ * The formula for the variances is as follows:
+ *
+ * - disp_var = disp_var_min + k_disp_to_disp * displacement
+ * - rot_var = rot_var_min + k_disp_to_rot * displacement + k_rot_to_rot * rotation
+ *
+ * See implementation for details.
+ */
+ Matrix6d computeCovFromMotion (const VectorXd& _data) const;
+
};
inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index fb38a8be279f9ed6572c729fdebe1551bb99f4ae..58d21a5cf834cc18372a197b3bbbead9894e46f6 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -54,10 +54,12 @@ class SolverManager
*/
enum class CovarianceBlocksToBeComputed : std::size_t
{
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
- GAUSS
+ ALL = 0, ///< All blocks and all cross-covariances
+ ALL_MARGINALS = 1, ///< All marginals
+ ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+ GAUSS = 3, ///< GAUSS: last frame P and V
+ GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T
+ GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T
};
/**
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index b586278c7e7f5a7cb949b78d2e90f58323031109..b89d49c8cac06092aaaada411598c20514fab574 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -9,13 +9,13 @@
#define TRUNK_SRC_SOLVER_QR_SOLVER_H_
//std includes
+#include <core/factor/factor_odom_2d.h>
#include <iostream>
#include <ctime>
//Wolf includes
#include "core/state_block/state_block.h"
#include "../factor_sparse.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_corner_2d.h"
#include "core/factor/factor_container.h"
#include "core/solver_suitesparse/sparse_utils.h"
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 5e1250ff066611a40af092e937324e840e58ae0e..418e0909fdfd7e32f42347dee4644cb6eba73c97 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -21,7 +21,7 @@ class HasStateBlocks
{
public:
HasStateBlocks();
- HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
+ HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
virtual ~HasStateBlocks();
const StateStructure& getStructure() const { return structure_; }
@@ -73,7 +73,10 @@ class HasStateBlocks
unsigned int getLocalSize(const StateStructure& _structure="") const;
// Perturb state
- void perturb(double amplitude = 0.01);
+ void perturb(double amplitude = 0.01);
+
+ protected:
+ void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
private:
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index cb89fa517f981153b84812b3382e852989d3f8a1..431524af94e218b17f2ce223ca47483c0d3cd058 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -12,6 +12,7 @@ class FrameBase;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/state_block/state_composite.h"
//std includes
@@ -46,9 +47,6 @@ class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_revers
return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
}
};
-// typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
-// typedef std::map<TimeStamp, FrameBasePtr>::const_iterator TrajectoryIter;
-// typedef std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator TrajectoryRevIter;
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
@@ -59,9 +57,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
FrameMap frame_map_;
protected:
- std::string frame_structure_; // Defines the structure of the Frames in the Trajectory.
- // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
- // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
+ StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
public:
TrajectoryBase();
@@ -104,14 +100,8 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
inline const FrameMap& TrajectoryBase::getFrameMap() const
{
return frame_map_;
- // return frame_map_;
}
-// inline FrameBasePtr TrajectoryBase::getLastFrame() const
-// {
-// auto it = static_cast<TrajectoryRevIter>(frame_map_.rbegin());
-// return *it;
-// }
inline FrameBasePtr TrajectoryBase::getFirstFrame() const
{
auto it = static_cast<TrajectoryIter>(frame_map_.begin());
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index c2d06be8d17f1c2075b225b86ab7e09cf06a7aad..706a4092c5b0a46f520f57129906898db7c2525c 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -16,19 +16,21 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
ParamsTreeManagerBase(_unique_name, _server)
{
n_frames = _server.getParam<unsigned int>(prefix + "/n_frames");
- fix_first_frame = _server.getParam<bool> (prefix + "/fix_first_frame");
+ n_fix_first_frames = _server.getParam<unsigned int>(prefix + "/n_fix_first_frames");
viral_remove_empty_parent = _server.getParam<bool> (prefix + "/viral_remove_empty_parent");
+ if (n_frames <= n_fix_first_frames)
+ throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!");
}
std::string print() const override
{
return ParamsTreeManagerBase::print() + "\n"
+ "n_frames: " + std::to_string(n_frames) + "\n"
- + "fix_first_frame: " + std::to_string(fix_first_frame) + "\n"
+ + "fix_first_frame: " + std::to_string(n_fix_first_frames) + "\n"
+ "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
}
unsigned int n_frames;
- bool fix_first_frame;
+ unsigned int n_fix_first_frames;
bool viral_remove_empty_parent;
};
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
index e810c11ba34699bbe9bc1537b5995d2670049862..f0e20f4b5eec08e6f322424f7353ca124b448e90 100644
--- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -38,7 +38,7 @@ class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
~TreeManagerSlidingWindowDualRate() override{}
- void keyFrameCallback(FrameBasePtr _key_frame) override;
+ void keyFrameCallback(FrameBasePtr _frame) override;
protected:
ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index d06149ba3f047bcca63f9e3d058e0220bbfb68bc..f29109d1862d134aa592df4b4c843cac6b0a8bd0 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -152,14 +152,6 @@ bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
}
-const StateStructure& CaptureBase::getStructure() const
-{
- if (getSensor())
- return getSensor()->getStructure();
- else
- return HasStateBlocks::getStructure();
-}
-
StateBlockPtr CaptureBase::getStateBlock(const char& _key) const
{
if (getSensor() and getSensor()->hasStateBlock(_key))
@@ -185,11 +177,9 @@ void CaptureBase::unfix()
void CaptureBase::move(FrameBasePtr _frm_ptr)
{
- WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
- WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr");
+ WOLF_WARN_COND(this->getFrame() == nullptr, "Moving Capture ", id(), " at ts=", getTimeStamp(), " not linked to any frame. Consider just linking it with link() instead of move()!");
- assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF");
- assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame");
+ assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!");
// Unlink
if (this->getFrame())
@@ -218,7 +208,7 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
}
else
{
- WOLF_WARN("Linking with a nullptr");
+ WOLF_WARN("Linking Capture ", id(), " to a nullptr");
}
}
@@ -227,7 +217,7 @@ void CaptureBase::setProblem(ProblemPtr _problem)
if (_problem == nullptr || _problem == this->getProblem())
return;
- assert(getFrame() and getFrame()->isKey());
+ assert(getFrame() && "Cannot set problem: Capture has no Frame!");
NodeBase::setProblem(_problem);
registerNewStateBlocks(_problem);
@@ -244,6 +234,10 @@ void CaptureBase::setProblem(ProblemPtr _problem)
void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
+ _stream << _tabs << "Cap" << id()
+ << " " << getType()
+ << " ts = " << std::setprecision(3) << getTimeStamp();
+
if(getSensor() != nullptr)
{
_stream << " -> Sen" << getSensor()->id();
@@ -261,34 +255,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
}
_stream << std::endl;
- if(getStateBlockMap().size() > 0)
- {
- if (_metric && _state_blocks){
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb)
- _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << std::endl;
- }
- }
- else if (_metric)
- {
- _stream << _tabs << (isFixed() ? "Fix" : "Est");
- _stream << ",\t x= ( " << std::setprecision(2) << getStateVector() << " )";
- _stream << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << " sb:";
- for (const auto& key : getStructure())
- {
- const auto& sb = getStateBlock(key);
- if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
- }
- _stream << std::endl;
- }
- }
+ if (getStateBlockMap().size() > 0)
+ printState(_metric, _state_blocks, _stream, _tabs);
}
void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -405,7 +373,7 @@ CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::os
match_any_prc_timetolerance = match_any_prc_timetolerance or (time_diff <= prc->getTimeTolerance());
}
inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
- << " ts =" << getTimeStamp() << ((frame->isKey()) ? "KFrm" : "Frm") << frame->id()
+ << " ts =" << getTimeStamp() << "Frm" << frame->id()
<< " ts = " << frame->getTimeStamp() << " their time difference (" << time_diff << ") does not match any time tolerance of"
<< " any processor in sensor " << getSensor()->id() << "\n";
log.assertTrue((match_any_prc_timetolerance), inconsistency_explanation);
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index bad56dae309cca61a119594e3b396444409e267b..16a535dc987deea66756f4c8468ac3005f2b3573 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -75,7 +75,9 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera
void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "CapM" << id() << " " << getType() << " ts = " << std::setprecision(3) << getTimeStamp();
+ _stream << _tabs << "CapM" << id()
+ << " " << getType()
+ << " ts = " << std::setprecision(3) << getTimeStamp();
if(getSensor() != nullptr)
{
@@ -83,11 +85,12 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
}
else
_stream << " -> Sen-";
+
if (auto OC = getOriginCapture())
{
- _stream << " -> OCap" << OC->id();
+ _stream << " -> Origin: Cap" << OC->id();
if (auto OF = OC->getFrame())
- _stream << " ; OFrm" << OF->id();
+ _stream << " ; Frm" << OF->id();
}
_stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
@@ -99,18 +102,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
}
_stream << std::endl;
- if (_state_blocks)
- for (const auto& sb : getStateBlockVec())
- {
- if(sb != nullptr)
- {
- _stream << _tabs << " " << "sb: ";
- _stream << (sb->isFixed() ? "Fix" : "Est");
- if (_metric)
- _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- _stream << " @ " << sb << std::endl;
- }
- }
+ if (getStateBlockMap().size() > 0)
+ printState(_metric, _state_blocks, _stream, _tabs);
_stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl;
if ( _metric && ! getBuffer().empty())
@@ -120,8 +113,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
// {
// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
-//// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
-//// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
// }
}
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 7f33435f917a913eb4bf9cbf7b541634ac2a8e1c..a2d7616c58181be35b9aae642e0e8e9f956b4879 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -99,12 +99,11 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
for(auto fr_ptr : *wolf_problem_->getTrajectory())
- if (fr_ptr->isKey())
- for (const auto& key : fr_ptr->getStructure())
- {
- const auto& sb = fr_ptr->getStateBlock(key);
- all_state_blocks.push_back(sb);
- }
+ for (const auto& key : fr_ptr->getStructure())
+ {
+ const auto& sb = fr_ptr->getStateBlock(key);
+ all_state_blocks.push_back(sb);
+ }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
@@ -131,23 +130,22 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
{
// first create a vector containing all state blocks
for(auto fr_ptr : *wolf_problem_->getTrajectory())
- if (fr_ptr->isKey())
- for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for (const auto& key1 : wolf_problem_->getFrameStructure())
+ {
+ const auto& sb1 = fr_ptr->getStateBlock(key1);
+ assert(isStateBlockRegisteredDerived(sb1));
+
+ for (const auto& key2 : wolf_problem_->getFrameStructure())
{
- const auto& sb1 = fr_ptr->getStateBlock(key1);
- assert(isStateBlockRegisteredDerived(sb1));
-
- for (const auto& key2 : wolf_problem_->getFrameStructure())
- {
- const auto& sb2 = fr_ptr->getStateBlock(key2);
- assert(isStateBlockRegisteredDerived(sb2));
-
- state_block_pairs.emplace_back(sb1, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
- if (sb1 == sb2)
- break;
- }
+ const auto& sb2 = fr_ptr->getStateBlock(key2);
+ assert(isStateBlockRegisteredDerived(sb2));
+
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
+ break;
}
+ }
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
@@ -233,34 +231,159 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// - Last KF: PV
// last KF
- FrameBasePtr last_frame = wolf_problem_->getLastFrame();
- if (not last_frame)
+ FrameBasePtr frame = wolf_problem_->getLastFrame();
+ if (not frame)
return false;
- // Error
- if (not last_frame->hasStateBlock('P') or
- not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or
- not last_frame->hasStateBlock('V') or
- not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')))
+ while (frame)
{
- WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P or V, returning...");
- WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found");
- WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
- WOLF_WARN_COND(not last_frame->getStateBlock('V'), "SolverCeres::computeCovariances: KF state block 'V' not found");
- WOLF_WARN_COND(last_frame->getStateBlock('V') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')), "SolverCeres::computeCovariances: KF state block 'V' not registered ", last_frame->getStateBlock('V'));
-
+ if (frame->hasStateBlock('P') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ frame->hasStateBlock('V') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('V')))
+ {
+ break;
+ }
+ //else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ frame = frame->getPreviousFrame();
+ }
+ if (not frame)
+ {
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and V, returning...");
return false;
}
// only marginals of P and V
- state_block_pairs.emplace_back(last_frame->getStateBlock('P'),
- last_frame->getStateBlock('P'));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('P')),
- getAssociatedMemBlockPtr(last_frame->getStateBlock('P')));
- state_block_pairs.emplace_back(last_frame->getStateBlock('V'),
- last_frame->getStateBlock('V'));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')),
- getAssociatedMemBlockPtr(last_frame->getStateBlock('V')));
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
+ "\nP @", frame->getStateBlock('P'),
+ "\nV @", frame->getStateBlock('V'));
+ state_block_pairs.emplace_back(frame->getStateBlock('P'),
+ frame->getStateBlock('P'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('P')));
+ state_block_pairs.emplace_back(frame->getStateBlock('V'),
+ frame->getStateBlock('V'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('V')));
+
+ break;
+ }
+ case CovarianceBlocksToBeComputed::GAUSS_TUCAN:
+ {
+ // State blocks:
+ // - Last KF: PV
+
+ // last KF
+ FrameBasePtr frame = wolf_problem_->getLastFrame();
+ if (not frame)
+ return false;
+
+ StateBlockPtr sb_gnss_T;
+ while (frame)
+ {
+ // get capture gnss
+ for (CaptureBasePtr cap : frame->getCaptureList())
+ if (cap->hasStateBlock('T'))
+ {
+ sb_gnss_T = cap->getStateBlock('T');
+ break;
+ }
+
+ if (frame->hasStateBlock('P') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ frame->hasStateBlock('O') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
+ frame->hasStateBlock('V') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('V')) and
+ sb_gnss_T and
+ isStateBlockRegisteredDerived(sb_gnss_T))
+ {
+ break;
+ }
+ // else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ frame = frame->getPreviousFrame();
+ }
+ if (not frame)
+ {
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, O, V and CaptureGnss with T, returning...");
+ return false;
+ }
+
+ // only marginals of P, O, V and T
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
+ "\nP @", frame->getStateBlock('P'),
+ "\nO @", frame->getStateBlock('O'),
+ "\nV @", frame->getStateBlock('V'),
+ "\nT @", sb_gnss_T);
+ state_block_pairs.emplace_back(frame->getStateBlock('P'),
+ frame->getStateBlock('P'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('P')));
+ state_block_pairs.emplace_back(frame->getStateBlock('O'),
+ frame->getStateBlock('O'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('O')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('O')));
+ state_block_pairs.emplace_back(frame->getStateBlock('V'),
+ frame->getStateBlock('V'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('V')));
+ state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
+ getAssociatedMemBlockPtr(sb_gnss_T));
+
+ break;
+ }
+ case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION:
+ {
+ // State blocks:
+ // - Last KF: PV
+
+ // last KF
+ FrameBasePtr frame = wolf_problem_->getLastFrame();
+ if (not frame)
+ return false;
+
+ StateBlockPtr sb_gnss_T;
+ while (frame)
+ {
+ // get capture gnss
+ for (CaptureBasePtr cap : frame->getCaptureList())
+ if (cap->hasStateBlock('T'))
+ {
+ sb_gnss_T = cap->getStateBlock('T');
+ break;
+ }
+
+ if (frame->hasStateBlock('P') and
+ isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
+ sb_gnss_T and
+ isStateBlockRegisteredDerived(sb_gnss_T))
+ {
+ break;
+ }
+ // else
+ WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
+ frame = frame->getPreviousFrame();
+ }
+ if (not frame)
+ {
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, returning...");
+ return false;
+ }
+
+ // only marginals of P and T
+ WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
+ "\nP @", frame->getStateBlock('P'),
+ "\nT @", sb_gnss_T);
+ state_block_pairs.emplace_back(frame->getStateBlock('P'),
+ frame->getStateBlock('P'));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
+ getAssociatedMemBlockPtr(frame->getStateBlock('P')));
+ state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
+ getAssociatedMemBlockPtr(sb_gnss_T));
break;
}
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index a0b4bc90005cb0ffa48c7ef21d87c758a7a52a36..c03fe00ad9b3a6018747b07572d0d6c1dbfd4a56 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -260,10 +260,6 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
- // if frame, should be key
- if (getCapture() and getFrame())
- assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
-
// set problem ( and register factor )
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
this->setProblem(_ftr_ptr->getProblem());
@@ -274,7 +270,7 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
auto frame_other = frm_ow.lock();
if(frame_other != nullptr)
{
- assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other.");
+ assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other.");
frame_other->addConstrainedBy(shared_from_this());
}
}
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index b864e27c8172671572026ce4f83b4d279bb1b669..c2af9127c6a2e028d98f4f5413490117bfbd7825 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -194,7 +194,6 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "id: " << id() << std::endl;
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 4)
for (auto c : getFactorList())
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 20ab9331867cd18963b5bc85b0f42fa1c01b4a2f..cbf11f07e9d8e6173ed3812c1152834fb275cf75 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -12,15 +12,13 @@ namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
+FrameBase::FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
const VectorComposite& _state) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- type_(_tp),
time_stamp_(_ts)
{
for (const auto& pair_key_vec : _state)
@@ -30,20 +28,18 @@ FrameBase::FrameBase(const FrameType & _tp,
StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
- addStateBlock(key, sb);
+ addStateBlock(key, sb, getProblem());
}
}
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
+FrameBase::FrameBase(const TimeStamp& _ts,
+ const StateStructure& _frame_structure,
const std::list<VectorXd>& _vectors) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- type_(_tp),
time_stamp_(_ts)
{
assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
@@ -55,15 +51,14 @@ FrameBase::FrameBase(const FrameType & _tp,
StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
- addStateBlock(key, sb);
+ addStateBlock(key, sb, getProblem());
vec_it++;
}
}
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
+FrameBase::FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _v_ptr) :
@@ -71,20 +66,19 @@ FrameBase::FrameBase(const FrameType & _tp,
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- type_(_tp),
time_stamp_(_ts)
{
if (_p_ptr)
{
- addStateBlock('P', _p_ptr);
+ addStateBlock('P', _p_ptr, getProblem());
}
if (_o_ptr)
{
- addStateBlock('O', _o_ptr);
+ addStateBlock('O', _o_ptr, getProblem());
}
if (_v_ptr)
{
- addStateBlock('V', _v_ptr);
+ addStateBlock('V', _v_ptr, getProblem());
}
}
@@ -116,8 +110,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
}
// Remove Frame State Blocks
- if ( isKey() )
- removeStateBlocks(getProblem());
+ removeStateBlocks(getProblem());
// remove from upstream
TrajectoryBasePtr T = trajectory_ptr_.lock();
@@ -133,20 +126,9 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts)
time_stamp_ = _ts;
}
-void FrameBase::setNonEstimated()
+void FrameBase::link(ProblemPtr _prb)
{
- // unregister if previously estimated
- if (isKey())
- for (const auto& sb : getStateBlockVec())
- getProblem()->notifyStateBlock(sb, REMOVE);
-
- type_ = NON_ESTIMATED;
-}
-
-void FrameBase::setKey(ProblemPtr _prb)
-{
- assert(_prb != nullptr && "setting Key fram with a null problem pointer");
- type_ = KEY;
+ assert(_prb != nullptr && "Trying to link Frame with a null problem pointer!");
this->link(_prb->getTrajectory());
}
@@ -287,7 +269,6 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
assert(this->getTrajectory() == nullptr && "linking an already linked frame");
- assert(isKey() && "Trying to link a non keyframe");
if(_trj_ptr)
{
@@ -297,14 +278,12 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
}
else
{
- WOLF_WARN("Linking with a nullptr");
+ WOLF_WARN("Linking Frame ", id(), " to a nullptr");
}
}
void FrameBase::setProblem(ProblemPtr _problem)
{
- assert(isKey() && "Trying to setProblem to a non keyframe");
-
if (_problem == nullptr || _problem == this->getProblem())
return;
@@ -317,7 +296,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << (isKey() ? "KFrm" : "Frm") << id()
+ _stream << _tabs << "Frm" << id()
<< " " << getStructure()
<< " ts = " << std::setprecision(3) << getTimeStamp()
<< ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
@@ -330,36 +309,7 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
}
_stream << std::endl;
- if (_metric && _state_blocks)
- {
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb)
- _stream << _tabs << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
- << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
- << " @ " << sb
- << std::endl;
- }
- }
- else if (_metric)
- {
- _stream << _tabs << " " << (isFixed() ? "Fix" : "Est");
- _stream << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )";
- _stream << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << _tabs << " " << "sb:";
- for (const auto& key : getStructure())
- {
- const auto& sb = getStateBlock(key);
- if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
- }
- _stream << std::endl;
- }
+ printState(_metric, _state_blocks, _stream, _tabs);
}
void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
@@ -377,7 +327,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
std::stringstream inconsistency_explanation;
if (_verbose) {
- _stream << _tabs << (isKey() ? "KFrm" : "Frm")
+ _stream << _tabs << "Frm"
<< id() << " @ " << _frm_ptr.get() << std::endl;
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
@@ -410,11 +360,6 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
<< " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
- // // check trajectory pointer
- // inconsistency_explanation << "Frame trajectory pointer is " << getTrajectory()
- // << " but trajectory pointer is" << trajectory_ptr << "\n";
- // log.assertTrue((getTrajectory() == T, inconsistency_explanation);
-
// check constrained_by
for (auto cby : getConstrainedByList())
{
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index c8f855dfb512a8e1438f07c0c9582db8d2c0f578..11e26334cfedde1e6ffcae6f4588a57651c51eaa 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -170,31 +170,7 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
}
_stream << std::endl;
- if (_metric && _state_blocks){
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << std::endl;
- }
- }
- else if (_metric)
- {
- _stream << _tabs << " " << (isFixed() ? "Fix" : "Est");
- _stream << ",\t x = ( " << std::setprecision(2) << getStateVector().transpose() << " )";
- _stream << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << _tabs << " " << "sb:";
- for (const auto& key : getStructure())
- {
- const auto& sb = getStateBlock(key);
- if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
- }
- _stream << std::endl;
- }
+ printState(_metric, _state_blocks, _stream, _tabs);
}
void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index bfcb33fa67482f2907ce1cdc715e4dacc44d60ed..40396509fd506ce25a65a8e510b9b4dde8818f64 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -325,7 +325,7 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim, //
const Eigen::VectorXd& _frame_state)
@@ -347,60 +347,73 @@ FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
state.emplace(key, _frame_state.segment(index, size));
+ // append new key to frame structure
+ if (frame_structure_.find(key) == std::string::npos) // not found
+ frame_structure_ += std::string(1,key);
+
index += size;
}
assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_,
- _time_stamp,
- _frame_structure,
- state);
+ auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
+ _time_stamp,
+ _frame_structure,
+ state);
+
return frm;
}
-FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
const StateStructure& _frame_structure, //
const SizeEigen _dim)
{
- return emplaceKeyFrame(_time_stamp,
- _frame_structure,
- getState(_time_stamp));
+ return emplaceFrame(_time_stamp,
+ _frame_structure,
+ getState(_time_stamp));
}
-FrameBasePtr Problem::emplaceKeyFrame (const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const VectorComposite& _frame_state)
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state)
{
- return FrameBase::emplaceKeyFrame<FrameBase>(getTrajectory(),
- _time_stamp,
- _frame_structure,
- _frame_state);
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _time_stamp,
+ _frame_structure,
+ _frame_state);
}
-FrameBasePtr Problem::emplaceKeyFrame (const TimeStamp& _time_stamp, //
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
const VectorComposite& _frame_state)
{
- return FrameBase::emplaceKeyFrame<FrameBase>(getTrajectory(),
- _time_stamp,
- getFrameStructure(),
- _frame_state);
+ // append new keys to frame structure
+ for (const auto& pair_key_vec : _frame_state)
+ {
+ const auto& key = pair_key_vec.first;
+ if (frame_structure_.find(key) == std::string::npos) // not found
+ frame_structure_ += std::string(1,key);
+ }
+
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _time_stamp,
+ getFrameStructure(),
+ _frame_state);
}
-FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
- const Eigen::VectorXd& _frame_state)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state)
{
- return emplaceKeyFrame(_time_stamp,
- this->getFrameStructure(),
- this->getDim(),
- _frame_state);
+ return emplaceFrame(_time_stamp,
+ this->getFrameStructure(),
+ this->getDim(),
+ _frame_state);
}
-FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
{
- return emplaceKeyFrame(_time_stamp,
- this->getFrameStructure(),
- this->getDim());
+ return emplaceFrame(_time_stamp,
+ this->getFrameStructure(),
+ this->getDim());
}
TimeStamp Problem::getTimeStamp ( ) const
@@ -415,10 +428,10 @@ TimeStamp Problem::getTimeStamp ( ) const
if (not ts.ok())
{
- const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
+ const auto& last_kf = trajectory_ptr_->getLastFrame();
- if (last_kf_or_aux)
- ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
+ if (last_kf)
+ ts = last_kf->getTimeStamp(); // Use last estimated frame's state
}
@@ -449,10 +462,10 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state_last;
- const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
+ const auto& last_kf = trajectory_ptr_->getLastFrame();
- if (last_kf_or_aux)
- state_last = last_kf_or_aux->getState(structure);
+ if (last_kf)
+ state_last = last_kf->getState(structure);
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
@@ -498,10 +511,10 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state_last;
- const auto& last_kf_or_aux = trajectory_ptr_->closestFrameToTimeStamp(_ts);
+ const auto& last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
- if (last_kf_or_aux)
- state_last = last_kf_or_aux->getState(structure);
+ if (last_kf)
+ state_last = last_kf->getState(structure);
else if (prior_options_ and prior_options_->mode != "nothing")
state_last = prior_options_->state;
@@ -639,31 +652,6 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
_processor_ptr->startCaptureProfiling();
}
-bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
-{
- // This should implement a black list of processors that have forbidden auxiliary frame creation
- // This decision is made at problem level, not at processor configuration level.
- // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
-
- // Currently allowing all processors to vote:
- return true;
-}
-
-void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
-{
- // TODO
-// if (_processor_ptr)
-// {
-// WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
-// }
-// else
-// {
-// WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
-// }
-//
-// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
-}
-
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
std::lock_guard<std::mutex> lock(mut_notifications_);
@@ -1011,7 +999,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceKeyFrame(_ts,
+ prior_keyframe = emplaceFrame(_ts,
prior_options_->state);
if (prior_options_->mode == "fix")
@@ -1174,12 +1162,9 @@ void Problem::perturb(double amplitude)
// Frames and Captures
for (auto& F : *(getTrajectory()))
{
- if (F->isKey())
- {
- F->perturb(amplitude);
- for (auto& C : F->getCaptureList())
- C->perturb(amplitude);
- }
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
}
// Landmarks
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 258e7c3d857a95f376b903c04977493be1d50e0d..e8f891b8c51098909b5db479f7e3d3a8139457c0 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -33,11 +33,6 @@ bool ProcessorBase::permittedKeyFrame()
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
-bool ProcessorBase::permittedAuxFrame()
-{
- return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this());
-}
-
void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other)
{
assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
@@ -119,7 +114,7 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
}
else
{
- WOLF_WARN("Linking with a nullptr");
+ WOLF_WARN("Linking Processor ", id(), " to a nullptr");
}
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index bf44bb4a94fcd0d498b7b64ac8e4ec32c84b3827..2fb9f2bab8cbd88b5239a72168610decda30c38f 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -8,7 +8,7 @@
#include "core/processor/processor_motion.h"
-
+#include "core/state_block/factory_state_block.h"
namespace wolf
{
@@ -236,7 +236,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
- keyframe_from_callback->setState(getState(ts_from_callback));
// find the capture whose buffer is affected by the new keyframe
auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k
@@ -247,6 +246,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
+ // update KF state (adding missing StateBlocks)
+ auto proc_state = getState(ts_from_callback);
+ for (auto pair_ckey_vec : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
+ keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
+ FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+ pair_ckey_vec.second,
+ false),
+ getProblem());
+ keyframe_from_callback->setState(proc_state);
+
// Find the capture acting as the buffer's origin
auto capture_origin = capture_existing->getOriginCapture();
@@ -337,7 +347,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
- keyframe_from_callback->setState(getState(ts_from_callback));
+
+ // update KF state (adding missing StateBlocks)
+ auto proc_state = getState(ts_from_callback);
+ for (auto pair_ckey_vec : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
+ keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
+ FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+ pair_ckey_vec.second,
+ false),
+ getProblem());
+ keyframe_from_callback->setState(proc_state);
auto & capture_existing = last_ptr_;
@@ -383,6 +403,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->setTimeStamp( ts );
last_ptr_->getFrame()->setTimeStamp( ts );
VectorComposite state_propa = getState( ts );
+ for (auto pair_ckey_vec : state_propa)
+ if (!last_ptr_->getFrame()->isInStructure(pair_ckey_vec.first))
+ last_ptr_->getFrame()->addStateBlock(pair_ckey_vec.first,
+ FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+ pair_ckey_vec.second,
+ false),
+ getProblem());
last_ptr_->getFrame()->setState( state_propa );
if (permittedKeyFrame() && voteForKeyFrame())
@@ -423,7 +450,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Set the frame of last_ptr as key
auto key_frame = last_ptr_->getFrame();
- key_frame ->setKey(getProblem());
+ key_frame ->link(getProblem());
// create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_);
@@ -432,9 +459,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto factor = emplaceFactor(key_feature, origin_ptr_);
// create a new frame
- auto frame_new = FrameBase::createNonKeyFrame<FrameBase>(getTimeStamp(),
- getStateStructure(),
- getProblem()->getState());
+ auto frame_new = std::make_shared<FrameBase>(getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
@@ -662,10 +689,10 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(),
- _ts_origin,
- getStateStructure(),
- _x_origin);
+ FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ _ts_origin,
+ getStateStructure(),
+ _x_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -676,7 +703,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
assert(_origin_frame->getTrajectory() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
- assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
TimeStamp origin_ts = _origin_frame->getTimeStamp();
@@ -687,15 +713,15 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
+ getCalibration(),
+ getCalibration(),
nullptr);
// ---------- LAST ----------
// Make non-key-frame for last Capture
- last_frame_ptr_ = FrameBase::createNonKeyFrame<FrameBase>(origin_ts,
- getStateStructure(),
- _origin_frame->getState());
+ last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts,
+ getStateStructure(),
+ _origin_frame->getState());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(last_frame_ptr_,
@@ -703,8 +729,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
+ getCalibration(),
+ getCalibration(),
origin_ptr_);
// clear and reset buffer
@@ -973,10 +999,10 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
{
_stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? " KFrm" : " Frm" )
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
<< getOrigin()->getFrame()->id() << std::endl;
if (getLast())
- _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? " KFrm" : " Frm")
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
<< getLast()->getFrame()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 2da7c330b59ffce0c7eceb11515e4f992a9eb3d9..d4f40c9ebea4250e313e2031650e33ef95f224ab 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -2,6 +2,7 @@
#include "core/sensor/sensor_odom_2d.h"
#include "core/math/covariance.h"
#include "core/state_block/state_composite.h"
+#include "core/factor/factor_odom_2d.h"
namespace wolf
{
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index c7be4d48c4d086d5d4f594dfa7849419d8cc5c30..8d8b190eef3ca6836e75b2868e40c56afea83fb4 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -6,12 +6,7 @@ namespace wolf
ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
- params_odom_3d_ (_params),
- k_disp_to_disp_ (0),
- k_disp_to_rot_ (0),
- k_rot_to_rot_ (0),
- min_disp_var_ (0.1), // around 10cm error
- min_rot_var_ (0.1) // around 6 degrees error
+ params_odom_3d_ (_params)
{
// Set constant parts of Jacobians
jacobian_delta_preint_.setIdentity(6,6);
@@ -29,13 +24,6 @@ void ProcessorOdom3d::configure(SensorBasePtr _sensor)
assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
-
- // we steal the parameters from the provided odom3d sensor.
- k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
- k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor();
- k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor();
- min_disp_var_ = sen_ptr->getMinDispVar();
- min_rot_var_ = sen_ptr->getMinRotVar();
}
void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 2e03aaafb52183adfd1843da41e0ca4ed3b49b3e..52421e86398bc049321e8b3dc5f75dc56d2d3001 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -81,10 +81,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
- FrameBasePtr kfrm = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(),
- incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
+ FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(kfrm);
// Process info
@@ -114,9 +114,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(frm);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
@@ -151,9 +151,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_old_frame->remove();
// Create new frame
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(frm);
// Detect new Features, initialize Landmarks, create Factors, ...
@@ -187,7 +187,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setKey(getProblem());
+ last_ptr_->getFrame()->link(getProblem());
// // make F; append incoming to new F
// FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
@@ -203,9 +203,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// make F; append incoming to new F
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
@@ -213,37 +213,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_ = nullptr;
}
- /* TODO: create an auxiliary frame
- else if (voteForAuxFrame() && permittedAuxFrame())
- {
- // We create an Auxiliary Frame
-
- // set AuxF on last
- last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setAuxiliary();
-
- // make F; append incoming to new F
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
- incoming_ptr_->link(frm);
-
- // Establish factors
- establishFactors();
-
- // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors
- getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
-
- // Advance this
- advanceDerived();
-
- // Replace last frame for a new one in incoming
- last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
- // do not remove! last_ptr_->remove();
- incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
-
- // Update pointers
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
- }*/
else
{
// We do not create a KF
@@ -252,9 +221,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
+ FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
@@ -307,11 +276,11 @@ void ProcessorTracker::computeProcessingStep()
if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
{
- if (last_ptr_->getFrame()->isKey())
+ if (last_ptr_->getFrame()->getProblem())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
- WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
+ WOLF_INFO("Pack's KF and last's Frame have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following for correctness:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
@@ -342,10 +311,10 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
{
_stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? " KFrm" : " Frm")
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
<< getOrigin()->getFrame()->id() << std::endl;
if (getLast())
- _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? " KFrm" : " Frm")
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << " Frm"
<< getLast()->getFrame()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 6d7dd0f50da30dc79e3c5c9cca980bc3364a0349..da6a88b75e98e0ada648f76155f5d12501c21c4d 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -214,7 +214,7 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
{
auto& ts = pair_ts_ftr.first;
auto& ftr = pair_ts_ftr.second;
- if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey())
+ if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->getProblem())
track_kf[ts] = ftr;
}
return track_kf;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0e2a89c12e888750073376f992353b5dd8abce96..0d2d54638d145f333efa0e349990beda54d1b17e 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -20,7 +20,6 @@ SensorBase::SensorBase(const std::string& _type,
NodeBase("SENSOR", _type),
HasStateBlocks(""),
hardware_ptr_(),
- calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
noise_std_(_noise_size),
noise_cov_(_noise_size, _noise_size),
@@ -42,7 +41,6 @@ SensorBase::SensorBase(const std::string& _type,
if (_intr_ptr)
addStateBlock('I', _intr_ptr, _intr_dyn);
- updateCalibSize();
}
SensorBase::SensorBase(const std::string& _type,
@@ -56,7 +54,6 @@ SensorBase::SensorBase(const std::string& _type,
NodeBase("SENSOR", _type),
HasStateBlocks(""),
hardware_ptr_(),
- calib_size_(0),
sensor_id_(++sensor_id_count_), // simple ID factory
noise_std_(_noise_std),
noise_cov_(_noise_std.size(), _noise_std.size()),
@@ -72,8 +69,6 @@ SensorBase::SensorBase(const std::string& _type,
if (_intr_ptr)
addStateBlock('I', _intr_ptr, _intr_dyn);
-
- updateCalibSize();
}
SensorBase::~SensorBase()
@@ -106,7 +101,6 @@ void SensorBase::fixExtrinsics()
if (sbp != nullptr)
sbp->fix();
}
- updateCalibSize();
}
void SensorBase::unfixExtrinsics()
@@ -117,7 +111,6 @@ void SensorBase::unfixExtrinsics()
if (sbp != nullptr)
sbp->unfix();
}
- updateCalibSize();
}
void SensorBase::fixIntrinsics()
@@ -131,7 +124,6 @@ void SensorBase::fixIntrinsics()
sbp->fix();
}
}
- updateCalibSize();
}
void SensorBase::unfixIntrinsics()
@@ -145,7 +137,6 @@ void SensorBase::unfixIntrinsics()
sbp->unfix();
}
}
- updateCalibSize();
}
void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
@@ -306,35 +297,6 @@ StateBlockPtr SensorBase::getIntrinsic() const
return getStateBlockDynamic('I');
}
-SizeEigen SensorBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (const auto& pair_key_sb : getStateBlockMap())
- {
- auto sb = pair_key_sb.second;
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXd SensorBase::getCalibration() const
-{
- SizeEigen index = 0;
- SizeEigen sz = getCalibSize();
- Eigen::VectorXd calib(sz);
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlockDynamic(key);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
capture_ptr->setSensor(shared_from_this());
@@ -453,7 +415,7 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
}
else
{
- WOLF_WARN("Linking with a nullptr");
+ WOLF_WARN("Linking Sensor ", id(), " to a nullptr");
}
}
@@ -464,39 +426,7 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
_stream << " -- " << getProcessorList().size() << "p";
_stream << std::endl;
- if (_metric && _state_blocks)
- {
- _stream << _tabs << " " << "sb: ";
- for (auto& key : getStructure())
- {
- auto sb = getStateBlockDynamic(key);
- if (sb)
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << " ";
- }
- _stream << std::endl;
- }
- else if (_metric)
- {
- _stream << _tabs << " " << "( ";
- for (auto& key : getStructure())
- {
- auto sb = getStateBlockDynamic(key);
- if (sb)
- _stream << sb->getState().transpose() << " ";
- }
- _stream << ")" << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << _tabs << " " << "sb: ";
- for (auto& key : getStructure())
- {
- auto sb = getStateBlockDynamic(key);
- if (sb)
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb << " ";
- }
- _stream << std::endl;
- }
+ printState(_metric, _state_blocks, _stream, _tabs);
}
void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index ec7a3b54d3f8163f44276bf8120cb482a3a2fdd7..a3fe8ee9aa2a81849fb0caa2789d785cdafcf22a 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -34,6 +34,17 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
return k_rot_to_rot_;
}
+Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+{
+ double d = fabs(_data(0));
+ double r = fabs(_data(1));
+
+ double dvar = k_disp_to_disp_ * d;
+ double rvar = k_rot_to_rot_ * r;
+
+ return (Matrix3d() << dvar, dvar, rvar).finished();
+}
+
}
// Register in the FactorySensor
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index 58b9b62ee9e111ce84887ba16e728a3494e40b7a..5954f505c0daf3e37588e0b78bf2b3b440b1052f 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -9,6 +9,7 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
+#include "core/math/rotations.h"
namespace wolf {
@@ -37,6 +38,21 @@ SensorOdom3d::~SensorOdom3d()
//
}
+Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
+{
+ double d = _data.head<3>().norm();
+ double r;
+ if (_data.size() == 6)
+ r = _data.tail<3>().norm();
+ else
+ r = 2 * acos(_data(6)); // arc cos of real part of quaternion
+
+ double dvar = min_disp_var_ + k_disp_to_disp_ * d;
+ double rvar = min_rot_var_ + k_disp_to_rot_ * d + k_rot_to_rot_ * r;
+
+ return (Matrix6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished();
+}
+
} // namespace wolf
// Register in the FactorySensor
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 5855fda6b33931665fb9240b715a7436ac1ed034..ce743db05620f2e5bea90fd6cb8512c9669d394f 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -54,4 +54,39 @@ void HasStateBlocks::perturb(double amplitude)
}
}
+void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ if (_metric && _state_blocks)
+ {
+ for (const auto &key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ if (sb)
+ _stream << _tabs << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << " @ " << sb << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est")
+ << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )"
+ << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto &key : getStructure())
+ {
+ const auto &sb = getStateBlock(key);
+ if (sb)
+ _stream << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] @ " << sb;
+ }
+ _stream << std::endl;
+ }
+}
+
}
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 23f967716a28e65d9aa09a01b1cb60a345f220e5..4a64613a3445bfead7565f38ae5527f01f2fde5c 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -6,7 +6,6 @@ namespace wolf {
TrajectoryBase::TrajectoryBase() :
NodeBase("TRAJECTORY", "TrajectoryBase")
{
-// WOLF_DEBUG("constructed T");
frame_map_ = FrameMap();
}
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 3c64b2ee87d2e26318788c4eddd547606edb6cb0..7d10c3167b5fe23aa279808e743bc4f8b4abb3da 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -5,18 +5,34 @@ namespace wolf
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+ int n_f = getProblem()->getTrajectory()->getFrameMap().size();
+ bool remove_first = (n_f > params_sw_->n_frames);
+ int n_fix = (n_f >= params_sw_->n_frames ?
+ params_sw_->n_fix_first_frames :
+ n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames));
- // remove first frame if too many frames
- if (n_f > params_sw_->n_frames)
+ auto frame = (remove_first ?
+ getProblem()->getTrajectory()->getFirstFrame()->getNextFrame() :
+ getProblem()->getTrajectory()->getFirstFrame());
+ int fixed_frames = 0;
+
+ // Fix n_fix first frames
+ while (fixed_frames < n_fix)
{
- if (params_sw_->fix_first_frame)
+ if (not frame)
+ break;
+ if (not frame->isFixed())
{
- WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
- auto second_frame = std::next(getProblem()->getTrajectory()->begin())->second;
- if (second_frame)
- second_frame->fix();
+ WOLF_DEBUG("TreeManagerSlidingWindow fixing frame ", frame->id());
+ frame->fix();
}
+ frame = frame->getNextFrame();
+ fixed_frames++;
+ }
+
+ // Remove first frame
+ if (remove_first)
+ {
WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_empty_parent);
}
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 2009cd89edf486af2efa892f58615a1e0fa05b0f..54fadc44d8ac752f8d522374e9ccdb8812241585 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -13,7 +13,7 @@ TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeMan
NodeBase::setType("TreeManagerSlidingWindowDualRate");
}
-void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
+void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
{
int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
@@ -21,7 +21,6 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
if (n_f <= params_swdr_->n_frames_recent)
return;
-
// REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
if (count_frames_ != 0)
{
@@ -48,25 +47,14 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
assert(cap_next->getOriginCapture() == cap_prev);
proc_motion->mergeCaptures(cap_prev, cap_next);
}
-
}
// remove frame
remove_recent_frame->remove(params_swdr_->viral_remove_empty_parent);
}
- // REMOVE OLDEST FRAME: when first recent frame is kept, remove oldest frame (if max frames reached)
- else if (n_f > params_swdr_->n_frames)
- {
- if (params_swdr_->fix_first_frame)
- {
- WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
- auto second_frame = *std::next(getProblem()->getTrajectory()->begin());
- if (second_frame)
- second_frame->fix();
- }
- WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
- getProblem()->getTrajectory()->getFirstFrame()->remove(params_swdr_->viral_remove_empty_parent);
- }
+ // Call tree manager sliding window
+ // It will remove oldest frame if tfirst recent frame has been kept
+ TreeManagerSlidingWindow::keyFrameCallback(_frame);
// iterate counter
count_frames_++;
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index 9c21e3842304e475146194bc04da7be9f73de396..bd168ee20b162426a85d7b1b78e508f2c00a8fe1 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -34,7 +34,6 @@ static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _f
YAML::Node keyframe_vote = config["keyframe_vote"];
params->voting_active = keyframe_vote["voting_active"] .as<bool>();
- params->voting_aux_active = keyframe_vote["voting_aux_active"].as<bool>();
params->max_time_span = keyframe_vote["max_time_span"] .as<double>();
params->max_buff_length = keyframe_vote["max_buff_length"] .as<SizeEigen>();
params->dist_traveled = keyframe_vote["dist_traveled"] .as<double>();
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4363e38a4d4df67a73ce92086b7a3ae071c4a561..2e7d21c96e9061674a1d5108e45913f298994a3e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -201,14 +201,14 @@ target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
+# FactorRelativePose2d class test
+wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
+target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_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 ${PLUGIN_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 ${PLUGIN_NAME})
-
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
diff --git a/test/dummy/factor_odom_2d_autodiff.h b/test/dummy/factor_odom_2d_autodiff.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7cefbb0a94b2ea54b52f0b0de915d660020c1c6
--- /dev/null
+++ b/test/dummy/factor_odom_2d_autodiff.h
@@ -0,0 +1,105 @@
+#ifndef FACTOR_ODOM_2d_AUTODIFF_H_
+#define FACTOR_ODOM_2d_AUTODIFF_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(FactorOdom2dAutodiff);
+
+//class
+class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>
+{
+ public:
+ FactorOdom2dAutodiff(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>("FactorOdom2dAutodiff",
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function, _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
+ {
+ //
+ }
+
+ ~FactorOdom2dAutodiff() override = default;
+
+ 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,
+ T* _residuals) const;
+
+// public:
+// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+// {
+// return std::make_shared<FactorOdom2d>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+// }
+
+};
+
+template<typename T>
+inline bool FactorOdom2dAutodiff::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+ const T* const _o2, 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);
+ T o1 = _o1[0];
+ T o2 = _o2[0];
+ Eigen::Matrix<T, 3, 1> expected_measurement;
+ Eigen::Matrix<T, 3, 1> er; // error
+
+ // Expected measurement
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
+ expected_measurement(2) = o2 - o1;
+
+ // Error
+ er = expected_measurement - getMeasurement();
+ er(2) = pi2pi(er(2));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper() * er;
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXd J(3,6);
+// J.row(0) = ((Jet<double, 6>)(er(0))).v;
+// J.row(1) = ((Jet<double, 6>)(er(1))).v;
+// J.row(2) = ((Jet<double, 6>)(er(2))).v;
+// J.row(0) = ((Jet<double, 6>)(res(0))).v;
+// J.row(1) = ((Jet<double, 6>)(res(1))).v;
+// J.row(2) = ((Jet<double, 6>)(res(2))).v;
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorOdom2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2d::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_ODOM_2d_AUTODIFF_H_
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 4c004cd72e14e9e69bb343e288ab86e3a5b9354e..3a0cbc4555c88bd40f0a656e2a73ff2a619654b6 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -100,6 +100,93 @@ TEST(CaptureBase, process)
ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
}
+TEST(CaptureBase, move_from_F_to_KF)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+ auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 1);
+ ASSERT_EQ(F->getCaptureList().size(), 1);
+
+ C->move(KF);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 2);
+ ASSERT_EQ(F->getCaptureList().size(), 0);
+ ASSERT_TRUE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_F_to_null)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ FrameBasePtr F0 = nullptr;
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", 0.0);
+
+ ASSERT_EQ(F->getCaptureList().size(), 1);
+
+ C->move(F0);
+
+ ASSERT_EQ(F->getCaptureList().size(), 0);
+ ASSERT_FALSE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_null_to_KF)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+ auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+ auto C = std::make_shared<CaptureBase>("Dummy", 0.0);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 1);
+
+ C->move(KF);
+
+ ASSERT_EQ(KF->getCaptureList().size(), 2);
+ ASSERT_TRUE(C->getProblem());
+}
+
+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 C = std::make_shared<CaptureBase>("Dummy", 0.0);
+
+ C->move(F);
+
+ ASSERT_EQ(F->getCaptureList().size(), 1);
+
+ ASSERT_FALSE(C->getProblem());
+}
+
+TEST(CaptureBase, move_from_KF_to_F)
+{
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ auto KF = problem->emplaceFrame(0.0); // dummy F object
+
+ auto F = std::make_shared<FrameBase>(0.0, nullptr); // dummy F object
+
+ auto C = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
+
+ ASSERT_DEATH(C->move(F), "");
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index ebca6bb09bf463ed022aa81a1629e0accc877ff8..21ed3167006049866bcbc481ec5bfa81c760a23c 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -42,7 +42,7 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
@@ -67,7 +67,7 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -81,7 +81,7 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -99,7 +99,7 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -119,7 +119,7 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -141,7 +141,7 @@ TEST(Emplace, ReturnDerived)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 8c3b353ccb9493be57f2d38a6ffe257a6375f3b8..dc3551d2c79d47973e4dca81d4046fa1f669661d 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV", 3);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0, problem_ptr->stateZero() );
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, problem_ptr->stateZero() );
// Capture
// auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 0acd77891bf1bae60fe41575edec629e9864999b..8c27c1e8a60eff512ac83ce511f7f0267f5198e8 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -5,16 +5,17 @@
* Author: jvallve
*/
-#include "core/utils/utils_gtest.h"
+#include "dummy/factor_odom_2d_autodiff.h"
+#include "dummy/factor_dummy_zero_1.h"
+#include "dummy/factor_dummy_zero_12.h"
+#include "core/factor/factor_odom_2d.h"
+#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/capture/capture_void.h"
#include "core/feature/feature_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
-#include "core/factor/factor_odom_2d_analytic.h"
#include "core/factor/factor_autodiff.h"
-#include "dummy/factor_dummy_zero_1.h"
-#include "dummy/factor_dummy_zero_12.h"
+
using namespace wolf;
using namespace Eigen;
@@ -350,8 +351,11 @@ TEST(FactorAutodiff, AutodiffDummy12)
TEST(FactorAutodiff, EmplaceOdom2d)
{
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1));
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2));
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -366,7 +370,7 @@ TEST(FactorAutodiff, EmplaceOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
ASSERT_TRUE(factor_ptr->getFeature());
ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -377,12 +381,14 @@ TEST(FactorAutodiff, EmplaceOdom2d)
TEST(FactorAutodiff, ResidualOdom2d)
{
+ ProblemPtr problem = Problem::create("PO", 2);
+
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -400,7 +406,7 @@ TEST(FactorAutodiff, ResidualOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
@@ -421,12 +427,14 @@ TEST(FactorAutodiff, ResidualOdom2d)
TEST(FactorAutodiff, JacobianOdom2d)
{
+ ProblemPtr problem = Problem::create("PO", 2);
+
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -443,7 +451,7 @@ TEST(FactorAutodiff, JacobianOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
@@ -499,12 +507,14 @@ TEST(FactorAutodiff, JacobianOdom2d)
TEST(FactorAutodiff, AutodiffVsAnalytic)
{
+ ProblemPtr problem = Problem::create("PO", 2);
+
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
+ FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -521,8 +531,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
- auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 3e6974cb91303c8f864db2816d801207eba309ad..54ce142a0dcb369522137e5ccf477728a609d4a8 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -5,8 +5,8 @@
* \author: jsola
*/
+#include <core/factor/factor_distance_3d.h>
#include "../include/core/ceres_wrapper/solver_ceres.h"
-#include "core/factor/factor_autodiff_distance_3d.h"
#include "core/problem/problem.h"
#include "core/math/rotations.h"
@@ -28,7 +28,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test
FrameBasePtr F1, F2;
CaptureBasePtr C2;
FeatureBasePtr f2;
- FactorAutodiffDistance3dPtr c2;
+ FactorDistance3dPtr c2;
Vector1d dist;
Matrix1d dist_cov;
@@ -55,9 +55,9 @@ class FactorAutodiffDistance3d_Test : public testing::Test
problem = Problem::create("PO", 3);
solver = std::make_shared<SolverCeres>(problem);
- F1 = problem->emplaceKeyFrame (1.0, pose1);
+ F1 = problem->emplaceFrame (1.0, pose1);
- F2 = problem->emplaceKeyFrame (2.0, pose2);
+ F2 = problem->emplaceFrame (2.0, pose2);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
}
@@ -68,7 +68,7 @@ TEST_F(FactorAutodiffDistance3d_Test, ground_truth)
double res;
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
c2->operator ()(pos1.data(), pos2.data(), &res);
ASSERT_NEAR(res, 0.0, 1e-5);
@@ -79,7 +79,7 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
double measurement = 1.400;
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
@@ -94,7 +94,7 @@ TEST_F(FactorAutodiffDistance3d_Test, solve)
double measurement = 1.400;
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index e1e035bf0ae084d309b51db84b4c49ac62440924..79663676a51ea74c3ea2fc23f5c32c5e18e1cf30 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -24,8 +24,8 @@ class FactorBaseTest : public testing::Test
void SetUp() override
{
- F0 = std::make_shared<FrameBase>(KEY, 0.0, nullptr);
- F1 = std::make_shared<FrameBase>(KEY, 1.0, nullptr);
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 9b572504fa686bd555ba3b5ec25f39d1a4fb7afd..1dbf21b97b609ae212b1e88cf16ef993c2e742dc 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -58,7 +58,7 @@ class FixtureFactorBlockDifference : public testing::Test
//FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
//FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
- KF1_ = problem_->emplaceKeyFrame(t1, problem_->stateZero());
+ 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 51661f8ab26d1257f43dd4d4cbcc8dd5237b80d8..4f3ece70d92af1deeb923ce811d7a09dd96ba543 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -159,11 +159,11 @@ class FactorDiffDriveTest : public testing::Test
processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
// frames
- F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
+ F0 = FrameBase::emplace<FrameBase>(trajectory,
0.0,
"PO",
std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
- F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
+ F1 = FrameBase::emplace<FrameBase>(trajectory,
1.0,
"PO",
std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index baee1dddcaf2c1fb998e3ae551f29a39a335fd5b..5638239af4199356a877f186ca93a9e398b9f557 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -1,7 +1,7 @@
+#include <core/factor/factor_odom_2d.h>
#include "../include/core/ceres_wrapper/solver_ceres.h"
#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"
@@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(TimeStamp(0), Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(TimeStamp(1), Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp
index 24daac332094bb8e65964eb0acdd9346341eb3f9..7a57f0354a19c2a53e8ce6adac68f07713a93664 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -37,8 +37,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(TimeStamp(1), delta);
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index 749d514d8cc4c0479cfdf5092dca6e5face76ea8..7ecc6ab0848695e7922d9e9fb83f92a73a95e861 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO", 2);
SolverCeres solver(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceKeyFrame(TimeStamp(0), problem->stateZero());
+FrameBasePtr frm0 = problem->emplaceFrame(TimeStamp(0), problem->stateZero());
// Capture, feature and factor from frm1 to frm0
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr);
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index 88433b4b4433ce7e5290c81b34766d13fb2f9831..c44374caa2a908080d36e5396a9e41db846b7fb7 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO", 3);
SolverCeres solver(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceKeyFrame(0, problem->stateZero() );
+FrameBasePtr frm0 = problem->emplaceFrame(0, problem->stateZero() );
// Capture, feature and factor
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr);
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_pose_2d.cpp
similarity index 84%
rename from test/gtest_factor_relative_2d_analytic.cpp
rename to test/gtest_factor_relative_pose_2d.cpp
index 93581a6ac63b18da056f84ea5caed95ca009ebd4..8a46caebb25d6ffc0d50d779fcd7642c11140074 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -1,7 +1,7 @@
#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
-#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
@@ -19,18 +19,18 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(1.0, Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-TEST(FactorRelative2dAnalytic, check_tree)
+TEST(FactorRelativePose2d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorRelative2dAnalytic, fix_0_solve)
+TEST(FactorRelativePose2d, fix_0_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -54,7 +54,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -71,7 +71,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
}
}
-TEST(FactorRelative2dAnalytic, fix_1_solve)
+TEST(FactorRelativePose2d, fix_1_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -95,7 +95,7 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 78321478cc5126e419ca4fc8fea53c5ed79caa71..f6e8e18f10247e1c819121bc9e4494dd26ac6efe 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -27,8 +27,8 @@ auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d
auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0, Vector3d::Zero() );
-FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(1, Vector3d::Zero() );
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
@@ -40,7 +40,7 @@ TEST(FactorRelativePose2dWithExtrinsics, check_tree)
TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < 1e2; i++)
{
// Random delta
Vector3d delta = Vector3d::Random() * 10;
@@ -88,7 +88,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < 1e2; i++)
{
// Random delta
Vector3d delta = Vector3d::Random() * 10;
@@ -136,7 +136,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < 1e2; i++)
{
// Random delta
Vector3d delta = Vector3d::Random() * 10;
@@ -184,7 +184,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < 1e2; i++)
{
// Random delta
Vector3d delta = Vector3d::Random() * 10;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 7167566535827fa6281057903e6f11dafdf3810d..fc170fe3589bc43c5816b04af4419dcab510c671 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,13 +5,13 @@
* Author: jsola
*/
+#include <core/factor/factor_odom_2d.h>
#include "core/utils/utils_gtest.h"
#include "core/frame/frame_base.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/capture/capture_motion.h"
#include <iostream>
@@ -22,7 +22,7 @@ using namespace wolf;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
// getters
ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -31,13 +31,11 @@ TEST(FrameBase, GettersAndSetters)
F->getTimeStamp(t);
ASSERT_EQ(t, 1);
ASSERT_FALSE(F->isFixed());
- ASSERT_EQ(F->isKey(), false);
- ASSERT_EQ(F->isKey(), false);
}
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -47,12 +45,10 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
- // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
- // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d());
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
@@ -69,8 +65,8 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
@@ -115,8 +111,6 @@ TEST(FrameBase, LinksToTree)
ASSERT_FALSE(F1->isFixed());
F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
-
- ASSERT_TRUE(F1->isKey());
}
#include "core/state_block/state_quaternion.h"
@@ -128,7 +122,7 @@ TEST(FrameBase, GetSetState)
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
- FrameBase F(NON_ESTIMATED, 1, sbp, sbq, sbv);
+ FrameBase F(1, sbp, sbq, sbv);
// Give values to vectors and vector blocks
VectorXd x(10), x1(10), x2(10);
@@ -152,12 +146,10 @@ TEST(FrameBase, GetSetState)
TEST(FrameBase, Constructor_composite)
{
- FrameBase F = FrameBase(KEY,
- 0.0,
+ FrameBase F = FrameBase(0.0,
"POV",
VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
- ASSERT_TRUE (F.isKey());
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
ASSERT_TRUE (F.getO()->hasLocalParametrization());
ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
@@ -165,12 +157,10 @@ TEST(FrameBase, Constructor_composite)
TEST(FrameBase, Constructor_vectors)
{
- FrameBase F = FrameBase(KEY,
- 0.0,
+ FrameBase F = FrameBase(0.0,
"POV",
{Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
- ASSERT_TRUE (F.isKey());
ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
ASSERT_TRUE (F.getO()->hasLocalParametrization());
ASSERT_EQ (F.getStateBlock('V')->getSize(), 3);
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index 646dd602e619b0c2d2fadd70eb6c6f94e7f79a5f..d9eb8a1be1a45d197d6c4cfcbf6cb8b8dad26a95 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -41,8 +41,8 @@ class HasStateBlocksTest : public testing::Test
sbo1 = std::make_shared<StateQuaternion>();
sbv1 = std::make_shared<StateBlock>(3); // size 3
- F0 = std::make_shared<FrameBase>(NON_ESTIMATED, 0.0, sbp0, sbo0); // non KF
- F1 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.0, nullptr); // non KF
+ F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
}
void TearDown() override{}
@@ -50,21 +50,6 @@ class HasStateBlocksTest : public testing::Test
};
-TEST_F(HasStateBlocksTest, Notifications_setKey_add)
-{
- Notification n;
- ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
- ASSERT_DEATH(F0->link(problem->getTrajectory()), "");
-
- // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
-
- F0->setKey(problem);
-
- ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
- ASSERT_EQ(n, ADD);
-}
-
TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
{
Notification n;
@@ -79,11 +64,11 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
// ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
- F0->addStateBlock('V', sbv0);
+ F0->addStateBlock('V', sbv0, nullptr);
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
- F0->setKey(problem);
+ F0->link(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
ASSERT_EQ(n, ADD);
@@ -100,9 +85,9 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
// first make KF, then add SB
// F1->link(problem->getTrajectory());
- F1->setKey(problem);
+ F1->link(problem);
- F1->addStateBlock('P', sbp1);
+ F1->addStateBlock('P', sbp1, problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
ASSERT_EQ(n, ADD);
@@ -111,7 +96,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
- F1->addStateBlock('V', sbv1);
+ F1->addStateBlock('V', sbv1, problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
ASSERT_EQ(n, ADD);
@@ -130,8 +115,8 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->addStateBlock('V', sbv0);
- F0->setKey(problem);
+ F0->addStateBlock('V', sbv0, nullptr);
+ F0->link(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
ASSERT_EQ(n, ADD);
@@ -157,7 +142,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
// Add again the same SB. This should crash
- ASSERT_DEATH( F0->addStateBlock('V', sbv0) , "" );
+ ASSERT_DEATH( F0->addStateBlock('V', sbv0, nullptr) , "" );
}
@@ -179,7 +164,7 @@ TEST_F(HasStateBlocksTest, stateBlockKey)
TEST_F(HasStateBlocksTest, getState_structure)
{
- F0->addStateBlock('V', sbv0); // now KF0 is POV
+ F0->addStateBlock('V', sbv0, nullptr); // now KF0 is POV
WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 3b525a12e5210de4eeb54ef213d8446bd24b8329..7f9afd039927db217e46dc4a2b1397e3529cf037 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -5,12 +5,12 @@
* \author: jsola
*/
+#include <core/factor/factor_odom_2d.h>
#include "core/utils/utils_gtest.h"
// Classes under test
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
// Wolf includes
@@ -76,27 +76,24 @@ void show(const ProblemPtr& problem)
for (FrameBasePtr F : *problem->getTrajectory())
{
- if (F->isKey())
+ cout << "----- Key Frame " << F->id() << " -----" << endl;
+ if (!F->getCaptureList().empty())
{
- cout << "----- Key Frame " << F->id() << " -----" << endl;
- if (!F->getCaptureList().empty())
+ auto C = F->getCaptureList().front();
+ if (!C->getFeatureList().empty())
{
- auto C = F->getCaptureList().front();
- if (!C->getFeatureList().empty())
- {
- auto f = C->getFeatureList().front();
- cout << " feature measure: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
- << endl;
- cout << " feature covariance: \n"
- << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
- }
+ auto f = C->getFeatureList().front();
+ cout << " feature measure: \n"
+ << F->getCaptureList().front()->getFeatureList().front()->getMeasurement().transpose()
+ << endl;
+ cout << " feature covariance: \n"
+ << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
}
- cout << " state: \n" << F->getStateVector().transpose() << endl;
- Eigen::MatrixXd cov;
- problem->getFrameCovariance(F,cov);
- cout << " covariance: \n" << cov << endl;
}
+ cout << " state: \n" << F->getStateVector().transpose() << endl;
+ Eigen::MatrixXd cov;
+ problem->getFrameCovariance(F,cov);
+ cout << " covariance: \n" << cov << endl;
}
}
@@ -125,21 +122,21 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
ProblemPtr Pr = Problem::create("PO", 2);
- SolverCeres solver(Pr);
+ SolverCeres solver(Pr);
// KF0 and absolute prior
FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceKeyFrame(t, Vector3d::Zero());
+ FrameBasePtr F1 = Pr->emplaceFrame(t, Vector3d::Zero());
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceKeyFrame(t, Vector3d::Zero());
+ FrameBasePtr F2 = Pr->emplaceFrame(t, Vector3d::Zero());
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
@@ -308,8 +305,6 @@ TEST(Odom2d, VoteForKfAndSolve)
int n = 0;
for (auto F : *problem->getTrajectory())
{
- if (!F->isKey()) break;
-
ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
@@ -423,7 +418,7 @@ TEST(Odom2d, 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->emplaceKeyFrame(t_split, x_split);
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
@@ -460,7 +455,7 @@ TEST(Odom2d, KF_callback)
problem->print(4,1,1,1);
x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_1 = problem->emplaceKeyFrame(t_split, x_split);
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 056fbcf0bb231962e109ea60f404a5a0cefd58f5..d1fce41e50c72c91fb520d748a8154d5d516a1e6 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -34,10 +34,10 @@ class BufferPackKeyFrameTest : public testing::Test
void SetUp(void) override
{
- f10 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(28),nullptr,nullptr,nullptr);
+ 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);
tt10 = 1.0;
tt20 = 1.0;
@@ -230,7 +230,7 @@ TEST_F(BufferPackKeyFrameTest, removeUpTo)
// Specifically, only f28 should remain
pack_kf_buffer.add(f28, tt28);
ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(22),nullptr,nullptr,nullptr);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index dc44348f5943d7702372e28ebfb4e8ea2a707876..181fb1ce43bc80a8a04034191973d5ac60635b1e 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -224,9 +224,10 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceKeyFrame(0, "PO" , 2, VectorXd(3) );
- FrameBasePtr f1 = P->emplaceKeyFrame(1, "PO" , 3, VectorXd(7) );
- FrameBasePtr f2 = P->emplaceKeyFrame(2, "POV", 3, VectorXd(10) );
+ FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2, VectorXd(3) );
+ FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, VectorXd(7) );
+ FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, VectorXd(10) );
+
// check that all frames are effectively in the trajectory
ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
@@ -239,6 +240,8 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f0->getProblem(), P);
ASSERT_EQ(f1->getProblem(), P);
ASSERT_EQ(f2->getProblem(), P);
+
+ ASSERT_EQ(P->getFrameStructure(), "POV");
}
TEST(Problem, StateBlocks)
@@ -261,7 +264,7 @@ TEST(Problem, StateBlocks)
auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 2 state blocks, estimated
- auto KF = P->emplaceKeyFrame(0, "PO", 3, xs3d );
+ auto KF = P->emplaceFrame(0, "PO", 3, xs3d );
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
// Notifications
@@ -320,7 +323,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceKeyFrame(0, "PO", 3, xs3d );
+ FrameBasePtr F = P->emplaceFrame(0, "PO", 3, xs3d );
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -356,7 +359,7 @@ TEST(Problem, perturb)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceKeyFrame(t, pose );
+ auto F = problem->emplaceFrame(t, pose );
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -385,21 +388,22 @@ TEST(Problem, perturb)
//// CHECK ALL STATE BLOCKS ///
- double prec = 1e-2;
+#define prec 1e-2
for (auto S : problem->getHardware()->getSensorList())
{
- ASSERT_TRUE (S->getP()->getState().isApprox(Vector2d(0,0), prec) );
- ASSERT_TRUE (S->getO()->getState().isApprox(Vector1d(0), prec) );
+ ASSERT_MATRIX_APPROX(S->getP()->getState(), Vector2d(0,0), prec );
+ ASSERT_MATRIX_APPROX(S->getO()->getState(), Vector1d(0), prec );
ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
}
- for (auto F : *problem->getTrajectory())
+ for (auto pair_T_F : problem->getTrajectory()->getFrameMap())
{
+ auto F = pair_T_F.second;
if (F->isFixed()) // fixed
{
- ASSERT_TRUE (F->getP()->getState().isApprox(Vector2d(0,0), prec) );
- ASSERT_TRUE (F->getO()->getState().isApprox(Vector1d(0), prec) );
+ ASSERT_MATRIX_APPROX (F->getP()->getState(), Vector2d(0,0), prec );
+ ASSERT_MATRIX_APPROX (F->getO()->getState(), Vector1d(0), prec );
}
else
{
@@ -416,8 +420,8 @@ TEST(Problem, perturb)
{
if ( L->isFixed() ) // fixed
{
- ASSERT_TRUE (L->getP()->getState().isApprox(Vector2d(1,2), prec) );
- ASSERT_TRUE (L->getO()->getState().isApprox(Vector1d(3), prec) );
+ ASSERT_MATRIX_APPROX (L->getP()->getState(), Vector2d(1,2), prec );
+ ASSERT_MATRIX_APPROX (L->getO()->getState(), Vector1d(3), prec );
}
else
{
@@ -446,7 +450,7 @@ TEST(Problem, check)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceKeyFrame(t, pose);
+ auto F = problem->emplaceFrame(t, pose);
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -564,6 +568,6 @@ TEST(Problem, getState)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "Problem.getState";
+// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 6a4b40d32c0908484e31b576310657aba5d11fb4..4e50c870348da1877b3a599c48158730e2df47ab 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -35,10 +35,9 @@ protected:
CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
{
for (FrameBasePtr kf : *getProblem()->getTrajectory())
- if (kf->isKey())
- for (CaptureBasePtr cap : kf->getCaptureList())
- if (cap != _capture)
- return cap;
+ for (CaptureBasePtr cap : kf->getCaptureList())
+ if (cap != _capture)
+ return cap;
return nullptr;
};
void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
@@ -87,7 +86,7 @@ TEST(ProcessorLoopClosure, installProcessor)
// new KF
t += dt;
- auto kf = problem->emplaceKeyFrame(t, x); //KF2
+ auto kf = problem->emplaceFrame(t, x); //KF2
// emplace a capture in KF
auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
proc_lc->captureCallback(capt_lc);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index d905bcad555b88e4cd92afed94771c2e7f46546d..e531defd5721e3374308d67e793a01f682701577 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -312,7 +312,7 @@ TEST_F(ProcessorMotion_test, mergeCaptures)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -389,7 +389,7 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -434,7 +434,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -479,7 +479,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index dc24beddd0643d7d64d21deeb59c2068752d50ec..876a4450d65e0da3231f1f9b91b9e56e3b97ac69 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -44,18 +44,10 @@ class ProcessorOdom3dTest : public ProcessorOdom3d
{
public:
ProcessorOdom3dTest();
-
- // getters :-D !!
- double& kdd() {return k_disp_to_disp_;}
- double& kdr() {return k_disp_to_rot_;}
- double& krr() {return k_rot_to_rot_;}
- double& dvar_min() {return min_disp_var_;}
- double& rvar_min() {return min_rot_var_;}
};
ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ParamsProcessorOdom3d>())
{
- dvar_min() = 0.5;
- rvar_min() = 0.25;
+ //
}
TEST(ProcessorOdom3d, computeCurrentDelta)
@@ -76,11 +68,8 @@ TEST(ProcessorOdom3d, computeCurrentDelta)
delta.head<3>() = delta_dp;
delta.tail<4>() = delta_dq.coeffs();
- // construct covariance from processor parameters and motion magnitudes
- double disp = data_dp.norm();
- double rot = data_do.norm();
- double dvar = prc.dvar_min() + prc.kdd()*disp;
- double rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot;
+ double dvar = 0.1;
+ double rvar = 0.2;
Vector6d diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
Matrix6d data_cov = diag.asDiagonal();
Matrix6d delta_cov = data_cov;
@@ -119,8 +108,6 @@ TEST(ProcessorOdom3d, deltaPlusDelta)
prc.deltaPlusDelta(D, d, dt, D_int);
ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10);
-// << "\nDpd : " << D_int.transpose()
-// << "\ncheck: " << D_int_check.transpose();
}
TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 7b6dc1c40fe5a4eabcdeae278cd823fa01403fbb..49593d64afa531c28a8a490de0ba562ed1c13bb2 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -321,7 +321,6 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
- ASSERT_TRUE(cap4->getFrame()->isKey());
ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id());
ASSERT_TRUE(problem->check(0));
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index 91fa716cb45d02c14bf9dad6068c4d6f4bae6780..3cd41dde30bc72ecc16fc27d70b8cde5fe327a7b 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -56,7 +56,7 @@ TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
while (true)
{
// Emplace Frame, Capture, feature and factor pose 2d
- FrameBasePtr F = P->emplaceKeyFrame(ts, P->stateZero());
+ FrameBasePtr F = P->emplaceFrame(ts, P->stateZero());
auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 0770b33aed43e5ebbc9a204e68428f7a0022c870..4e772c1bb74747d80a6ceda04d62a679f323a4e3 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -51,7 +51,7 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
while (true)
{
// Emplace Frame, Capture, feature and factor pose 2d
- FrameBasePtr F = P->emplaceKeyFrame(ts, P->stateZero());
+ FrameBasePtr F = P->emplaceFrame(ts, P->stateZero());
auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 72bf3f7d8ce8124c4005daf4a9a82ba119504d5e..a14c436ddc0864f7b55e1ef9db1fd451ecf494ea 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -37,11 +37,11 @@ class TrackMatrixTest : public testing::Test
// unlinked frames
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- F0 = FrameBase::createNonKeyFrame<FrameBase>(0.0, nullptr);
- F1 = FrameBase::createNonKeyFrame<FrameBase>(1.0, nullptr);
- F2 = FrameBase::createNonKeyFrame<FrameBase>(2.0, nullptr);
- F3 = FrameBase::createNonKeyFrame<FrameBase>(3.0, nullptr);
- F4 = FrameBase::createNonKeyFrame<FrameBase>(4.0, nullptr);
+ 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);
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
@@ -52,8 +52,8 @@ class TrackMatrixTest : public testing::Test
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
- F0->setKey(problem);
- F4->setKey(problem);
+ F0->link(problem);
+ F4->link(problem);
// link captures
C0->link(F0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index ff103bc097ba9d166ef76a97fcde20b15199507a..7c483113468573388e5cca87ebf1bf243f41a911 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -32,15 +32,14 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceKeyFrame( 1, Eigen::Vector3d::Zero() );
- FrameBasePtr F2 = P->emplaceKeyFrame( 2, Eigen::Vector3d::Zero() );
- // FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() );
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(),
-// P->getDim(),
- std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
- FrameBasePtr F4 = FrameBase::createNonKeyFrame<FrameBase>(4, P->getFrameStructure(),
-// P->getDim(),
- std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
+ FrameBasePtr F1 = P->emplaceFrame( 1, Eigen::Vector3d::Zero() );
+ FrameBasePtr F2 = P->emplaceFrame( 2, Eigen::Vector3d::Zero() );
+ FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+ // P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
+ FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameStructure(),
+ // P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
FrameBasePtr KF; // closest key-frame queried
@@ -80,23 +79,23 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceKeyFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceFrame(1, Eigen::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->emplaceKeyFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceFrame(2, Eigen::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);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(),
-// P->getDim(),
- std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
+ FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+ // P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::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 15bb1778940d6e2d1e80ad05e195caba66b6417d..9c684c7bc0f3f30199a8d86e2ca9bf4b8aacb87f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
- auto F0 = P->emplaceKeyFrame(0, "PO", 3, VectorXd(7) );
+ auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
P->keyFrameCallback(F0, nullptr, 0);
ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 0c5e7c2aec97277425289f3f28a9ba3d61dece1c..486ef3df9fccd76d266765a0486b1c09dbbc65d6 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -72,6 +72,9 @@ TEST(TreeManagerSlidingWindow, autoConf)
TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
{
+ // window size: 3
+ // first 2 frames fixed
+
ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
ParamsServer server = ParamsServer(parser.getParams());
@@ -87,7 +90,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -101,9 +104,12 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
+ // Check F1 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -117,9 +123,13 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
+ // Check F1 and F2 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_TRUE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -131,15 +141,18 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
- // Checks
+ // Check F1 (virally) removed
EXPECT_TRUE(F1->isRemoving());
EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
EXPECT_TRUE(C12->isRemoving()); // Virally removed
EXPECT_TRUE(f12->isRemoving()); // Virally removed
- EXPECT_TRUE(F2->isFixed()); //Fixed
+ // Check F2 and F3 fixed
+ EXPECT_TRUE(F2->isFixed());
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -151,7 +164,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
- // Checks
+ // Check F1 and F2 (virally) removed
EXPECT_TRUE(F1->isRemoving());
EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
EXPECT_TRUE(C12->isRemoving()); // Virally removed
@@ -163,8 +176,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
EXPECT_TRUE(C23->isRemoving()); // Virally removed
EXPECT_TRUE(f23->isRemoving()); // Virally removed
-
- EXPECT_TRUE(F3->isFixed()); //Fixed
+ // Check F3 and F4 fixed
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_TRUE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
}
TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
@@ -184,7 +199,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -198,9 +213,12 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
+ // Check no frames fixed
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -214,9 +232,13 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
+ // Check no frames fixed
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -233,10 +255,13 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
EXPECT_FALSE(C12->isRemoving()); //Not virally removed
EXPECT_FALSE(f12->isRemoving()); //Not virally removed
- EXPECT_FALSE(F2->isFixed()); //Not fixed
+ // Check no frames fixed
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -260,7 +285,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
EXPECT_FALSE(C23->isRemoving()); //Not virally removed
EXPECT_FALSE(f23->isRemoving()); //Not virally removed
- EXPECT_FALSE(F3->isFixed()); //Not fixed
+ // Check no frames fixed
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
}
int main(int argc, char **argv)
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index ae4112bc3c27d88e103694ffd0824f4b7b35de2d..dbc9f67c3aef8911b4568548d99f1722768ff598 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -76,6 +76,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
{
/* sliding window dual rate:
* n_frames: 5
+ * n_fix_first_frames: 2
* n_frames_recent: 3
* rate_old_frames: 2
*/
@@ -90,6 +91,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* ( ) ( ) ( )( )(F1)
+ * fix fix
*/
auto F1 = P->getTrajectory()->getLastFrame();
ASSERT_TRUE(F1 != nullptr);
@@ -109,15 +111,16 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(c1->isRemoving());
EXPECT_FALSE(C1->isRemoving());
EXPECT_FALSE(f1->isRemoving());
-
+ // Check no frame fixed
EXPECT_FALSE(F1->isFixed());
/* FRAME 2 ----------------------------------------------------------
*
* Sliding window:
* ( ) ( ) ( )(F1)(F2)
+ * fix fix
*/
- auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -142,7 +145,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(c12->isRemoving());
EXPECT_FALSE(C12->isRemoving());
EXPECT_FALSE(f12->isRemoving());
-
+ // Check no frame fixed
EXPECT_FALSE(F1->isFixed());
EXPECT_FALSE(F2->isFixed());
@@ -150,8 +153,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* ( ) ( ) (F1)(F2)(F3)
+ * fix fix
*/
- auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -185,6 +189,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(C23->isRemoving());
EXPECT_FALSE(f23->isRemoving());
+ // Check no frame fixed
EXPECT_FALSE(F1->isFixed());
EXPECT_FALSE(F2->isFixed());
EXPECT_FALSE(F3->isFixed());
@@ -193,8 +198,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* ( ) (F1)(F2)(F3)(F4)
+ * fix fix
*/
- auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -236,7 +242,8 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(C34->isRemoving());
EXPECT_FALSE(f34->isRemoving());
- EXPECT_FALSE(F1->isFixed());
+ // Check F1 fixed
+ EXPECT_TRUE(F1->isFixed());
EXPECT_FALSE(F2->isFixed());
EXPECT_FALSE(F3->isFixed());
EXPECT_FALSE(F4->isFixed());
@@ -245,8 +252,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* ( ) (F1) (F3)(F4)(F5)
+ * fix fix
*/
- auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -296,7 +304,8 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(C45->isRemoving());
EXPECT_FALSE(f45->isRemoving());
- EXPECT_FALSE(F1->isFixed());
+ // Check F1 fixed
+ EXPECT_TRUE(F1->isFixed());
EXPECT_FALSE(F3->isFixed());
EXPECT_FALSE(F4->isFixed());
EXPECT_FALSE(F5->isFixed());
@@ -305,8 +314,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* (F1) (F3)(F4)(F5)(F6)
+ * fix fix
*/
- auto F6 = P->emplaceKeyFrame(TimeStamp(6), "PO", 3, state);
+ auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
P->keyFrameCallback(F6, nullptr, 0);
// absolute factor
@@ -364,8 +374,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(C56->isRemoving());
EXPECT_FALSE(f56->isRemoving());
- EXPECT_FALSE(F1->isFixed());
- EXPECT_FALSE(F3->isFixed());
+ // Check F1 and F3 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_TRUE(F3->isFixed());
EXPECT_FALSE(F4->isFixed());
EXPECT_FALSE(F5->isFixed());
EXPECT_FALSE(F6->isFixed());
@@ -374,8 +385,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* (F1) (F3) (F5)(F6)(F7)
+ * fix fix
*/
- auto F7 = P->emplaceKeyFrame(TimeStamp(7), "PO", 3, state);
+ auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
P->keyFrameCallback(F7, nullptr, 0);
// absolute factor
@@ -441,8 +453,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(C67->isRemoving());
EXPECT_FALSE(f67->isRemoving());
- EXPECT_FALSE(F1->isFixed());
- EXPECT_FALSE(F3->isFixed());
+ // Check F1 and F3 fixed
+ EXPECT_TRUE(F1->isFixed());
+ EXPECT_TRUE(F3->isFixed());
EXPECT_FALSE(F5->isFixed());
EXPECT_FALSE(F6->isFixed());
EXPECT_FALSE(F7->isFixed());
@@ -451,8 +464,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
*
* Sliding window:
* (F3) (F5)(F6)(F7)(F8)
+ * fix fix
*/
- auto F8 = P->emplaceKeyFrame(TimeStamp(8), "PO", 3, state);
+ auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
P->keyFrameCallback(F8, nullptr, 0);
// absolute factor
@@ -526,8 +540,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
EXPECT_FALSE(C78->isRemoving());
EXPECT_FALSE(f78->isRemoving());
+ // Check F1 and F3 fixed
EXPECT_TRUE(F3->isFixed());
- EXPECT_FALSE(F5->isFixed());
+ EXPECT_TRUE(F5->isFixed());
EXPECT_FALSE(F6->isFixed());
EXPECT_FALSE(F7->isFixed());
EXPECT_FALSE(F8->isFixed());
@@ -537,6 +552,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
{
/* sliding window dual rate:
* n_frames: 5
+ * n_fix_first_frames: 0
* n_frames_recent: 3
* rate_old_frames: 2
*/
@@ -578,7 +594,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) ( ) ( )(F1)(F2)
*/
- auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -612,7 +628,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) ( ) (F1)(F2)(F3)
*/
- auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -655,7 +671,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) (F1)(F2)(F3)(F4)
*/
- auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -707,7 +723,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) (F1) (F3)(F4)(F5)
*/
- auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -767,7 +783,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* (F1) (F3)(F4)(F5)(F6)
*/
- auto F6 = P->emplaceKeyFrame(TimeStamp(6), "PO", 3, state);
+ auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
P->keyFrameCallback(F6, nullptr, 0);
// absolute factor
@@ -836,7 +852,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* (F1) (F3) (F5)(F6)(F7)
*/
- auto F7 = P->emplaceKeyFrame(TimeStamp(7), "PO", 3, state);
+ auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
P->keyFrameCallback(F7, nullptr, 0);
// absolute factor
@@ -913,7 +929,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* (F3) (F5)(F6)(F7)(F8)
*/
- auto F8 = P->emplaceKeyFrame(TimeStamp(8), "PO", 3, state);
+ auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
P->keyFrameCallback(F8, nullptr, 0);
// absolute factor
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
index 0337cdc0bbc589d0d03054107643e632bb6be7b4..7e0b0ccb110c0545ff24a9726c035124f0d1686a 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -35,7 +35,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: true
- voting_aux_active: false
max_time_span: 1.95 # seconds
max_buff_length: 999 # motion deltas
dist_traveled: 999 # meters
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index d8f443220bf71786c1e95ca77ae9e68705d1d576..6a69b63a6918e13b0f492c7b51ef6dfe3c97ba94 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -40,7 +40,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: false
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index b61f9936d4ccb8fc705d2ca92717f2a2ad67c402..879421e7915581f52355d1874ebb78f302b768df 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -39,7 +39,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: false
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 22242febd640b616bebe4ab0410867b0e373a5be..704e8530ed2c8b8eb99ac35c29c36f62d01a3fb4 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -14,7 +14,7 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
n_frames: 3
- fix_first_frame: true
+ n_fix_first_frames: 2
viral_remove_empty_parent: true
sensors:
-
@@ -38,7 +38,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: false
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index add5eff760050fecbde63c827bb75e77c531f86e..701fc6fbfb71e35c800563224cf7a86cce6d846b 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -14,7 +14,7 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
n_frames: 3
- fix_first_frame: false
+ n_fix_first_frames: 0
viral_remove_empty_parent: false
sensors:
-
@@ -38,7 +38,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: false
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index 32ef665e0a6d92b94e9342d22c5e4ca9952613f1..a7f0f7434fb8a71c74e3aa3f15b8dc9f6ea7c067 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -16,5 +16,5 @@ config:
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
- fix_first_frame: true
+ n_fix_first_frames: 2
viral_remove_empty_parent: true
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
index 6b4795fb3ad70c77b02c9cffc74b964abfe64141..cae3df67f036430481cd936ea31a9d2b4c0bca9a 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -16,5 +16,5 @@ config:
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
- fix_first_frame: false
+ n_fix_first_frames: 0
viral_remove_empty_parent: false
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index b59022518aaf931890368185fb9b489611cda257..47d81f409a5ed09d37ba3b307c2fb23152759791 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -22,7 +22,7 @@ config:
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
- fix_first_frame: true
+ n_fix_first_frames: 2
viral_remove_empty_parent: true
sensors:
-
@@ -46,7 +46,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: true
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 10 # meters
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index eba780939b0e4521ba7a5d571ed47d63749adc6e..494d27be3f0b9e3c68ef33919451a0f7e6ba065c 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -40,7 +40,6 @@ config:
time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: true
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 10 # meters
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 672421b5d074b7fd627bf955a879e3f10a3544e0..0fe5e5c6d360e239edf207dba1f88329341a0632 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -4,7 +4,6 @@ time_tolerance: 0.01 # seconds
keyframe_vote:
voting_active: false
- voting_aux_active: false
max_time_span: 0.2 # seconds
max_buff_length: 10 # motion deltas
dist_traveled: 0.5 # meters