diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 787f27623a37d9510cea7293d1efb98ce1afb475..fc389145b8db006e5907a71453cd66b56a78ecd8 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -131,6 +131,7 @@ int main()
params_sen_odo["states"]["O"]["dynamic"] = false;
params_sen_odo["name"] = "Sensor Odometry";
+ params_sen_odo["enabled"] = true;
params_sen_odo["k_disp_to_disp"] = 0.1;
params_sen_odo["k_disp_to_rot"] = 0.1;
params_sen_odo["k_rot_to_rot"] = 0.1;
@@ -168,6 +169,7 @@ int main()
params_sen_rb["states"]["O"]["dynamic"] = false;
params_sen_rb["name"] = "Sensor Range Bearing";
+ params_sen_rb["enabled"] = true;
params_sen_rb["noise_range_metres_std"] = 0.1;
params_sen_rb["noise_bearing_degrees_std"] = 1.0;
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index b41f0391a7de60679d917ab85af5711ae594a61a..8005adfbb620574aeb9c43df2aba98d5f4e84cc1 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -26,7 +26,7 @@
namespace wolf
{
ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params)
- : ProcessorBase("ProcessorRangeBearing", 2, _params)
+ : ProcessorBase("ProcessorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
{
//
}
@@ -128,14 +128,13 @@ ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2
Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
{
- Trf H_w_r = transform(getProblem()->getState().vector("PO"));
+ Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
return H_w_r * H_r_s * lmk_s;
}
Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{
- // Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
- Trf H_w_r = transform(getProblem()->getState().vector("PO"));
+ Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
return (H_w_r * H_r_s).inverse() * lmk_w;
}
diff --git a/include/core/common/node_state_blocks.h b/include/core/common/node_state_blocks.h
index 418abd564337d693abfb72ed262f3f977731709b..9dc0f134afb61c8eef96b522c291f68b00f4027a 100644
--- a/include/core/common/node_state_blocks.h
+++ b/include/core/common/node_state_blocks.h
@@ -84,13 +84,22 @@ class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<Nod
*
* \param _key Key of the new state.
* \param _type Types of the new state.
- * \param _vectors Vector of the new state.
+ * \param _vector Vector of the new state.
* \param _fixed If the new state is fixed (i.e. not estimated).
**/
StateBlockPtr emplaceStateBlock(const char key,
- const std::string& _type,
- const Eigen::VectorXd& _vector,
- const bool& _fixed);
+ const std::string& _type,
+ const Eigen::VectorXd& _vector,
+ const bool& _fixed);
+ /** \brief Emplace a state block with zero state
+ *
+ * \param _key Key of the new state.
+ * \param _type Types of the new state.
+ * \param _fixed If the new state is fixed (i.e. not estimated).
+ **/
+ StateBlockPtr emplaceStateBlockZero(const char key,
+ const std::string& _type,
+ const bool& _fixed);
protected:
virtual void removeStateBlock(const char& _sb_key);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 91cc837f3e66fd57136a47f09bd12938d81373e5..73eab66545543855df753984a5fb5fb55d3607d6 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -73,7 +73,6 @@ class Problem : public std::enable_shared_from_this<Problem>
std::map<unsigned int, MotionProviderPtr> motion_provider_map_;
SizeEigen dim_;
- StateKeys frame_keys_;
TypeComposite frame_types_;
VectorComposite first_frame_values_;
PriorComposite first_frame_priors_;
@@ -94,14 +93,14 @@ class Problem : public std::enable_shared_from_this<Problem>
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(SizeEigen _dim,
- const TypeComposite& _frame_structure = {},
- LoaderPtr _loader = nullptr); // USE create() below !!
+ const TypeComposite& _frame_types = {},
+ LoaderPtr _loader = nullptr); // USE create() below !!
void setup();
public:
static ProblemPtr create(SizeEigen _dim,
- const TypeComposite& _frame_structure = {},
- LoaderPtr _loader = nullptr); // USE THIS AS A CONSTRUCTOR!
+ const TypeComposite& _frame_types = {},
+ LoaderPtr _loader = nullptr); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(const std::string& _input_yaml_file,
std::vector<std::string> _primary_schema_folders = {},
LoaderPtr _loader = nullptr,
@@ -115,13 +114,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// Properties -----------------------------------------
LoaderPtr getLoader();
SizeEigen getDim() const;
- StateKeys getFrameKeys() const;
- bool hasFrameKeys(StateKeys _keys) const;
TypeComposite getFrameTypes(StateKeys _keys) const;
protected:
- void addFrameKeys(StateKeys _keys);
- void addFrameTypes(const TypeComposite& _structure);
+ void addFrameTypes(const TypeComposite& _types);
// Tree manager --------------------------------------
public:
@@ -245,11 +241,11 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys = "");
+ FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys);
/** \brief Emplace frame from timestamps and state
* \param _time_stamp Time stamp of the frame
- * \param _frame_state State; must be part of the problem's frame structure
+ * \param _frame_state State; must be part of the problem's frame state_keys
*
* - The frame_types are taken from Problem
*
@@ -263,7 +259,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Emplace frame from timestamp and state vector
* \param _time_stamp Time stamp of the frame
* \param _frame_keys String containing the keys of the desired frame. Empty means all keys available.
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ * \param _frame_state State vector; must match the size and format of the chosen frame state_keys
*
* - The frame_types are taken from Problem
*
@@ -276,6 +272,17 @@ class Problem : public std::enable_shared_from_this<Problem>
const StateKeys& _frame_keys,
const Eigen::VectorXd& _frame_state);
+ /** \brief Take a frame and emplace to it the missing states with the frame timestamp
+ * \param _frame_ptr Frame to which states have to be emplaced
+ * \param _frame_keys the keys that the frame is requested to have.
+ *
+ * - The frame_types are taken from Problem.
+ * - Only the missing keys are emplaced.
+ * - The states are taken from Problem::getState(_frame_ptr->getTimeStamp(), _frame_keys)
+ * - The states are emplaced to the frame as unfixed.
+ */
+ void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const StateKeys& _frame_keys);
+
// Frame getters
FrameBaseConstPtr getLastFrame() const;
FrameBasePtr getLastFrame();
@@ -300,13 +307,13 @@ class Problem : public std::enable_shared_from_this<Problem>
// State getters
TimeStamp getTimeStamp() const;
- VectorComposite getState(const StateKeys& _structure = "") const;
- VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const;
- VectorComposite getOdometry(const StateKeys& _structure = "") const;
+ VectorComposite getState(const StateKeys& _keys) const;
+ VectorComposite getState(const TimeStamp& _ts, const StateKeys& _keys) const;
+ VectorComposite getOdometry(const StateKeys& _keys) const;
// Zero state provider
- VectorComposite stateZero(TypeComposite _types = {}) const;
- VectorComposite stateZero(const StateKeys& _structure) const;
+ VectorComposite stateZero(TypeComposite _types) const;
+ VectorComposite stateZero(const StateKeys& _keys) const;
// Map branch -----------------------------------------
MapBaseConstPtr getMap() const;
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index b9c5d7b4e52ed923a398e5bd06af718bde6740c8..70175fb70179e5d30dc2d8bd6c1433e3aa1b33b9 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -36,9 +36,9 @@ class MotionProvider
virtual ~MotionProvider();
// Queries to the processor:
- virtual TimeStamp getTimeStamp() const = 0;
- virtual VectorComposite getState(StateKeys _structure = "") const = 0;
- virtual VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const = 0;
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState(StateKeys _keys = "") const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts, StateKeys _keys = "") const = 0;
VectorComposite getOdometry() const;
void setOdometry(const VectorComposite&);
@@ -47,21 +47,6 @@ class MotionProvider
unsigned int getOrder() const;
void setOrder(unsigned int);
- public:
- TypeComposite getStateTypes() const
- {
- return state_types_;
- };
- StateKeys getStateKeys() const
- {
- return state_types_.getKeys();
- };
- void setStateTypes(const TypeComposite& _state_structure)
- {
- state_types_ = _state_structure;
- };
- void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
-
protected:
TypeComposite state_types_;
bool is_state_provider_;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b4d22b4035c5a69a1f1927083b571c2196ef771b..73bf60933c51fc1c12ffbc35a9cf63b9252c3a0f 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -119,7 +119,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
YAML::Node params_;
BufferFrame buffer_frame_;
BufferCapture buffer_capture_;
- unsigned int dim_compatible_; ///< Dimension compatibility of the processor: 2: 2D, 3: 3D, 0: both
+ TypeComposite state_types_; ///< State type composite of frames created or used by this processor
private:
SensorBaseWPtr sensor_ptr_;
@@ -142,15 +142,17 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
/** \brief constructor
*
* \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: type name
- * \param _dim_compatible TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR:
- * Which dimension is the processor compatible (2: 2D, 3: 3D, 0: both)
+ * \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR:
+ * The composite of state types of the frames created or used by this processor
* \param _params params yaml node
*/
- ProcessorBase(const std::string& _type, unsigned int _dim_compatible, const YAML::Node& _params);
+ ProcessorBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params);
~ProcessorBase() override;
- virtual void configure(SensorBasePtr _sensor) = 0;
- virtual void remove();
- bool hasChildren() const override;
+ virtual void configure(SensorBasePtr _sensor) = 0;
+ virtual void remove();
+ bool hasChildren() const override;
+ const TypeComposite& getStateTypes() const;
+ StateKeys getStateKeys() const;
unsigned int id() const;
@@ -232,7 +234,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
public:
YAML::Node getParams() const;
bool isMotionProvider() const;
- unsigned int getDimCompatible() const;
bool applyLossFunction() const;
bool isVotingActive() const;
double getTimeTolerance() const;
@@ -331,9 +332,14 @@ inline SensorBasePtr ProcessorBase::getSensor()
return sensor_ptr_.lock();
}
-inline unsigned int ProcessorBase::getDimCompatible() const
+inline StateKeys ProcessorBase::getStateKeys() const
{
- return dim_compatible_;
+ return state_types_.getKeys();
+}
+
+inline const TypeComposite& ProcessorBase::getStateTypes() const
+{
+ return state_types_;
}
inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index 89439337b62230545b9addf883c16cb1632aa736..969502b2c4c28f3cf2225c61f6a7a071b669caa1 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -38,7 +38,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal);
- void configure(SensorBasePtr _sensor) override{};
+ void configure(SensorBasePtr _sensor) override {};
protected:
TrackMatrix track_matrix_;
@@ -103,7 +103,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
*
* It does nothing for now
*/
- void postProcess() override{};
+ void postProcess() override {};
void advanceDerived() override;
void resetDerived() override;
@@ -120,6 +120,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
const VectorComposite& _pose_sen) const;
// PROCESSOR PARAMETERS
+ int dim_; ///< dimension of the problem (2D or 3D)
bool use_orientation_; ///< use orientation measure or not when emplacing factors
unsigned int new_features_for_keyframe_; ///< for keyframe voting: amount of new features with respect to origin
///< (sufficient condition if more than min_features_for_keyframe)
@@ -140,7 +141,11 @@ class ProcessorLandmarkExternal : public ProcessorTracker
};
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(const YAML::Node& _params)
- : ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params),
+ : ProcessorTracker("ProcessorLandmarkExternal",
+ {{'P', _params["dimension"].as<int>() == 2 ? "StatePoint2d" : "StatePoint3d"},
+ {'O', _params["dimension"].as<int>() == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params),
+ dim_(_params["dimension"].as<int>()),
use_orientation_(_params["use_orientation"].as<bool>()),
new_features_for_keyframe_(_params["keyframe_vote"]["new_features_for_keyframe"].as<unsigned int>()),
time_span_(_params["keyframe_vote"]["time_span"].as<double>()),
@@ -150,6 +155,6 @@ inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(const YAML::Node& _p
match_dist_th_unknown_(_params["match_dist_th_unknown"].as<double>()),
track_length_th_(_params["track_length_th"].as<unsigned int>()),
close_loops_by_id_(_params["close_loops_by_id"].as<bool>()),
- close_loops_by_type_(_params["close_loops_by_type"].as<bool>()){};
+ close_loops_by_type_(_params["close_loops_by_type"].as<bool>()) {};
} // namespace wolf
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index a04ed6f0204d348371272d84b67e9c4a023c2426..b923ea90db0ad58a7234772f2c0566ca63198f05 100644
--- a/include/core/processor/processor_loop_closure.h
+++ b/include/core/processor/processor_loop_closure.h
@@ -52,7 +52,7 @@ struct MatchLoopClosure
class ProcessorLoopClosure : public ProcessorBase
{
public:
- ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params);
+ ProcessorLoopClosure(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params);
~ProcessorLoopClosure() override = default;
void configure(SensorBasePtr _sensor) override{};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 62c75554606f0db12d8d9558c46efc87ffa43b36..4d8dcee2844eafebb27865d38329c6c6eb263c7e 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -129,7 +129,6 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
public:
ProcessorMotion(const std::string& _type,
TypeComposite _state_types,
- int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
index bfd7582b1d7544780cc41eef1805870878bcbbc2..22396e638119baad77fb36715472f1d380c4d2fb 100644
--- a/include/core/processor/processor_pose.h
+++ b/include/core/processor/processor_pose.h
@@ -78,7 +78,11 @@ WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d);
namespace wolf
{
template <unsigned int DIM>
-inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params) : ProcessorBase("ProcessorPose", DIM, _params)
+inline ProcessorPose<DIM>::ProcessorPose(const YAML::Node& _params)
+ : ProcessorBase(
+ "ProcessorPose",
+ {{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params)
{
static_assert(DIM == 2 or DIM == 3);
}
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 54eade13bf31e4dad72d1d3ab88748fbc670e2bd..147f3caccd8133a5f53166d4064c7d9b24c8df9c 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -88,7 +88,6 @@ class ProcessorTracker : public ProcessorBase
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- FrameBasePtr last_frame_ptr_;
FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new
///< key-frame creation.
@@ -98,7 +97,6 @@ class ProcessorTracker : public ProcessorBase
known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
FeatureBasePtrList
known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
- StateKeys state_keys_; ///< Keys of frames created or used by this processor
// PARAMS
unsigned int min_features_for_keyframe_; ///< minimum nbr. of features to vote for keyframe
@@ -110,17 +108,13 @@ class ProcessorTracker : public ProcessorBase
establish_factors_profiling_, postprocess_profiling_;
public:
- ProcessorTracker(const std::string& _type, const StateKeys& _structure, int _dim, const YAML::Node& _params);
+ ProcessorTracker(const std::string& _type, const TypeComposite& state_types_, const YAML::Node& _params);
~ProcessorTracker() override;
- const StateKeys& getStateKeys() const;
-
virtual CaptureBaseConstPtr getOrigin() const;
virtual CaptureBasePtr getOrigin();
virtual CaptureBaseConstPtr getLast() const;
virtual CaptureBasePtr getLast();
- virtual FrameBaseConstPtr getLastFrame() const;
- virtual FrameBasePtr getLastFrame();
virtual CaptureBaseConstPtr getIncoming() const;
virtual CaptureBasePtr getIncoming();
@@ -187,7 +181,7 @@ class ProcessorTracker : public ProcessorBase
* - initializing counters, flags, or any derived variables
* - initializing algorithms needed for processing the derived data
*/
- virtual void preProcess(){};
+ virtual void preProcess() {};
/** Post-process
*
@@ -199,7 +193,7 @@ class ProcessorTracker : public ProcessorBase
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
- virtual void postProcess(){};
+ virtual void postProcess() {};
/** \brief Tracker function
* \return The number of successful tracks.
@@ -307,11 +301,6 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
-inline const StateKeys& ProcessorTracker::getStateKeys() const
-{
- return state_keys_;
-}
-
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
@@ -337,16 +326,6 @@ inline CaptureBasePtr ProcessorTracker::getLast()
return last_ptr_;
}
-inline FrameBaseConstPtr ProcessorTracker::getLastFrame() const
-{
- return last_frame_ptr_;
-}
-
-inline FrameBasePtr ProcessorTracker::getLastFrame()
-{
- return last_frame_ptr_;
-}
-
inline CaptureBaseConstPtr ProcessorTracker::getIncoming() const
{
return incoming_ptr_;
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index eebf6e782877e4a285678c0028bbba433a86b4d7..79a4c3ca8a8d1f3c5ac141c26128d32722312394 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -86,10 +86,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
public:
/** \brief Constructor with type
*/
- ProcessorTrackerFeature(const std::string& _type,
- const StateKeys& _structure,
- int _dim,
- const YAML::Node& _params);
+ ProcessorTrackerFeature(const std::string& _type, const TypeComposite& state_types_, const YAML::Node& _params);
~ProcessorTrackerFeature() override;
protected:
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index ebf22d98ccd5962191e5fd5e436f0ae4106c7dd2..bf8eb2143b81dd017d0aa01a3e99acbcb9348ac0 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -82,9 +82,9 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
- ProcessorTrackerLandmark(const std::string& _type,
- const StateKeys& _structure,
- const YAML::Node& _params_tracker_landmark);
+ ProcessorTrackerLandmark(const std::string& _type,
+ const TypeComposite& _state_types,
+ const YAML::Node& _params_tracker_landmark);
~ProcessorTrackerLandmark() override;
protected:
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index c9494cac8e17187c287c0ab68f87084fbc29e28f..c876c66bdda4bd66bca5cc3a61194be9a4780315 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -84,7 +84,7 @@ inline std::string FactoryStateBlock::getClass() const
{
return "FactoryStateBlock";
}
-typedef Factory<std::shared_ptr<StateBlock>> FactoryStateBlockIdentity;
+typedef Factory<std::shared_ptr<StateBlock>, bool> FactoryStateBlockIdentity;
template <>
inline std::string FactoryStateBlockIdentity::getClass() const
{
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 46533f9de0650810a38f9ba510c66f2d205bb7a5..f0739a87ada759e2e7b87f9d7a7ef1cdb55b820a 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -38,9 +38,9 @@
{ \
return std::make_shared<StateBlockClass>(_state, _fixed); \
} \
- static StateBlockPtr createIdentity() \
+ static StateBlockPtr createIdentity(bool _fixed) \
{ \
- return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false); \
+ return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), _fixed); \
}
// Fwd references
@@ -116,8 +116,8 @@ class StateBlock : public std::enable_shared_from_this<StateBlock>
bool _transformable = true);
///< Explicitly not copyable/movable
- StateBlock(const StateBlock& o) = delete;
- StateBlock(StateBlock&& o) = delete;
+ StateBlock(const StateBlock& o) = delete;
+ StateBlock(StateBlock&& o) = delete;
StateBlock& operator=(const StateBlock& o) = delete;
/** \brief Destructor
diff --git a/schema/processor/ProcessorLandmarkExternal.schema b/schema/processor/ProcessorLandmarkExternal.schema
index c929de2307515879a73a2eb51d839f82aafc939e..611400c7c63f29ebd61179c451f8c411a504d5b2 100644
--- a/schema/processor/ProcessorLandmarkExternal.schema
+++ b/schema/processor/ProcessorLandmarkExternal.schema
@@ -1,5 +1,11 @@
follow: ProcessorTracker.schema
+dimension:
+ _mandatory: true
+ _type: int
+ _options: [2, 3]
+ _doc: "Dimension of the problem representation is 2D or 3D."
+
quality_th:
_mandatory: true
_type: double
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
index 8ea9d6b0d86f2518328b84b2eaa33a18c3883d57..a8ecdce30be2b3e11c6889d7b7a878b85480bb23 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -118,6 +118,38 @@ StateBlockPtr NodeStateBlocks::emplaceStateBlock(const char _key,
return sb;
}
+StateBlockPtr NodeStateBlocks::emplaceStateBlockZero(const char _key, const std::string& _type, const bool& _fixed)
+{
+ // check key available
+ if (state_blocks_.count(_key))
+ {
+ throw std::runtime_error(std::string("NodeStateBlocks::emplaceStateBlockZero: key ") + _key +
+ " already in the node.");
+ }
+ // check correct type
+ checkTypeComposite({{_key, _type}});
+ if (state_types_.count(_key))
+ {
+ if (state_types_.at(_key) != _type)
+ throw std::runtime_error(
+ std::string("NodeStateBlocks::emplaceStateBlockZero: Wrong input type: " + _type + " for key ") +
+ _key + ". It should be " + state_types_.at(_key));
+ }
+ else
+ state_types_[_key] = _type;
+
+ // create state block
+ auto sb = FactoryStateBlockIdentity::create(_type, _fixed);
+
+ // store state block
+ state_blocks_.emplace(_key, sb);
+
+ // register to problem (if linked)
+ if (getProblem()) getProblem()->notifyStateBlock(sb, ADD);
+
+ return sb;
+}
+
void NodeStateBlocks::removeStateBlock(const char& _key)
{
StateBlockPtr _sb = getStateBlock(_key);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 965a0aa506d3127489bb7b53cc8722f3e070bcea..2c5d981e15d701e0b65d581555a504dd0cd0df47 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -46,7 +46,6 @@ Problem::Problem(SizeEigen _dim, const TypeComposite& _frame_types, LoaderPtr _l
tree_manager_(nullptr),
motion_provider_map_(),
dim_(_dim),
- frame_keys_(),
// reseved state types (P -> position, O -> orientation, V -> velocity)
frame_types_({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")},
{'O', (dim_ == 2 ? "StateAngle" : "StateQuaternion")},
@@ -316,10 +315,6 @@ ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node, st
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
- // Dimension check
- if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
- throw std::runtime_error("Processor not compatible with the Problem dimension.");
-
// Link
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
@@ -368,10 +363,6 @@ ProcessorBasePtr Problem::installProcessor(SensorBasePtr _sensor_corr
ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(prc_type, processor_node, _schema_folders);
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
- // Dimension check
- if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
- throw std::runtime_error("Processor not compatible with the Problem dimension.");
-
// Add processor
prc_ptr->configure(_sensor_corresponding);
prc_ptr->link(_sensor_corresponding);
@@ -422,10 +413,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _params_yaml
ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(prc_type, processor_node, _schema_folders);
if (not prc_ptr) throw std::runtime_error("Processor could not be created.");
- // Dimension check
- if (prc_ptr->getDimCompatible() != 0 and prc_ptr->getDimCompatible() != this->getDim())
- throw std::runtime_error("Processor not compatible with the Problem dimension.");
-
// Add processor
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
@@ -464,25 +451,20 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
const VectorComposite& _frame_state,
const PriorComposite& _frame_priors)
{
- // check input _frame_specs
+ // check input _frame_types
checkTypeComposite(_frame_types);
// add the types of the keys that are not in frame_types_ (also checks that are consistent)
addFrameTypes(_frame_types);
- // add the keys that are not in frame_keys_
- addFrameKeys(_frame_state.getKeys());
-
return FrameBase::emplace<FrameBase>(getTrajectory(), _time_stamp, _frame_types, _frame_state, _frame_priors);
}
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const StateKeys& _frame_keys)
{
if (not frame_types_.has(_frame_keys))
- {
- throw std::runtime_error("Problem::emplaceFrame: any unknown type, asked for " + _frame_keys +
+ throw std::runtime_error("Problem::emplaceFrame: any unknown type. Asked for key " + _frame_keys +
" but problem only know the types of " + frame_types_.getKeys());
- }
auto state = getState(_time_stamp, _frame_keys);
@@ -492,11 +474,8 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const StateKeys
FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const VectorComposite& _frame_state)
{
if (not frame_types_.has(_frame_state.getKeys()))
- {
- throw std::runtime_error("Problem::emplaceFrame: any unknown type, given vectors of " +
- _frame_state.getKeys() + " but problem only know the types of " +
- frame_types_.getKeys());
- }
+ throw std::runtime_error("Problem::emplaceFrame: any unknown type. given vectors for keys " + _frame_state.getKeys() +
+ " but problem only know the types of " + frame_types_.getKeys());
return Problem::emplaceFrame(_time_stamp, getFrameTypes(_frame_state.getKeys()), _frame_state);
}
@@ -506,10 +485,8 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
const Eigen::VectorXd& _frame_state)
{
if (not frame_types_.has(_frame_keys))
- {
- throw std::runtime_error("Problem::emplaceFrame: any unknown type, given vectors of " + _frame_keys +
+ throw std::runtime_error("Problem::emplaceFrame: any unknown type. given vectors for keys " + _frame_keys +
" but problem only know the types of " + frame_types_.getKeys());
- }
VectorComposite vec_composite;
@@ -534,6 +511,32 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
return Problem::emplaceFrame(_time_stamp, getFrameTypes(vec_composite.getKeys()), vec_composite);
}
+void Problem::emplaceStatesToFrame(FrameBasePtr _frame_ptr, const StateKeys& _frame_keys)
+{
+ if (not _frame_ptr) throw std::runtime_error("Problem::emplaceStatesToFrame: the provided frame is nullptr");
+
+ if (not frame_types_.has(_frame_keys))
+ throw std::runtime_error("Problem::emplaceStatesToFrame: any unknown type. Asked for key " + _frame_keys +
+ " but problem only know the types of " + frame_types_.getKeys());
+
+ // missing keys
+ StateKeys missing_keys;
+ for (auto key : _frame_keys)
+ if (not _frame_ptr->getState().has(key)) missing_keys.push_back(key);
+
+ // if no missing keys, return the frame
+ if (missing_keys.empty()) return;
+
+ WOLF_DEBUG("Emplacing missing states (", missing_keys, ") to frame ", _frame_ptr->id());
+
+ // get state of missing keys
+ auto state = getState(_frame_ptr->getTimeStamp(), _frame_keys);
+
+ // emplace states to the frame (unfixed)
+ for (auto vec_pair : state)
+ _frame_ptr->emplaceStateBlock(vec_pair.first, frame_types_.at(vec_pair.first), vec_pair.second, false);
+}
+
TimeStamp Problem::getTimeStamp() const
{
TimeStamp ts = TimeStamp::Invalid();
@@ -556,14 +559,19 @@ TimeStamp Problem::getTimeStamp() const
VectorComposite Problem::getState(const StateKeys& _keys) const
{
- StateKeys keys = (_keys == "" ? getFrameKeys() : _keys);
+ if (not frame_types_.has(_keys))
+ throw std::runtime_error("Problem::getState: unknown key! Asked for " + _keys +
+ " but installed processors only work with " + frame_types_.getKeys());
+
+ WOLF_WARN_COND(_keys.empty(), "Problem::getState: empty keys provided. Returning empty state.");
+ if (_keys.empty()) return VectorComposite();
VectorComposite state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
for (auto prc_pair : motion_provider_map_)
{
- auto prc_state = prc_pair.second->getState(keys);
+ auto prc_state = prc_pair.second->getState(_keys);
// transfer processor vector blocks to problem state
for (auto pair_key_vec : prc_state)
@@ -576,7 +584,7 @@ VectorComposite Problem::getState(const StateKeys& _keys) const
}
// If all keys are filled return
- if (state.has(keys))
+ if (state.has(_keys))
{
return state;
}
@@ -588,12 +596,12 @@ VectorComposite Problem::getState(const StateKeys& _keys) const
auto last_kf = trajectory_ptr_->getLastFrame();
if (last_kf)
- state_last = last_kf->getState(keys);
+ state_last = last_kf->getState(_keys);
else if (not first_frame_values_.empty())
state_last = first_frame_values_;
- for (auto key : keys)
+ for (auto key : _keys)
{
if (state.has(key) == 0)
{
@@ -603,23 +611,34 @@ VectorComposite Problem::getState(const StateKeys& _keys) const
state.emplace(key, state_last_it->second);
else
+ {
+ WOLF_WARN("Could not find any state for key ", key, ". Filling with zero state.");
state.emplace(key, stateZero(std::string(1, key)).at(key));
+ }
}
}
return state;
}
-VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _structure) const
+VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _keys) const
{
- StateKeys structure = (_structure == "" ? getFrameKeys() : _structure);
+ if (not frame_types_.has(_keys))
+ throw std::runtime_error("Problem::getState: unknown key! Asked for " + _keys +
+ " but installed processors only work with " + frame_types_.getKeys());
+
+ WOLF_WARN_COND(_keys.empty(), "Empty keys provided. Returning empty state.");
+ if (_keys.empty()) return VectorComposite();
+
+ WOLF_WARN_COND(not _ts.ok(), "Wrong timestamp provided. Returning empty state.");
+ if (not _ts.ok()) return VectorComposite();
VectorComposite state;
// compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
for (auto prc_pair : motion_provider_map_)
{
- auto prc_state = prc_pair.second->getState(_ts, structure);
+ auto prc_state = prc_pair.second->getState(_ts, _keys);
// transfer processor vector blocks to problem state
for (auto pair_key_vec : prc_state)
@@ -630,7 +649,7 @@ VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _struct
}
// If all keys are filled return
- if (state.size() == structure.size())
+ if (state.size() == _keys.size())
{
return state;
}
@@ -643,12 +662,12 @@ VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _struct
auto last_kf = trajectory_ptr_->closestFrameToTimeStamp(_ts);
if (last_kf)
- state_last = last_kf->getState(structure);
+ state_last = last_kf->getState(_keys);
else if (not first_frame_values_.empty())
state_last = first_frame_values_;
- for (auto key : structure)
+ for (auto key : _keys)
{
if (state.count(key) == 0)
{
@@ -658,16 +677,24 @@ VectorComposite Problem::getState(const TimeStamp& _ts, const StateKeys& _struct
state.emplace(key, state_last_it->second);
else
+ {
+ WOLF_WARN("Could not find any state for key ", key, ". Filling with zero state.");
state.emplace(key, stateZero(std::string(1, key)).at(key));
+ }
}
}
return state;
}
-VectorComposite Problem::getOdometry(const StateKeys& _structure) const
+VectorComposite Problem::getOdometry(const StateKeys& _keys) const
{
- StateKeys structure = (_structure == "" ? getFrameKeys() : _structure);
+ if (not frame_types_.has(_keys))
+ throw std::runtime_error("Problem::getState: unknown key! Asked for " + _keys +
+ " but installed processors only work with " + frame_types_.getKeys());
+
+ WOLF_WARN_COND(_keys.empty(), "Empty keys provided. Returning empty state.");
+ if (_keys.empty()) return VectorComposite();
VectorComposite odom_state;
@@ -686,7 +713,7 @@ VectorComposite Problem::getOdometry(const StateKeys& _structure) const
}
// check for empty blocks and fill them with the prior, or with zeros in the worst case
- for (auto key : structure)
+ for (auto key : _keys)
{
if (odom_state.count(key) == 0)
{
@@ -696,7 +723,10 @@ VectorComposite Problem::getOdometry(const StateKeys& _structure) const
odom_state.emplace(key, state_last_it->second);
else
+ {
+ WOLF_WARN("Could not find any odometry for key ", key, ". Filling with zero state.");
odom_state.emplace(key, stateZero(std::string(1, key)).at(key));
+ }
}
}
@@ -708,15 +738,8 @@ SizeEigen Problem::getDim() const
return dim_;
}
-StateKeys Problem::getFrameKeys() const
-{
- return frame_keys_;
-}
-
TypeComposite Problem::getFrameTypes(StateKeys _keys) const
{
- if (_keys == "") return frame_types_(frame_keys_);
-
if (not frame_types_.has(_keys))
throw std::runtime_error("Problem::getFrameTypes: frame_types_ does not have any of the required keys");
@@ -726,29 +749,19 @@ TypeComposite Problem::getFrameTypes(StateKeys _keys) const
return types;
}
-bool Problem::hasFrameKeys(StateKeys _keys) const
-{
- for (auto input_key : _keys)
- if (frame_keys_.find(input_key) == std::string::npos) return false;
-
- return true;
-}
-
-void Problem::addFrameKeys(StateKeys _keys)
-{
- for (auto input_key : _keys)
- if (frame_keys_.find(input_key) == std::string::npos) frame_keys_.push_back(input_key);
-}
-
void Problem::addFrameTypes(const TypeComposite& _types)
{
for (auto pair : _types)
- if (not frame_types_.has(pair.first)) // new key not found in problem structure -> append!
+ if (not frame_types_.has(pair.first)) // new key not found in problem keys -> append!
frame_types_.emplace(pair.first, pair.second);
else
{
WOLF_DEBUG_COND(frame_types_.at(pair.first) == pair.second,
- "Type already in 'frame_types_' doing nothing.");
+ "Type '",
+ pair.second,
+ "' already in 'frame_types_' in key '",
+ pair.first,
+ "' doing nothing.");
if (frame_types_.at(pair.first) != pair.second)
{
WOLF_ERROR("Trying to append type '",
@@ -761,8 +774,6 @@ void Problem::addFrameTypes(const TypeComposite& _types)
throw std::runtime_error("Problem::addFrameTypes: Different type already defined in this key.");
}
}
- // add also keys
- addFrameKeys(_types.getKeys());
}
void Problem::setTreeManager(TreeManagerBasePtr _gm)
@@ -795,18 +806,6 @@ void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
// add to map ordered by priority
motion_provider_map_.emplace(_motion_provider_ptr->getOrder(), _motion_provider_ptr);
-
- // check consistent types
- for (auto new_key : _motion_provider_ptr->getStateTypes().getKeys())
- if (frame_types_.has(new_key) and frame_types_[new_key] != _motion_provider_ptr->getStateTypes().at(new_key))
- {
- throw std::runtime_error("Problem::addMotionProvider: inconsistent type for key " +
- std::string(1, new_key) + " previously registered as " + frame_types_[new_key] +
- " and now as " + _motion_provider_ptr->getStateTypes().at(new_key));
- }
-
- // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key
- addFrameTypes(_motion_provider_ptr->getStateTypes());
}
void Problem::removeMotionProvider(MotionProviderPtr proc)
@@ -814,16 +813,10 @@ void Problem::removeMotionProvider(MotionProviderPtr proc)
WOLF_WARN_COND(motion_provider_map_.count(proc->getOrder()) == 0, "Missing processor");
motion_provider_map_.erase(proc->getOrder());
-
- // rebuild frame structure with remaining motion processors
- frame_keys_.clear();
- for (auto pm : motion_provider_map_) addFrameKeys(pm.second->getStateTypes().getKeys());
}
VectorComposite Problem::stateZero(TypeComposite _types) const
{
- if (_types.empty()) _types = frame_types_(frame_keys_);
-
VectorComposite state;
for (auto&& type_pair : _types)
{
@@ -838,8 +831,6 @@ VectorComposite Problem::stateZero(const StateKeys& _keys) const
throw std::runtime_error("Problem::stateZero any unknown type... asked for " + _keys +
" but only have types for " + frame_types_.getKeys());
- StateKeys keys = (_keys == "" ? getFrameKeys() : _keys);
-
return stateZero(getFrameTypes(_keys));
}
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index 7b36f391298091a588ccf4157a0a2938f32d05b8..05303de561b39e68f68bd04901bbf9f4448f9710 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -16,7 +16,8 @@
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include <core/processor/motion_provider.h>
+#include "core/processor/motion_provider.h"
+#include "core/state_block/factory_state_block.h"
#include "core/problem/problem.h"
using namespace wolf;
@@ -27,26 +28,38 @@ MotionProvider::MotionProvider(const TypeComposite& _state_types, const YAML::No
state_provider_order_(_params["state_provider_order"] ? _params["state_provider_order"].as<unsigned int>() : 0)
{
checkTypeComposite(_state_types);
-}
-void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr)
-{
- WOLF_DEBUG_COND(not isStateProvider(),
- "MotionProvider processor with 'state_provider'=false. Not adding this processor");
- if (isStateProvider())
- {
- _prb_ptr->addMotionProvider(_motion_ptr);
- }
- setOdometry(_prb_ptr->stateZero(state_types_));
+ // initialize odometry
+ for (auto&& type_pair : _state_types)
+ odometry_.emplace(type_pair.first, FactoryStateBlockIdentityVector::create(type_pair.second));
}
void MotionProvider::setOdometry(const VectorComposite& _odom)
{
std::lock_guard<std::mutex> lock(mut_odometry_);
- if (not _odom.has(getStateKeys()))
+ if (not _odom.has(state_types_.getKeys()))
throw std::runtime_error("MotionProvider::setOdometry(): any key missing in _odom.");
- for (auto key : getStateKeys()) odometry_[key] = _odom.at(key); // overwrite/insert only keys of the state_types_
+ for (auto key : state_types_.getKeys())
+ {
+ // check vector goodness
+ try
+ {
+ auto sb = FactoryStateBlock::create(state_types_.at(key), _odom.at(key), false);
+ }
+ catch (const std::exception& e)
+ {
+ WOLF_ERROR("Wrong odometry input for key '",
+ key,
+ "'. Could not create a stateblock of type '",
+ state_types_.at(key),
+ "' with the provided vector. Error: ",
+ e.what());
+ continue;
+ }
+ // store
+ odometry_[key] = _odom.at(key); // overwrite only
+ }
}
wolf::VectorComposite MotionProvider::getOdometry() const
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index c33dfc2db53adcaa15b03090ffd1fc41ba0527ca..90102584c1449d815c8547b205df712e6fd79b71 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -25,16 +25,18 @@ namespace wolf
{
unsigned int ProcessorBase::processor_id_count_ = 0;
-ProcessorBase::ProcessorBase(const std::string& _type, unsigned int _dim_compatible, const YAML::Node& _params)
+ProcessorBase::ProcessorBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params)
: NodeBase("PROCESSOR", _type, _params["name"].as<std::string>()),
processor_id_(++processor_id_count_),
params_(Clone(_params)),
- dim_compatible_(_dim_compatible),
+ state_types_(_state_types),
sensor_ptr_(),
capture_period_mean_(0),
prev_capture_stamp_(TimeStamp::Invalid())
{
- assert(dim_compatible_ == 0 or dim_compatible_ == 2 or dim_compatible_ == 3);
+ // check input state_types_
+ checkTypeComposite(state_types_);
+
WOLF_DEBUG("Constructed ProcessorBase - ", getType(), " id: ", id(), " name: ", getName());
}
@@ -104,10 +106,16 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture)
}
prev_capture_stamp_ = _capture->getTimeStamp();
- // apply prior in problem if not done (very first capture)
+ // apply first frame options in problem if not done (very first capture)
if (getProblem() and not getProblem()->isFirstFrameApplied())
+ {
+ WOLF_DEBUG(getType(),
+ " - ",
+ getName(),
+ ": applying first frame options to problem with ts = ",
+ _capture->getTimeStamp());
getProblem()->applyFirstFrameOptions(_capture->getTimeStamp());
-
+ }
// asking if capture should be stored
if (storeCapture(_capture)) buffer_capture_.emplace(_capture);
@@ -159,15 +167,16 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
{
if (_problem == nullptr or _problem == this->getProblem()) return;
- if (dim_compatible_ != 0 and dim_compatible_ != _problem->getDim())
- throw std::runtime_error("Processor works with " + std::to_string(dim_compatible_) + "D but problem is " +
- std::to_string(_problem->getDim()) + "D");
-
+ // set problem
NodeBase::setProblem(_problem);
+ // add types to problem (checks compatibility with other processors' types)
+ _problem->addFrameTypes(state_types_);
+
// adding motion provider to the motion providers vector
auto motion_provider_ptr = std::dynamic_pointer_cast<MotionProvider>(shared_from_this());
- if (motion_provider_ptr) motion_provider_ptr->addToProblem(_problem, motion_provider_ptr);
+ if (motion_provider_ptr and motion_provider_ptr->isStateProvider())
+ _problem->addMotionProvider(motion_provider_ptr);
}
void ProcessorBase::printHeader(int _depth,
diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
index dd41de9c9596fb1524625add1a1644e6a1eaecb7..a70ca60c9fb75ca116c694b48d9db643906e1441 100644
--- a/src/processor/processor_fixed_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -25,7 +25,7 @@
namespace wolf
{
ProcessorFixedWingModel::ProcessorFixedWingModel(const YAML::Node& _params)
- : ProcessorBase("ProcessorFixedWingModel", 3, _params),
+ : ProcessorBase("ProcessorFixedWingModel", {{'V', "StateVector3d"}}, _params),
velocity_local_(_params["velocity_local"].as<Eigen::Vector3d>().normalized()),
angle_stdev_(_params["angle_stdev"].as<double>()),
min_vel_norm_(_params["min_vel_norm"].as<double>())
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index ca15feb4327bb547ac540059f108662cc87dc9c1..e1b164265a65198e986b7aeb868eaf5bf32dbe06 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -37,7 +37,6 @@ void ProcessorLandmarkExternal::preProcess()
{
new_features_incoming_.clear();
- auto dim = getProblem()->getDim();
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks)
throw std::runtime_error(
@@ -58,16 +57,16 @@ void ProcessorLandmarkExternal::preProcess()
MatrixXd cov;
if (not use_orientation_)
{
- assert(detection.measure.size() >= dim);
- assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
+ assert(detection.measure.size() >= dim_);
+ assert(detection.covariance.rows() >= dim_ and detection.covariance.rows() == detection.covariance.cols());
- meas = detection.measure.head(dim);
- cov = detection.covariance.topLeftCorner(dim, dim);
+ meas = detection.measure.head(dim_);
+ cov = detection.covariance.topLeftCorner(dim_, dim_);
}
else
{
- assert(detection.measure.size() == (dim == 2 ? 3 : 7));
- assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and
+ assert(detection.measure.size() == (dim_ == 2 ? 3 : 7));
+ assert(detection.covariance.rows() == (dim_ == 2 ? 3 : 6) and
detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure;
@@ -341,7 +340,7 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
}
else
{
- if (getProblem()->getDim() == 2)
+ if (dim_ == 2)
{
VectorComposite pose_s1 = SE2::compose(_pose1, _pose_sen);
VectorComposite pose_s2 = SE2::compose(_pose2, _pose_sen);
@@ -378,7 +377,7 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr,
"ProcessorLandmarkExternal::detectionDistance: Missing any required geometric information");
}
- if (getProblem()->getDim() == 2)
+ if (dim_ == 2)
{
auto pose_s = SE2::compose(_pose_frm, _pose_sen);
Eigen::Vector2d p = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>();
@@ -577,7 +576,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
assert(_landmark);
// 2D - Relative Position
- if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
+ if (dim_ == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
{
return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(
_feature,
@@ -590,7 +589,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
applyLossFunction());
}
// 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
+ else if (dim_ == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
{
return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(
_feature,
@@ -604,7 +603,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
TOP_LMK);
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
+ else if (dim_ == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
{
return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(
_feature,
@@ -617,7 +616,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
applyLossFunction());
}
// 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
+ else if (dim_ == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
{
return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
_feature,
@@ -656,7 +655,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExtern
LandmarkBasePtr lmk;
// 2D - Relative Position
- if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
+ if (dim_ == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -669,7 +668,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExtern
getProblem()->getMap(), VectorComposite({{'P', lmk_p}}), PriorComposite{});
}
// 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
+ else if (dim_ == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -684,7 +683,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExtern
getProblem()->getMap(), VectorComposite({{'P', lmk_p}, {'O', Vector1d(lmk_o)}}), PriorComposite{});
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
+ else if (dim_ == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
{
Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -697,7 +696,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExtern
getProblem()->getMap(), VectorComposite({{'P', lmk_p}}), PriorComposite{});
}
// 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
+ else if (dim_ == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
{
Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
@@ -732,13 +731,13 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
if (not _landmark) throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
// 2D - Relative Position
- if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
+ if (dim_ == 2 and (_feature->getMeasurement().size() == 2 or not use_orientation_))
{
// nothing to do (we assume P already in landmark)
return;
}
// 2D - Relative Pose
- else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
+ else if (dim_ == 2 and _feature->getMeasurement().size() == 3 and use_orientation_)
{
// no orientation, add it
if (not _landmark->getO())
@@ -752,13 +751,13 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
}
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
+ else if (dim_ == 3 and (_feature->getMeasurement().size() == 3 or not use_orientation_))
{
// nothing to do (we assume P already in landmark)
return;
}
// 3D - Relative Pose
- else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
+ else if (dim_ == 3 and _feature->getMeasurement().size() == 7 and use_orientation_)
{
// no orientation, add it
if (not _landmark->getO())
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 957a080b36e86b1d4dbeca6a64bdac34d7e71d9c..cad2e26f876b1649cb0271bc92d203d91579025f 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -20,8 +20,8 @@
namespace wolf
{
-ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, const YAML::Node& _params)
- : ProcessorBase(_type, _dim, _params)
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params)
+ : ProcessorBase(_type, _state_types, _params)
{
//
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 56aea70f8f2fc2492970ba410498b16d98081910..abad4de30d6f6eb0b01839040e69216825dcfaf6 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -23,14 +23,13 @@ namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
TypeComposite _state_types,
- int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
const YAML::Node& _params)
- : ProcessorBase(_type, _dim, _params),
+ : ProcessorBase(_type, _state_types, _params),
MotionProvider(_state_types, _params),
processing_step_(RUNNING_WITHOUT_KF),
bootstrapping_(false),
@@ -300,7 +299,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
if (not keyframe_from_callback->has(pair_key_vec.first))
keyframe_from_callback->emplaceStateBlock(
pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
- keyframe_from_callback->setState(proc_state);
+ else
+ keyframe_from_callback->getStateBlock(pair_key_vec.first)->setState(pair_key_vec.second);
// Find the capture acting as the buffer's origin
auto capture_origin = capture_existing->getOriginCapture();
@@ -393,7 +393,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
if (not keyframe_from_callback->has(pair_key_vec.first))
keyframe_from_callback->emplaceStateBlock(
pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
- keyframe_from_callback->setState(proc_state);
+ else
+ keyframe_from_callback->getStateBlock(pair_key_vec.first)->setState(pair_key_vec.second);
auto& capture_existing = last_ptr_;
@@ -441,7 +442,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
if (not last_ptr_->getFrame()->has(pair_key_vec.first))
last_ptr_->getFrame()->emplaceStateBlock(
pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
- last_ptr_->getFrame()->setState(state_propa);
+ else
+ last_ptr_->getFrame()->getStateBlock(pair_key_vec.first)->setState(pair_key_vec.second);
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -765,14 +767,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
"ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
// add (zero) states to origin_frame if missing
- if (not _origin_frame->has(getStateKeys()))
- for (auto key : getStateKeys())
- if (not _origin_frame->has(key))
- _origin_frame->emplaceStateBlock(
- key,
- getStateTypes().at(key),
- getProblem()->stateZero(std::string(1, key)).vector(std::string(1, key)),
- false);
+ for (auto key : getStateKeys())
+ if (not _origin_frame->has(key)) _origin_frame->emplaceStateBlockZero(key, getStateTypes().at(key), false);
TimeStamp origin_ts = _origin_frame->getTimeStamp();
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index d8414746e1a5fb8224373428117837d30313bace..4fd2531e7ab5f6c9612dcf4e98f64e549ada753a 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -25,7 +25,7 @@
namespace wolf
{
ProcessorOdom2d::ProcessorOdom2d(const YAML::Node& _params)
- : ProcessorMotion("ProcessorOdom2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, 2, 3, 3, 3, 2, 0, _params)
+ : ProcessorMotion("ProcessorOdom2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, 3, 3, 3, 2, 0, _params)
{
//
}
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index ed4d97aeb4afbf51d83dfac5a3961644447d9517..fef1b7819564327961e51c92bc30eafd97c32a35 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -23,7 +23,7 @@
namespace wolf
{
ProcessorOdom3d::ProcessorOdom3d(const YAML::Node& _params)
- : ProcessorMotion("ProcessorOdom3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, 3, 7, 7, 6, 6, 0, _params)
+ : ProcessorMotion("ProcessorOdom3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, 7, 7, 6, 6, 0, _params)
{
//
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 0f58985a31a3d4c01c7eeb08c023fc57e4541d0b..0b8c2f7a35051c7b32dd88b19dd980e9224c091e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -24,17 +24,14 @@
namespace wolf
{
-ProcessorTracker::ProcessorTracker(const std::string& _type,
- const StateKeys& _state_keys,
- int _dim,
- const YAML::Node& _params)
- : ProcessorBase(_type, _dim, _params),
+ProcessorTracker::ProcessorTracker(const std::string& _type,
+ const TypeComposite& _state_types,
+ const YAML::Node& _params)
+ : ProcessorBase(_type, _state_types, _params),
processing_step_(FIRST_TIME_WITHOUT_KEYFRAME),
origin_ptr_(nullptr),
last_ptr_(nullptr),
incoming_ptr_(nullptr),
- last_frame_ptr_(nullptr),
- state_keys_(_state_keys),
min_features_for_keyframe_(_params["keyframe_vote"]["min_features_for_keyframe"].as<unsigned int>()),
max_new_features_(_params["max_new_features"].as<int>())
{
@@ -94,6 +91,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(keyframe_from_callback);
}
+ // Emplace missing states to KF
+ getProblem()->emplaceStatesToFrame(keyframe_from_callback, getStateKeys());
+
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything
// to do. TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnownProfiling();
@@ -101,11 +101,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Reset this
last_ptr_ = incoming_ptr_; // Before calling resetDerived() since it will assume KF in last
resetDerived();
+
// Update pointers
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = keyframe_from_callback;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
@@ -121,7 +121,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
WOLF_DEBUG("PT ", getName(), " Incoming capture has not been processed by another processor!")
// make a new KF at incoming
- FrameBasePtr keyframe = getProblem()->emplaceFrame(incoming_ptr_->getTimeStamp(), state_keys_);
+ FrameBasePtr keyframe = getProblem()->emplaceFrame(incoming_ptr_->getTimeStamp(), getStateKeys());
// Append incoming to KF
incoming_ptr_->link(keyframe);
@@ -137,11 +137,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Reset this
last_ptr_ = incoming_ptr_; // Before calling resetDerived() since it will assume KF in last
resetDerived();
+
// Update pointers
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = keyframe;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
@@ -169,11 +169,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITHOUT_KEYFRAME: {
WOLF_DEBUG("PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME");
- // Make a NON KEY Frame to hold incoming capture
- FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(state_keys_),
- getProblem()->getState(incoming_ptr_->getTimeStamp()));
-
// Process known information
processKnownProfiling();
@@ -185,11 +180,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Reset this
resetDerived();
+
// Update pointers
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frame;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
@@ -204,6 +199,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
" callback unpacked with ts= ",
keyframe_from_callback->getTimeStamp());
+ // Emplace missing states to KF
+ getProblem()->emplaceStatesToFrame(keyframe_from_callback, getStateKeys());
+
+ // Process known information
processKnownProfiling();
// chack if the received KF has a capture of this sensor, and if it matches with last_ptr
@@ -225,10 +224,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->link(keyframe_from_callback);
}
- // Create new NON KEY frame for incoming
- FrameBasePtr frame = std::make_shared<FrameBase>(
- incoming_ptr_->getTimeStamp(), getProblem()->getFrameTypes(state_keys_), getProblem()->getState());
-
// Detect new Features, initialize Landmarks, ...
processNewProfiling(max_new_features_);
@@ -237,11 +232,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Reset this
resetDerived();
+
// Update pointers
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frame;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
@@ -256,47 +251,33 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
processNewProfiling(max_new_features_);
// We create a KF
- // set KF on last
- last_frame_ptr_->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_frame_ptr_->link(getProblem());
- last_ptr_->link(last_frame_ptr_);
+ FrameBasePtr keyframe = getProblem()->emplaceFrame(last_ptr_->getTimeStamp(), getStateKeys());
+ last_ptr_->link(keyframe);
// Establish factors
establishFactorsProfiling();
// Call the new keyframe callback in order to let the other processors to join
- getProblem()->keyFrameCallback(last_frame_ptr_, shared_from_this());
-
- // make NON KEY frame; append incoming to new frame
- FrameBasePtr frame =
- std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(state_keys_),
- getProblem()->getState(incoming_ptr_->getTimeStamp()));
+ getProblem()->keyFrameCallback(keyframe, shared_from_this());
// Reset this
resetDerived();
+
// Update pointers
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frame;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
}
else
{
// We do not create a KF
- // Replace last frame for a new NON KEY frame in incoming
- FrameBasePtr frame =
- std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameTypes(state_keys_),
- getProblem()->getState(incoming_ptr_->getTimeStamp()));
-
// Advance this
advanceDerived();
+
// Update pointers
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frame;
- incoming_ptr_ = nullptr;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
}
break;
}
@@ -330,7 +311,7 @@ void ProcessorTracker::computeProcessingStep()
if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), getTimeTolerance()))
processing_step_ = FIRST_TIME_WITH_KEYFRAME;
- else // ! last && ! pack(incoming)
+ else
processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
break;
@@ -346,19 +327,7 @@ void ProcessorTracker::computeProcessingStep()
default:
if (buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance()))
- {
- if (last_frame_ptr_->getProblem())
- {
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_INFO("Received KF and last's Frame have matching time stamps (i.e. below time tolerances)");
- WOLF_INFO("Check the following for correctness:");
- WOLF_INFO(" - You have all processors installed before starting receiving any data");
- WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
- WOLF_ERROR("Received KF and last's KF have matching time stamps (i.e. below time tolerances).");
- }
processing_step_ = RUNNING_WITH_KEYFRAME;
- }
else
processing_step_ = RUNNING_WITHOUT_KEYFRAME;
break;
@@ -379,8 +348,7 @@ void ProcessorTracker::printHeader(int _depth,
<< " Frm" << getOrigin()->getFrame()->id() << std::endl;
if (getLast())
_stream << _tabs << " "
- << "l: Cap" << getLast()->id() << " - "
- << " Frm" << getLastFrame()->id() << std::endl;
+ << "l: Cap" << getLast()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " "
<< "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 7c6d618bed2429b2e6c42b62facb3196973a36e4..3d9380b0ca1131b1accf1a2277e7bfe4d5d9926a 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -20,11 +20,10 @@
namespace wolf
{
-ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
- const StateKeys& _structure,
- int _dim,
- const YAML::Node& _params)
- : ProcessorTracker(_type, _structure, _dim, _params)
+ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
+ const TypeComposite& _frame_types,
+ const YAML::Node& _params)
+ : ProcessorTracker(_type, _frame_types, _params)
{
}
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 0a2307ea19d9a05db10510c2fe0fc0256be0f2c4..71ad36f151c65d124697183d155e28e840c39711 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -23,10 +23,10 @@
namespace wolf
{
-ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
- const StateKeys& _structure,
- const YAML::Node& _params)
- : ProcessorTracker(_type, _structure, 0, _params)
+ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
+ const TypeComposite& _state_types,
+ const YAML::Node& _params)
+ : ProcessorTracker(_type, _state_types, _params)
{
//
}
diff --git a/test/dummy/processor_loop_closure_dummy.cpp b/test/dummy/processor_loop_closure_dummy.cpp
index f7709d8ee39488446c4b3251aed18c41270e1f3a..d45e900c4e762df6d9a827a50598050f48e4e626 100644
--- a/test/dummy/processor_loop_closure_dummy.cpp
+++ b/test/dummy/processor_loop_closure_dummy.cpp
@@ -23,7 +23,7 @@ using namespace Eigen;
namespace wolf
{
ProcessorLoopClosureDummy::ProcessorLoopClosureDummy(const YAML::Node& _params)
- : ProcessorLoopClosure("ProcessorLoopClosureDummy", 2, _params)
+ : ProcessorLoopClosure("ProcessorLoopClosureDummy", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
{
}
bool ProcessorLoopClosureDummy::voteFindLoopClosures(CaptureBasePtr cap)
diff --git a/test/dummy/processor_motion_provider_dummy.cpp b/test/dummy/processor_motion_provider_dummy.cpp
index c5219fde3102aba924f21a5d417b8a405df5f50e..ee31343895b469eb49607a5d5da4bd4d0ee941e9 100644
--- a/test/dummy/processor_motion_provider_dummy.cpp
+++ b/test/dummy/processor_motion_provider_dummy.cpp
@@ -21,7 +21,7 @@
namespace wolf
{
ProcessorMotionProviderDummy::ProcessorMotionProviderDummy(const YAML::Node& _params)
- : ProcessorBase("ProcessorMotionProviderDummy", 2, _params),
+ : ProcessorBase("ProcessorMotionProviderDummy", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
MotionProvider({{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
{
}
@@ -55,12 +55,12 @@ TimeStamp ProcessorMotionProviderDummy::getTimeStamp() const
return TimeStamp(0);
};
-VectorComposite ProcessorMotionProviderDummy::getState(StateKeys _structure) const
+VectorComposite ProcessorMotionProviderDummy::getState(StateKeys _keys) const
{
return getOdometry();
};
-VectorComposite ProcessorMotionProviderDummy::getState(const TimeStamp& _ts, StateKeys _structure) const
+VectorComposite ProcessorMotionProviderDummy::getState(const TimeStamp& _ts, StateKeys _keys) const
{
return getOdometry();
};
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index 35955e32d5819a3b06a25a56a24005a4104f0acc..a9a1e70a6e2c224b11864b6251c6f3ab0a73c17c 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -45,9 +45,9 @@ class ProcessorMotionProviderDummy : public ProcessorBase, public MotionProvider
bool voteForKeyFrame() const override;
TimeStamp getTimeStamp() const override;
- VectorComposite getState(StateKeys _structure = "") const override;
+ VectorComposite getState(StateKeys _keys = "") const override;
- VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, StateKeys _keys = "") const override;
};
} /* namespace wolf */
diff --git a/test/dummy/processor_motion_provider_dummy_pov.cpp b/test/dummy/processor_motion_provider_dummy_pov.cpp
index 402b5163dce89b04d826267b73c80d15b007ddba..4cb0850a3c9f48cebf4be207e7a79ffbe1b16adf 100644
--- a/test/dummy/processor_motion_provider_dummy_pov.cpp
+++ b/test/dummy/processor_motion_provider_dummy_pov.cpp
@@ -21,7 +21,7 @@
namespace wolf
{
ProcessorMotionProviderDummyPOV::ProcessorMotionProviderDummyPOV(const YAML::Node& _params)
- : ProcessorBase("ProcessorMotionProviderDummyPOV", 2, _params),
+ : ProcessorBase("ProcessorMotionProviderDummyPOV", {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}}, _params),
MotionProvider({{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}}, _params)
{
}
@@ -55,12 +55,12 @@ TimeStamp ProcessorMotionProviderDummyPOV::getTimeStamp() const
return TimeStamp(0);
};
-VectorComposite ProcessorMotionProviderDummyPOV::getState(StateKeys _structure) const
+VectorComposite ProcessorMotionProviderDummyPOV::getState(StateKeys _keys) const
{
return getOdometry();
};
-VectorComposite ProcessorMotionProviderDummyPOV::getState(const TimeStamp& _ts, StateKeys _structure) const
+VectorComposite ProcessorMotionProviderDummyPOV::getState(const TimeStamp& _ts, StateKeys _keys) const
{
return getOdometry();
};
diff --git a/test/dummy/processor_motion_provider_dummy_pov.h b/test/dummy/processor_motion_provider_dummy_pov.h
index 948f8528fddcf6b2b5e8c009698428205968e06b..b76e4699e972a6d771c921240ed956ea520e2891 100644
--- a/test/dummy/processor_motion_provider_dummy_pov.h
+++ b/test/dummy/processor_motion_provider_dummy_pov.h
@@ -45,9 +45,9 @@ class ProcessorMotionProviderDummyPOV : public ProcessorBase, public MotionProvi
bool voteForKeyFrame() const override;
TimeStamp getTimeStamp() const override;
- VectorComposite getState(StateKeys _structure = "") const override;
+ VectorComposite getState(StateKeys _keys = "") const override;
- VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, StateKeys _keys = "") const override;
};
} /* namespace wolf */
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index e230ff1247d23b6420b75a524c4d246f795e50c8..8a1c6e72cb733460655c31ee9f1705024d79c14a 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -166,7 +166,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
};
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(const YAML::Node& _params)
- : ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", "PO", 0, _params),
+ : ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
n_tracks_lost_(_params["n_tracks_lost"].as<unsigned int>())
{
//
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 7d78446a9ab48b6d755104e8ba2f560f509e94fc..56fde47bc37952d7a87bd6216ac95ddee2dfd8ef 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -25,7 +25,7 @@
namespace wolf
{
ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(const YAML::Node& _params)
- : ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params),
+ : ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
n_landmarks_lost_(_params["n_landmarks_lost"].as<unsigned int>())
{
//
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 9b24898d82ede6476a536ccac244011fde9c99fd..9fc1ae902ebb664fea928bf0e088376f71167251 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -124,8 +124,8 @@ TEST(CaptureBase, emplaceDriftFactor)
auto sensor = problem->installSensor(wolf_dir + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_dir});
- auto F0 = problem->emplaceFrame(0.0);
- auto F1 = problem->emplaceFrame(1.0);
+ auto F0 = problem->emplaceFrame(0.0, "PO");
+ auto F1 = problem->emplaceFrame(1.0, "PO");
CaptureBasePtr C0 = CaptureBase::emplace<CaptureBase>(
F0,
@@ -204,7 +204,7 @@ TEST(CaptureBase, move_from_F_to_KF)
{
ProblemPtr problem = Problem::create(2);
- auto KF = problem->emplaceFrame(0.0); // dummy F object
+ auto KF = problem->emplaceFrame(0.0, ""); // dummy F object
auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
@@ -244,7 +244,7 @@ TEST(CaptureBase, move_from_null_to_KF)
{
ProblemPtr problem = Problem::create(2);
- auto KF = problem->emplaceFrame(0.0); // dummy F object
+ auto KF = problem->emplaceFrame(0.0, ""); // dummy F object
auto KC = CaptureBase::emplace<CaptureBase>(KF, "Dummy", 0.0);
@@ -277,7 +277,7 @@ TEST(CaptureBase, move_from_KF_to_F)
{
ProblemPtr problem = Problem::create(2);
- auto KF = problem->emplaceFrame(0.0); // dummy F object
+ auto KF = problem->emplaceFrame(0.0, ""); // dummy F object
auto F = std::make_shared<FrameBase>(0.0, TypeComposite(), VectorComposite()); // dummy F object
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index b4d0697aade652283842deabf9c4e9175812f207..8845a3c70d1a66a59e3f5f1f5323a5661754db4b 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -61,7 +61,7 @@ class FixtureFactorBlockDifference : public testing::Test
problem_->stateZero("POV"),
PriorComposite{{'P', Prior("fix")}, {'O', Prior("fix")}, {'V', Prior("fix")}});
- KF1_ = problem_->emplaceFrame(t1, problem_->stateZero());
+ KF1_ = problem_->emplaceFrame(t1, problem_->stateZero("POV"));
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 56a01ecfec206af0a2b5b2dcec825f7e4d612a8d..0ddbf90136834b7150c8c8af1fe63bab5063a872 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -69,7 +69,7 @@ class FactorDiffDriveTest : public testing::Test
{wolf_dir + "/test/dummy", wolf_dir + "/schema"}));
// frames
- F0 = problem->emplaceFrame(0.0, problem->stateZero());
+ F0 = problem->emplaceFrame(0.0, problem->stateZero("PO"));
F1 = problem->emplaceFrame(1.0, VectorComposite{{'P', Vector2d(1, 0)}, {'O', Vector1d(0)}});
// captures
@@ -385,7 +385,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_TRACE("\n", report);
- WOLF_TRACE("x1 : ", problem->getState(N * dt).vector("PO").transpose());
+ WOLF_TRACE("x1 : ", problem->getState(N * dt, "PO").vector("PO").transpose());
WOLF_TRACE("x2 : ", F2->getStateVector("PO").transpose());
WOLF_TRACE("calib_preint : ", calib_preint.transpose());
WOLF_TRACE("calib_pert : ", calib_pert.transpose());
@@ -393,8 +393,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
WOLF_TRACE("calib_gt : ", calib_gt.transpose());
ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-6);
- ASSERT_POSE2d_APPROX(problem->getState(N * dt).vector("PO"), x1, 1e-6);
- ASSERT_POSE2d_APPROX(problem->getState(2 * N * dt).vector("PO"), x2, 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(N * dt, "PO").vector("PO"), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(2 * N * dt, "PO").vector("PO"), x2, 1e-6);
std::cout << "\n\n" << std::endl;
}
@@ -507,15 +507,15 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_TRACE("\n", report);
- WOLF_TRACE("x1 : ", problem->getState(N * dt).vector("PO").transpose());
+ WOLF_TRACE("x1 : ", problem->getState(N * dt, "PO").vector("PO").transpose());
WOLF_TRACE("x2 : ", F2->getStateVector("PO").transpose());
WOLF_TRACE("calib_preint : ", calib_preint.transpose());
WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
WOLF_TRACE("calib_GT : ", calib_gt.transpose());
ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
- ASSERT_POSE2d_APPROX(problem->getState(N * dt).vector("PO"), x1, 1e-2);
- ASSERT_POSE2d_APPROX(problem->getState(2 * N * dt).vector("PO"), x2, 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(N * dt, "PO").vector("PO"), x1, 1e-2);
+ ASSERT_POSE2d_APPROX(problem->getState(2 * N * dt, "PO").vector("PO"), x2, 1e-6);
}
int main(int argc, char** argv)
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index b50443f1f902f6fa8481386487dbb20ec9e26dff..1020aa3ce30a46decb5482c1308f972070d8d1d1 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -151,17 +151,17 @@ TEST(FactoryStateBlock, creator_Params)
TEST(FactoryStateBlockIdentity, creator_idendity_non_registered_key)
{
// non registered -> throw
- ASSERT_THROW(auto sba = FactoryStateBlockIdentity::create("A"), std::runtime_error);
+ ASSERT_THROW(auto sba = FactoryStateBlockIdentity::create("A", false), std::runtime_error);
}
TEST(FactoryStateBlockIdentity, creator_StateBlock)
{
- ASSERT_THROW(auto sbp = FactoryStateBlockIdentity::create("StateBlock"), std::runtime_error);
+ ASSERT_THROW(auto sbp = FactoryStateBlockIdentity::create("StateBlock", false), std::runtime_error);
}
TEST(FactoryStateBlockIdentity, creator_Quaternion)
{
- auto sbq = FactoryStateBlockIdentity::create("StateQuaternion");
+ auto sbq = FactoryStateBlockIdentity::create("StateQuaternion", false);
ASSERT_EQ(sbq->getSize(), 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -172,7 +172,7 @@ TEST(FactoryStateBlockIdentity, creator_Quaternion)
TEST(FactoryStateBlockIdentity, creator_Angle)
{
- auto sba = FactoryStateBlockIdentity::create("StateAngle");
+ auto sba = FactoryStateBlockIdentity::create("StateAngle", false);
ASSERT_EQ(sba->getSize(), 1);
ASSERT_EQ(sba->getLocalSize(), 1);
@@ -183,7 +183,7 @@ TEST(FactoryStateBlockIdentity, creator_Angle)
TEST(FactoryStateBlockIdentity, creator_Homogeneous3d)
{
- auto sbh = FactoryStateBlockIdentity::create("StateHomogeneous3d");
+ auto sbh = FactoryStateBlockIdentity::create("StateHomogeneous3d", false);
ASSERT_EQ(sbh->getSize(), 4);
ASSERT_EQ(sbh->getLocalSize(), 3);
@@ -194,7 +194,7 @@ TEST(FactoryStateBlockIdentity, creator_Homogeneous3d)
TEST(FactoryStateBlockIdentity, creator_Point)
{
- auto sbp2d = FactoryStateBlockIdentity::create("StatePoint2d");
+ auto sbp2d = FactoryStateBlockIdentity::create("StatePoint2d", false);
ASSERT_EQ(sbp2d->getSize(), 2);
ASSERT_EQ(sbp2d->getLocalSize(), 2);
@@ -202,7 +202,7 @@ TEST(FactoryStateBlockIdentity, creator_Point)
ASSERT_TRUE(sbp2d->isValid());
ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbp2d->getState(), Constants::EPS_SMALL);
- auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d");
+ auto sbp3d = FactoryStateBlockIdentity::create("StatePoint3d", false);
ASSERT_EQ(sbp3d->getSize(), 3);
ASSERT_EQ(sbp3d->getLocalSize(), 3);
@@ -213,7 +213,7 @@ TEST(FactoryStateBlockIdentity, creator_Point)
TEST(FactoryStateBlockIdentity, creator_Vector)
{
- auto sbv2d = FactoryStateBlockIdentity::create("StateVector2d");
+ auto sbv2d = FactoryStateBlockIdentity::create("StateVector2d", false);
ASSERT_EQ(sbv2d->getSize(), 2);
ASSERT_EQ(sbv2d->getLocalSize(), 2);
@@ -221,7 +221,7 @@ TEST(FactoryStateBlockIdentity, creator_Vector)
ASSERT_TRUE(sbv2d->isValid());
ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbv2d->getState(), Constants::EPS_SMALL);
- auto sbv3d = FactoryStateBlockIdentity::create("StateVector3d");
+ auto sbv3d = FactoryStateBlockIdentity::create("StateVector3d", false);
ASSERT_EQ(sbv3d->getSize(), 3);
ASSERT_EQ(sbv3d->getLocalSize(), 3);
@@ -232,16 +232,27 @@ TEST(FactoryStateBlockIdentity, creator_Vector)
TEST(FactoryStateBlockIdentity, creator_Params)
{
- auto sb1 = FactoryStateBlockIdentity::create("StateParams1");
- auto sb2 = FactoryStateBlockIdentity::create("StateParams2");
- auto sb3 = FactoryStateBlockIdentity::create("StateParams3");
- auto sb4 = FactoryStateBlockIdentity::create("StateParams4");
- auto sb5 = FactoryStateBlockIdentity::create("StateParams5");
- auto sb6 = FactoryStateBlockIdentity::create("StateParams6");
- auto sb7 = FactoryStateBlockIdentity::create("StateParams7");
- auto sb8 = FactoryStateBlockIdentity::create("StateParams8");
- auto sb9 = FactoryStateBlockIdentity::create("StateParams9");
- auto sb10 = FactoryStateBlockIdentity::create("StateParams10");
+ auto sb1 = FactoryStateBlockIdentity::create("StateParams1", false);
+ auto sb2 = FactoryStateBlockIdentity::create("StateParams2", true);
+ auto sb3 = FactoryStateBlockIdentity::create("StateParams3", false);
+ auto sb4 = FactoryStateBlockIdentity::create("StateParams4", false);
+ auto sb5 = FactoryStateBlockIdentity::create("StateParams5", true);
+ auto sb6 = FactoryStateBlockIdentity::create("StateParams6", false);
+ auto sb7 = FactoryStateBlockIdentity::create("StateParams7", true);
+ auto sb8 = FactoryStateBlockIdentity::create("StateParams8", false);
+ auto sb9 = FactoryStateBlockIdentity::create("StateParams9", true);
+ auto sb10 = FactoryStateBlockIdentity::create("StateParams10", false);
+
+ ASSERT_EQ(sb1->isFixed(), false);
+ ASSERT_EQ(sb2->isFixed(), true);
+ ASSERT_EQ(sb3->isFixed(), false);
+ ASSERT_EQ(sb4->isFixed(), false);
+ ASSERT_EQ(sb5->isFixed(), true);
+ ASSERT_EQ(sb6->isFixed(), false);
+ ASSERT_EQ(sb7->isFixed(), true);
+ ASSERT_EQ(sb8->isFixed(), false);
+ ASSERT_EQ(sb9->isFixed(), true);
+ ASSERT_EQ(sb10->isFixed(), false);
ASSERT_EQ(sb1->getSize(), 1);
ASSERT_EQ(sb2->getSize(), 2);
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index 57ca89c7652c33250fbb30ac81bd42786aa90992..f2c315db09ec55127b69b0a50cdddd65c5345136 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -96,15 +96,6 @@ TEST_F(MotionProviderTest, install)
ASSERT_TRUE(mp3->isStateProvider());
ASSERT_EQ(mp2->getOrder(), 1);
ASSERT_EQ(mp3->getOrder(), 2);
- ASSERT_TRUE(mp1->getStateTypes().has('P'));
- ASSERT_TRUE(mp1->getStateTypes().has('O'));
- ASSERT_FALSE(mp1->getStateTypes().has('V'));
- ASSERT_TRUE(mp2->getStateTypes().has('P'));
- ASSERT_TRUE(mp2->getStateTypes().has('O'));
- ASSERT_FALSE(mp2->getStateTypes().has('V'));
- ASSERT_TRUE(mp3->getStateTypes().has('P'));
- ASSERT_TRUE(mp3->getStateTypes().has('O'));
- ASSERT_TRUE(mp3->getStateTypes().has('V'));
// Only 2 and 3 in problem::motion_provider_map_
ASSERT_EQ(problem->getMotionProviderMap().size(), 2);
@@ -161,7 +152,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom3_get.at('V'), odom3.at('V'), 1e-9);
// problem::getOdometry(): by priority P and O should come from mp2 and V from mp3
- auto odom_get = problem->getOdometry();
+ auto odom_get = problem->getOdometry("PO");
EXPECT_TRUE(odom_get.count('P') == 1);
EXPECT_TRUE(odom_get.count('O') == 1);
EXPECT_TRUE(odom_get.count('V') == 1);
@@ -170,7 +161,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom_get.at('V'), odom3.at('V'), 1e-9);
// problem::getState(): by priority P and O should come from mp2 and V from mp3
- auto state_get = problem->getState();
+ auto state_get = problem->getState("POV");
EXPECT_TRUE(state_get.count('P') == 1);
EXPECT_TRUE(state_get.count('O') == 1);
EXPECT_TRUE(state_get.count('V') == 1);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d8363c132c80b55d0cfba9a706f5cb4bc3762e83..737b19422c4452b7f238f3bbc58d3a89db5140ae 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -56,10 +56,6 @@ TEST(Problem, create)
ASSERT_EQ(P, P->getHardware()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getMap()->getProblem());
-
- // check frame structure through the state size
- ASSERT_EQ(P->getFrameKeys().size(), 0);
- ASSERT_FALSE(P->hasFrameKeys("POV"));
}
TEST(Problem, Installers)
@@ -94,10 +90,8 @@ TEST(Problem, emplaceFirstFrame_PO_2d)
TimeStamp t0(0);
VectorComposite first_frame_values{{'P', Vector2d(1, 2)}, {'O', Vector1d(3)}};
PriorComposite first_frame_priors{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))},
- {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}};
+ {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}};
P->emplaceFirstFrame(t0, {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, first_frame_values, first_frame_priors);
- ASSERT_EQ(P->getFrameKeys().size(), 2);
- ASSERT_TRUE(P->hasFrameKeys("PO"));
WOLF_INFO("printing...");
P->print(4, 1, 1, 1);
@@ -106,7 +100,7 @@ TEST(Problem, emplaceFirstFrame_PO_2d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd)0);
// check that the state is correct
- ASSERT_POSE2d_APPROX(first_frame_values.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL);
+ ASSERT_POSE2d_APPROX(first_frame_values.vector("PO"), P->getState("PO").vector("PO"), Constants::EPS_SMALL);
// check that the tree has one frame without captures
TrajectoryBasePtr T = P->getTrajectory();
@@ -150,11 +144,9 @@ TEST(Problem, emplaceFirstFrame_PO_3d)
TimeStamp t0(0);
VectorComposite first_frame_values{{'P', Vector3d(1, 2, 3)}, {'O', Vector4d(4, 5, 6, 7).normalized()}};
PriorComposite first_frame_priors{{'P', Prior("factor", Vector3d::Constant(sqrt(0.1)))},
- {'O', Prior("factor", Vector3d::Constant(sqrt(0.1)))}};
+ {'O', Prior("factor", Vector3d::Constant(sqrt(0.1)))}};
P->emplaceFirstFrame(
t0, {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, first_frame_values, first_frame_priors);
- ASSERT_EQ(P->getFrameKeys().size(), 2);
- ASSERT_TRUE(P->hasFrameKeys("PO"));
WOLF_INFO("printing...");
P->print(4, 1, 1, 1);
@@ -163,7 +155,7 @@ TEST(Problem, emplaceFirstFrame_PO_3d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd)0);
// check that the state is correct
- ASSERT_POSE3d_APPROX(first_frame_values.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL);
+ ASSERT_POSE3d_APPROX(first_frame_values.vector("PO"), P->getState("PO").vector("PO"), Constants::EPS_SMALL);
// check that the tree has one frame without captures
TrajectoryBasePtr T = P->getTrajectory();
@@ -208,12 +200,8 @@ TEST(Problem, emplaceFrame_factory_2D)
ProblemPtr P = Problem::create(2);
FrameBasePtr f1 = P->emplaceFrame(0, "PO", xpo);
- ASSERT_EQ(P->getFrameKeys().size(), 2);
- ASSERT_TRUE(P->hasFrameKeys("PO"));
FrameBasePtr f2 = P->emplaceFrame(1, "POV", xpov);
- ASSERT_EQ(P->getFrameKeys().size(), 3);
- ASSERT_TRUE(P->hasFrameKeys("POV"));
// check that all frames are effectively in the trajectory
ASSERT_EQ(P->getTrajectory()->size(), (SizeStd)2);
@@ -250,9 +238,6 @@ TEST(Problem, emplaceFrame_factory_3D)
// check that all frames are linked to Problem
ASSERT_EQ(f1->getProblem(), P);
ASSERT_EQ(f2->getProblem(), P);
-
- ASSERT_EQ(P->getFrameKeys().size(), 3);
- ASSERT_TRUE(P->hasFrameKeys("POV"));
}
TEST(Problem, StateBlocks)
@@ -620,52 +605,40 @@ TEST(Problem, getState)
P->print(4, 1, 1, 1);
// get at t = origin
- WOLF_DEBUG("P (0) = ", P->getState(0, "P")); // partial structure
- WOLF_DEBUG("PO(0) = ", P->getState(0, "PO")); // all but explicit structure
- WOLF_DEBUG("x (0) = ", P->getState(TimeStamp(0))); // own structure
+ WOLF_DEBUG("P (0) = ", P->getState(0, "P"));
+ WOLF_DEBUG("PO(0) = ", P->getState(0, "PO"));
ASSERT_EQ(P->getState(0, "P").size(), 1);
ASSERT_EQ(P->getState(0, "PO").size(), 2);
- ASSERT_EQ(P->getState(TimeStamp(0)).size(), 2);
// get at t = before KF
WOLF_DEBUG("P (1) = ", P->getState(1, "P"));
WOLF_DEBUG("PO(1) = ", P->getState(1, "PO"));
- WOLF_DEBUG("x (1) = ", P->getState(1));
ASSERT_EQ(P->getState(1, "P").size(), 1);
ASSERT_EQ(P->getState(1, "PO").size(), 2);
- ASSERT_EQ(P->getState(TimeStamp(1)).size(), 2);
// get at t = KF
WOLF_DEBUG("P (2) = ", P->getState(2, "P"));
WOLF_DEBUG("PO(2) = ", P->getState(2, "PO"));
- WOLF_DEBUG("x (2) = ", P->getState(2));
ASSERT_EQ(P->getState(2, "P").size(), 1);
ASSERT_EQ(P->getState(2, "PO").size(), 2);
- ASSERT_EQ(P->getState(TimeStamp(2)).size(), 2);
// get at t = after last KF
WOLF_DEBUG("P (3) = ", P->getState(3, "P"));
WOLF_DEBUG("PO(3) = ", P->getState(3, "PO"));
- WOLF_DEBUG("x (3) = ", P->getState(3));
ASSERT_EQ(P->getState(3, "P").size(), 1);
ASSERT_EQ(P->getState(3, "PO").size(), 2);
- ASSERT_EQ(P->getState(TimeStamp(3)).size(), 2);
// get at t = last processed capture
WOLF_DEBUG("P (3.9) = ", P->getState(3.9, "P"));
WOLF_DEBUG("PO(3.9) = ", P->getState(3.9, "PO"));
- WOLF_DEBUG("x (3.9) = ", P->getState(3.9));
ASSERT_EQ(P->getState(3.9, "P").size(), 1);
ASSERT_EQ(P->getState(3.9, "PO").size(), 2);
- ASSERT_EQ(P->getState(TimeStamp(3.9)).size(), 2);
// get at t = current state
WOLF_DEBUG("P () = ", P->getState("P"));
WOLF_DEBUG("PO() = ", P->getState("PO"));
- WOLF_DEBUG("x () = ", P->getState());
ASSERT_EQ(P->getState("P").size(), 1);
ASSERT_EQ(P->getState("PO").size(), 2);
- ASSERT_EQ(P->getState().size(), 2);
}
TEST(Problem, transform)
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index 169d54c38c5470bbb308ffeef9b1fec41d69e5fb..5b82bb2acd1ce0d696507cd9c3105a2909ceba01 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -59,7 +59,7 @@ class ProcessorLandmarkExternalTest : public testing::Test
VectorComposite state_robot, state_sensor;
std::vector<VectorComposite> state_landmarks;
- virtual void SetUp(){};
+ virtual void SetUp() {};
void initProblem(int _dim,
bool _orientation,
int _mode,
@@ -133,6 +133,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Take parameters from YAML and modify some of them
YAML::Node params = YAML::LoadFile(wolf_dir + "/test/yaml/processor_landmark_external.yaml");
+ params["dimension"] = dim;
params["time_tolerance"] = dt / 2;
params["use_orientation"] = orientation;
params["quality_th"] = _quality_th;
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 9fd70a8123e9d50c8d6f6a6c1e6d34bb2ca70abf..072297eb4de696e285fe2a1b56f63969b54cbcf5 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -111,10 +111,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState("PO").vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, getState_structure)
@@ -136,7 +136,7 @@ TEST_F(ProcessorMotion_test, getState_structure)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
ASSERT_TRUE(processor->getState("P").count('P'));
@@ -167,7 +167,7 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
capture->setData(data);
capture->setDataCovariance(data_cov);
capture->process();
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
problem->print(2, 1, 1, 1);
@@ -203,10 +203,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState("PO").vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
@@ -229,10 +229,10 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState("PO").vector("PO"), (Vector3d() << 9, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
@@ -254,10 +254,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState("PO").vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
@@ -281,10 +281,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState("PO").vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
@@ -307,10 +307,10 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
- ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState("PO").vector("PO"), (Vector3d() << 0, 0, 0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, mergeCaptures)
@@ -332,13 +332,13 @@ TEST_F(ProcessorMotion_test, mergeCaptures)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target, "PO");
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target =
CaptureBase::emplace<CaptureMotion>(F_target, "CaptureOdom2d", t_target, sensor, data, nullptr);
@@ -404,13 +404,13 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target, "PO");
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);
@@ -442,13 +442,13 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target, "PO");
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);
@@ -479,13 +479,13 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState("PO").vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(t_target);
+ FrameBasePtr F_target = problem->emplaceFrame(t_target, "PO");
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);
diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp
index fb68aa20c3b2d92df6241383b9c12273f062151c..978d694f18ba62a719797fe347dabadd6a27e9a8 100644
--- a/test/gtest_processor_odom_2d.cpp
+++ b/test/gtest_processor_odom_2d.cpp
@@ -392,9 +392,9 @@ TEST(ProcessorOdom2d, KF_callback)
for (int n = 1; n <= N; n++)
{
t += dt;
- WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).vector("PO").transpose());
+ WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t, "PO").vector("PO").transpose());
WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- ASSERT_POSE2d_APPROX(problem->getState(t).vector("PO"), integrated_pose_vector[n], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(t, "PO").vector("PO"), integrated_pose_vector[n], 1e-6);
}
}
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 0c33991a6652b8c82fde2cab790cf974642b7f15..72505e093bbd5c59e696f7bc286ea5d00a7d3397 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -164,14 +164,12 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
{
- FrameBasePtr frm = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 0, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr frm = problem->emplaceFrame(0, problem->stateZero("P"));
CaptureBasePtr cap = CaptureBase::emplace<CaptureVoid>(frm, 0, sensor);
FeatureBasePtr ftr =
FeatureBase::emplace<FeatureBase>(cap, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
- FrameBasePtr frm_other = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 1, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr frm_other = problem->emplaceFrame(1, problem->stateZero("P"));
CaptureBasePtr cap_other = CaptureBase::emplace<CaptureVoid>(frm_other, 1, sensor);
FeatureBasePtr ftr_other = FeatureBase::emplace<FeatureBase>(
cap_other, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
@@ -193,14 +191,12 @@ TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors)
{
// Put a capture on last_ptr_
- FrameBasePtr last_frm = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 0, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr last_frm = problem->emplaceFrame(0, problem->stateZero("P"));
CaptureBasePtr last_cap = CaptureBase::emplace<CaptureVoid>(last_frm, 0, sensor);
processor->setLast(last_cap);
// Put a capture on incoming_ptr_
- FrameBasePtr inc_frm = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 1, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr inc_frm = problem->emplaceFrame(1, problem->stateZero("P"));
CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor);
processor->setInc(inc_cap);
@@ -283,6 +279,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_EQ(processor->getLast()->getFeatureList().size(), processor->getParams()["max_new_features"].as<int>() - 3);
ASSERT_TRUE(problem->check(0));
+ problem->print(4, 1, 0, 0);
+
// 5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
WOLF_INFO("Fifth time...");
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index d8d508173f496238eac28740e89a2473012758e9..e6ae208475b3ff3eca0d4c01505c169ecef8cd56 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -209,8 +209,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
{
- FrameBasePtr frm = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 0, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr frm = problem->emplaceFrame(0, problem->stateZero("P"));
CaptureBasePtr cap = CaptureBase::emplace<CaptureVoid>(frm, 0, sensor);
FeatureBasePtr ftr =
FeatureBase::emplace<FeatureBase>(cap, "DUMMY FEATURE", Eigen::Vector1d::Ones(), Eigen::MatrixXd::Ones(1, 1));
@@ -233,14 +232,12 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
TEST_F(ProcessorTrackerLandmarkDummyTest, establishFactors)
{
// Put a capture on last_ptr_
- FrameBasePtr last_frm = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 0, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr last_frm = problem->emplaceFrame(0, problem->stateZero("P"));
CaptureBasePtr last_cap = CaptureBase::emplace<CaptureVoid>(last_frm, 0, sensor);
processor->setLast(last_cap);
// Put a capture on incoming_ptr_
- FrameBasePtr inc_frm = FrameBase::emplace<FrameBase>(
- problem->getTrajectory(), 1, problem->getFrameTypes("P"), problem->stateZero("P"));
+ FrameBasePtr inc_frm = problem->emplaceFrame(1, problem->stateZero("P"));
CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor);
processor->setInc(inc_cap);
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index 772ed0c8bcb30f49ff73c31021b899bd9b0a0aa7..0fb4ef3d889df64228d5dae599de5fe366e9a3f9 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -1170,17 +1170,17 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
capture_bl->setData(data);
capture_bl->process();
- WOLF_INFO("state: ", problem->getState().vector("PO").transpose());
+ WOLF_INFO("state: ", problem->getState("PO").vector("PO").transpose());
ASSERT_POSE3d_APPROX(
- problem->getState().vector("PO").transpose(), problem_bl->getState().vector("PO").transpose(), 1e-12);
+ problem->getState("PO").vector("PO").transpose(), problem_bl->getState("PO").vector("PO").transpose(), 1e-12);
// Solve
solver->solve();
solver_bl->solve();
ASSERT_POSE3d_APPROX(
- problem->getState().vector("PO").transpose(), problem_bl->getState().vector("PO").transpose(), 1e-12);
+ problem->getState("PO").vector("PO").transpose(), problem_bl->getState("PO").vector("PO").transpose(), 1e-12);
ASSERT_TRUE(problem->check());
}
diff --git a/test/yaml/processor_landmark_external.yaml b/test/yaml/processor_landmark_external.yaml
index b70dcc32f26786e0acdb2a313b8964523100ada1..da0b76f89cef744152c5ee5b9c9a4ff2948c18e6 100644
--- a/test/yaml/processor_landmark_external.yaml
+++ b/test/yaml/processor_landmark_external.yaml
@@ -9,6 +9,8 @@ keyframe_vote:
min_features_for_keyframe: 1
new_features_for_keyframe: 1000
+dimension: 3
+
time_tolerance: 0.05
apply_loss_function: false
max_new_features: -1