diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3cfdcb44eff879f3ea3934c3a79fc9ec6c4f4fd5..75b2269f91251bf68673b37ba86962ee03878e1a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -209,6 +209,7 @@ SET(HDRS_FRAME
include/core/frame/frame_base.h
)
SET(HDRS_STATE_BLOCK
+ include/core/state_block/factory_state_block.h
include/core/state_block/has_state_blocks.h
include/core/state_block/local_parametrization_angle.h
include/core/state_block/local_parametrization_base.h
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index e418b30c14377b15dff3eb606911a05fd7118cfa..93e14f38c41ad1639c09810b4a26631e0ddc61e5 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -141,7 +141,8 @@ int main()
TimeStamp t(0.0); // t : 0.0
Vector3d x(0,0,0);
Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
+ FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
+ std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 5ab1ad1dd20a6b03411ea5cc458bde99b2b1fca8..39d5558eb7e2925352a78694531aefdea16ee6f1 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -9,6 +9,7 @@
// wolf core includes
#include "core/common/wolf.h"
#include "core/capture/capture_odom_2d.h"
+#include "core/processor/processor_motion.h"
#include "core/yaml/parser_yaml.h"
#include "core/ceres_wrapper/ceres_manager.h"
@@ -118,9 +119,11 @@ int main()
// recover sensor pointers and other stuff for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->getSensor("sen odom");
SensorBasePtr sensor_rb = problem->getSensor("sen rb");
- TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp();
-
+ // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
+ TimeStamp t(0.0);
+ FrameBasePtr KF1 = problem->applyPriorOptions(t);
+ std::static_pointer_cast<ProcessorMotion>(problem->getProcessorIsMotion())->setOrigin(KF1);
// SELF CALIBRATION ===================================================
// These few lines control whether we calibrate some sensor parameters or not.
@@ -272,72 +275,79 @@ int main()
*
* P: wolf tree status ---------------------
Hardware
- S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
- Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
- Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
- o: C7 - F3 // origin at Capture 7, Frame 3
- l: C10 - F4 // last at Capture 10, frame 4
- S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
- Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
- Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pt2 RANGE BEARING // Processor 2: type Range and Bearing
+ Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
+ sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
+ PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
+ o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
+ l: Cap8 - Frm4 // last at Capture 8, Frame 4
+ Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
+ sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
+ Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type rande and bearing
Trajectory
- KF1 <-- c3 // KeyFrame 1, constrained by Factor 3
- Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector
- sb: Est Est // State's pos and orient are estimated
- C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute
- f1 FIX <-- // Feature 1, type Fix
- m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
- c1 FIX --> A // Factor 1, type FIX, it is Absolute
- CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
- C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
- f2 RANGE BEARING <-- // Feature 2, type R+B
- m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
- c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1
- KF2 <-- c6
- Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
- sb: Est Est
- CM3 ODOM 2d -> S1 [Sta, Sta] <--
- f3 ODOM 2d <--
+ KFrm1 <-- Fac4 //KeyFrame 1, constrained by Factor 4
+ P[Est] = ( -4.4e-12 1.5e-09 ) //Position is estimated
+ O[Est] = ( 2.6e-09 ) //Orientation is estimated
+ Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
+ Ftr1 trk0 prior <-- // Position prior
+ m = ( 0 0 )
+ Fac1 FactorBlockAbsolute --> Abs
+ Ftr2 trk0 prior <-- // Orientation prior
+ m = ( 0 )
+ Fac2 FactorBlockAbsolute --> Abs
+ CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
+ buffer size : 0
+ Cap4 CaptureRangeBearing -> Sen2 <--
+ Ftr3 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac3 RANGE BEARING --> Lmk1
+ KFrm2 <-- Fac7
+ P[Est] = ( 1 5.7e-09 )
+ O[Est] = ( 5.7e-09 )
+ CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <-- // Capture 3 of type motion odom2d from sensor 1.
+ // Its origin is at Capture 2; Frame 1
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr4 trk0 ProcessorOdom2d <--
m = ( 1 0 0 )
- c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
- C9 RANGE BEARING -> S2 [Sta, Sta] <--
- f4 RANGE BEARING <--
- m = ( 1.41 2.36 )
- c4 RANGE BEARING --> L1
- f5 RANGE BEARING <--
- m = ( 1 1.57 )
- c5 RANGE BEARING --> L2
- KF3 <--
- Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
- sb: Est Est
- CM7 ODOM 2d -> S1 [Sta, Sta] <--
- f6 ODOM 2d <--
+ Fac4 FactorOdom2d --> Frm1
+ Cap7 CaptureRangeBearing -> Sen2 <--
+ Ftr5 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac5 RANGE BEARING --> Lmk1
+ Ftr6 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac6 RANGE BEARING --> Lmk2
+ KFrm3 <--
+ P[Est] = ( 2 1.2e-08 )
+ O[Est] = ( 6.6e-09 )
+ CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr7 trk0 ProcessorOdom2d <--
m = ( 1 0 0 )
- c6 ODOM 2d --> F2
- C12 RANGE BEARING -> S2 [Sta, Sta] <--
- f7 RANGE BEARING <--
- m = ( 1.41 2.36 )
- c7 RANGE BEARING --> L2
- f8 RANGE BEARING <--
- m = ( 1 1.57 )
- c8 RANGE BEARING --> L3
- F4 <--
- Est, ts=2, x = ( 0.11 -0.045 0.26 )
- sb: Est Est
- CM10 ODOM 2d -> S1 [Sta, Sta] <--
+ Fac7 FactorOdom2d --> Frm2
+ Cap9 CaptureRangeBearing -> Sen2 <--
+ Ftr8 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac8 RANGE BEARING --> Lmk2
+ Ftr9 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac9 RANGE BEARING --> Lmk3
+ Frm4 <--
+ P[Est] = ( 2 0 )
+ O[Est] = ( 0 )
+ CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
+ buffer size : 1
+ delta preint : (0 0 0)
Map
- L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
- Est, x = ( 1 2 ) // L4 state is estimated, state vector
- sb: Est // L4 has 1 state block estimated
- L2 POINT 2d <-- c5 c7
- Est, x = ( 2 2 )
- sb: Est
- L3 POINT 2d <-- c8
- Est, x = ( 3 2 )
- sb: Est
+ Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by facotrs 3 & 5
+ P[Est] = ( 1 2 )
+ Lmk2 LandmarkPoint2d <-- Fac6 Fac8
+ P[Est] = ( 2 2 )
+ Lmk3 LandmarkPoint2d <-- Fac9
+ P[Est] = ( 3 2 )
-----------------------------------------
+
*
* ============= GENERAL WOLF NOTES ==================
*
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 1077fe00cb442147b03a36ffddba5b3b37016e8d..90e1c1616c1d32955bc50ccf04eae73e0ef01159 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -10,7 +10,7 @@ config:
frame_structure: "PO" # keyframes have position and orientation
dimension: 2 # space is 2d
prior:
- timestamp: 0.0
+ mode: "factor"
state: [0,0,0]
cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
time_tolerance: 0.1
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index d97810fe3f6fb83f14ee2e802ea8f07b2049485f..1510362e65c0a457f8982eec5e8e5cc130798668 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -244,7 +244,7 @@ class Factory
bool registerCreator(const std::string& _type, CreatorCallback createFn);
bool unregisterCreator(const std::string& _type);
TypeBasePtr create(const std::string& _type, TypeInput... _input);
- std::string getClass();
+ std::string getClass() const;
private:
CallbackMap callbacks_;
@@ -301,7 +301,7 @@ inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
}
template<class TypeBase, typename... TypeInput>
-inline std::string Factory<TypeBase, TypeInput...>::getClass()
+inline std::string Factory<TypeBase, TypeInput...>::getClass() const
{
return "Factory<class TypeBase>";
}
@@ -319,7 +319,7 @@ class LandmarkBase;
typedef Factory<LandmarkBase,
const YAML::Node&> FactoryLandmark;
template<>
-inline std::string FactoryLandmark::getClass()
+inline std::string FactoryLandmark::getClass() const
{
return "FactoryLandmark";
}
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 738d3558a41124377dffa39731ed1c859d09f873..5fc2f8f14ccffc52653c6f43fa0c07fdb03be825 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -170,7 +170,7 @@ struct ParamsProcessorBase;
typedef Factory<ParamsProcessorBase,
const std::string&> FactoryParamsProcessor;
template<>
-inline std::string FactoryParamsProcessor::getClass()
+inline std::string FactoryParamsProcessor::getClass() const
{
return "FactoryParamsProcessor";
}
@@ -180,7 +180,7 @@ typedef Factory<ProcessorBase,
const std::string&,
const ParamsProcessorBasePtr> FactoryProcessor;
template<>
-inline std::string FactoryProcessor::getClass()
+inline std::string FactoryProcessor::getClass() const
{
return "FactoryProcessor";
}
@@ -193,7 +193,7 @@ typedef Factory<ProcessorBase,
const std::string&,
const ParamsServer&> AutoConfFactoryProcessor;
template<>
-inline std::string AutoConfFactoryProcessor::getClass()
+inline std::string AutoConfFactoryProcessor::getClass() const
{
return "AutoConfFactoryProcessor";
}
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 67ef5f597108f7ddc3bc264fb1c0cbc218bdba0a..ff0574aaadc6a52015a6c9e4f076a1ec64d968c0 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -212,9 +212,9 @@ namespace wolf
// ParamsSensor factory
struct ParamsSensorBase;
typedef Factory<ParamsSensorBase,
- const std::string&> FactoryParamsSensor;
+ const std::string&> FactoryParamsSensor;
template<>
-inline std::string FactoryParamsSensor::getClass()
+inline std::string FactoryParamsSensor::getClass() const
{
return "FactoryParamsSensor";
}
@@ -222,10 +222,11 @@ inline std::string FactoryParamsSensor::getClass()
// Sensor factory
typedef Factory<SensorBase,
const std::string&,
- const Eigen::VectorXd&, const ParamsSensorBasePtr> FactorySensor;
+ const Eigen::VectorXd&,
+ const ParamsSensorBasePtr> FactorySensor;
template<>
-inline std::string FactorySensor::getClass()
+inline std::string FactorySensor::getClass() const
{
return "FactorySensor";
}
@@ -240,7 +241,7 @@ typedef Factory<SensorBase,
const ParamsServer&> AutoConfFactorySensor;
template<>
-inline std::string AutoConfFactorySensor::getClass()
+inline std::string AutoConfFactorySensor::getClass() const
{
return "AutoConfFactorySensor";
}
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index b055217828d887169f8e732138b43a4f2067c62a..e3cc58a4a0b6d269026312f9c946a490d1045923 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -214,7 +214,7 @@ typedef Factory<SolverManager,
const ParamsServer&> FactorySolver;
template<>
-inline std::string FactorySolver::getClass()
+inline std::string FactorySolver::getClass() const
{
return "FactorySolver";
}
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
new file mode 100644
index 0000000000000000000000000000000000000000..c353121b724011b993942d5a567166963a7440d0
--- /dev/null
+++ b/include/core/state_block/factory_state_block.h
@@ -0,0 +1,51 @@
+/*
+ * factory_state_block.h
+ *
+ * Created on: Apr 27, 2020
+ * Author: jsola
+ */
+
+#ifndef STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+#define STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+
+#include "core/common/factory.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+// State blocks factory
+typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock;
+template<>
+inline std::string FactoryStateBlock::getClass() const
+{
+ return "FactoryStateBlock";
+}
+template<>
+inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed)
+{
+ typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
+
+ if (creator_callback_it == 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::get().registerCreator(#StateBlockType, StateBlockType::create); } \
+
+#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
+ namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
+ FactoryStateBlock::get().registerCreator(#Key, StateBlockType::create); } \
+
+
+
+
+}
+
+
+#endif /* STATE_BLOCK_FACTORY_STATE_BLOCK_H_ */
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 05a87f0f25bb2af1466da5a5a5b4608b64f2a883..8b714b64c8b3bd82699fab1d845887e2bd74e10c 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -20,6 +20,8 @@ class StateAngle : public StateBlock
StateAngle(double _angle = 0.0, bool _fixed = false);
virtual ~StateAngle();
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
inline StateAngle::StateAngle(double _angle, bool _fixed) :
@@ -33,6 +35,12 @@ inline StateAngle::~StateAngle()
//
}
+inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<1,1>::check(_state);
+ return std::make_shared<StateAngle>(_state(0), _fixed);
+}
+
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index ab3bb7147f3c81f57a3436216412274394acc04a..5d3aa1fc2e9694211d388592c8aa2605cee00824 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -157,6 +157,11 @@ public:
/** \brief Remove this state_block from the problem
**/
//void removeFromProblem(ProblemPtr _problem_ptr);
+
+ /** \brief Factory creator
+ *
+ */
+ static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
};
} // namespace wolf
@@ -292,6 +297,11 @@ inline double* StateBlock::getStateData()
return state_.data();
}
+inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ return std::make_shared<StateBlock>(_state, _fixed);
+}
+
}// namespace wolf
#endif
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 4760a52b7faa2bf188b520534b8512722d209f63..f9a0c099841c025671bff1b51d50df86fccff51a 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -19,6 +19,8 @@ class StateHomogeneous3d : public StateBlock
StateHomogeneous3d(bool _fixed = false);
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
virtual ~StateHomogeneous3d();
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
@@ -40,6 +42,12 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
// The local_param_ptr_ pointer is already removed by the base class
}
+inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<4,1>::check(_state);
+ return std::make_shared<StateHomogeneous3d>(_state, _fixed);
+}
+
} // namespace wolf
#endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 3732201633ca04eeac5815b9948c8e9d7261f852..817d436aef72c9b128ce97dd8569f380a575ce1b 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -20,6 +20,8 @@ class StateQuaternion : public StateBlock
StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
virtual ~StateQuaternion();
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
@@ -45,6 +47,12 @@ inline StateQuaternion::~StateQuaternion()
// The local_param_ptr_ pointer is already removed by the base class
}
+inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<4,1>::check(_state);
+ return std::make_shared<StateQuaternion>(_state, _fixed);
+}
+
} // namespace wolf
#endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index 91f2b9531d91bdad952760a0670ceece4240e86a..a56dae93e138c0f99cc51f73884a50be9f252707 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -23,7 +23,7 @@ struct ParamsTreeManagerBase;
typedef Factory<ParamsTreeManagerBase,
const std::string&> FactoryParamsTreeManager;
template<>
-inline std::string FactoryParamsTreeManager::getClass()
+inline std::string FactoryParamsTreeManager::getClass() const
{
return "FactoryParamsTreeManager";
}
@@ -33,7 +33,7 @@ typedef Factory<TreeManagerBase,
const std::string&,
const ParamsTreeManagerBasePtr> FactoryTreeManager;
template<>
-inline std::string FactoryTreeManager::getClass()
+inline std::string FactoryTreeManager::getClass() const
{
return "FactoryTreeManager";
}
@@ -46,7 +46,7 @@ typedef Factory<TreeManagerBase,
const std::string&,
const ParamsServer&> AutoConfFactoryTreeManager;
template<>
-inline std::string AutoConfFactoryTreeManager::getClass()
+inline std::string AutoConfFactoryTreeManager::getClass() const
{
return "AutoConfFactoryTreeManager";
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c7b35c78394971965bf2f15476209dad3d2206c1..f2d41a420415f8b0f9fb3ac9f6b5e286036c1160 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -175,6 +175,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Prior
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
+ WOLF_TRACE("Prior mode: ", prior_mode);
if (prior_mode == "nothing")
{
problem->setPriorOptions(prior_mode);
@@ -188,8 +189,9 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
else
{
+ WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
- _server.getParam<double>("problem/prior/timestamp"),
+ _server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<Eigen::VectorXd>("problem/prior/state"));
}
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index bfe2c6cd1ac780d058421d96c64e745b378c3b88..5b3abbf52bbc5715b9613635494a1719f94f17e8 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -82,7 +82,13 @@ unsigned int ProcessorTrackerFeature::processKnown()
matches_last_from_incoming_.clear();
known_features_incoming_.clear();
- if (!last_ptr_ || known_features_last_.empty())
+ if (!last_ptr_)
+ {
+ WOLF_TRACE("null last, returning...");
+ return 0;
+ }
+
+ if (known_features_last_.empty())
{
WOLF_TRACE("Empty last feature list, returning...");
return 0;
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c..030febd46ba50917d8681bd2ce3bb6ee8be55e23 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -55,3 +55,32 @@ void StateBlock::perturb(double amplitude)
}
}
+
+#include "core/state_block/factory_state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#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);
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);
+
+StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 1)
+ return StateAngle::create(_state, _fixed);
+ if (_state.size() == 4)
+ return StateQuaternion::create(_state, _fixed);
+
+ 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;
+}
+
+//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
+namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \
+ FactoryStateBlock::get().registerCreator("O", wolf::create_orientation); }
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index dc7cfae429f38b83a414229f78a70b01cc5ba53f..b0309585c0a4a1a16b2ce0582669e2e75cf41d68 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -47,6 +47,10 @@ add_library(dummy ${SRC_DUMMY})
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PROJECT_NAME})
+# Converter from String to various types used by the parameters server
+wolf_add_gtest(gtest_converter gtest_converter.cpp)
+target_link_libraries(gtest_converter ${PROJECT_NAME})
+
# FactorBase class test
wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
target_link_libraries(gtest_factor_base ${PROJECT_NAME})
@@ -55,9 +59,9 @@ target_link_libraries(gtest_factor_base ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
-# Converter from String to various types used by the parameters server
-wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PROJECT_NAME})
+# FactoryStateBlock factory test
+wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
+target_link_libraries(gtest_factory_state_block ${PROJECT_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbded073c2e9a226c0f66b1feb34388bd5c06dfc
--- /dev/null
+++ b/test/gtest_factory_state_block.cpp
@@ -0,0 +1,157 @@
+/*
+ * gtest_factory_state_block.cpp
+ *
+ * Created on: Apr 28, 2020
+ * Author: jsola
+ */
+
+#include "core/common/wolf.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/sensor/factory_sensor.h"
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.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)
+{
+ auto sbp = FactoryStateBlock::get().create("P", Eigen::Vector3d(1,2,3), false);
+ auto sbv = FactoryStateBlock::get().create("V", Eigen::Vector2d(4,5), false);
+ auto sbw = FactoryStateBlock::get().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());
+}
+
+TEST(FactoryStateBlock, creator_StateBlock)
+{
+ auto sbp = FactoryStateBlock::get().create("StateBlock", Eigen::Vector3d(1,2,3), false);
+ auto sbv = FactoryStateBlock::get().create("StateBlock", Eigen::Vector2d(4,5), true);
+ auto sbw = FactoryStateBlock::get().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());
+}
+
+TEST(FactoryStateBlock, creator_Quaternion)
+{
+ auto sbq = FactoryStateBlock::get().create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getLocalSize(), 3);
+ ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Angle)
+{
+ auto sba = FactoryStateBlock::get().create("StateAngle", Eigen::Vector1d(1), false);
+
+ ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getLocalSize(), 1);
+ ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Homogeneous3d)
+{
+ auto sbh = FactoryStateBlock::get().create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_H)
+{
+ auto sbh = FactoryStateBlock::get().create("H", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_quaternion)
+{
+ auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getLocalSize(), 3);
+ ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_angle)
+{
+ auto sba = FactoryStateBlock::get().create("O", Eigen::Vector1d(1), false);
+
+ ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getLocalSize(), 1);
+ ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_wrong_size)
+{
+ ASSERT_THROW(auto sba = FactoryStateBlock::get().create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}