diff --git a/.clang-format b/.clang-format
index d7ec3ad2d8fa7d63422d17a014db88614d844159..8f284dd854c7257bc8e4af2bb9e9a056acf81d6c 100644
--- a/.clang-format
+++ b/.clang-format
@@ -19,7 +19,7 @@ AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
-BinPackArguments: true
+BinPackArguments: false
BinPackParameters: false
BreakBeforeBraces: Custom
BraceWrapping:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8d17620362c9176e48b0a5f694359f5ba4ab1b89..6c1ea91516a75e5a5408335fa7c2bcbebf0c72cd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,7 +33,7 @@ include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+ set(CMAKE_CXX_STANDARD 14)
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
@@ -313,6 +313,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_quaternion.cpp
src/state_block/prior.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/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/problem/problem.h b/include/core/problem/problem.h
index 027b1b4e8c3e4418a9c702a4300272da046e5e91..bb3cb379ae80a582ec3ba90db10cc013487a5ce9 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -348,6 +348,7 @@ class Problem : public std::enable_shared_from_this<Problem>
// All branches -------------------------------------------
// perturb states
void perturb(double amplitude = 0.01);
+ // transform states
void transform(const VectorComposite& _transformation);
bool isTransformed() const;
void resetTransformed();
@@ -411,7 +412,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S, T:F, M:L ; 2: H:S:p, T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
- * \param constr_by: show factors pointing to F, f and L.
+ * \param constr_by : show factors pointing to F, C, f and L.
* \param metric : show metric info (status, time stamps, state vectors, measurements)
* \param state_blocks : show state blocks
*/
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index ab77f4afa6d7ad4e6abb1e243afbbc7a7dab945d..578a8b0acdbf2fcfeb9b8b26656c676774fb67c2 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -128,25 +128,17 @@ 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); \
+ }
}
\ No newline at end of file
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 5b936a73835aa5bd578b88b77462c091c9bbfbcf..4857055c7392fffe063a38c574a68aaeb67e555b 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -70,8 +70,9 @@ class HasStateBlocks
std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
- // Emplace base state blocks.
- StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, const Eigen::VectorXd& _state, bool _fixed);
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
virtual void registerNewStateBlocks(ProblemPtr _problem);
@@ -162,6 +163,17 @@ inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
return state_block_const_map_.erase(_sb_type);
}
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_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.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_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 ebe6b02c6c8c8e63e1d8f55836326f2eddd7d702..cce71edc7b2cd3d105ea228ef099ab6de3281d4f 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -31,6 +31,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;
@@ -45,6 +46,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 dea672e85aa339355318892368e6ce3efc3c9f5e..2f967122cc9db3c4b5928e75b7dbdfd655b3c8ab 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -180,16 +180,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);
-
};
@@ -344,10 +346,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 13453f63c5d1e99b9306f7cf549a1a9488e2fcd4..a4c34c448563e7126198f0218effe1a1d16ce0c8 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/include/core/state_block/state_block_derived.h
@@ -28,11 +28,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>
@@ -44,6 +44,7 @@ class StateParams : public StateBlock
{
// non transformable! --> do nothing
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
typedef StateParams<1> StateParams1;
@@ -59,11 +60,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
@@ -77,16 +78,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
@@ -100,15 +102,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
@@ -123,16 +126,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
@@ -146,6 +150,7 @@ class StateVector3d : public StateBlock
{
if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
}
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
-} // namespace wolf
\ No newline at end of file
+} // namespace wolf
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 2b18216ff1a1994b06b5bc6c8cf87928cf0038c1..83c07d4c43c396d33352bceb6f4e8a44b6e5e191 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -286,10 +286,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;
@@ -359,16 +355,4 @@ 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;
-}
-
}
\ No newline at end of file
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 266c0ef1925bf90725d317eb9eb17a4ccb1b3299..72b1834617074da8a53a89a1097d28d8cdfeebb3 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 "yaml-schema-cpp/yaml_conversion.hpp"
@@ -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_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 514769f5b94ec8934b462194e9c901b4deb64796..1819213b7cf468cd764092b8bf3db18339bf6033 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -27,6 +27,7 @@
*/
#include "core/sensor/sensor_diff_drive.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
namespace wolf
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 7e399e7d48e1af44d75c743c9c8168c5c748b8fc..c348c5109ac0f34a5ff90705428bc6a2c1a4427d 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -26,20 +26,6 @@
namespace wolf
{
-StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type,
- ProblemPtr _problem,
- const Eigen::VectorXd& _state,
- bool _fixed)
-{
- 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 = FactoryStateBlock::create(std::string(1,_sb_type), _state, _fixed);
-
- addStateBlock(_sb_type, sb, _problem);
-
- return sb;
-}
-
StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
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.");
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 42d1a4d476b848a94ddb6659e03fbc0e51b52aab..a1d831c460e628d1a4571e287bd3fd1c942117d5 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);
@@ -87,22 +86,12 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
if (_state.size() == 4)
return StateQuaternion::create(_state, _fixed);
- throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
+ throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
return nullptr;
}
-StateBlockPtr create_pos_vel(const Eigen::VectorXd& _state, bool _fixed)
-{
- if (_state.size() != 2 and _state.size() != 3)
- throw std::runtime_error("Wrong vector size for position/velocity. Must be 2 or 3 (2D or 3D).");
-
- return StateBlock::create(_state, _fixed);
-}
-
-namespace{
- const bool __attribute__((used)) create_orientationRegisteredWithO = FactoryStateBlock::registerCreator("O", wolf::create_orientation);
- const bool __attribute__((used)) create_positionRegisteredWithO = FactoryStateBlock::registerCreator("P", wolf::create_pos_vel);
- const bool __attribute__((used)) create_velocityRegisteredWithO = FactoryStateBlock::registerCreator("V", wolf::create_pos_vel);
-}
-}
+//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
+namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \
+ FactoryStateBlock::registerCreator("O", wolf::create_orientation); }
+}
\ No newline at end of file
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 7c04d53978d133a192df8245739a866180d282c4..4ae33e98db23cee63948cc3a76e121f66c3e75c7 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 e8823de12eda0f672114b57f16cd38e74af85337..88de4a9b44c01961378c0267aeed29c41a805198 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -24,6 +24,7 @@
#include "core/capture/capture_base.h"
#include "core/sensor/factory_sensor.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -65,9 +66,9 @@ TEST(CaptureBase, Static_sensor_params)
TEST(CaptureBase, Dynamic_sensor_params)
{
SensorBasePtr S = FactorySensorYaml::create("SensorDiffDrive", "SensorDiffDrive", wolf_root + "/test/yaml/sensor_diff_drive_dynamic.yaml", {wolf_root});
- StateBlockPtr p(std::make_shared<StateBlock>(2));
- StateBlockPtr o(std::make_shared<StateAngle>() );
- StateBlockPtr i(std::make_shared<StateBlock>(3));
+ StateBlockPtr p(std::make_shared<StatePoint2d>(Vector2d::Zero()));
+ StateBlockPtr o(std::make_shared<StateAngle>(0) );
+ StateBlockPtr i(std::make_shared<StateParams2>(Vector2d::Zero()));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query capture blocks
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index ef45a2d9d33f7fb35bb921f59e4e6b84a6b1cadb..a18e266ccc164878cb100a90e0fcfae3dbb5a3d9 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.h"
@@ -71,7 +72,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());
}
@@ -106,7 +107,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);
@@ -120,7 +121,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);
@@ -138,7 +139,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);
@@ -158,7 +159,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<SensorOdom>(P->getHardware(),
"dummy_name",
2,
@@ -185,7 +186,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 ce5058b1b6489c137588b086f3acdf7e4f9496c9..718cf14ba4731b4b754de43812f0d785cfc72efa 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/factory_sensor.h"
@@ -45,7 +46,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);
@@ -62,18 +63,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 e1a328927679631ea3bb63357b08d009908a5cf2..3f6c88ac226b0fab73b3ab8b4f5c21e649056dbe 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"
@@ -136,7 +137,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 8c15d647982fe4cefaa98ecf07e68d860edb4984..7468d09e36469d1ad8ee49d61bc97bc8e8774201 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::runtime_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 dc7ea50ecb32b1cbd40a26a446b311c282abfa9b..ac3e52514df922b77473e912d7e10ef297904afd 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -28,6 +28,8 @@
#include "core/sensor/sensor_odom.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>
@@ -39,7 +41,7 @@ std::string wolf_root = _WOLF_ROOT_DIR;
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);
@@ -52,7 +54,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);
@@ -62,7 +64,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());
@@ -79,8 +81,8 @@ TEST(FrameBase, LinksToTree)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
auto S = P->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
- 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>());
@@ -130,16 +132,16 @@ TEST(FrameBase, Frames)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
auto S = P->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
- 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));
@@ -387,8 +389,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 5f12596a9d39ae9d0da598b5f8eddf90613a9f71..183d6dfbbc6489dd94da9875e224ef4cbbd072a9 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 874682995a2b49dcf8eed3314c064e4f709d8aca..fbc835ae1624f9dda8c139c9f8bedf131cb54787 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -42,6 +42,10 @@
#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 "core/landmark/landmark_base.h"
+#include "core/math/SE3.h"
#include <iostream>
@@ -395,7 +399,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++;
@@ -403,8 +407,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();
}
@@ -481,7 +485,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++)
@@ -497,8 +501,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();
}
@@ -614,10 +618,128 @@ TEST(Problem, getState)
}
+TEST(Problem, transform)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ auto S = P->getHardware()->getSensorList().front();
+
+ // Add dynamic sensor params
+ Vector3d param_sensor = Vector3d(3, 2, 1);
+ S->addStateBlock('b', std::make_shared<StateParams3>(param_sensor, false), true); // Estimated ; Dynamic
+
+ Vector6d data;
+ data << 0, 0.2, 0, 0, 0, 0; // advance on Y
+ auto C = std::make_shared<CaptureOdom3d>(0.0, S, Vector6d::Zero(), 0.01 * Matrix6d::Identity());
+
+ // Move until we get one keyframe
+ for (TimeStamp t = 0.0; t <= 3.0; t += 0.2)
+ {
+ C->setTimeStamp(t);
+ if (t > 0.0) C->setData(data); // first time data is zero!
+ C->process();
+ }
+
+ // Add dynamic sensor params to captures
+ for (auto t_f : P->getTrajectory()->getFrameMap())
+ {
+ for (auto c : t_f.second->getCapturesOf(S))
+ {
+ c->addStateBlock('b', std::make_shared<StateParams3>(S->getStateBlock('b')->getState()), P);
+ }
+ }
+
+ // Add a couple of lmks
+ LandmarkBase::emplace<LandmarkBase>(P->getMap(), //
+ "Point3d",
+ std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)));
+ LandmarkBase::emplace<LandmarkBase>(P->getMap(),
+ "Pose3d",
+ std::make_shared<StatePoint3d>(Vector3d(1, 2, 3)),
+ std::make_shared<StateQuaternion>(Vector4d(0, 0, 0, 1)));
+
+ // P->print(2, 0, 1, 1); // reference
+
+ ///////////////////////////
+ // Define transformations
+
+ // translation
+ Quaterniond q_w1_w0 = Quaterniond::Identity(); // no rotation
+ VectorComposite pose_w1_w0("PO", {Vector3d(1, 0, 0), q_w1_w0.coeffs()}); // translation X: 1m
+
+ // rotation
+ Quaterniond q_w2_w1(AngleAxisd(M_PI_2, Vector3d(1, 0, 0))); // rotation X: 90deg
+ VectorComposite pose_w2_w1("PO", {Vector3d(0, 0, 0), q_w2_w1.coeffs()}); // no translation,
+
+ // rotation + translation
+ Quaterniond q_w3_w2(AngleAxisd(-M_PI_2, Vector3d(1, 0, 0))); // rotation X: -90deg
+ VectorComposite pose_w3_w2("PO", {Vector3d(1, 0, 0), q_w3_w2.coeffs()}); // translation X: 1m
+
+ // State of the problem in the initial frame "w0"
+ VectorComposite pose_w0_r0 = P->getTrajectory()->getFirstFrame()->getState();
+ VectorComposite pose_w0_r1 = P->getTrajectory()->getLastFrame()->getState();
+ Vector3d p_w0_l0 = P->getMap()->getLandmarkList().front()->getState("P").vector("P"); // Point3d
+ VectorComposite pose_w0_l1 = P->getMap()->getLandmarkList().back()->getState();
+
+ //////////////////////
+ // Apply successive changes of reference frame w0->w1->w2->w3 and compare with transformed states
+ //////////////////////
+
+ //////////////////////
+ // w0->w1: Translation
+ P->transform(pose_w1_w0);
+ // Expected values
+ VectorComposite pose_w1_r0 = SE3::compose(pose_w1_w0, pose_w0_r0);
+ VectorComposite pose_w1_r1 = SE3::compose(pose_w1_w0, pose_w0_r1);
+ Vector3d p_w1_l0 = pose_w1_w0.at('P') + q_w1_w0 * p_w0_l0;
+ VectorComposite pose_w1_l1 = SE3::compose(pose_w1_w0, pose_w0_l1);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w1_r0.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w1_r1.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w1_l0, 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w1_l1.vector("PO"), 1e-10);
+ // P->print(2, 0, 1, 1);
+
+ //////////////////////
+ // w1->w2: Rotation
+ P->transform(pose_w2_w1);
+ // Expected values
+ VectorComposite pose_w2_r0 = SE3::compose(pose_w2_w1, pose_w1_r0);
+ VectorComposite pose_w2_r1 = SE3::compose(pose_w2_w1, pose_w1_r1);
+ Vector3d p_w2_l0 = pose_w2_w1.at('P') + q_w2_w1 * p_w1_l0;
+ VectorComposite pose_w2_l1 = SE3::compose(pose_w2_w1, pose_w1_l1);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w2_r0.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w2_r1.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w2_l0, 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w2_l1.vector("PO"), 1e-10);
+ // P->print(2, 0, 1, 1);
+
+ //////////////////////
+ // w2->w3: Translation + Rotation
+ P->transform(pose_w3_w2);
+ // Expected values
+ VectorComposite pose_w3_r0 = SE3::compose(pose_w3_w2, pose_w2_r0);
+ VectorComposite pose_w3_r1 = SE3::compose(pose_w3_w2, pose_w2_r1);
+ Vector3d p_w3_l0 = pose_w3_w2.at('P') + q_w3_w2 * p_w2_l0;
+ VectorComposite pose_w3_l1 = SE3::compose(pose_w3_w2, pose_w2_l1);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getFirstFrame()->getState().vector("PO"), pose_w3_r0.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getTrajectory()->getLastFrame()->getState().vector("PO"), pose_w3_r1.vector("PO"), 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().front()->getState("P").vector("P"), p_w3_l0, 1e-10);
+ ASSERT_MATRIX_APPROX(P->getMap()->getLandmarkList().back()->getState().vector("PO"), pose_w3_l1.vector("PO"), 1e-10);
+ // P->print(2, 0, 1, 1);
+
+ // sensor parameters should not be transformed
+ ASSERT_MATRIX_APPROX(S->getStateBlock('b')->getState(), param_sensor, 1e-10);
+
+}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index fa60723ff68b6eb86106403ea0c828b2844365a3..613857d108325657276fdaadc138650b543f5540 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -37,6 +37,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>
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index 53e4a4a9c90624d48e72e1e514453bcdfe59f22b..0504619c7489de34b566ab509b569b3f81fa8193 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;
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 773f85b02f4c53deab3b97a49391eb5f630a49a0..4baf6e8b53885999c718b73280cc5bb6c8d83593 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"
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 64ab59ce841a85ac94c2f63dbc77ac1f51b80aba..38d865ffa5869234ccdc577ad30892057f3ffce5 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 fbc1dc1186a5095fce16d8e4804c1d6352498b33..bcb2c08329f8901ca106f25cb355c88a2984dd98 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));
}
};
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
index 8af72b6347377f3737d2e5a4222bd9862da37a81..00cc15c553ad7d068553016b7d782be29a5f4eaa 100644
--- a/test/yaml/params_problem_autosetup.yaml
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -50,7 +50,7 @@ config:
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
- unmeasured_perturbation_std: 0.00111
+ unmeasured_perturbation_std: 0.001
state_getter: true
state_priority: 1