diff --git a/.gitignore b/.gitignore
index ea3b11ae3f1e21c872d7f069c6d6b56ca83c2a54..b690efc9b285d63727f435a9db3ee8b2039aee82 100644
--- a/.gitignore
+++ b/.gitignore
@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
build_release/
.clangd
wolfcore.found
+/wolf.found
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b84a4aa218a28e25b767d517f055f0c901bf8e8d..976709646ff5d2d052630537fe6178a4251c1653 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -292,6 +292,7 @@ SET(HDRS_TREE_MANAGER
include/core/tree_manager/factory_tree_manager.h
include/core/tree_manager/tree_manager_base.h
include/core/tree_manager/tree_manager_sliding_window.h
+ include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
)
SET(HDRS_YAML
include/core/yaml/parser_yaml.h
@@ -381,6 +382,7 @@ SET(SRCS_SOLVER
)
SET(SRCS_TREE_MANAGER
src/tree_manager/tree_manager_sliding_window.cpp
+ src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
)
SET(SRCS_YAML
src/yaml/parser_yaml.cpp
diff --git a/README.md b/README.md
index ad5473fec99242beacf3889f0818e3f4cd93caa7..c1e960469ccdcaf2b9918905b7e10536c9e9af59 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
WOLF - Windowed Localization Frames
===================================
-For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
\ No newline at end of file
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
\ No newline at end of file
diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h
index 27e5d758a84821dcaada80e9062c917c7af50020..1717be546ab820d24e13985227dd19753530440e 100644
--- a/include/core/ceres_wrapper/iteration_update_callback.h
+++ b/include/core/ceres_wrapper/iteration_update_callback.h
@@ -16,10 +16,11 @@ namespace wolf {
class IterationUpdateCallback : public ceres::IterationCallback
{
public:
- explicit IterationUpdateCallback(ProblemPtr _problem, bool verbose = false)
+ explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
: problem_(_problem)
- , verbose_(verbose)
+ , min_num_iterations_(min_num_iterations)
, initial_cost_(0)
+ , verbose_(verbose)
{}
~IterationUpdateCallback() {}
@@ -29,8 +30,9 @@ class IterationUpdateCallback : public ceres::IterationCallback
if (summary.iteration == 0)
initial_cost_ = summary.cost;
- if (problem_->getStateBlockNotificationMapSize() != 0 or
- problem_->getFactorNotificationMapSize() != 0)
+ else if (summary.iteration >= min_num_iterations_ and
+ (problem_->getStateBlockNotificationMapSize() != 0 or
+ problem_->getFactorNotificationMapSize() != 0))
{
if (summary.cost >= initial_cost_)
{
@@ -48,8 +50,9 @@ class IterationUpdateCallback : public ceres::IterationCallback
private:
ProblemPtr problem_;
- bool verbose_;
+ int min_num_iterations_;
double initial_cost_;
+ bool verbose_;
};
}
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index e4306aa8ca126185a6343435405c1bcb5940e650..62e2ab1a2da15ecec5ea372150a3c83c8b6b3bea 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -23,6 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver
{
bool update_immediately = false;
+ int min_num_iterations = 1;
ceres::Solver::Options solver_options;
ceres::Problem::Options problem_options;
ceres::Covariance::Options covariance_options;
@@ -39,15 +40,21 @@ struct ParamsCeres : public ParamsSolver
loadHardcodedValues();
// stop solver whenever the problem is updated (via ceres::iterationCallback)
- update_immediately = _server.getParam<bool>(prefix + "update_immediately");
+ update_immediately = _server.getParam<bool>(prefix + "update_immediately");
+ if (update_immediately)
+ min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations");
// ceres solver options
- solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
+ solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
+ solver_options.num_threads = _server.getParam<int>(prefix + "n_threads");
+ covariance_options.num_threads = _server.getParam<int>(prefix + "n_threads");
}
void loadHardcodedValues()
{
solver_options = ceres::Solver::Options();
+ solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH;
+ solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG;
problem_options = ceres::Problem::Options();
covariance_options = ceres::Covariance::Options();
problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
@@ -62,7 +69,6 @@ struct ParamsCeres : public ParamsSolver
#else
covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
#endif
- covariance_options.num_threads = 1;
covariance_options.apply_loss_function = false;
}
@@ -76,6 +82,7 @@ class SolverCeres : public SolverManager
std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
+ // this map is only for handling automatic destruction of localParametrizationWrapper objects
std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
ceres::Solver::Summary summary_;
@@ -99,10 +106,10 @@ class SolverCeres : public SolverManager
std::unique_ptr<ceres::Problem>& getCeresProblem();
- void computeCovariances(CovarianceBlocksToBeComputed _blocks
+ bool computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- void computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
+ bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
bool hasConverged() override;
@@ -116,6 +123,10 @@ class SolverCeres : public SolverManager
const Eigen::SparseMatrixd computeHessian() const;
+ virtual int numStateBlocksDerived() const override;
+
+ virtual int numFactorsDerived() const override;
+
protected:
bool checkDerived(std::string prefix="") const override;
@@ -138,7 +149,14 @@ class SolverCeres : public SolverManager
bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
- bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) override;
+
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
@@ -158,16 +176,39 @@ inline ceres::Solver::Options& SolverCeres::getSolverOptions()
inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
- && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
+ return fac_2_residual_idx_.count(fac_ptr) == 1 and
+ fac_2_costfunction_.count(fac_ptr) == 1;
}
-inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr)
+inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
{
- return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end()
- && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+ if (state_blocks_.count(state_ptr) == 0)
+ return false;
+ return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
+inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
+{
+ if (state_blocks_.count(st) == 0)
+ return false;
+ return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
+};
+
+inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
+{
+ return state_blocks_local_param_.count(st) == 1;
+};
+
+inline int SolverCeres::numStateBlocksDerived() const
+{
+ return ceres_problem_->NumParameterBlocks();
+}
+
+inline int SolverCeres::numFactorsDerived() const
+{
+ return ceres_problem_->NumResidualBlocks();
+};
+
} // namespace wolf
#endif
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index acfa2231c8ba692497d3ce69c9dcccd11f2e4382..84595cc3f6fd7c72374a30cb324d92ad2efbfd10 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -18,6 +18,7 @@ class SensorBase;
// std
#include <memory>
#include <map>
+#include <chrono>
namespace wolf {
@@ -274,6 +275,18 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
unsigned int id() const;
+ // PROFILING
+ unsigned int n_capture_callback_;
+ unsigned int n_kf_callback_;
+ std::chrono::microseconds duration_capture_;
+ std::chrono::microseconds duration_kf_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
+ void startCaptureProfiling();
+ void stopCaptureProfiling();
+ void startKFProfiling();
+ void stopKFProfiling();
+
protected:
/** \brief process an incoming capture
*
@@ -393,6 +406,26 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
+inline void ProcessorBase::startCaptureProfiling()
+{
+ start_capture_ = std::chrono::high_resolution_clock::now();
+}
+
+inline void ProcessorBase::stopCaptureProfiling()
+{
+ duration_capture_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_capture_);
+}
+
+inline void ProcessorBase::startKFProfiling()
+{
+ start_kf_ = std::chrono::high_resolution_clock::now();
+}
+
+inline void ProcessorBase::stopKFProfiling()
+{
+ duration_kf_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_kf_);
+}
+
inline bool ProcessorBase::isVotingActive() const
{
return params_->voting_active;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 1f62a82a39dfcf5ae2de6183d341051d5785b45c..6a0f15a90885f5438c18c2cf01ed714c488ba599 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -247,11 +247,11 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
double updateDt();
void integrateOneStep();
- void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
+ void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
TimeStamp ts_split,
const FrameBasePtr& keyframe_target,
- const wolf::CaptureMotionPtr& capture_target);
+ const wolf::CaptureMotionPtr& capture_target) const;
/** Pre-process incoming Capture
*
@@ -416,14 +416,26 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const = 0;
+ /** \brief merge two consecutive capture motions into the second one
+ * Merge two consecutive capture motions into the second one.
+ * The first capture motion is not removed.
+ * If the second capture has feature and factor emplaced, they are replaced by a new ones.
+ * @param cap_prev : the first capture motion to be merged (input)
+ * @param cap_target : the second capture motion (modified)
+ */
+ void mergeCaptures(CaptureMotionConstPtr cap_prev,
+ CaptureMotionPtr cap_target);
+
protected:
/** \brief emplace a CaptureMotion
- * \param _ts time stamp
+ * \param _frame_own frame owning the Capture to create
* \param _sensor Sensor that produced the Capture
+ * \param _ts time stamp
* \param _data a vector of motion data
- * \param _sata_cov covariances matrix of the motion data data
- * \param _frame_own frame owning the Capture to create
- * \param _frame_origin frame acting as the origin of motions for the MorionBuffer of the created MotionCapture
+ * \param _data_cov covariances matrix of the motion data data
+ * \param _calib calibration vector
+ * \param _calib_preint calibration vector used during pre-integration
+ * \param _capture_origin_ptr capture acting as the origin of motions for the MorionBuffer of the created MotionCapture
*/
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
@@ -491,18 +503,19 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
protected:
// helpers to avoid allocation
- double dt_; ///< Time step
- Eigen::VectorXd x_; ///< current state
- Eigen::VectorXd delta_; ///< current delta
- Eigen::MatrixXd delta_cov_; ///< current delta covariance
- Eigen::VectorXd delta_integrated_; ///< integrated delta
- Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
- Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
- Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
- Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
- Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
- Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
- Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+ mutable double dt_; ///< Time step
+ mutable Eigen::VectorXd x_; ///< current state
+ mutable Eigen::VectorXd delta_; ///< current delta
+ mutable Eigen::MatrixXd delta_cov_; ///< current delta covariance
+ mutable Eigen::VectorXd delta_integrated_; ///< integrated delta
+ mutable Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
+ mutable Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
+ mutable Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+ mutable Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
+ mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
+ mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
+
+ Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 17341101c0d2ee4ee7074d220fda006e42fd0b31..c2dce560c5a701f2d5131c34c31fa489da96a4f2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -56,7 +56,8 @@ class SolverManager
{
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
+ ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+ GAUSS
};
/**
@@ -69,6 +70,11 @@ class SolverManager
FULL
};
+ // PROFILING
+ unsigned int n_solve_;
+ std::chrono::microseconds duration_manager_;
+ std::chrono::microseconds duration_solver_;
+
protected:
ProblemPtr wolf_problem_;
@@ -96,9 +102,9 @@ class SolverManager
*/
std::string solve(const ReportVerbosity report_level);
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+ virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
+ virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
virtual bool hasConverged() = 0;
@@ -119,31 +125,47 @@ class SolverManager
ReportVerbosity getVerbosity() const;
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
+ virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
+
+ virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
+
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+ virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+ virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) final;
+
+ virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+ virtual int numFactors() const final;
+ virtual int numStateBlocks() const final;
+ virtual int numStateBlocksFloating() const final;
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
+ virtual int numFactorsDerived() const = 0;
+ virtual int numStateBlocksDerived() const = 0;
- bool check(std::string prefix="") const;
+ virtual bool check(std::string prefix="") const final;
protected:
std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
std::set<FactorBasePtr> factors_;
+ std::set<StateBlockPtr> floating_state_blocks_;
virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
- virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+ double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
private:
// SolverManager functions
- void addFactor(const FactorBasePtr& fac_ptr);
- void removeFactor(const FactorBasePtr& fac_ptr);
- void addStateBlock(const StateBlockPtr& state_ptr);
- void removeStateBlock(const StateBlockPtr& state_ptr);
- void updateStateBlockState(const StateBlockPtr& state_ptr);
- void updateStateBlockStatus(const StateBlockPtr& state_ptr);
- void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr);
+ virtual void addFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
protected:
// Derived virtual functions
@@ -154,9 +176,13 @@ class SolverManager
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
virtual bool checkDerived(std::string prefix="") const = 0;
+ virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+ virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) = 0;
+ virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
};
// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index 2236c1d62e94c6223f13529d2be55d9fd60baf00..460835cb9bf03c92cfb73f8516248816212c6571 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -27,7 +27,7 @@ class LocalParametrizationAngle : public LocalParametrizationBase
bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
-
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
inline LocalParametrizationAngle::LocalParametrizationAngle() :
@@ -64,6 +64,13 @@ inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>&
return true;
}
+inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
+{
+ //Any real is a valid angle because we use the pi2pi function. Also
+ //the types don't match. In this case the argument is
+ //Eigen::Map not Eigen::VectorXd
+ return true;
+}
} /* namespace wolf */
#endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 14f2ce38f1dd55b6571884710eb7ed7b7640f985..8494ec3a89215af8287584c8255b241af3486b45 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -31,7 +31,7 @@ class LocalParametrizationBase{
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
-
+ virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
};
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index 30785bb2cae80e94117d6895657b96617b6adefb..b8bbb981c6cec93c9e4fbfa29049a12f16d137a9 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -47,6 +47,7 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
+ bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 75804f85dabc9d9480ef045944f195fe1a472bb9..c637ab6bb07a23b0e5dd5528d6a1c240327e1473 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -73,6 +73,12 @@ public:
bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
+ // inline bool isValid(const Eigen::VectorXd& state) override;
+ // template<QuaternionDeltaReference DeltaReference>
+ bool isValid(const Eigen::VectorXd& _state, double tolerance) override
+ {
+ return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
+ }
};
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index c8706e2b6d081802d4a3b2590538b2e45df0584b..5406fe0c105c67d87e4793ba86a2e52f81bbfa87 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -159,6 +159,8 @@ public:
void plus(const Eigen::VectorXd& _dv);
+ bool isValid(double tolerance = Constants::EPS);
+
static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
};
@@ -319,7 +321,10 @@ inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fi
{
return std::make_shared<StateBlock>(_state, _fixed);
}
-
+inline bool StateBlock::isValid(double tolerance)
+{
+ return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
+}
}// namespace wolf
#endif
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 0ff5f31ed2734b7840518bf18dfc32073dac4cec..ce44fe003a8dfa58bb9a938e804af3e26fe576df 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -1,7 +1,7 @@
#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
-#include "../tree_manager/tree_manager_base.h"
+#include "core/tree_manager/tree_manager_base.h"
namespace wolf
{
@@ -15,20 +15,20 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsTreeManagerBase(_unique_name, _server)
{
- n_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames");
- fix_first_key_frame = _server.getParam<bool> (prefix + "/fix_first_key_frame");
+ n_frames = _server.getParam<unsigned int>(prefix + "/n_frames");
+ fix_first_frame = _server.getParam<bool> (prefix + "/fix_first_frame");
viral_remove_empty_parent = _server.getParam<bool> (prefix + "/viral_remove_empty_parent");
}
std::string print() const
{
return "\n" + ParamsTreeManagerBase::print() + "\n"
- + "n_key_frames: " + std::to_string(n_key_frames) + "\n"
- + "fix_first_key_frame: " + std::to_string(fix_first_key_frame) + "\n"
+ + "n_frames: " + std::to_string(n_frames) + "\n"
+ + "fix_first_frame: " + std::to_string(fix_first_frame) + "\n"
+ "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
}
- unsigned int n_key_frames;
- bool fix_first_key_frame;
+ unsigned int n_frames;
+ bool fix_first_frame;
bool viral_remove_empty_parent;
};
@@ -43,7 +43,7 @@ class TreeManagerSlidingWindow : public TreeManagerBase
~TreeManagerSlidingWindow() override{}
- void keyFrameCallback(FrameBasePtr _key_frame) override;
+ void keyFrameCallback(FrameBasePtr _frame) override;
protected:
ParamsTreeManagerSlidingWindowPtr params_sw_;
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
new file mode 100644
index 0000000000000000000000000000000000000000..e9fcbd1332fd2d4eca3f7afa04c2aee167ad1980
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -0,0 +1,51 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
+
+struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
+{
+ ParamsTreeManagerSlidingWindowDualRate() = default;
+ ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerSlidingWindow(_unique_name, _server)
+ {
+ n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent");
+ assert(n_frames_recent <= n_frames);
+ rate_old_frames = _server.getParam<unsigned int>(prefix + "/rate_old_frames");
+ }
+ std::string print() const
+ {
+ return "\n" + ParamsTreeManagerBase::print() + "\n"
+ + "n_frames_recent: " + std::to_string(n_frames_recent) + "\n"
+ + "rate_old_frames: " + std::to_string(rate_old_frames) + "\n";
+ }
+
+ unsigned int n_frames_recent, rate_old_frames;
+};
+
+class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
+{
+ public:
+ TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
+ ;
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+
+ ~TreeManagerSlidingWindowDualRate() override{}
+
+ void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
+ unsigned int count_frames_;
+ //TrajectoryIter first_recent_frame_it_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ */
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 94df0d2c9d77aba7e61ee7f70b87aafc927d6fa6..47990c97d0190f0780a475780fc6c7294501a102 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -56,7 +56,6 @@ CaptureBase::CaptureBase(const std::string& _type,
CaptureBase::~CaptureBase()
{
- removeStateBlocks(getProblem());
}
void CaptureBase::remove(bool viral_remove_empty_parent)
@@ -224,6 +223,8 @@ void CaptureBase::setProblem(ProblemPtr _problem)
if (_problem == nullptr || _problem == this->getProblem())
return;
+ assert(getFrame() and getFrame()->isKey());
+
NodeBase::setProblem(_problem);
registerNewStateBlocks(_problem);
@@ -233,8 +234,6 @@ 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();
@@ -247,7 +246,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
_stream << " <-- ";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
@@ -257,7 +257,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ if (sb)
+ _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << std::endl;
}
}
else if (_metric)
@@ -272,7 +273,8 @@ void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
for (const auto& key : getStructure())
{
const auto& sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
@@ -284,7 +286,8 @@ void CaptureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_b
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 3)
for (auto f : getFeatureList())
- f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (f)
+ f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index da17a57af80971b21f4a69322ba0c2ade54225d4..bad56dae309cca61a119594e3b396444409e267b 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -108,7 +108,7 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
_stream << (sb->isFixed() ? "Fix" : "Est");
if (_metric)
_stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- _stream << std::endl;
+ _stream << " @ " << sb << std::endl;
}
}
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 033b0f9100b2befc728157cb0fb8d282d9808dd0..e0995c7be03642c74d9e75b4ce2f266a165d81f3 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -25,7 +25,9 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
if (params_ceres_->update_immediately)
- getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_, params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
+ getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
+ params_ceres_->min_num_iterations,
+ params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
}
SolverCeres::~SolverCeres()
@@ -51,7 +53,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
// run Ceres Solver
ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
- // if termination by user (Iteration callback, update and solve again)
+ /*/ if termination by user (Iteration callback, update and solve again)
// solve until max iterations reached
if (params_ceres_->update_immediately)
{
@@ -68,7 +70,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
}
getSolverOptions().max_num_iterations = max_num_iterations;
- }
+ }*/
std::string report;
//return report
@@ -80,14 +82,11 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
return report;
}
-void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
+bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
{
// update problem
update();
- // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
- wolf_problem_->clearCovariance();
-
// CREATE DESIRED COVARIANCES LIST
std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
std::vector<std::pair<const double*, const double*>> double_pairs;
@@ -115,12 +114,17 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
}
// double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++)
+ {
+ assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
for (unsigned int j = i; j < all_state_blocks.size(); j++)
{
+ assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
+
state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
getAssociatedMemBlockPtr(all_state_blocks[j]));
}
+ }
break;
}
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
@@ -129,26 +133,36 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
for(auto fr_ptr : *wolf_problem_->getTrajectory())
if (fr_ptr->isKey())
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);
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())
for (auto sb : l_ptr->getUsedStateBlockVec())
+ {
+ assert(isStateBlockRegisteredDerived(sb));
for(auto sb2 : l_ptr->getUsedStateBlockVec())
{
+ assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
if (sb == sb2)
break;
}
+ }
// // loop all marginals (PO marginals)
// for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
// {
@@ -167,6 +181,9 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
//robot-robot
auto last_key_frame = wolf_problem_->getLastFrame();
+ assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
+ assert(isStateBlockRegisteredDerived(last_key_frame->getO()));
+
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO());
@@ -187,6 +204,8 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
{
+ assert(isStateBlockRegisteredDerived(*state_it));
+
// robot - landmark
state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
@@ -198,20 +217,65 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
// landmark marginal
for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
{
- state_block_pairs.emplace_back(*state_it, *next_state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
+ assert(isStateBlockRegisteredDerived(*next_state_it));
+
+ state_block_pairs.emplace_back(*state_it, *next_state_it);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
getAssociatedMemBlockPtr((*next_state_it)));
}
}
}
break;
}
+ case CovarianceBlocksToBeComputed::GAUSS:
+ {
+ // State blocks:
+ // - Last KF: PV
+
+ // last KF
+ FrameBasePtr last_frame = wolf_problem_->getLastFrame();
+ if (not last_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')))
+ {
+ 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'));
+
+ 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')));
+
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
+
}
//std::cout << "pairs... " << double_pairs.size() << std::endl;
// COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get()))
{
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
+
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
@@ -220,34 +284,39 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got switch: " << std::endl << cov << std::endl;
}
+ return true;
}
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
+ return false;
}
-void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+
+bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
{
//std::cout << "SolverCeres: computing covariances..." << std::endl;
// update problem
update();
- // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
- wolf_problem_->clearCovariance();
-
// CREATE DESIRED COVARIANCES LIST
std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
std::vector<std::pair<const double*, const double*>> double_pairs;
// double loop all against all (without repetitions)
- for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++){
+ for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
+ {
if (*st_it1 == nullptr){
continue;
}
+ assert(isStateBlockRegisteredDerived(*st_it1));
+
for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
{
if (*st_it2 == nullptr){
continue;
}
+ assert(isStateBlockRegisteredDerived(*st_it2));
+
state_block_pairs.emplace_back(*st_it1, *st_it2);
double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
getAssociatedMemBlockPtr((*st_it2)));
@@ -258,6 +327,10 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
// COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get()))
+ {
+ // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
+ wolf_problem_->clearCovariance();
+
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
@@ -266,13 +339,16 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
}
- else
- std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
+ return true;
+ }
+
+ WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
+ return false;
}
void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
{
- assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
+ assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
auto cost_func_ptr = createCostFunction(fac_ptr);
fac_2_costfunction_[fac_ptr] = cost_func_ptr;
@@ -281,6 +357,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
{
+ assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
}
@@ -312,7 +389,7 @@ void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
{
assert(state_ptr);
- assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added");
+ assert(!isStateBlockRegisteredDerived(state_ptr) && "adding a state block already added");
assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
ceres::LocalParameterization* local_parametrization_ptr = nullptr;
@@ -339,7 +416,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
{
//std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
assert(state_ptr);
- assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres");
+ assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
+
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
state_blocks_local_param_.erase(state_ptr);
}
@@ -347,6 +425,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
{
assert(state_ptr != nullptr);
+ assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres");
+
if (state_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
else
@@ -549,7 +629,7 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
if (!sb->isFixed())
{
- assert(state_blocks_local_param_.count(sb) != 0 && "factor involving a state block not added");
+ assert(state_blocks_.count(sb) != 0 && "factor involving a state block not added");
assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
@@ -579,6 +659,14 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
return H;
}
+bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param)
+{
+ return state_blocks_local_param_.count(st) == 1
+ && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
+ && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
+ == state_blocks_local_param_.at(st).get();
+}
} // namespace wolf
#include "core/solver/factory_solver.h"
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 69133345e0f57c28e3135aa3b50e0d99e179be50..b864e27c8172671572026ce4f83b4d279bb1b669 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -182,7 +182,8 @@ void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _s
{
_stream << " <--\t";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
if (_metric)
@@ -193,10 +194,12 @@ 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())
- c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (c)
+ c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index f7c7ce6ea257a48f73c68003b8c1efbfeb4f6a6b..20ab9331867cd18963b5bc85b0f42fa1c01b4a2f 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -303,12 +303,13 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
void FrameBase::setProblem(ProblemPtr _problem)
{
+ assert(isKey() && "Trying to setProblem to a non keyframe");
+
if (_problem == nullptr || _problem == this->getProblem())
return;
NodeBase::setProblem(_problem);
- if (this->isKey())
- registerNewStateBlocks(_problem);
+ registerNewStateBlocks(_problem);
for (auto cap : capture_list_)
cap->setProblem(_problem);
@@ -324,7 +325,8 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
{
_stream << " <-- ";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
@@ -333,10 +335,12 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << _tabs << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
- << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
- << std::endl;
+ if (sb)
+ _stream << _tabs << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << " @ " << sb
+ << std::endl;
}
}
else if (_metric)
@@ -351,7 +355,8 @@ void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _sta
for (const auto& key : getStructure())
{
const auto& sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
@@ -362,7 +367,8 @@ void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blo
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 2)
for (auto C : getCaptureList())
- C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (C)
+ C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index 72f07b8e216e6ca830ec9021dbb7ca1144d09860..e6e5a15c3c216d2abfa9242a48bb97081e22d016 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -30,7 +30,8 @@ void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
for (auto S : getSensorList())
- S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (S)
+ S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index af2d01b68e40c33b129f799d93fe3d133459671b..c8f855dfb512a8e1438f07c0c9582db8d2c0f578 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -165,7 +165,8 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
{
_stream << "\t<-- ";
for (auto cby : getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
@@ -173,7 +174,8 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
for (const auto& key : getStructure())
{
auto sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << std::endl;
}
}
else if (_metric)
@@ -188,7 +190,8 @@ void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _
for (const auto& key : getStructure())
{
const auto& sb = getStateBlock(key);
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 67c37971b4e0517017f03b9cd798080fcd40808d..7f6db136f0d1644bb4d2ff193fc3c7d776fdfa26 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -99,7 +99,8 @@ void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_block
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
for (auto L : getLandmarkList())
- L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (L)
+ L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b151a6929ed93ecb4c231ffcd409df89ee3c36d1..bfcb33fa67482f2907ce1cdc715e4dacc44d60ed 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -605,6 +605,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+ // pause processor profiling
+ if (_processor_ptr)
+ _processor_ptr->stopCaptureProfiling();
+
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor && (processor != _processor_ptr) )
@@ -629,6 +633,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
// notify tree manager
if (tree_manager_)
tree_manager_->keyFrameCallback(_keyframe_ptr);
+
+ // resume processor profiling
+ if (_processor_ptr)
+ _processor_ptr->startCaptureProfiling();
}
bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 5c769cde325442b711ff36c8a18f557a289ed6f6..a4ff214e113e41c67725bae938988ff3d10d857b 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -12,7 +12,11 @@ ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessor
processor_id_(++processor_id_count_),
params_(_params),
dim_(_dim),
- sensor_ptr_()
+ sensor_ptr_(),
+ n_capture_callback_(0),
+ n_kf_callback_(0),
+ duration_capture_(0),
+ duration_kf_(0)
{
// WOLF_DEBUG("constructed +p" , id());
}
@@ -37,6 +41,9 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _
assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
+ // profiling
+ n_kf_callback_++;
+ startKFProfiling();
// asking if key frame should be stored
if (storeKeyFrame(_keyframe_ptr))
@@ -46,6 +53,8 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _
if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
processKeyFrame(_keyframe_ptr, _time_tol_other);
+ // profiling
+ stopKFProfiling();
}
void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
@@ -53,6 +62,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ // profiling
+ n_capture_callback_++;
+ startCaptureProfiling();
+
// apply prior in problem if not done (very first capture)
if (getProblem() && !getProblem()->isPriorSet())
getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
@@ -64,6 +77,9 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
// if trigger, process directly without buffering
if (triggerInCapture(_capture_ptr))
processCapture(_capture_ptr);
+
+ // profiling
+ stopCaptureProfiling();
}
void ProcessorBase::remove()
@@ -220,7 +236,6 @@ void BufferPackKeyFrame::print(void) const
void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
-
}
void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f56b8c80a344e864f16be3ca2f8f006a0996052f..d18ae7c312f4e19a98ed1ae9522b2b06aad2fd41 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -54,10 +54,45 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
-void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
+
+void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+ CaptureMotionPtr cap_target)
+{
+ assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions");
+
+ // add prev buffer (discarding the first zero motion)
+ cap_target->getBuffer().pop_front();
+ cap_target->getBuffer().insert(cap_target->getBuffer().begin(),
+ cap_prev->getBuffer().begin(),
+ cap_prev->getBuffer().end());
+
+ // change origin
+ cap_target->setOriginCapture(cap_prev->getOriginCapture());
+
+ // change calibration params for preintegration from origin
+ cap_target->setCalibrationPreint(getCalibration(cap_prev->getOriginCapture()));
+
+ // reintegrate buffer
+ reintegrateBuffer(cap_target);
+
+ // replace existing feature and factor (if they exist)
+ if (!cap_target->getFeatureList().empty())
+ {
+ // remove feature and factor (automatically)
+ cap_target->getFeatureList().back()->remove();
+
+ assert(cap_target->getFeatureList().empty());// there should be one feature only
+
+ // emplace new feature and factor
+ auto new_feature = emplaceFeature(cap_target);
+ emplaceFactor(new_feature, cap_prev->getOriginCapture());
+ }
+}
+
+void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
TimeStamp _ts_split,
const FrameBasePtr& _keyframe_target,
- const wolf::CaptureMotionPtr& _capture_target)
+ const CaptureMotionPtr& _capture_target) const
{
/** we are doing this:
*
@@ -732,7 +767,7 @@ void ProcessorMotion::integrateOneStep()
statePlusDelta(odometry_, delta_, dt_, odometry_);
}
-void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
+void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
{
VectorXd calib = _capture_ptr->getCalibrationPreint();
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 91867f39528b54cc7335fcca91e7fcc108757576..2e03aaafb52183adfd1843da41e0ca4ed3b49b3e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -83,8 +83,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr kfrm = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(),
incoming_ptr_->getTimeStamp(),
- getStateStructure(),
- getProblem()->getState(getStateStructure()));
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(kfrm);
// Process info
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 3804962f62fcbfba13b56e7c64866be7497eeaa8..cda95e5ed6259e7ef11d94d01c3fe939ffe61941 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -91,7 +91,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
if (known_features_last_.empty())
{
- WOLF_TRACE("Empty last feature list, returning...");
+ WOLF_DEBUG("Empty last feature list, returning...");
return 0;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index fefc4d332eca061b041585bf42c6248fb985d992..82fbe684558bd9791763b70c0bbedab68514b8f0 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -198,7 +198,7 @@ void SensorBase::registerNewStateBlocks() const
auto key = pair_key_sbp.first;
auto sbp = pair_key_sbp.second;
- if (sbp && !isStateBlockInCapture(key))
+ if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key))
getProblem()->notifyStateBlock(sbp, ADD);
}
}
@@ -479,7 +479,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
for (auto& key : getStructure())
{
auto sb = getStateBlockDynamic(key);
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ if (sb)
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ) @ " << sb << " ";
}
_stream << std::endl;
}
@@ -489,7 +490,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
for (auto& key : getStructure())
{
auto sb = getStateBlockDynamic(key);
- _stream << sb->getState().transpose() << " ";
+ if (sb)
+ _stream << sb->getState().transpose() << " ";
}
_stream << ")" << std::endl;
}
@@ -499,7 +501,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
for (auto& key : getStructure())
{
auto sb = getStateBlockDynamic(key);
- _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ if (sb)
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb << " ";
}
_stream << std::endl;
}
@@ -511,7 +514,8 @@ void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_bl
if (_depth >= 2)
for (auto p : getProcessorList())
- p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ if (p)
+ p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 4a74ca8194f9b2b6111fb69d4c97bb7a4875b60e..8a9e15d6e81b88905ffdd3fb81b0635013ebf426 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -12,6 +12,9 @@ SolverManager::SolverManager(const ProblemPtr& _problem) :
SolverManager::SolverManager(const ProblemPtr& _problem,
const ParamsSolverPtr& _params) :
+ n_solve_(0),
+ duration_manager_(0),
+ duration_solver_(0),
wolf_problem_(_problem),
params_(_params)
{
@@ -75,10 +78,19 @@ void SolverManager::update()
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
+ std::set<StateBlockPtr> new_floating_state_blocks;
for (auto state_pair : state_blocks_)
{
auto state_ptr = state_pair.first;
+ // Check for "floating" state blocks (not involved in any factor -> not observable problem)
+ if (state_blocks_2_factors_.at(state_ptr).empty())
+ {
+ WOLF_DEBUG("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart.");
+ new_floating_state_blocks.insert(state_ptr);
+ continue;
+ }
+
// state update
if (state_ptr->stateUpdated())
updateStateBlockState(state_ptr);
@@ -92,6 +104,20 @@ void SolverManager::update()
updateStateBlockLocalParametrization(state_ptr);
}
+ // REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call)
+ for (auto state_ptr : new_floating_state_blocks)
+ {
+ removeStateBlock(state_ptr);
+ floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()!
+ }
+ // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock)
+ for (auto state_ptr : floating_state_blocks_)
+ {
+ state_ptr->resetStateUpdated();
+ state_ptr->resetFixUpdated();
+ state_ptr->resetLocalParamUpdated();
+ }
+
#ifdef _WOLF_DEBUG
assert(check("after update()"));
#endif
@@ -109,11 +135,16 @@ std::string SolverManager::solve()
std::string SolverManager::solve(const ReportVerbosity report_level)
{
+ auto start = std::chrono::high_resolution_clock::now();
+ n_solve_++;
+
// update problem
update();
// call derived solver
+ auto start_derived = std::chrono::high_resolution_clock::now();
std::string report = solveDerived(report_level);
+ duration_solver_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_derived);
// update StateBlocks with optimized state value.
/// @todo whatif someone has changed the state notification during opti ??
@@ -128,6 +159,8 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
}
+ duration_manager_ += std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+
return report;
}
@@ -136,7 +169,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
// Warning if adding an already added
if (factors_.count(fac_ptr) != 0)
{
- WOLF_WARN("Tried to add a factor that was already added !");
+ WOLF_WARN("Tried to add the factor ", fac_ptr->id(), " that was already added !");
return;
}
@@ -150,10 +183,20 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
for (const auto& st : fac_ptr->getStateBlockPtrVector())
if (state_blocks_.count(st) == 0)
{
- WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
- // put back notification in problem (will be added next update() call) and do nothing
- wolf_problem_->notifyFactor(fac_ptr, ADD);
- return;
+ // Check if it was stored as a 'floating' state block
+ if (floating_state_blocks_.count(st) == 1)
+ {
+ WOLF_DEBUG("SolverManager::addFactor(): Factor ", fac_ptr->id(), " involves state block ", st, " stored as 'floating'. Adding the state block to the solver.");
+ floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()!
+ addStateBlock(st);
+ }
+ else
+ {
+ WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+ // put back notification in problem (will be added next update() call) and do nothing
+ wolf_problem_->notifyFactor(fac_ptr, ADD);
+ return;
+ }
}
// factors
@@ -176,7 +219,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
// Warning if removing a missing factor
if (factors_.count(fac_ptr) == 0)
{
- WOLF_WARN("Tried to remove a factor that is missing !");
+ WOLF_WARN("Tried to remove factor ", fac_ptr->id(), " that is missing !");
return;
}
@@ -200,12 +243,19 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
// Warning if adding an already added state block
if (state_blocks_.count(state_ptr) != 0)
{
- WOLF_WARN("Tried to add a StateBlock that was already added !");
+ WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that was already added !");
+ return;
+ }
+ // Warning if adding a floating state block
+ if (floating_state_blocks_.count(state_ptr) != 0)
+ {
+ WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that is stored as floating !");
return;
}
assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+ assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)");
// stateblock maps
state_blocks_.emplace(state_ptr, state_ptr->getState());
@@ -222,10 +272,16 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
{
+ // check if stored as 'floating' state block
+ if (floating_state_blocks_.count(state_ptr) == 1)
+ {
+ floating_state_blocks_.erase(state_ptr);
+ return;
+ }
// Warning if removing a missing state block
if (state_blocks_.count(state_ptr) == 0)
{
- WOLF_WARN("Tried to remove a StateBlock that was not added !");
+ WOLF_WARN("Tried to remove the StateBlock ", state_ptr, " that was not added !");
return;
}
@@ -270,8 +326,12 @@ void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
{
+ assert(state_ptr && "SolverManager::updateStateBlockState null state block");
+ assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block");
+ assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
+ assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size());
+
Eigen::VectorXd new_state = state_ptr->getState();
- // We assume the same size for the states in both WOLF and the solver.
std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
// reset flag
state_ptr->resetStateUpdated();
@@ -291,8 +351,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
+ {
+ WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ }
return it->second;
}
@@ -301,8 +363,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
- throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ {
+ WOLF_ERROR("Tried to retrieve the memory block const ptr of an unregistered StateBlock: ", state_ptr);
+ throw std::runtime_error("Tried to retrieve the memory block const ptr of an unregistered StateBlock !");
+ }
return it->second.data();
}
@@ -311,8 +375,10 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
auto it = state_blocks_.find(state_ptr);
if (it == state_blocks_.end())
- throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
-
+ {
+ WOLF_ERROR("Tried to retrieve the memory block ptr of an unregistered StateBlock: ", state_ptr);
+ throw std::runtime_error("Tried to retrieve the memory block ptr of an unregistered StateBlock !");
+ }
return it->second.data();
}
@@ -321,14 +387,67 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const
return params_->verbose;
}
-bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
+bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
{
- return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);
+ return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+}
+
+bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
+{
+ return floating_state_blocks_.count(state_ptr) == 1;
}
bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return isFactorRegisteredDerived(fac_ptr);
+ return factors_.count(fac_ptr) == 1 and isFactorRegisteredDerived(fac_ptr);
+}
+
+bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->isFixed();
+
+ return isStateBlockFixedDerived(st);
+}
+
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->getLocalParametrization() == local_param;
+
+ return hasThisLocalParametrizationDerived(st, local_param);
+};
+
+bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->hasLocalParametrization();
+
+ return hasLocalParametrizationDerived(st);
+};
+
+int SolverManager::numFactors() const
+{
+ return factors_.size();
+}
+
+int SolverManager::numStateBlocks() const
+{
+ return state_blocks_.size() + floating_state_blocks_.size();
+}
+
+int SolverManager::numStateBlocksFloating() const
+{
+ return floating_state_blocks_.size();
}
double SolverManager::getPeriod() const
@@ -357,15 +476,32 @@ bool SolverManager::check(std::string prefix) const
ok = false;
}
- // factor involving state block in factors_
- for (auto fac : sb_fac_it->second)
+ // no factors involving state block
+ if (sb_fac_it->second.empty())
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix);
+ ok = false;
+ }
+ else
{
- if (factors_.count(fac) == 0)
+ // factor involving state block in factors_
+ for (auto fac : sb_fac_it->second)
{
- WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
- ok = false;
+ if (factors_.count(fac) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+ ok = false;
+ }
}
}
+
+ // can't be in both state_blocks_ and floating_state_blocks_
+ if (floating_state_blocks_.count(sb_fac_it->first) == 1)
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix);
+ ok = false;
+ }
+
sb_vec_it++;
sb_fac_it++;
}
@@ -384,9 +520,38 @@ bool SolverManager::check(std::string prefix) const
}
}
- // checkDerived
+ // CHECK DERIVED ----------------------
ok &= checkDerived(prefix);
+ // CHECK IF DERIVED IS UP TO DATE ----------------------
+ // state blocks registered in derived solver
+ for (auto sb : state_blocks_)
+ if (!isStateBlockRegisteredDerived(sb.first))
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+ ok = false;
+ }
+
+ // factors registered in derived solver
+ for (auto fac : factors_)
+ if (!isFactorRegisteredDerived(fac))
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+ ok = false;
+ }
+
+ // numbers
+ if (numStateBlocksDerived() != state_blocks_.size())
+ {
+ WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+ ok = false;
+ }
+ if (numFactorsDerived() != numFactors())
+ {
+ WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+ ok = false;
+ }
+
return ok;
}
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index b2fa1c111e028445333236e01384144aeafc31a6..846b14a49934c70c0d3879efd03f454ffab74605 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -60,5 +60,9 @@ bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _
return true;
}
+bool LocalParametrizationHomogeneous::isValid(const Eigen::VectorXd& _state, double tolerance)
+{
+ return _state.size() == global_size_;
+}
} // namespace wolf
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 8ab61006c9e46acfdeee6ad735257bdf9d776ff7..23f967716a28e65d9aa09a01b1cb60a345f220e5 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -91,8 +91,8 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
for (auto F : *this)
- F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
-
+ if (F)
+ F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 623c7bec91214b55f7d2d8f2034dbba3e01c5ccb..3c64b2ee87d2e26318788c4eddd547606edb6cb0 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -3,32 +3,22 @@
namespace wolf
{
-void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
+void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_kf(0);
- FrameBasePtr first_KF(nullptr), second_KF(nullptr);
- for (auto frm : *getProblem()->getTrajectory())
- {
- if (frm->isKey())
- {
- n_kf++;
- if (first_KF == nullptr)
- first_KF = frm;
- else if (second_KF == nullptr)
- second_KF = frm;
- }
- }
+ int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
- // remove first KF if too many KF
- if (n_kf > params_sw_->n_key_frames)
+ // remove first frame if too many frames
+ if (n_f > params_sw_->n_frames)
{
- WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
- first_KF->remove(params_sw_->viral_remove_empty_parent);
- if (params_sw_->fix_first_key_frame)
+ if (params_sw_->fix_first_frame)
{
WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
- second_KF->fix();
+ auto second_frame = std::next(getProblem()->getTrajectory()->begin())->second;
+ if (second_frame)
+ second_frame->fix();
}
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..2009cd89edf486af2efa892f58615a1e0fa05b0f
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -0,0 +1,86 @@
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/capture/capture_motion.h"
+#include "core/processor/processor_motion.h"
+
+namespace wolf
+{
+
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) :
+ TreeManagerSlidingWindow(_params),
+ params_swdr_(_params),
+ count_frames_(0)
+{
+ NodeBase::setType("TreeManagerSlidingWindowDualRate");
+}
+
+void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
+{
+ int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+
+ // recent segment not complete
+ 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)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
+ FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
+ params_swdr_->n_frames_recent)->second;
+ FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
+ params_swdr_->n_frames_recent - 1)->second;
+
+ // compose motion captures for all processors motion
+ for (auto is_motion : getProblem()->getProcessorIsMotionList())
+ {
+ auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion);
+ if (proc_motion == nullptr)
+ continue;
+
+ auto cap_prev = std::static_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
+ auto cap_next = std::static_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));
+
+ // merge captures (if exist)
+ if (cap_prev and cap_next)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow merging capture ", cap_prev->id(), " with ", cap_next->id());
+ 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);
+ }
+
+ // iterate counter
+ count_frames_++;
+ if (count_frames_ == params_swdr_->rate_old_frames)
+ count_frames_ = 0;
+}
+
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowDualRate);
+} // namespace wolf
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 88bcfc88a48286994ec38aee96c3cb44eb25a200..4363e38a4d4df67a73ce92086b7a3ae071c4a561 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -139,6 +139,10 @@ target_link_libraries(gtest_shared_from_this ${PLUGIN_NAME})
wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
target_link_libraries(gtest_solver_manager ${PLUGIN_NAME})
+# SolverManagerMultithread test
+wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp)
+target_link_libraries(gtest_solver_manager_multithread ${PLUGIN_NAME})
+
# StateBlock class test
wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
target_link_libraries(gtest_state_block ${PLUGIN_NAME})
@@ -250,15 +254,23 @@ wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
IF (Ceres_FOUND)
-# SolverCeres test
-wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
-target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME})
+ # SolverCeres test
+ wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
+ target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME})
+
+ # SolverCeresMultithread test
+ wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
+ target_link_libraries(gtest_solver_ceres_multithread ${PLUGIN_NAME})
ENDIF(Ceres_FOUND)
# TreeManagerSlidingWindow class test
wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME})
+# TreeManagerSlidingWindowDualRate class test
+wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME})
+
# yaml conversions
wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME})
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 15db720a3c42f8151dd8745dc35d0232fc0fe885..885f955d217b5a6afcd8084ab5d951af26e5b374 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy);
class SolverManagerDummy : public SolverManager
{
public:
- std::list<FactorBasePtr> factors_;
+ std::set<FactorBasePtr> factors_derived_;
std::map<StateBlockPtr,bool> state_block_fixed_;
std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
@@ -26,35 +26,34 @@ class SolverManagerDummy : public SolverManager
{
};
- bool isStateBlockRegistered(const StateBlockPtr& st) override
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override
{
- return state_blocks_.find(st)!=state_blocks_.end();
+ return state_block_fixed_.at(st);
};
- bool isStateBlockFixed(const StateBlockPtr& st) const
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
{
- return state_block_fixed_.at(st);
+ return state_block_local_param_.at(st) == local_param;
};
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
{
- return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+ return state_block_local_param_.at(st) != nullptr;
};
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+ virtual int numStateBlocksDerived() const override
{
- return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
- };
+ return state_block_fixed_.size();
+ }
- bool hasLocalParametrization(const StateBlockPtr& st) const
+ virtual int numFactorsDerived() const override
{
- return state_block_local_param_.find(st) != state_block_local_param_.end();
+ return factors_derived_.size();
};
- void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
- void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {};
- bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;};
- bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;};
+ bool computeCovariances(const CovarianceBlocksToBeComputed blocks) override {return false;};
+ bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override {return false;};
+
// The following are dummy implementations
bool hasConverged() override { return true; }
@@ -62,19 +61,17 @@ class SolverManagerDummy : public SolverManager
double initialCost() override { return double(1); }
double finalCost() override { return double(0); }
-
-
protected:
bool checkDerived(std::string prefix="") const override {return true;}
std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
void addFactorDerived(const FactorBasePtr& fac_ptr) override
{
- factors_.push_back(fac_ptr);
+ factors_derived_.insert(fac_ptr);
};
void removeFactorDerived(const FactorBasePtr& fac_ptr) override
{
- factors_.remove(fac_ptr);
+ factors_derived_.erase(fac_ptr);
};
void addStateBlockDerived(const StateBlockPtr& state_ptr) override
{
@@ -92,10 +89,16 @@ class SolverManagerDummy : public SolverManager
};
void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
{
- if (state_ptr->getLocalParametrization() == nullptr)
- state_block_local_param_.erase(state_ptr);
- else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
+ {
+ return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
+ };
+
+ bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
+ {
+ return factors_derived_.count(fac_ptr) == 1;
};
};
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 657ddc48a3ee49d8aefa709379415e780641aeec..dc44348f5943d7702372e28ebfb4e8ea2a707876 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -113,7 +113,7 @@ TEST(Problem, SetOrigin_PO_2d)
VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
P->setPriorFactor(x0, s0, t0, 1.0);
-
+ WOLF_INFO("printing.-..");
P->print(4,1,1,1);
// check that no sensor has been added
@@ -246,7 +246,7 @@ TEST(Problem, StateBlocks)
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7d xs3d;
+ Eigen::Vector7d xs3d; xs3d << 1,2,3,0,0,0,1; // required normalized quaternion (solver manager checks this)
Eigen::Vector3d xs2d;
// 2 state blocks, fixed
@@ -564,6 +564,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.getState";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 743b0fcc56a4088ceb87073a6941200fd74ccd33..d905bcad555b88e4cd92afed94771c2e7f46546d 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -289,6 +289,83 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
+TEST_F(ProcessorMotion_test, mergeCaptures)
+{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ // copy initial buffer
+ MotionBuffer initial_buffer = C_source->getBuffer();
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+
+ // merge captures
+ processor->mergeCaptures(C_target, C_source);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+
+ // Check that merged buffer is the initial one (before splitting)
+ EXPECT_EQ(C_source->getBuffer().size(), initial_buffer.size());
+
+ auto init_it = initial_buffer.begin();
+ auto res_it = C_source->getBuffer().begin();
+ while (init_it != initial_buffer.end())
+ {
+ EXPECT_EQ(init_it->data_size_, init_it->data_size_);
+ EXPECT_EQ(init_it->delta_size_, init_it->delta_size_);
+ EXPECT_EQ(init_it->cov_size_, init_it->cov_size_);
+ EXPECT_EQ(init_it->calib_size_, init_it->calib_size_);
+ EXPECT_EQ(init_it->ts_, init_it->ts_);
+
+ EXPECT_MATRIX_APPROX(init_it->data_ ,init_it->data_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->data_cov_,init_it->data_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_, init_it->delta_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_cov_, init_it->delta_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_integr_, init_it->delta_integr_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->delta_integr_cov_, init_it->delta_integr_cov_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_delta_, init_it->jacobian_delta_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_delta_integr_, init_it->jacobian_delta_integr_, 1e-12);
+ EXPECT_MATRIX_APPROX(init_it->jacobian_calib_, init_it->jacobian_calib_, 1e-12);
+
+ init_it++;
+ res_it++;
+ }
+}
+
TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
{
// Prior
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index a05fd74de52247abb7cc3047861d2ece21d5963c..e5efa2dff18a27131a13cb173e064edad2d0a3c2 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -1,5 +1,5 @@
/*
- * gtest_solver_ceres.cpp
+ * gtest_solver_ptr.cpp
*
* Created on: Jun, 2018
* Author: jvallve
@@ -13,6 +13,7 @@
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_angle.h"
#include "core/state_block/local_parametrization_quaternion.h"
@@ -27,604 +28,1490 @@
using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(SolverCeresWrapper);
-class SolverCeresWrapper : public SolverCeres
+/*
+ * Following tests are the same as in gtest_solver_manager.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
{
- public:
- SolverCeresWrapper(const ProblemPtr& wolf_problem) :
- SolverCeres(wolf_problem)
- {
- };
-
- bool isStateBlockRegisteredSolverCeres(const StateBlockPtr& st)
- {
- return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st)
- {
- return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- int numStateBlocks()
- {
- return ceres_problem_->NumParameterBlocks();
- };
-
- int numFactors()
- {
- return ceres_problem_->NumResidualBlocks();
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
- {
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
- ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
- };
-
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
- };
-
-};
+ return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+ VectorXd::Zero(sb_ptr->getSize()),
+ MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+ sb_ptr,
+ nullptr,
+ false);
+}
TEST(SolverCeres, Create)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // check double ointers to branches
- ASSERT_EQ(P, solver_ceres->getProblem());
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverCeres, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleAddStateBlock)
+TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // add stateblock again
+ // notify stateblock again
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, UpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddUpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
// Fix state block
sb_ptr->fix();
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock localparam
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock localparam
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(solver_ceres->numStateBlocks(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddRemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check no stateblocks
- ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(solver_ceres->numStateBlocks(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveUpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
- // update solver
- solver_ceres->update();
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleRemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // update solver
+ solver_ptr->update();
+
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddFactor)
+TEST(SolverCeres, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Fix
+ sb_ptr->fix();
+
+ // Set State
+ sb_ptr->setState(2*sb_ptr->getState());
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame( 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 1);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverCeres, DoubleAddFactor)
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverCeres, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add factor again
- P->notifyFactor(c,ADD);
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 1);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddRemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleRemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // update solver
- solver_ceres->update();
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // update solver
- solver_ceres->update();
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // update solver
- solver_ceres->update();
+ // Fix state block
+ sb_ptr->fix();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddStateBlockLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
- // update solver
- solver_ceres->update();
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // check stateblock
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
// Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
- // update solver
- solver_ceres->update();
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Remove local param
sb_ptr->removeLocalParametrization();
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FactorsRemoveLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 3);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // remove local param
- F->getO()->removeLocalParametrization();
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // check local param
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(F->getO()));
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD is posponed
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FactorsUpdateLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
{
ProblemPtr P = Problem::create("PO", 3);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
- // remove local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getO()->setLocalParametrization(local_param_ptr);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Fix
+ sb_ptr->fix();
+
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),local_param_ptr));
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverCeres, OnlyFactor_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, OnlyFactor_AddRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // update solver
+ solver_ptr->update(); // nothing to update
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
int main(int argc, char **argv)
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91fa716cb45d02c14bf9dad6068c4d6f4bae6780
--- /dev/null
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -0,0 +1,81 @@
+/*
+ * gtest_solver_ptr.cpp
+ *
+ * Created on: Jun, 2018
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_angle.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+
+#include "core/solver/solver_manager.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/local_parametrization_wrapper.h"
+
+#include "ceres/ceres.h"
+
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_manager_multithread.cpp
+ * (modifications should be applied to both tests)
+ */
+
+TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceKeyFrame(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);
+
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 30bf42ca390c4488c5227be99a2c59a5a1cb98c3..2eb3f46e927270a7e61abb03734d01898722f471 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,8 +13,8 @@
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
-#include "core/state_block/local_parametrization_base.h"
-#include "core/state_block/local_parametrization_angle.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
#include <iostream>
@@ -23,571 +23,1490 @@
using namespace wolf;
using namespace Eigen;
+/*
+ * Following tests are the same as in gtest_solver_ceres.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+ return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+ VectorXd::Zero(sb_ptr->getSize()),
+ MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+ sb_ptr,
+ nullptr,
+ false);
+}
+
TEST(SolverManager, Create)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
- // check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblem());
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverManager, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleAddStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- // add stateblock again
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, UpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Remove local param
sb_ptr->removeLocalParametrization();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancel out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check state block
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add state_block
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // update solver
+ solver_ptr->update();
+
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdatedStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Fix
sb_ptr->fix();
// Set State
- Vector2d state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Check flags have been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
// == When an ADD is notified: a ADD notification should be stored ==
- // add state_block
+ // notify state block
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ // check notifications
Notification notif;
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
// Set State --> FLAG
- state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Fix --> FLAG
sb_ptr->unfix();
// Check flag has been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// Check flags have been reset
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
P->notifyStateBlock(sb_ptr,ADD);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// == UPDATES should clear the list of notifications ==
- // add state_block
+ // notify state block
P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
P->notifyStateBlock(sb_ptr,ADD);
P->notifyStateBlock(sb_ptr,REMOVE);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverManager, StateBlockAndFactor_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_Fix)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddFactor)
+TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // check factor
- ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // factor ADD is posponed
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // add factor
- P->notifyFactor(c,REMOVE);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, REMOVE);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Fix
+ sb_ptr->fix();
+
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
- // add factor
- P->notifyFactor(c,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
- // notification
- ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
- ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverManager, DoubleRemoveFactor)
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverManager, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero() );
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, MultiThreadingTruncatedNotifications)
+TEST(SolverManager, OnlyFactor_AddRemove)
{
- double Dt = 5.0;
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
-
- // loop updating (without sleep)
- std::thread t([&](){
- auto start_t = std::chrono::high_resolution_clock::now();
- while (true)
- {
- solver_manager_ptr->update();
- if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
- break;
- }});
-
- // loop emplacing and removing frames (window of 10 KF)
- auto start = std::chrono::high_resolution_clock::now();
- TimeStamp ts(0);
- while (true)
- {
- // Emplace Frame, Capture, feature and factor pose 2d
- FrameBasePtr F = P->emplaceKeyFrame(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);
-
- ts += 1.0;
-
- if (P->getTrajectory()->getFrameMap().size() > 10)
- (*P->getTrajectory()->begin())->remove();
-
- if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
- break;
- }
-
- t.join();
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
int main(int argc, char **argv)
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0770b33aed43e5ebbc9a204e68428f7a0022c870
--- /dev/null
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -0,0 +1,75 @@
+/*
+ * gtest_solver_manager.cpp
+ *
+ * Created on: Jun, 2018
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+
+#include "core/problem/problem.h"
+#include "core/sensor/sensor_base.h"
+#include "core/state_block/state_block.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
+#include "dummy/solver_manager_dummy.h"
+
+#include <iostream>
+#include <thread>
+
+using namespace wolf;
+using namespace Eigen;
+
+/*
+ * Following tests are the same as in gtest_solver_ceres_multithread.cpp
+ * (modifications should be applied to both tests)
+ */
+
+TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceKeyFrame(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);
+
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index b8ff8240e7e2c7e97fb4f56aeadb72d2b51a0027..0c5e7c2aec97277425289f3f28a9ba3d61dece1c 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -24,7 +24,7 @@ TEST(TreeManagerSlidingWindow, make_shared)
P->setTreeManager(GM);
- ASSERT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), GM);
}
TEST(TreeManagerSlidingWindow, createParams)
@@ -35,12 +35,12 @@ TEST(TreeManagerSlidingWindow, createParams)
auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
P->setTreeManager(GM);
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- ASSERT_EQ(P->getTreeManager(), GM);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
}
TEST(TreeManagerSlidingWindow, createParamServer)
@@ -52,12 +52,12 @@ TEST(TreeManagerSlidingWindow, createParamServer)
auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
P->setTreeManager(GM);
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- ASSERT_EQ(P->getTreeManager(), GM);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
}
TEST(TreeManagerSlidingWindow, autoConf)
@@ -67,7 +67,7 @@ TEST(TreeManagerSlidingWindow, autoConf)
ProblemPtr P = Problem::autoSetup(server);
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
}
TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
@@ -80,7 +80,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 1 ----------------------------------------------------------
auto F1 = P->getTrajectory()->getLastFrame();
- ASSERT_TRUE(F1 != nullptr);
+ EXPECT_TRUE(F1 != nullptr);
Vector7d state = F1->getStateVector();
Vector7d zero_disp(state);
@@ -100,7 +100,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
// Check no frame removed
- ASSERT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
@@ -116,7 +116,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
// Check no frame removed
- ASSERT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
@@ -132,11 +132,11 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
// Checks
- ASSERT_TRUE(F1->isRemoving());
- ASSERT_TRUE(c12->isRemoving());
- ASSERT_TRUE(C12->isRemoving()); //Virally removed
- ASSERT_TRUE(f12->isRemoving()); //Virally removed
- ASSERT_TRUE(F2->isFixed()); //Fixed
+ 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
// FRAME 5 ----------------------------------------------------------
auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
@@ -152,18 +152,19 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
// Checks
- ASSERT_TRUE(F1->isRemoving());
- ASSERT_TRUE(c12->isRemoving());
- ASSERT_TRUE(C12->isRemoving()); //Virally removed
- ASSERT_TRUE(f12->isRemoving()); //Virally removed
- ASSERT_TRUE(F2->isRemoving());
- ASSERT_TRUE(c2->isRemoving());
- ASSERT_TRUE(C2->isRemoving()); //Virally removed
- ASSERT_TRUE(f2->isRemoving()); //Virally removed
- ASSERT_TRUE(c23->isRemoving());
- ASSERT_TRUE(C23->isRemoving()); //Virally removed
- ASSERT_TRUE(f23->isRemoving()); //Virally removed
- ASSERT_TRUE(F3->isFixed()); //Fixed
+ 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->isRemoving());
+ EXPECT_TRUE(c2->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C2->isRemoving()); // Virally removed
+ EXPECT_TRUE(f2->isRemoving()); // Virally removed
+ 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
}
TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
@@ -176,7 +177,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 1 (prior) ----------------------------------------------------------
auto F1 = P->getTrajectory()->getLastFrame();
- ASSERT_TRUE(F1 != nullptr);
+ EXPECT_TRUE(F1 != nullptr);
Vector7d state = F1->getStateVector();
Vector7d zero_disp(state);
@@ -196,7 +197,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
// Check no frame removed
- ASSERT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
@@ -212,7 +213,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
// Check no frame removed
- ASSERT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
@@ -228,11 +229,11 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
// Checks
- ASSERT_TRUE(F1->isRemoving());
- ASSERT_TRUE(c12->isRemoving());
- ASSERT_FALSE(C12->isRemoving()); //Not virally removed
- ASSERT_FALSE(f12->isRemoving()); //Not virally removed
- ASSERT_FALSE(F2->isFixed()); //Not fixed
+ EXPECT_TRUE(F1->isRemoving());
+ 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
// FRAME 5 ----------------------------------------------------------
auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
@@ -248,18 +249,18 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
// Checks
- ASSERT_TRUE(F1->isRemoving());
- ASSERT_TRUE(c12->isRemoving());
- ASSERT_TRUE(C12->isRemoving());
- ASSERT_TRUE(f12->isRemoving());
- ASSERT_TRUE(F2->isRemoving());
- ASSERT_TRUE(c2->isRemoving());
- ASSERT_TRUE(C2->isRemoving());
- ASSERT_TRUE(f2->isRemoving());
- ASSERT_TRUE(c23->isRemoving());
- ASSERT_FALSE(C23->isRemoving()); //Not virally removed
- ASSERT_FALSE(f23->isRemoving()); //Not virally removed
- ASSERT_FALSE(F3->isFixed()); //Not fixed
+ EXPECT_TRUE(F1->isRemoving());
+ EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ 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
}
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
new file mode 100644
index 0000000000000000000000000000000000000000..ae4112bc3c27d88e103694ffd0824f4b7b35de2d
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -0,0 +1,1066 @@
+#include "core/utils/utils_gtest.h"
+
+
+#include "core/problem/problem.h"
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/capture/capture_void.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_odom_3d.h"
+#include "core/factor/factor_pose_3d.h"
+#include "core/solver/factory_solver.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManagerSlidingWindowDualRate, make_shared)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+ auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+ auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", ParamsGM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, autoConf)
+{
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
+{
+ /* sliding window dual rate:
+ * n_frames: 5
+ * n_frames_recent: 3
+ * rate_old_frames: 2
+ */
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ /* FRAME 1 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )( )(F1)
+ */
+ auto F1 = P->getTrajectory()->getLastFrame();
+ ASSERT_TRUE(F1 != nullptr);
+ ASSERT_FALSE(F1->getCaptureList().empty());
+ auto C1 = F1->getCaptureList().front();
+ ASSERT_FALSE(C1->getFeatureList().empty());
+ auto f1 = C1->getFeatureList().front();
+ ASSERT_FALSE(f1->getFactorList().empty());
+ auto c1 = f1->getFactorList().front();
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+
+ /* FRAME 2 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )(F1)(F2)
+ */
+ auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+
+ /* FRAME 3 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) (F1)(F2)(F3)
+ */
+ auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+
+ /* FRAME 4 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1)(F2)(F3)(F4)
+ */
+ auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+
+ /* FRAME 5 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1) (F3)(F4)(F5)
+ */
+ auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+
+ /* FRAME 6 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3)(F4)(F5)(F6)
+ */
+ auto F6 = P->emplaceKeyFrame(TimeStamp(6), "PO", 3, state);
+ P->keyFrameCallback(F6, nullptr, 0);
+
+ // absolute factor
+ auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov);
+ auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false);
+ // displacement
+ auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
+ auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+
+ /* FRAME 7 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3) (F5)(F6)(F7)
+ */
+ auto F7 = P->emplaceKeyFrame(TimeStamp(7), "PO", 3, state);
+ P->keyFrameCallback(F7, nullptr, 0);
+
+ // absolute factor
+ auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov);
+ auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false);
+ // displacement
+ auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
+ auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C45->isRemoving()); // Virally removed
+ EXPECT_TRUE(f45->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+
+ /* FRAME 8 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F3) (F5)(F6)(F7)(F8)
+ */
+ auto F8 = P->emplaceKeyFrame(TimeStamp(8), "PO", 3, state);
+ P->keyFrameCallback(F8, nullptr, 0);
+
+ // absolute factor
+ auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov);
+ auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false);
+ // displacement
+ auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
+ auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false);
+
+ // Checks
+ EXPECT_TRUE(F1->isRemoving()); // First frame removed
+ EXPECT_TRUE(c1->isRemoving());
+ EXPECT_TRUE(C1->isRemoving());
+ EXPECT_TRUE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C23->isRemoving()); // Virally removed
+ EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
+ EXPECT_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_TRUE(C45->isRemoving()); // Virally removed
+ EXPECT_TRUE(f45->isRemoving()); // Virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F8->isRemoving());
+ EXPECT_FALSE(c8->isRemoving());
+ EXPECT_FALSE(C8->isRemoving());
+ EXPECT_FALSE(f8->isRemoving());
+ EXPECT_FALSE(c78->isRemoving());
+ EXPECT_FALSE(C78->isRemoving());
+ EXPECT_FALSE(f78->isRemoving());
+
+ EXPECT_TRUE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+ EXPECT_FALSE(F8->isFixed());
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
+{
+ /* sliding window dual rate:
+ * n_frames: 5
+ * n_frames_recent: 3
+ * rate_old_frames: 2
+ */
+
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ /* FRAME 1 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )( )(F1)
+ */
+ auto F1 = P->getTrajectory()->getLastFrame();
+ ASSERT_TRUE(F1 != nullptr);
+ ASSERT_FALSE(F1->getCaptureList().empty());
+ auto C1 = F1->getCaptureList().front();
+ ASSERT_FALSE(C1->getFeatureList().empty());
+ auto f1 = C1->getFeatureList().front();
+ ASSERT_FALSE(f1->getFactorList().empty());
+ auto c1 = f1->getFactorList().front();
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+
+ /* FRAME 2 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) ( )(F1)(F2)
+ */
+ auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+
+ /* FRAME 3 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) ( ) (F1)(F2)(F3)
+ */
+ auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+
+ // Check no frame removed
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+
+ /* FRAME 4 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1)(F2)(F3)(F4)
+ */
+ auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_FALSE(F2->isRemoving());
+ EXPECT_FALSE(c2->isRemoving());
+ EXPECT_FALSE(C2->isRemoving());
+ EXPECT_FALSE(f2->isRemoving());
+ EXPECT_FALSE(c12->isRemoving());
+ EXPECT_FALSE(C12->isRemoving());
+ EXPECT_FALSE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ EXPECT_FALSE(c23->isRemoving());
+ EXPECT_FALSE(C23->isRemoving());
+ EXPECT_FALSE(f23->isRemoving());
+
+ EXPECT_FALSE(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F2->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+
+ /* FRAME 5 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * ( ) (F1) (F3)(F4)(F5)
+ */
+ auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ 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(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+
+ /* FRAME 6 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3)(F4)(F5)(F6)
+ */
+ auto F6 = P->emplaceKeyFrame(TimeStamp(6), "PO", 3, state);
+ P->keyFrameCallback(F6, nullptr, 0);
+
+ // absolute factor
+ auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov);
+ auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false);
+ // displacement
+ auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
+ auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
+ auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ 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(F4->isRemoving());
+ EXPECT_FALSE(c4->isRemoving());
+ EXPECT_FALSE(C4->isRemoving());
+ EXPECT_FALSE(f4->isRemoving());
+ EXPECT_FALSE(c34->isRemoving());
+ EXPECT_FALSE(C34->isRemoving());
+ EXPECT_FALSE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_FALSE(c45->isRemoving());
+ EXPECT_FALSE(C45->isRemoving());
+ EXPECT_FALSE(f45->isRemoving());
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F4->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+
+ /* FRAME 7 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F1) (F3) (F5)(F6)(F7)
+ */
+ auto F7 = P->emplaceKeyFrame(TimeStamp(7), "PO", 3, state);
+ P->keyFrameCallback(F7, nullptr, 0);
+
+ // absolute factor
+ auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov);
+ auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false);
+ // displacement
+ auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
+ auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
+ auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false);
+
+ // Checks
+ EXPECT_FALSE(F1->isRemoving());
+ EXPECT_FALSE(c1->isRemoving());
+ EXPECT_FALSE(C1->isRemoving());
+ EXPECT_FALSE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ 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_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C45->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f45->isRemoving()); // Not virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F1->isFixed());
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+
+ /* FRAME 8 ----------------------------------------------------------
+ *
+ * Sliding window:
+ * (F3) (F5)(F6)(F7)(F8)
+ */
+ auto F8 = P->emplaceKeyFrame(TimeStamp(8), "PO", 3, state);
+ P->keyFrameCallback(F8, nullptr, 0);
+
+ // absolute factor
+ auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov);
+ auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false);
+ // displacement
+ auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
+ auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
+ auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false);
+
+ // Checks
+ EXPECT_TRUE(F1->isRemoving()); // First frame removed
+ EXPECT_TRUE(c1->isRemoving());
+ EXPECT_TRUE(C1->isRemoving());
+ EXPECT_TRUE(f1->isRemoving());
+
+ EXPECT_TRUE(F2->isRemoving());
+ EXPECT_TRUE(c2->isRemoving());
+ EXPECT_TRUE(C2->isRemoving());
+ EXPECT_TRUE(f2->isRemoving());
+ EXPECT_TRUE(c12->isRemoving());
+ EXPECT_TRUE(C12->isRemoving());
+ EXPECT_TRUE(f12->isRemoving());
+
+ EXPECT_FALSE(F3->isRemoving());
+ EXPECT_FALSE(c3->isRemoving());
+ EXPECT_FALSE(C3->isRemoving());
+ EXPECT_FALSE(f3->isRemoving());
+ 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_TRUE(F4->isRemoving());
+ EXPECT_TRUE(c4->isRemoving());
+ EXPECT_TRUE(C4->isRemoving());
+ EXPECT_TRUE(f4->isRemoving());
+ EXPECT_TRUE(c34->isRemoving());
+ EXPECT_TRUE(C34->isRemoving());
+ EXPECT_TRUE(f34->isRemoving());
+
+ EXPECT_FALSE(F5->isRemoving());
+ EXPECT_FALSE(c5->isRemoving());
+ EXPECT_FALSE(C5->isRemoving());
+ EXPECT_FALSE(f5->isRemoving());
+ EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame
+ EXPECT_FALSE(C45->isRemoving()); // Not virally removed
+ EXPECT_FALSE(f45->isRemoving()); // Not virally removed
+
+ EXPECT_FALSE(F6->isRemoving());
+ EXPECT_FALSE(c6->isRemoving());
+ EXPECT_FALSE(C6->isRemoving());
+ EXPECT_FALSE(f6->isRemoving());
+ EXPECT_FALSE(c56->isRemoving());
+ EXPECT_FALSE(C56->isRemoving());
+ EXPECT_FALSE(f56->isRemoving());
+
+ EXPECT_FALSE(F7->isRemoving());
+ EXPECT_FALSE(c7->isRemoving());
+ EXPECT_FALSE(C7->isRemoving());
+ EXPECT_FALSE(f7->isRemoving());
+ EXPECT_FALSE(c67->isRemoving());
+ EXPECT_FALSE(C67->isRemoving());
+ EXPECT_FALSE(f67->isRemoving());
+
+ EXPECT_FALSE(F8->isRemoving());
+ EXPECT_FALSE(c8->isRemoving());
+ EXPECT_FALSE(C8->isRemoving());
+ EXPECT_FALSE(f8->isRemoving());
+ EXPECT_FALSE(c78->isRemoving());
+ EXPECT_FALSE(C78->isRemoving());
+ EXPECT_FALSE(f78->isRemoving());
+
+ EXPECT_FALSE(F3->isFixed());
+ EXPECT_FALSE(F5->isFixed());
+ EXPECT_FALSE(F6->isFixed());
+ EXPECT_FALSE(F7->isFixed());
+ EXPECT_FALSE(F8->isFixed());
+}
+
+TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
+{
+ // SLIDING WINDOW DUAL RATE
+ ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+ ProblemPtr problem = Problem::autoSetup(server);
+ SolverManagerPtr solver = FactorySolver::create("SolverCeres", problem, server);
+
+ // BASELINE
+ ParserYaml parser_bl = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml", wolf_root);
+ ParamsServer server_bl = ParamsServer(parser_bl.getParams());
+ ProblemPtr problem_bl = Problem::autoSetup(server_bl);
+ SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server);
+
+ // aux variables
+ Vector7d data;
+ Matrix6d data_cov = Matrix6d::Identity();
+ TimeStamp t(0.0);
+ double dt = 1;
+ CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t,
+ problem->getSensor("odom"),
+ data,
+ data_cov);
+ CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t,
+ problem_bl->getSensor("odom"),
+ data,
+ data_cov);
+
+ // START MOTION CAPTURES
+ for (int i = 0; i<20; i++)
+ {
+ t += dt;
+ WOLF_INFO("-------------------------- t: ", t);
+
+ // random movement
+ data.setRandom(); data.tail<4>().normalize();
+
+ // sliding window process
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->process();
+
+ // baseline process
+ capture_bl->setTimeStamp(t);
+ capture_bl->setData(data);
+ capture_bl->process();
+
+ WOLF_INFO("state: ", problem->getState().vector("PO").transpose());
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+ problem_bl->getState().vector("PO").transpose(),
+ 1e-12);
+
+ // Solve
+ solver->solve();
+ solver_bl->solve();
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+ problem_bl->getState().vector("PO").transpose(),
+ 1e-12);
+
+ ASSERT_TRUE(problem->check());
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 96ad94f72af7e8de1b9200a8e5ea44e3bc952a85..22242febd640b616bebe4ab0410867b0e373a5be 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -4,8 +4,6 @@ config:
dimension: 3
prior:
mode: "factor"
- # state: [0,0,0,0,0,0,1]
- # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
$state:
P: [0,0,0]
O: [0,0,0,1]
@@ -15,8 +13,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
- n_key_frames: 3
- fix_first_key_frame: true
+ n_frames: 3
+ fix_first_frame: true
viral_remove_empty_parent: true
sensors:
-
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index 9ae33a2af5ab0695bd44948d33d2f9aa104cbc74..add5eff760050fecbde63c827bb75e77c531f86e 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -4,8 +4,6 @@ config:
dimension: 3
prior:
mode: "factor"
- # state: [0,0,0,0,0,0,1]
- # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
$state:
P: [0,0,0]
O: [0,0,0,1]
@@ -15,8 +13,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
- n_key_frames: 3
- fix_first_key_frame: false
+ n_frames: 3
+ fix_first_frame: false
viral_remove_empty_parent: false
sensors:
-
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..32ef665e0a6d92b94e9342d22c5e4ca9952613f1
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -0,0 +1,20 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindowDualRate"
+ n_frames: 5
+ n_frames_recent: 3
+ rate_old_frames: 2
+ fix_first_frame: true
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..6b4795fb3ad70c77b02c9cffc74b964abfe64141
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -0,0 +1,20 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindowDualRate"
+ n_frames: 5
+ n_frames_recent: 3
+ rate_old_frames: 2
+ fix_first_frame: false
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..b59022518aaf931890368185fb9b489611cda257
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -0,0 +1,55 @@
+config:
+ solver:
+ period: 1
+ verbose: 2
+ update_immediately: false
+ max_num_iterations: 10
+ n_threads: 2
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindowDualRate"
+ n_frames: 5
+ n_frames_recent: 3
+ rate_old_frames: 2
+ fix_first_frame: true
+ viral_remove_empty_parent: true
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 10 # meters
+ angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
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
new file mode 100644
index 0000000000000000000000000000000000000000..eba780939b0e4521ba7a5d571ed47d63749adc6e
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -0,0 +1,49 @@
+config:
+ solver:
+ period: 1
+ verbose: 2
+ update_immediately: false
+ max_num_iterations: 10
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 10 # meters
+ angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111