diff --git a/CMakeLists.txt b/CMakeLists.txt
index 618b7bfa941ed63f724e8f67a559e151a8ab4351..97718b5cd9bae711a7f64d8be64dce06085aa749 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -314,6 +314,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
+ src/state_block/state_block_derived.cpp
src/state_block/state_composite.cpp
)
SET(SRCS_TRAJECTORY
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 29a27f5d9c1662a6e6ab7bd4c9d7c02515476c4e..707bb45b02d2880cf461bd161d3f3137f9897737 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
// Ceres wrapper
SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);
@@ -147,8 +147,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
// store
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index bc8982ca2ce696719aab35105f059639d6c72f23..ed612ffcad7f3e1299f8e02d434c61969705b718 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -84,7 +84,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
- SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(Eigen::VectorXd::Zero(1)), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
Eigen::SparseMatrix<double> Lambda(0,0);
@@ -151,8 +151,8 @@ int main(int argc, char** argv)
bNum.clear();
// add frame to problem
- FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_full = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
+ FrameBasePtr vertex_frame_ptr_prun = new FrameBase(TimeStamp(0), std::make_shared<StatePoint2d>(vertex_pose.head(2)), std::make_shared<StateAngle>(vertex_pose.tail(1)));
wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
index c2c11187eedd016c49116f84ee14141067d7fa47..2eee6e3b8d43638a010b7959d700b7d1b4fa05a0 100644
--- a/demos/hello_wolf/landmark_point_2d.cpp
+++ b/demos/hello_wolf/landmark_point_2d.cpp
@@ -27,12 +27,13 @@
*/
#include "landmark_point_2d.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
- LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
+ LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
{
setId(_id);
}
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 7073082e34be71b45e60b7671211e1617432a124..affd707e29a7eacadee870b584ff586920049b17 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -28,6 +28,7 @@
#include "sensor_range_bearing.h"
#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
@@ -36,7 +37,7 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
SensorBase("SensorRangeBearing",
- std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
+ std::make_shared<StatePoint2d>(_extrinsics.head<2>(), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
nullptr,
_noise_std)
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index e634dac6f2de62d79296525dd6ca2631b4041108..d85e2cdebeacd0bbc1d39220cacd562f018e291b 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -181,10 +181,10 @@ int main(int argc, char *argv[])
Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(std::make_shared<StateBlock>(gps_pose.head(2)), std::make_shared<StateBlock>(gps_pose.tail(1)), gps_std);
- SensorLaser2D laser_1_sensor(std::make_shared<StateBlock>(laser_1_pose.head(2)), std::make_shared<StateBlock>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
- SensorLaser2D laser_2_sensor(std::make_shared<StateBlock>(laser_2_pose.head(2)), std::make_shared<StateBlock>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), std::make_shared<StateAngle>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(std::make_shared<StatePoint2d>(laser_1_pose.head(2)), std::make_shared<StateAngle>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
+ SensorLaser2D laser_2_sensor(std::make_shared<StatePoint2d>(laser_2_pose.head(2)), std::make_shared<StateAngle>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
// Initial pose
pose_odom << 2, 8, 0;
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index c440584c0539625d93b4fa4bc5bbafaa1cad1461..5c90da7b1c297d89b9db00364008e696e7316cfa 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -136,30 +136,19 @@ inline std::string FactoryStateBlock::getClass() const
return "FactoryStateBlock";
}
-template<>
-inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen::VectorXd& _state, bool _fixed)
-{
- typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
-
- if (creator_callback_it == get().callbacks_.end())
- // not found: return StateBlock base
- return std::make_shared<StateBlock>(_state, _fixed);
-
- // Invoke the creation function
- return (creator_callback_it->second)(_state, _fixed);
-}
-
-#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
- namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
- FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); } \
-
-#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
- namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
- FactoryStateBlock::registerCreator(#Key, StateBlockType::create); } \
-
-
-
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED StateBlockType##Registered = \
+ FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \
+ }
+#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
+ namespace \
+ { \
+ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
+ FactoryStateBlock::registerCreator(#Key, StateBlockType::create); \
+ }
}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 90ba34708a5ffc71a3c8b2f38f14d0a039c76c32..40ddcebbe520cc05e3fcb18dd47683f2cd10d25b 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -82,10 +82,6 @@ class HasStateBlocks
template<typename SB, typename ... Args>
std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
- // Emplace base state blocks.
- template<typename ... Args>
- StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
-
// Register/remove state blocks to/from wolf::Problem
virtual void registerNewStateBlocks(ProblemPtr _problem);
virtual void removeStateBlocks(ProblemPtr _problem);
@@ -186,17 +182,6 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_typ
return sb;
}
-template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
-{
- assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
-
- addStateBlock(_sb_type, sb, _problem);
-
- return sb;
-}
-
inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 7e97022b4618dbdbd6896dcfc4261c936fd76cb8..30a1c4dc3c5b16d0a5ddd238d7f3569e5abc8987 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -39,6 +39,7 @@ class StateAngle : public StateBlock
{
public:
StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true);
+ StateAngle(const Vector1d& _angle, bool _fixed = false, bool _transformable = true);
~StateAngle() override;
@@ -53,6 +54,12 @@ inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
state_(0) = pi2pi(_angle);
}
+inline StateAngle::StateAngle(const Vector1d& _angle, bool _fixed, bool _transformable) :
+ StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
+{
+ state_(0) = pi2pi(_angle(0));
+}
+
inline StateAngle::~StateAngle()
{
//
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 2572cc9b64a729f36496ac403fb18700846dc43c..2dbab85a741ddbe489f5e369c1a7cf2d5df12d9e 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -182,16 +182,18 @@ public:
bool isTransformable() const {
return transformable_;
- };
+ }
- virtual void transform(const VectorComposite& _transformation) { };
+ void setTransformable(bool _trf = true) {transformable_ = _trf;}
+
+ void setNonTransformable() {transformable_ = false;}
+
+ virtual void transform(const VectorComposite& _transformation) = 0;
void plus(const Eigen::VectorXd& _dv);
bool isValid(double tolerance = Constants::EPS);
- static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
-
};
@@ -346,10 +348,6 @@ inline Eigen::VectorXd StateBlock::zero() const
return identity();
}
-inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
-{
- return std::make_shared<StateBlock>(_state, _fixed);
-}
inline bool StateBlock::isValid(double tolerance)
{
return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
index 541b79a19453f34cedf13343de1caee2d2fca74b..5d1377520098ae65e5a43b361f48d6061266de37 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/include/core/state_block/state_block_derived.h
@@ -29,11 +29,11 @@ namespace wolf
/**
* @brief State block for general parameters
- *
+ *
* This state block cannot be transformed with a geometric frame transformation.
- *
+ *
* This state block cannot have a local parametrization
- *
+ *
* @tparam size the size of the state block's vector
*/
template <size_t size>
@@ -45,6 +45,7 @@ class StateParams : public StateBlock
{
// non transformable! --> do nothing
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
typedef StateParams<1> StateParams1;
@@ -60,11 +61,11 @@ typedef StateParams<10> StateParams10;
/**
* @brief State block for general 2D points and positions
- *
+ *
* This state block can be transformed with a geometric frame transformation.
* This is controlled at construction time by parameter _transformable,
* or later via setTransformable(bool).
- *
+ *
* This state block cannot have a local parametrization
*/
class StatePoint2d : public StateBlock
@@ -78,16 +79,17 @@ class StatePoint2d : public StateBlock
{
if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
/**
* @brief State block for general 2D vectors
- *
+ *
* This state block can be transformed with a geometric frame transformation.
* This is controlled at construction time by parameter _transformable,
* or later via setTransformable(bool).
* Being a vector, the geometric frame transformation will rotate the vector.
- *
+ *
* This state block cannot have a local parametrization
*/
class StateVector2d : public StateBlock
@@ -101,15 +103,16 @@ class StateVector2d : public StateBlock
{
if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
/**
* @brief State block for general 3D points and positions
- *
+ *
* This state block can be transformed with a geometric frame transformation.
* This is controlled at construction time by parameter _transformable,
* or later via setTransformable(bool).
- *
+ *
* This state block cannot have a local parametrization
*/
class StatePoint3d : public StateBlock
@@ -124,16 +127,17 @@ class StatePoint3d : public StateBlock
if (transformable_)
setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
/**
* @brief State block for general 3D vectors
- *
+ *
* This state block can be transformed with a geometric frame transformation.
* This is controlled at construction time by parameter _transformable,
* or later via setTransformable(bool).
* Being a vector, the geometric frame transformation will rotate the vector.
- *
+ *
* This state block cannot have a local parametrization
*/
class StateVector3d : public StateBlock
@@ -147,8 +151,10 @@ class StateVector3d : public StateBlock
{
if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
+
} // namespace wolf
#endif // STATE_BLOCK_DERIVED_H_
\ No newline at end of file
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index dcfa71155597334de8ff4a3daedbb760314f1856..3a2e72aa39e3a4ade898355452295b02673b0713 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -294,10 +294,6 @@ class StateBlockComposite
template<typename SB, typename ... Args>
std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
- // Emplace base state blocks.
- template<typename ... Args>
- inline StateBlockPtr emplace(const char& _sb_type, Args&&... _args_of_base_state_block_constructor);
-
unsigned int remove(const char& _sb_type);
bool has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool has(const StateBlockPtr& _sb) const;
@@ -367,18 +363,6 @@ inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_ty
return sb;
}
-template<typename ... Args>
-inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type,
- Args &&... _args_of_base_state_block_constructor)
-{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
-
- add(_sb_type, sb);
-
- return sb;
-}
-
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index e7acd4f0d90ef0883eea1af949928b0458e25a44..bbda7dcbadebe059e619b72602be1eeff5a33254 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -23,9 +23,10 @@
#include "core/landmark/landmark_base.h"
#include "core/factor/factor_base.h"
#include "core/map/map_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.h"
#include "core/common/factory.h"
#include "core/yaml/yaml_conversion.h"
@@ -216,7 +217,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
Eigen::VectorXd pos = _node["position"] .as< Eigen::VectorXd >();
bool pos_fixed = _node["position fixed"] .as< bool >();
- StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
+ StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
StateBlockPtr ori_sb = nullptr;
if (_node["orientation"])
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index b0e08b4b59240b359457565c95071d2b7c1385e6..623066614b764486e4cf568e892d8fed95c90cbf 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -90,8 +90,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -119,8 +120,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Issue KF callback with new KF
getProblem()->keyFrameCallback(keyframe, shared_from_this());
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -163,8 +165,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Establish factors
establishFactors();
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = keyframe;
@@ -199,8 +202,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Establish factors
establishFactors();
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
@@ -237,8 +241,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_ ->link(frame);
- // Update pointers
+ // Reset this
resetDerived();
+ // Update pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
@@ -249,9 +254,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
// We do not create a KF
- // Advance this
- advanceDerived();
-
// Replace last frame for a new NON KEY frame in incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
@@ -259,6 +261,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(frame);
last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
+ // Advance this
+ advanceDerived();
// Update pointers
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 996d48263c113555ba055422153bf10459bfb564..21d9622c3366f33eea12ce94ef9a332d14cebc64 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -56,14 +56,20 @@ SensorBase::SensorBase(const std::string& _type,
noise_cov_.setZero();
if (_p_ptr)
+ {
+ _p_ptr->setNonTransformable();
addStateBlock('P', _p_ptr, _p_dyn);
+ }
if (_o_ptr)
+ {
+ _o_ptr->setNonTransformable();
addStateBlock('O', _o_ptr, _o_dyn);
-
+ }
if (_intr_ptr)
+ {
addStateBlock('I', _intr_ptr, _intr_dyn);
-
+ }
}
SensorBase::SensorBase(const std::string& _type,
@@ -87,11 +93,15 @@ SensorBase::SensorBase(const std::string& _type,
setNoiseStd(_noise_std);
if (_p_ptr)
+ {
+ _p_ptr->setNonTransformable();
addStateBlock('P', _p_ptr, _p_dyn);
-
+ }
if (_o_ptr)
+ {
+ _o_ptr->setNonTransformable();
addStateBlock('O', _o_ptr, _o_dyn);
-
+ }
if (_intr_ptr)
addStateBlock('I', _intr_ptr, _intr_dyn);
}
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index f5c4e6aa21c79b2e2d3b0875fdefb8c0108cb51a..256def47937b9cd42e94b72b2169e4a3e72fc897 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -27,21 +27,23 @@
*/
#include "core/sensor/sensor_diff_drive.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
namespace wolf
{
-SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
- ParamsSensorDiffDrivePtr _intrinsics) :
- SensorBase("SensorDiffDrive",
- std::make_shared<StateBlock>(_extrinsics.head(2), true),
- std::make_shared<StateAngle>(_extrinsics(2), true),
- std::make_shared<StateBlock>(3, false, nullptr, false), 2),
- params_diff_drive_(_intrinsics)
+SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics, ParamsSensorDiffDrivePtr _intrinsics)
+ : SensorBase("SensorDiffDrive",
+ std::make_shared<StatePoint2d>(_extrinsics.head(2), true),
+ std::make_shared<StateAngle>(_extrinsics(2), true),
+ std::make_shared<StateParams3>(
+ Vector3d(_intrinsics->radius_left, _intrinsics->radius_right, _intrinsics->wheel_separation),
+ false),
+ 2),
+ params_diff_drive_(_intrinsics)
{
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
- getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
unfixIntrinsics();
if(params_diff_drive_->set_intrinsics_prior)
@@ -53,7 +55,6 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
setNoiseStd(noise_sigma);
-
}
SensorDiffDrive::~SensorDiffDrive()
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index 711545f66b48b99fcfaf891954db3ddba7b56d7a..9a4a9d1f1f41323e3209da09afcf968ef3dc69b4 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -20,17 +20,19 @@
//
//--------LICENSE_END--------
#include "core/sensor/sensor_odom_2d.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
namespace wolf {
SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) :
- SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+ SensorBase("SensorOdom2d", std::make_shared<StatePoint2d>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_rot_to_rot_(_intrinsics.k_rot_to_rot)
{
assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
+ getStateBlock('P')->setNonTransformable();
+ getStateBlock('O')->setNonTransformable();
//
}
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index b44561ff402bef4971b211cfa2eae43724457fc3..112dc72eb98a50dfdf9f34bbceba7d3994078758 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -28,14 +28,14 @@
#include "core/sensor/sensor_odom_3d.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/math/rotations.h"
namespace wolf {
SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) :
- SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ SensorBase("SensorOdom3d", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_disp_to_rot_(_intrinsics.k_disp_to_rot),
k_rot_to_rot_(_intrinsics.k_rot_to_rot),
@@ -43,6 +43,8 @@ SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSe
min_rot_var_(_intrinsics.min_rot_var)
{
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
+ getStateBlock('P')->setNonTransformable();
+ getStateBlock('O')->setNonTransformable();
noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
setNoiseCov(noise_cov_); // sets also noise_std_
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 87f2880b8f3e58e1898470572e630375bd792d8c..0d12027c9d6b880a98931b4b0e3939a3f2150f0d 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.cpp
@@ -28,14 +28,14 @@
#include "core/sensor/sensor_pose.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/math/rotations.h"
namespace wolf {
SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
- SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+ SensorBase("SensorPose", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
std_p_(_intrinsics.std_p),
std_o_(_intrinsics.std_o)
{
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 513d6d4627627a029e3ebe61bab550497d369634..5fe479daf2fdab0245b223f76075f05d7e2e4001 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -74,7 +74,6 @@ void StateBlock::plus(const Eigen::VectorXd &_dv)
#include "core/state_block/state_homogeneous_3d.h"
namespace wolf{
-WOLF_REGISTER_STATEBLOCK(StateBlock);
WOLF_REGISTER_STATEBLOCK(StateQuaternion);
WOLF_REGISTER_STATEBLOCK(StateAngle);
WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5bc71bbc0252beb8c951a6bc37d5f5a66884ea0
--- /dev/null
+++ b/src/state_block/state_block_derived.cpp
@@ -0,0 +1,108 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/factory_state_block.h"
+
+namespace wolf
+{
+StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Point2d.");
+}
+
+StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Point3d.");
+}
+
+StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Vector2d.");
+}
+
+StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Vector3d.");
+}
+
+StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2)
+ return std::make_shared<StatePoint2d>(_state, _fixed);
+ else if (_state.size() == 3)
+ return std::make_shared<StatePoint3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Point.");
+}
+
+StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 2)
+ return std::make_shared<StateVector2d>(_state, _fixed);
+ else if (_state.size() == 3)
+ return std::make_shared<StateVector3d>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Vector.");
+}
+
+template <size_t size>
+StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for Params.");
+}
+
+namespace
+{
+const bool __attribute__((used)) create_pointRegisteredWithP =
+ FactoryStateBlock::registerCreator("P", wolf::create_point);
+const bool __attribute__((used)) create_vectorRegisteredWithP =
+ FactoryStateBlock::registerCreator("V", wolf::create_vector);
+} // namespace
+
+WOLF_REGISTER_STATEBLOCK(StatePoint2d);
+WOLF_REGISTER_STATEBLOCK(StateVector2d);
+WOLF_REGISTER_STATEBLOCK(StatePoint3d);
+WOLF_REGISTER_STATEBLOCK(StateVector3d);
+
+WOLF_REGISTER_STATEBLOCK(StateParams1);
+WOLF_REGISTER_STATEBLOCK(StateParams2);
+WOLF_REGISTER_STATEBLOCK(StateParams3);
+WOLF_REGISTER_STATEBLOCK(StateParams4);
+WOLF_REGISTER_STATEBLOCK(StateParams5);
+WOLF_REGISTER_STATEBLOCK(StateParams6);
+WOLF_REGISTER_STATEBLOCK(StateParams7);
+WOLF_REGISTER_STATEBLOCK(StateParams8);
+WOLF_REGISTER_STATEBLOCK(StateParams9);
+WOLF_REGISTER_STATEBLOCK(StateParams10);
+
+} // namespace wolf
\ No newline at end of file
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 54525c8682b4c80d95e3e38b2ef34784e4a84cfe..b72b6a3e677d29889cf80a14a81fc32d163944b2 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -29,6 +29,8 @@
#include "processor_tracker_landmark_dummy.h"
#include "core/landmark/landmark_base.h"
#include "factor_landmark_dummy.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
namespace wolf
{
@@ -124,8 +126,8 @@ LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _f
//std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
"LandmarkBase",
- std::make_shared<StateBlock>(2),
- std::make_shared<StateBlock>(1));
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(0));
}
FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index aa36078108a936109484bd4a8d761c258105d236..aaf3aa4fbaca881a24b520ecb85795d5fb0f9b7d 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -29,6 +29,7 @@
#include "core/utils/utils_gtest.h"
#include "core/capture/capture_base.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -58,9 +59,9 @@ TEST(CaptureBase, ConstructorWithSensor)
TEST(CaptureBase, Static_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", p, o, i, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
@@ -77,10 +78,10 @@ TEST(CaptureBase, Static_sensor_params)
TEST(CaptureBase, Dynamic_sensor_params)
{
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(2));
- SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
+ SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StatePoint2d>(Vector2d::Zero()), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Vector2d::Zero()), 2, true, true, true)); // last 3 'true' mark dynamic
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 0aa294035d9f712b706883636e46cd1e4ae62593..956f6eb4e76f44b333ea5253a0023c0ec4fadac5 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -28,7 +28,8 @@
#include "core/utils/utils_gtest.h"
-
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -64,7 +65,7 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
@@ -89,7 +90,7 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(0,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -103,7 +104,7 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -121,7 +122,7 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -141,7 +142,7 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -163,7 +164,7 @@ TEST(Emplace, ReturnDerived)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 7a1a330916e5ba4a5e7c28f784bd6bfd21278da7..a2f8dfaf9f6bfe3a08ae392937b186377476e28e 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -30,6 +30,7 @@
#include "dummy/factor_dummy_zero_1.h"
#include "dummy/factor_dummy_zero_12.h"
+#include "core/state_block/state_block_derived.h"
#include "core/factor/factor_relative_pose_2d.h"
#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -44,7 +45,7 @@ using namespace Eigen;
TEST(FactorAutodiff, AutodiffDummy1)
{
- StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ StateBlockPtr sb = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
@@ -61,18 +62,18 @@ TEST(FactorAutodiff, AutodiffDummy1)
TEST(FactorAutodiff, AutodiffDummy12)
{
- StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
- StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
- StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
- StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
- StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
- StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
- StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
- StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
- StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
- StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
- StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
- StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+ StateBlockPtr sb1 = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateParams2>(Eigen::Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateParams3>(Eigen::Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateParams4>(Eigen::Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateParams5>(Eigen::Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateParams6>(Eigen::Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateParams7>(Eigen::Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateParams8>(Eigen::Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateParams9>(Eigen::Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateParams10>(Eigen::Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateParams<11>>(Eigen::VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateParams<12>>(Eigen::VectorXd::Random(12));
std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
std::vector<Eigen::MatrixXd> J;
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 0f89a11003bd0e297ed6b3df283b6aba536ca5ea..f4755e11139f07ba42d537e0fc4e070c300cf9a3 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -25,6 +25,7 @@
#include "core/common/wolf.h"
#include "core/utils/logging.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_pose.h"
@@ -117,9 +118,9 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
// Create sensor to be able to initialize (a camera for instance)
S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose",
- std::make_shared<StateBlock>(pos_camera, true),
+ std::make_shared<StatePoint3d>(pos_camera, true),
std::make_shared<StateQuaternion>(vquat_camera, true),
- std::make_shared<StateBlock>(Vector4d::Zero(), false), // fixed
+ std::make_shared<StateParams4>(Vector4d::Zero(), false), // fixed
Vector1d::Zero());
// ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
@@ -142,7 +143,7 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
- std::make_shared<StateBlock>(pos_landmark),
+ std::make_shared<StatePoint3d>(pos_landmark),
std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
}
};
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 6332a69928a558cfdede9125a73fd7ab7f835338..9822acd60d37ae94d4b73ef45a30ab5207eccbd0 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -31,79 +31,21 @@
#include "core/common/wolf.h"
#include "core/state_block/factory_state_block.h"
#include "core/sensor/factory_sensor.h"
-
+#include "core/state_block/state_block_derived.h"
using namespace wolf;
-/*
-// You may use this to make some methods of Foo public
-WOLF_PTR_TYPEDEFS(FooPublic);
-class FooPublic : public Foo
-{
- // You may use this to make some methods of Foo public
-}
-
-class TestInit : public testing::Test
-{
- public:
- // You may use this to initialize stuff
- // Combine it with TEST_F(FooTest, testName) { }
- void SetUp() override
- {
- // Init all you want here
- // e.g. FooPublic foo;
- }
- void TearDown() override {} // you can delete this if unused
-};
-
-TEST_F(TestInit, testName)
-{
- // Use class TestInit
-}
-*/
-
-//TEST(FactoryStateBlock, get_getClass)
-//{
-// const auto& f = FactoryStateBlock::get();
-//
-// const std::string& s = f.getClass();
-//
-// ASSERT_EQ(s, "FactoryStateBlock");
-//}
-TEST(FactoryStateBlock, creator_default)
+TEST(FactoryStateBlock, creator_non_registered_key)
{
- auto sbp = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
- auto sbv = FactoryStateBlock::create("V", Eigen::Vector2d(4,5), false);
- auto sbw = FactoryStateBlock::create("W", Eigen::Vector1d(6), false);
-
- ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
-
- ASSERT_FALSE(sbp->hasLocalParametrization());
- ASSERT_FALSE(sbv->hasLocalParametrization());
- ASSERT_FALSE(sbw->hasLocalParametrization());
+ // non registered -> throw
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("A", Eigen::Vector1d(6), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_StateBlock)
{
- auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false);
- auto sbv = FactoryStateBlock::create("StateBlock", Eigen::Vector2d(4,5), true);
- auto sbw = FactoryStateBlock::create("StateBlock", Eigen::Vector1d(6), false);
-
- ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
- ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
-
- ASSERT_FALSE(sbp->isFixed());
- ASSERT_TRUE (sbv->isFixed());
- ASSERT_FALSE(sbw->isFixed());
-
- ASSERT_FALSE(sbp->hasLocalParametrization());
- ASSERT_FALSE(sbv->hasLocalParametrization());
- ASSERT_FALSE(sbw->hasLocalParametrization());
+ ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false), std::runtime_error);
}
TEST(FactoryStateBlock, creator_Quaternion)
@@ -165,6 +107,109 @@ TEST(FactoryStateBlock, creator_O_is_wrong_size)
ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
}
+TEST(FactoryStateBlock, creator_Point)
+{
+ auto sba = FactoryStateBlock::create("StatePoint2d", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("StatePoint3d", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_P)
+{
+ auto sba = FactoryStateBlock::create("P", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_Vector)
+{
+ auto sba = FactoryStateBlock::create("StateVector2d", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("StateVector3d", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_V)
+{
+ auto sba = FactoryStateBlock::create("V", Eigen::Vector2d(1,2), false);
+
+ ASSERT_EQ(sba->getSize() , 2);
+ ASSERT_EQ(sba->getLocalSize(), 2);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ sba = FactoryStateBlock::create("V", Eigen::Vector3d(1,2,3), false);
+
+ ASSERT_EQ(sba->getSize() , 3);
+ ASSERT_EQ(sba->getLocalSize(), 3);
+ ASSERT_FALSE(sba->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_Params)
+{
+ auto sb1 = FactoryStateBlock::create("StateParams1", Eigen::Vector1d::Ones(), false);
+ auto sb2 = FactoryStateBlock::create("StateParams2", Eigen::Vector2d::Ones(), false);
+ auto sb3 = FactoryStateBlock::create("StateParams3", Eigen::Vector3d::Ones(), false);
+ auto sb4 = FactoryStateBlock::create("StateParams4", Eigen::Vector4d::Ones(), false);
+ auto sb5 = FactoryStateBlock::create("StateParams5", Eigen::Vector5d::Ones(), false);
+ auto sb6 = FactoryStateBlock::create("StateParams6", Eigen::Vector6d::Ones(), false);
+ auto sb7 = FactoryStateBlock::create("StateParams7", Eigen::Vector7d::Ones(), false);
+ auto sb8 = FactoryStateBlock::create("StateParams8", Eigen::Vector8d::Ones(), false);
+ auto sb9 = FactoryStateBlock::create("StateParams9", Eigen::Vector9d::Ones(), false);
+ auto sb10 = FactoryStateBlock::create("StateParams10", Eigen::Vector10d::Ones(), false);
+
+ ASSERT_EQ(sb1->getSize(), 1);
+ ASSERT_EQ(sb2->getSize(), 2);
+ ASSERT_EQ(sb3->getSize(), 3);
+ ASSERT_EQ(sb4->getSize(), 4);
+ ASSERT_EQ(sb5->getSize(), 5);
+ ASSERT_EQ(sb6->getSize(), 6);
+ ASSERT_EQ(sb7->getSize(), 7);
+ ASSERT_EQ(sb8->getSize(), 8);
+ ASSERT_EQ(sb9->getSize(), 9);
+ ASSERT_EQ(sb10->getSize(), 10);
+
+ ASSERT_EQ(sb1->getLocalSize(), 1);
+ ASSERT_FALSE(sb1->hasLocalParametrization());
+
+ // fails
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index eb61f798d41b2aec792b864dffb39cb1cdb7d098..64dbd6d85484e2ac6ed270b470f674107d146cb6 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -33,6 +33,8 @@
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_motion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
@@ -42,7 +44,7 @@ using namespace wolf;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
// getters
ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -55,7 +57,7 @@ TEST(FrameBase, GettersAndSetters)
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -65,7 +67,7 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
@@ -85,8 +87,8 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
@@ -139,16 +141,16 @@ TEST(FrameBase, Frames)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
+ auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
// tree is now consistent
ASSERT_TRUE(P->check(0));
@@ -396,8 +398,8 @@ TEST(FrameBase, Frames)
TEST(FrameBase, GetSetState)
{
// Create PQV_3d state blocks
- StateBlockPtr sbp = make_shared<StateBlock>(3);
- StateBlockPtr sbv = make_shared<StateBlock>(3);
+ StateBlockPtr sbp = make_shared<StatePoint3d>(Vector3d::Zero());
+ StateBlockPtr sbv = make_shared<StateVector3d>(Vector3d::Zero());
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index e60bc875310148a2fc0a127884b4fb1b3b77ab6a..2b02be288217cca57703ab98b5b9c7a40f32d505 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -34,6 +34,8 @@
#include "core/sensor/sensor_base.h"
#include "core/landmark/landmark_base.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -55,12 +57,12 @@ class HasStateBlocksTest : public testing::Test
{
problem = Problem::create("POV", 3);
- sbp0 = std::make_shared<StateBlock>(3); // size 3
+ sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo0 = std::make_shared<StateQuaternion>();
- sbv0 = std::make_shared<StateBlock>(3); // size 3
- sbp1 = std::make_shared<StateBlock>(3); // size 3
+ sbv0 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
+ sbp1 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo1 = std::make_shared<StateQuaternion>();
- sbv1 = std::make_shared<StateBlock>(3); // size 3
+ sbv1 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 5c4b2b7893837316736ab7a69a49f1b08c169b9b..9ca7544ee6c9f28013f4037642de45504eca0b92 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -31,7 +31,8 @@
#include "core/problem/problem.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "core/yaml/yaml_conversion.h"
@@ -51,11 +52,11 @@ TEST(MapYaml, save_2d)
o2 << 2;
o3 << 3;
- StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
- StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
- StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
- StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
- StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
+ StateBlockPtr p1_sb = std::make_shared<StatePoint2d>(p1);
+ StateBlockPtr p2_sb = std::make_shared<StatePoint2d>(p2);
+ StateBlockPtr o2_sb = std::make_shared<StateAngle>(o2(0));
+ StateBlockPtr p3_sb = std::make_shared<StatePoint2d>(p3, true);
+ StateBlockPtr o3_sb = std::make_shared<StateAngle>(o3(0), true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
@@ -174,10 +175,10 @@ TEST(MapYaml, save_3d)
q2 << 0, 1, 0, 0;
q3 << 0, 0, 1, 0;
- auto p1_sb = std::make_shared<StateBlock>(p1);
- auto p2_sb = std::make_shared<StateBlock>(p2);
+ auto p1_sb = std::make_shared<StatePoint2d>(p1);
+ auto p2_sb = std::make_shared<StatePoint2d>(p2);
auto q2_sb = std::make_shared<StateQuaternion>(q2);
- auto p3_sb = std::make_shared<StateBlock>(p3, true);
+ auto p3_sb = std::make_shared<StatePoint2d>(p3, true);
auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p1_sb);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 522246910cc6bd3944f6924a832fbf028cb2b75c..88f76ed653605423a85eedd7ac2cba8ab4815605 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -43,6 +43,8 @@
#include "core/capture/capture_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
@@ -388,7 +390,7 @@ TEST(Problem, perturb)
for (int j = 0; j< 2 ; j++)
{
- auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto sb = std::make_shared<StatePoint3d>(Vector3d(1,1,1));
auto cap = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
}
i++;
@@ -396,8 +398,8 @@ TEST(Problem, perturb)
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
- auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateAngle>(3);
auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
if (l==0) L->fix();
}
@@ -479,7 +481,7 @@ TEST(Problem, check)
for (int j = 0; j< 2 ; j++)
{
- auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto sb = std::make_shared<StatePoint3d>(Vector3d(1,1,1));
auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
for (int k = 0; k<2; k++)
@@ -495,8 +497,8 @@ TEST(Problem, check)
for (int l = 0; l < 2; l++)
{
- auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
- auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto sb1 = std::make_shared<StatePoint2d>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateAngle>(3);
auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
if (l==0) L->fix();
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 89341815653b03ed0d189b7d91c8e1c759f9bd9f..5840118ff7c4b4ee12ed7413f2ae3e64e896ffd0 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -36,6 +36,8 @@
#include "core/capture/capture_void.h"
#include "core/problem/problem.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
// STL
#include <iterator>
@@ -104,9 +106,9 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorTrackerDummy",
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+ std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateAngle>(0),
+ std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
proc_trk_params->time_tolerance = dt/2;
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index b2bcb1990ccd266756c7f106478b79aadff3cc88..4f7500423edb371503bafc23169b12775ff3d5c5 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -20,15 +20,17 @@
//
//--------LICENSE_END--------
+#include "core/processor/processor_fixed_wing_model.h"
+
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
// STL
#include <iterator>
#include <iostream>
-#include "../include/core/processor/processor_fixed_wing_model.h"
using namespace wolf;
using namespace Eigen;
@@ -52,7 +54,7 @@ class ProcessorFixWingModelTest : public testing::Test
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
- std::make_shared<StateBlock>(Vector3d::Zero()),
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
nullptr,
2);
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 9623ecbaa1dd1560650c8cb9e96c044c87d18e9b..57a0d4cbdb0dc23509fb4ef9f7f691d14c36cbea 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -24,6 +24,8 @@
#include "core/problem/problem.h"
#include "core/capture/capture_base.h"
#include "core/factor/factor_relative_pose_2d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include "dummy/processor_loop_closure_dummy.h"
@@ -47,8 +49,8 @@ class ProcessorLoopClosureTest : public testing::Test
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
- std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()),
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(0),
nullptr,
2);
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 4ed3c0846017e15fe5882607ff1807fbbbbc93f5..69ed41d60e75af3346156192b145dff16c15bf31 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -25,6 +25,8 @@
#include "core/sensor/factory_sensor.h"
#include "dummy/processor_tracker_landmark_dummy.h"
#include "core/capture/capture_void.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace wolf;
@@ -285,8 +287,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
Eigen::Vector1d::Ones(),
Eigen::MatrixXd::Ones(1, 1)));
LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("LandmarkBase",
- std::make_shared<StateBlock>(1),
- std::make_shared<StateBlock>(1)));
+ std::make_shared<StateParams1>(Vector1d(0)),
+ std::make_shared<StateAngle>(0)));
FactorBasePtr fac = processor->callEmplaceFactor(ftr, lmk);
ASSERT_EQ(fac->getFeature(),ftr);
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index 89bf93042391ae90c0f12560922795029ff6d704..3fb6f4abadd3528878978b95764fc1359aaf9148 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -30,7 +30,7 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
@@ -57,7 +57,7 @@ using namespace Eigen;
StateBlockPtr createStateBlock()
{
Vector4d state; state << 1, 0, 0, 0;
- return std::make_shared<StateBlock>(state);
+ return std::make_shared<StateParams4>(state);
}
FactorBasePtr createFactor(StateBlockPtr sb_ptr)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index e66342ba48d3ba7e2c6248e249e364540c1c9537..65056864243e2104071e4a5349961efb56d6b7aa 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -37,6 +37,8 @@
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
#include <iostream>
#include <thread>
@@ -52,7 +54,7 @@ using namespace Eigen;
StateBlockPtr createStateBlock()
{
Vector4d state; state << 1, 0, 0, 0;
- return std::make_shared<StateBlock>(state);
+ return std::make_shared<StateParams4>(state);
}
FactorBasePtr createFactor(StateBlockPtr sb_ptr)
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index e1696bcc92f15f5f87a1aeed24fbed554e8cdbab..17ad54a8ce833aecb23ac1735b2d4c7c93439d59 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -40,10 +40,10 @@ using namespace Eigen;
using namespace std;
using namespace wolf;
-TEST(StateBlock, block_perturb)
+TEST(StateBlock, point_perturb)
{
- Vector3d x(10, 20, 30);
- StateBlock sb(x);
+ Vector3d x(10, 20, 30);
+ StatePoint3d sb(x);
sb.perturb(0.5);
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 74dfe643ab18b1658a774fa782fe22e0a9284a66..5631d5fddeb26014baeb2698817481a602239a5e 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -27,6 +27,7 @@
*/
#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/utils/utils_gtest.h"
@@ -46,11 +47,11 @@ class StateBlockCompositeInit : public testing::Test
void SetUp() override
{
- sbp = states.emplace('P', Vector3d(1,2,3));
- sbv = states.emplace('V', Vector3d(4,5,6));
+ sbp = states.emplace<StatePoint3d>('P', Vector3d(1,2,3));
+ sbv = states.emplace<StateVector3d>('V', Vector3d(4,5,6));
sbq = states.emplace<StateQuaternion>('Q', Vector4d(.5,.5,.5,.5));
- sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ sbx = std::make_shared<StateVector3d>(Vector3d(7,8,9));
}
};