diff --git a/demos/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h
index 8bc32a93d452629b5c69358c5d751c070eeba1a6..10fb8fa05f14fc4801effce4fb734c90a9ea2e90 100644
--- a/demos/hello_wolf/capture_range_bearing.h
+++ b/demos/hello_wolf/capture_range_bearing.h
@@ -21,7 +21,7 @@ class CaptureRangeBearing : public CaptureBase
{
public:
CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
- virtual ~CaptureRangeBearing();
+ ~CaptureRangeBearing() override;
const VectorXi& getIds () const;
const int& getId (int _i) const;
diff --git a/demos/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
index 226a6335e7966a0d3bdd7042c017913bc64b419b..978f88846d5dee608a47a01c101676a0bfd23037 100644
--- a/demos/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.h
@@ -45,12 +45,12 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
//
}
- virtual ~FactorRangeBearing()
+ ~FactorRangeBearing() override
{
//
}
- virtual std::string getTopology() const override
+ std::string getTopology() const override
{
return "LMK";
}
diff --git a/demos/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h
index 8571a8fb933976c740440c9d4d9ba8a2fb3d8f20..b924d29eea0a22a80267ace90718f9a66f54dac1 100644
--- a/demos/hello_wolf/feature_range_bearing.h
+++ b/demos/hello_wolf/feature_range_bearing.h
@@ -19,7 +19,7 @@ class FeatureRangeBearing : public FeatureBase
{
public:
FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
- virtual ~FeatureRangeBearing();
+ ~FeatureRangeBearing() override;
};
} // namespace wolf
diff --git a/demos/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h
index 0ec660c9e70944a8de9dd152649dc78d8a85b86b..79287af069fb5b5da0ef7186333b90fc18664bbb 100644
--- a/demos/hello_wolf/landmark_point_2d.h
+++ b/demos/hello_wolf/landmark_point_2d.h
@@ -19,7 +19,7 @@ class LandmarkPoint2d : public LandmarkBase
{
public:
LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
- virtual ~LandmarkPoint2d();
+ ~LandmarkPoint2d() override;
};
} /* namespace wolf */
diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
index 1203b217e4f07c3e02a287f63b0bace4ba46ede2..bdcefb3a7141627523a6f2111307fc776abbadbb 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -46,31 +46,31 @@ class ProcessorRangeBearing : public ProcessorBase
typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(ParamsProcessorBasePtr _params);
- virtual ~ProcessorRangeBearing() {/* empty */}
- virtual void configure(SensorBasePtr _sensor) override;
+ ~ProcessorRangeBearing() override {/* empty */}
+ void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
protected:
// Implementation of pure virtuals from ProcessorBase
- virtual void processCapture (CaptureBasePtr _capture) override;
- virtual void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
- virtual bool triggerInCapture (CaptureBasePtr) const override { return true;};
- virtual bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
- virtual bool voteForKeyFrame () const override {return false;}
+ void processCapture (CaptureBasePtr _capture) override;
+ void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+ bool triggerInCapture (CaptureBasePtr) const override { return true;};
+ bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+ bool voteForKeyFrame () const override {return false;}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
- virtual bool storeKeyFrame(FrameBasePtr) override;
+ bool storeKeyFrame(FrameBasePtr) override;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
- virtual bool storeCapture(CaptureBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override;
private:
// control variables
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 491efff4f533edc993051482de7d0e84562a790a..59c217fe0f3f9f5f70973dca8a2ef4b060a6696a 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -45,7 +45,7 @@ class SensorRangeBearing : public SensorBase
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
- virtual ~SensorRangeBearing();
+ ~SensorRangeBearing() override;
};
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index c9aa0cd1980d55577179a07be2c6c819055d2b4b..edd07448729ba69ddda1fdc447b2610c22cd298c 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -126,18 +126,20 @@ namespace wolf
*
* Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>:
* it can only exist once in your application.
- * To access it, use the static method get(),
+ * To access it, we internally use the static method get(),
*
* \code
* Factory<MyTypeBase, MyTypeInput>::get()
* \endcode
*
- * where, of course, you better make use of the appropriate typedef in place of ````Factory<MyTypeBase, MyTypeInput>````.
+ * but this is a private method.
*
- * You can then call the methods you like, e.g. to create a landmark, you use:
+ * The interesting methods are designed static, and are the ones responsible for accessing the Factory instance via .get().
+ *
+ * You must therefore call the methods you like statically, e.g. to create a landmark, you use:
*
* \code
- * FactoryLandmark::get().create(TYPE, args...); // see below for creating objects ...
+ * FactoryLandmark::create(TYPE, args...); // see below for creating objects ...
* \endcode
*
* #### Write creator methods (in your derived object classes)
@@ -172,7 +174,7 @@ namespace wolf
* that knows how to create your specific object, e.g.:
*
* \code
- * FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
+ * FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* \endcode
*
* #### Automatic registration
@@ -180,7 +182,7 @@ namespace wolf
* For example, in sensor_camera_yaml.cpp we find the line:
*
* \code
- * const bool registered_camera_intr = FactoryParamsSensor::get().registerCreator("ParamsSensorCamera", createParamsSensorCamera);
+ * const bool registered_camera_intr = FactoryParamsSensor::registerCreator("ParamsSensorCamera", createParamsSensorCamera);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
@@ -191,7 +193,7 @@ namespace wolf
* The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of the object type.
*
* \code
- * Factory<MyTypeBase, MyTypeInput>::get().unregisterCreator("ParamsSensorCamera");
+ * Factory<MyTypeBase, MyTypeInput>::unregisterCreator("ParamsSensorCamera");
* \endcode
*
* #### Create objects using the factory
@@ -201,13 +203,13 @@ namespace wolf
* To create e.g. a LandmarkPolyline2d from a YAML node you type:
*
* \code
- * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("LandmarkPolyline2d", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* or even better, make use of the convenient typedefs:
*
* \code
- * LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create("LandmarkPolyline2d", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = FactoryLandmark::create("LandmarkPolyline2d", lmk_yaml_node);
* \endcode
*
* ### Examples
@@ -247,7 +249,7 @@ namespace wolf
* // Register landmark creator (put the register code inside an unnamed namespace):
* namespace
* {
- * const bool registered_lmk_polyline_2d = FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
+ * const bool registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* }
*
* \endcode
@@ -280,9 +282,9 @@ class Factory
typedef std::map<std::string, CreatorCallback> CallbackMap;
public:
- bool registerCreator(const std::string& _type, CreatorCallback createFn);
- bool unregisterCreator(const std::string& _type);
- TypeBasePtr create(const std::string& _type, TypeInput... _input);
+ static bool registerCreator(const std::string& _type, CreatorCallback createFn);
+ static bool unregisterCreator(const std::string& _type);
+ static TypeBasePtr create(const std::string& _type, TypeInput... _input);
std::string getClass() const;
private:
@@ -291,7 +293,7 @@ class Factory
// Singleton ---------------------------------------------------
// This class is a singleton. The code below guarantees this.
// See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
- public:
+ private:
static Factory& get();
public:
Factory(const Factory&) = delete;
@@ -304,11 +306,11 @@ class Factory
template<class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
{
- bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
+ bool reg = get().callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
if (reg)
- std::cout << std::setw(26) << std::left << getClass() << " <-- registered " << _type << std::endl;
+ std::cout << std::setw(26) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
else
- std::cout << std::setw(26) << std::left << getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
+ std::cout << std::setw(26) << std::left << get().getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
return reg;
}
@@ -316,17 +318,17 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
template<class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
{
- return callbacks_.erase(_type) == 1;
+ return get().callbacks_.erase(_type) == 1;
}
template<class TypeBase, typename... TypeInput>
inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
{
- typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
+ typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
- if (creator_callback_it == callbacks_.end())
+ if (creator_callback_it == get().callbacks_.end())
// not found
- throw std::runtime_error(getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
+ throw std::runtime_error(get().getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
// Invoke the creation function
return (creator_callback_it->second)(std::forward<TypeInput>(_input)...);
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index a4ea7b1dff8484a89885edd424a6ac899331d3d7..16f1d677024f4dd97968dbfda45c0e3aaca82723 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -35,7 +35,7 @@ class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2,
virtual ~FactorOdom2dCloseloop() = default;
- virtual std::string getTopology() const override
+ std::string getTopology() const override
{
return std::string("LOOP");
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 5c45c103c33b77542a1d136b870d6a0f88c177eb..91a94d274b36d0023a0ba267583f4b67726d8665 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -63,12 +63,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
StateBlockPtr _o_ptr = nullptr,
StateBlockPtr _v_ptr = nullptr);
- FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
- const SizeEigen _dim,
- const Eigen::VectorXd& _x);
-
FrameBase(const FrameType & _tp,
const TimeStamp& _ts,
const std::string _frame_structure,
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 9d101848eb54966e970a48d08416548a259b7154..e36e85a6d109572070bf2d317d81349df73f824a 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -75,7 +75,7 @@ namespace wolf
* To create a ProcessorOdom2d, you type:
*
* \code
- * auto prc_odom2d_ptr = FactoryProcessor::get().create("ProcessorOdom2d", "main odometry", params_ptr);
+ * auto prc_odom2d_ptr = FactoryProcessor::create("ProcessorOdom2d", "main odometry", params_ptr);
* \endcode
*
* #### Example 1 : Create a sensor and its processor
@@ -90,14 +90,14 @@ namespace wolf
* // Note: ProcessorOdom2d::create() is already registered, automatically.
*
* // First create the sensor (See FactorySensor for details)
- * SensorBasePtr sensor_ptr = FactorySensor::get().create ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
+ * SensorBasePtr sensor_ptr = FactorySensor::create ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
*
* // To create a odometry integrator, provide a type="ProcessorOdom2d", a name="main odometry", and a pointer to the parameters struct:
*
* auto params = make_shared<ParamsProcessorOdom2d>({...}); // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
*
* ProcessorBasePtr processor_ptr =
- * FactoryProcessor::get().create ( "ProcessorOdom2d" , "main odometry" , params );
+ * FactoryProcessor::create ( "ProcessorOdom2d" , "main odometry" , params );
*
* // Bind processor to sensor
* sensor_ptr->addProcessor(processor_ptr);
@@ -126,7 +126,7 @@ inline std::string FactoryParamsProcessor::getClass() const
#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
- wolf::FactoryProcessor::get().registerCreator(#ProcessorType, ProcessorType::create); } \
+ wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
typedef Factory<ProcessorBase,
const std::string&,
@@ -140,7 +140,7 @@ inline std::string AutoConfFactoryProcessor::getClass() const
#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType) \
namespace{ const bool WOLF_UNUSED ProcessorType##AutoConfRegistered = \
- wolf::AutoConfFactoryProcessor::get().registerCreator(#ProcessorType, ProcessorType::create); } \
+ wolf::AutoConfFactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
} /* namespace wolf */
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 3e787dd56cf710390cce53ed81d3b3800b2445b6..fb75e84cd1c27c6f3681d1a6a5065ba6d29ac9f4 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -79,7 +79,7 @@ namespace wolf
* To create e.g. a SensorCamera, you type:
*
* \code
- * auto camera_ptr = FactorySensor::get().create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
+ * auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
* \endcode
*
* where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
@@ -119,14 +119,14 @@ namespace wolf
* Put the code either at global scope (you must define a dummy variable for this),
* \code
* namespace {
- * const bool registered_camera = FactorySensor::get().registerCreator("SensorCamera", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
* }
* main () { ... }
* \endcode
* or inside your main(), where a direct call is possible:
* \code
* main () {
- * FactorySensor::get().registerCreator("SensorCamera", SensorCamera::create);
+ * FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
* ...
* }
* \endcode
@@ -135,7 +135,7 @@ namespace wolf
* Put the code at the last line of the sensor_xxx.cpp file,
* \code
* namespace {
- * const bool registered_camera = FactorySensor::get().registerCreator("SensorCamera", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
* }
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
@@ -163,11 +163,11 @@ namespace wolf
*
* // Create a pointer to the struct of sensor parameters stored in a YAML file ( see FactoryParamsSensor )
* ParamsSensorCameraPtr intrinsics_1 =
- * FactoryParamsSensor::get().create("ParamsSensorCamera",
+ * FactoryParamsSensor::create("ParamsSensorCamera",
* camera_1.yaml);
*
* SensorBasePtr camera_1_ptr =
- * FactorySensor::get().create ( "SensorCamera" ,
+ * FactorySensor::create ( "SensorCamera" ,
* "Front-left camera" ,
* extrinsics_1 ,
* intrinsics_1 );
@@ -177,11 +177,11 @@ namespace wolf
* Eigen::VectorXd extrinsics_2(7);
*
* ParamsSensorCameraPtr intrinsics_2 =
- * FactoryParamsSensor::get().create("ParamsSensorCamera",
+ * FactoryParamsSensor::create("ParamsSensorCamera",
* camera_2.yaml);
*
* SensorBasePtr camera_2_ptr =
- * FactorySensor::get().create( "SensorCamera" ,
+ * FactorySensor::create( "SensorCamera" ,
* "Front-right camera" ,
* extrinsics_2 ,
* intrinsics_2 );
@@ -214,7 +214,7 @@ inline std::string FactoryParamsSensor::getClass() const
#define WOLF_REGISTER_SENSOR(SensorType) \
namespace{ const bool WOLF_UNUSED SensorType##Registered = \
- FactorySensor::get().registerCreator(#SensorType, SensorType::create); } \
+ FactorySensor::registerCreator(#SensorType, SensorType::create); } \
typedef Factory<SensorBase,
@@ -229,7 +229,7 @@ inline std::string AutoConfFactorySensor::getClass() const
#define WOLF_REGISTER_SENSOR_AUTO(SensorType) \
namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered = \
- AutoConfFactorySensor::get().registerCreator(#SensorType, SensorType::create); } \
+ AutoConfFactorySensor::registerCreator(#SensorType, SensorType::create); } \
} /* namespace wolf */
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index fc2031cdb245ee6364aa9de3bb38e834812e22a7..3e13e598fbd2726532333f2a71d4eabc5adcc1fb 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -37,7 +37,7 @@ inline std::string FactorySolver::getClass() const
#define WOLF_REGISTER_SOLVER(SolverType) \
namespace{ const bool WOLF_UNUSED SolverType##Registered = \
- wolf::FactorySolver::get().registerCreator(#SolverType, SolverType::create); } \
+ wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
} /* namespace wolf */
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index 0ffb0f23bbd9d18896c68f01b2c3f48183f39cc7..efa0db6d76410f4437180b3bc9f2c844c2b878d5 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -68,19 +68,19 @@ namespace wolf
* To create a StateQuaternion, you type:
*
* \code
- * auto sq_ptr = FactoryStateBlock::get().create("StateQuaternion", Vector4d(1,0,0,0), false);
+ * auto sq_ptr = FactoryStateBlock::create("StateQuaternion", Vector4d(1,0,0,0), false);
* \endcode
*
* If your problem has dimension 3 (e.g. is 3D), you can use the key "O" to create a StateQuaternion,
*
* \code
- * auto sq_ptr = FactoryStateBlock::get().create("O", Vector4d(1,0,0,0), false);
+ * auto sq_ptr = FactoryStateBlock::create("O", Vector4d(1,0,0,0), false);
* \endcode
*
* However if your problem has dimension 2 (e.g. is 2D), the key "O" will create a StateAngle,
*
* \code
- * auto sa_ptr = FactoryStateBlock::get().create("O", Vector1d(angle_in_radians), false);
+ * auto sa_ptr = FactoryStateBlock::create("O", Vector1d(angle_in_radians), false);
* \endcode
*
* Note: It is an error to provide state vectors of the wrong size (4 for 3D orientation, 1 for 2D).
@@ -88,22 +88,22 @@ namespace wolf
* To create StateBlocks to store 2D position and velocity, you type:
*
* \code
- * auto sp2_ptr = FactoryStateBlock::get().create("StateBlock", Vector2d(1,2), false);
- * auto sv2_ptr = FactoryStateBlock::get().create("StateBlock", Vector2d(1,2), false);
+ * auto sp2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false);
+ * auto sv2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false);
* \endcode
*
* To create StateBlocks to store 2D position and velocity, you can also use the key letters:
*
* \code
- * auto sp2_ptr = FactoryStateBlock::get().create("P", Vector2d(1,2), false);
- * auto sv2_ptr = FactoryStateBlock::get().create("V", Vector2d(1,2), false);
+ * auto sp2_ptr = FactoryStateBlock::create("P", Vector2d(1,2), false);
+ * auto sv2_ptr = FactoryStateBlock::create("V", Vector2d(1,2), false);
* \endcode
*
* To create StateBlocks to store 3D position and velocity, you type:
*
* \code
- * auto sp3_ptr = FactoryStateBlock::get().create("P", Vector3d(1,2,3), false);
- * auto sv3_ptr = FactoryStateBlock::get().create("V", Vector3d(1,2,3), false);
+ * auto sp3_ptr = FactoryStateBlock::create("P", Vector3d(1,2,3), false);
+ * auto sv3_ptr = FactoryStateBlock::create("V", Vector3d(1,2,3), false);
* \endcode
*
* Note: for base state blocks, the size is determined by the size of the provided vector parameter `VectorXd& _state`.
@@ -118,9 +118,9 @@ inline std::string FactoryStateBlock::getClass() const
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);
+ typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
- if (creator_callback_it == callbacks_.end())
+ if (creator_callback_it == get().callbacks_.end())
// not found: return StateBlock base
return std::make_shared<StateBlock>(_state, _fixed);
@@ -130,11 +130,11 @@ inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const E
#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
- FactoryStateBlock::get().registerCreator(#StateBlockType, StateBlockType::create); } \
+ FactoryStateBlock::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); } \
+ FactoryStateBlock::registerCreator(#Key, StateBlockType::create); } \
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 9462efe7a6dd314ff2ab94cbf923ee79f5c4e21e..8880fc0a2cfb09a46712a502601a801a1f923a39 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -301,9 +301,9 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_struct
for (const char key : structure)
{
const auto& sb = getStateBlock(key);
- if (!sb){
- WOLF_ERROR("Stateblock key ", key, " not in the structure");
- }
+
+ assert(sb != nullptr && "Requested StateBlock key not in the structure");
+
state.segment(index,sb->getSize()) = sb->getState();
index += sb->getSize();
}
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 44fdc98d588d46cbdf996ff94ec788f38bf7ce9c..0d3cafcc80c602a4de1bf59b9ce7c2ddd9123c9a 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -49,8 +49,6 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd
VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
- unsigned int size(const StateStructure& _structure) const;
-
Eigen::VectorXd vector(const StateStructure& _structure) const;
/**
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index a56dae93e138c0f99cc51f73884a50be9f252707..c9e91935fe52e08b2ea1bf0c264b5e67ca5ef4e1 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -40,7 +40,7 @@ inline std::string FactoryTreeManager::getClass() const
#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
- wolf::FactoryTreeManager::get().registerCreator(#TreeManagerType, TreeManagerType::create); } \
+ wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
typedef Factory<TreeManagerBase,
const std::string&,
@@ -54,7 +54,7 @@ inline std::string AutoConfFactoryTreeManager::getClass() const
#define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType) \
namespace{ const bool WOLF_UNUSED TreeManagerType##AutoConfRegistered = \
- wolf::AutoConfFactoryTreeManager::get().registerCreator(#TreeManagerType, TreeManagerType::create); } \
+ wolf::AutoConfFactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
} /* namespace wolf */
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index de9fc606991d9f3b9cc73d52f130123c07e3d6e1..4147b98ca8dcd67d76fb732f11cbbee85cea9df8 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -28,7 +28,7 @@ FrameBase::FrameBase(const FrameType & _tp,
const auto& key = pair_key_vec.first;
const auto& vec = pair_key_vec.second;
- StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+ StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
addStateBlock(key, sb);
}
@@ -54,7 +54,7 @@ FrameBase::FrameBase(const FrameType & _tp,
const auto& key = string(1,ckey);
const auto& vec = *vec_it;
- StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+ StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
addStateBlock(key, sb);
@@ -89,65 +89,6 @@ FrameBase::FrameBase(const FrameType & _tp,
}
}
-FrameBase::FrameBase(const FrameType & _tp,
- const TimeStamp& _ts,
- const std::string _frame_structure,
- const SizeEigen _dim,
- const Eigen::VectorXd& _x) :
- NodeBase("FRAME", "FrameBase"),
- HasStateBlocks(_frame_structure),
- trajectory_ptr_(),
- frame_id_(++frame_id_count_),
- type_(_tp),
- time_stamp_(_ts)
-{
- if(_frame_structure.compare("PO") == 0 and _dim == 2){
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- this->setType("PO 2d");
- }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- this->setType("PO 3d");
- }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXd::Zero(10);
- assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
- StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- addStateBlock("V", v_ptr);
- this->setType("POV 3d");
- }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXd::Zero(10);
- assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
- StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
- StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3 ) ) );
- StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.segment <3> (7 ) ) );
- StateBlockPtr c_ptr ( std::make_shared<StateBlock> (_x.segment <3> (10) ) );
- StateBlockPtr cd_ptr ( std::make_shared<StateBlock> (_x.segment <3> (13) ) );
- StateBlockPtr Lc_ptr ( std::make_shared<StateBlock> (_x.segment <3> (16) ) );
- addStateBlock("P", p_ptr);
- addStateBlock("O", o_ptr);
- addStateBlock("V", v_ptr);
- addStateBlock("C", c_ptr);
- addStateBlock("D", cd_ptr);
- addStateBlock("L", Lc_ptr);
- this->setType("POVCDL 3d");
- }else{
- std::cout << _frame_structure << " ^^ " << _dim << std::endl;
- throw std::runtime_error("Unknown frame structure");
- }
-
-
-}
FrameBase::~FrameBase()
{
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index ada2bef9648e15ca6a2809d9bf35dc88f2e454ac..0fdd18775a2ea50c2ca6ee51ffaeaff9db7c85fa 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -311,7 +311,7 @@ bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bo
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::get().registerCreator("LandmarkBase", LandmarkBase::create);
+const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
}
} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 4c13360effa052b61b189042a95d2ec92bedfa4d..67c37971b4e0517017f03b9cd798080fcd40808d 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -49,7 +49,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
for (unsigned int i = 0; i < nlandmarks; i++)
{
YAML::Node lmk_node = map["landmarks"][i];
- LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create(lmk_node["type"].as<std::string>(), lmk_node);
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
lmk_ptr->link(shared_from_this());
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 225e6f1ed184b3b51959ab4f125c1dbb9066eaec..4d2462f72d4e9bbf2fc6cb2c453389ea02028309 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -172,7 +172,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
if (tree_manager_type != "None" and tree_manager_type != "none")
- problem->setTreeManager(AutoConfFactoryTreeManager::get().create(tree_manager_type, "tree manager", _server));
+ problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
// Prior
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
@@ -213,7 +213,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const Eigen::VectorXd& _extrinsics, //
ParamsSensorBasePtr _intrinsics)
{
- SensorBasePtr sen_ptr = FactorySensor::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
+ SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -228,7 +228,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
if (_intrinsics_filename != "")
{
assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
- ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::get().create(_sen_type, _intrinsics_filename);
+ ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename);
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
}
else
@@ -240,7 +240,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const ParamsServer& _server)
{
- SensorBasePtr sen_ptr = AutoConfFactorySensor::get().create(_sen_type, _unique_sensor_name, _server);
+ SensorBasePtr sen_ptr = AutoConfFactorySensor::create(_sen_type, _unique_sensor_name, _server);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -257,7 +257,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return ProcessorBasePtr();
}
- ProcessorBasePtr prc_ptr = FactoryProcessor::get().create(_prc_type, _unique_processor_name, _prc_params);
+ ProcessorBasePtr prc_ptr = FactoryProcessor::create(_prc_type, _unique_processor_name, _prc_params);
//Dimension check
int prc_dim = prc_ptr->getDim();
@@ -283,7 +283,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
else
{
assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
- ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::get().create(_prc_type, _params_filename);
+ ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::create(_prc_type, _params_filename);
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
}
}
@@ -297,7 +297,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
- ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::get().create(_prc_type, _unique_processor_name, _server);
+ ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::create(_prc_type, _unique_processor_name, _server);
//Dimension check
int prc_dim = prc_ptr->getDim();
@@ -331,12 +331,35 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
const SizeEigen _dim, //
const Eigen::VectorXd& _frame_state)
{
+ VectorComposite state;
+ SizeEigen index = 0;
+ SizeEigen size = 0;
+ for (const auto& ckey : _frame_structure)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+
+ if (key == "O")
+ {
+ if (_dim == 2) size = 1;
+ else size = 4;
+ }
+ else
+ size = _dim;
+
+ assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
+
+ state.emplace(key, _frame_state.segment(index, size));
+
+ index += size;
+ }
+
+ assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
+
auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
_frame_key_type,
_time_stamp,
_frame_structure,
- _dim,
- _frame_state);
+ state);
return frm;
}
@@ -440,6 +463,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
state.insert(pair_key_vec);
}
}
+
// check for empty blocks and fill them with zeros
for (const auto& ckey : frame_structure_)
{
@@ -483,6 +507,15 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
state.insert(pair_key_vec);
}
}
+
+ // check for empty blocks and fill them with zeros
+ for (const auto& ckey : frame_structure_)
+ {
+ const auto& key = string(1,ckey);
+ if (state.count(key) == 0)
+ state.emplace(key, stateZero(key).at(key));
+ }
+
return state;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7b5d5dec09eadb5aa13b84d9340c760adba3f7c4..ef801eb6d525375272fd95d89a50eec9bed4dc1e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -201,7 +201,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// make F; append incoming to new F
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getStateVector());
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
@@ -250,7 +250,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Replace last frame for a new one in incoming
FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getStateVector());
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index ea2c9a4cd4f32b4a1c49e1bfec6ad28efb397cbe..0280e2657a3e5bb0bd0dcc184b6800844ee68ef8 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -73,5 +73,5 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \
- FactoryStateBlock::get().registerCreator("O", wolf::create_orientation); }
+ FactoryStateBlock::registerCreator("O", wolf::create_orientation); }
}
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 612b8e53ec55d7e5b787a5aa0c49312d21dfe4a9..90b7f47c6b5ffa2a28374b5673827d325224430f 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -62,18 +62,6 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l
}
-unsigned int VectorComposite::size(const StateStructure &_structure) const
-{
- unsigned int size = 0;
- for (const auto& ckey : _structure)
- {
- std::string key(1,ckey); // ckey is char
- const VectorXd& v = this->at(key);
- size += v.size();
- }
- return size;
-}
-
Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
{
// traverse once with unordered_map access
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index 2d1aa8200790d5d5484b7c24c1f8dc78304a3218..d37d8c022de839efbdeddbdacae80007168a282e 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -497,13 +497,11 @@ void ParserYAML::parse()
YAML::Node n;
n = loadYAML(generatePath(file_));
-
if (n.Type() == YAML::NodeType::Map)
{
for (auto it : n)
{
auto node = it.second;
- // WOLF_INFO("WUT ", it.first);
std::vector<std::string> tags = std::vector<std::string>();
if (it.first.as<std::string>() != "config")
walkTreeR(node, tags, it.first.as<std::string>());
@@ -515,7 +513,15 @@ void ParserYAML::parse()
if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
node_key != "ROS subscriber" and node_key != "ROS publisher")
{
- walkTreeR(itt.second, tags, node_key);
+ std::regex rr("follow");
+ if (not std::regex_match(node_key, rr))
+ {
+ walkTreeR(itt.second, tags, node_key);
+ }
+ else
+ {
+ walkTree(itt.second.as<std::string>(), tags, "");
+ }
}
}
}
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index 0a0884454f126a46e169c5edc25d26b3bb04ef64..9c21e3842304e475146194bc04da7be9f73de396 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -48,7 +48,7 @@ static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _f
}
// Register in the FactorySensor
-const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::get().registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
+const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index b8ef1a083111a877b5b283c13d751df88a5af20d..a33b7e3c7d1cf0b73374aa10510083bf9e47f9fd 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -40,7 +40,7 @@ static ParamsSensorBasePtr createParamsSensorOdom2d(const std::string & _filenam
}
// Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::get().registerCreator("SensorOdom2d", createParamsSensorOdom2d);
+const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::registerCreator("SensorOdom2d", createParamsSensorOdom2d);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index 5b700eb8210b337d59f1186be90b869fe123874f..edba0bd17d21b86508e01a407b4dea5bf41f91bd 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -42,7 +42,7 @@ static ParamsSensorBasePtr createParamsSensorOdom3d(const std::string & _filenam
}
// Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::get().registerCreator("SensorOdom3d", createParamsSensorOdom3d);
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorOdom3d", createParamsSensorOdom3d);
} // namespace [unnamed]
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 713fd1c1b26624b84612a1e721eb4924db08777c..f2e8e5f6014811ea3b99c2843fa4b0e8e94c5b72 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -163,14 +163,12 @@ class FactorDiffDriveTest : public testing::Test
KEY,
0.0,
"PO",
- 2,
- Vector3d(0,0,0));
+ std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
F1 = FrameBase::emplace<FrameBase>(trajectory,
KEY,
1.0,
"PO",
- 2,
- Vector3d(1,0,0));
+ std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index fbded073c2e9a226c0f66b1feb34388bd5c06dfc..726fa7e47566cae2c1071b7c3511ace30f03308e 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -41,20 +41,20 @@ TEST_F(TestInit, testName)
}
*/
-TEST(FactoryStateBlock, get_getClass)
-{
- const auto& f = FactoryStateBlock::get();
-
- const std::string& s = f.getClass();
-
- ASSERT_EQ(s, "FactoryStateBlock");
-}
+//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);
+ 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);
@@ -67,9 +67,9 @@ TEST(FactoryStateBlock, creator_default)
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);
+ 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);
@@ -86,7 +86,7 @@ TEST(FactoryStateBlock, creator_StateBlock)
TEST(FactoryStateBlock, creator_Quaternion)
{
- auto sbq = FactoryStateBlock::get().create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+ auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
ASSERT_EQ(sbq->getSize() , 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -95,7 +95,7 @@ TEST(FactoryStateBlock, creator_Quaternion)
TEST(FactoryStateBlock, creator_Angle)
{
- auto sba = FactoryStateBlock::get().create("StateAngle", Eigen::Vector1d(1), false);
+ auto sba = FactoryStateBlock::create("StateAngle", Eigen::Vector1d(1), false);
ASSERT_EQ(sba->getSize() , 1);
ASSERT_EQ(sba->getLocalSize(), 1);
@@ -104,7 +104,7 @@ TEST(FactoryStateBlock, creator_Angle)
TEST(FactoryStateBlock, creator_Homogeneous3d)
{
- auto sbh = FactoryStateBlock::get().create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+ auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
ASSERT_EQ(sbh->getSize() , 4);
ASSERT_EQ(sbh->getLocalSize(), 3);
@@ -113,7 +113,7 @@ TEST(FactoryStateBlock, creator_Homogeneous3d)
TEST(FactoryStateBlock, creator_H)
{
- auto sbh = FactoryStateBlock::get().create("H", Eigen::Vector4d(1,2,3,4), false);
+ auto sbh = FactoryStateBlock::create("H", Eigen::Vector4d(1,2,3,4), false);
ASSERT_EQ(sbh->getSize() , 4);
ASSERT_EQ(sbh->getLocalSize(), 3);
@@ -122,7 +122,7 @@ TEST(FactoryStateBlock, creator_H)
TEST(FactoryStateBlock, creator_O_is_quaternion)
{
- auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
+ auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4), false);
ASSERT_EQ(sbq->getSize() , 4);
ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -131,7 +131,7 @@ TEST(FactoryStateBlock, creator_O_is_quaternion)
TEST(FactoryStateBlock, creator_O_is_angle)
{
- auto sba = FactoryStateBlock::get().create("O", Eigen::Vector1d(1), false);
+ auto sba = FactoryStateBlock::create("O", Eigen::Vector1d(1), false);
ASSERT_EQ(sba->getSize() , 1);
ASSERT_EQ(sba->getLocalSize(), 1);
@@ -140,7 +140,7 @@ TEST(FactoryStateBlock, creator_O_is_angle)
TEST(FactoryStateBlock, creator_O_is_wrong_size)
{
- ASSERT_THROW(auto sba = FactoryStateBlock::get().create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+ ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
}
int main(int argc, char **argv)
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 8ec6ebcae42990451909902ee8f245c35fbf16c4..20e272cfa698f904e3bb63c6d7730b11e0fe4568 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -227,17 +227,13 @@ TEST(Problem, emplaceFrame_factory)
FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3, VectorXd(7) );
FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3, VectorXd(10) );
- // std::cout << "f0: type PO 2d? " << f0->getType() << std::endl;
- // std::cout << "f1: type PO 3d? " << f1->getType() << std::endl;
- // std::cout << "f2: type POV 3d? " << f2->getType() << std::endl;
-
- ASSERT_EQ(f0->getType().compare("PO 2d"), 0);
- ASSERT_EQ(f1->getType().compare("PO 3d"), 0);
- ASSERT_EQ(f2->getType().compare("POV 3d"), 0);
-
// check that all frames are effectively in the trajectory
ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
+ ASSERT_EQ(f0->getStateVector().size(), 3 );
+ ASSERT_EQ(f1->getStateVector().size(), 7 );
+ ASSERT_EQ(f2->getStateVector().size(), 10);
+
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
ASSERT_EQ(f1->getProblem(), P);
@@ -497,5 +493,6 @@ TEST(Problem, check)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 4d5a420119526c2fc1984aecfc22e01b40d0084b..56f705aa3b9c3689a076989fcd8e30896d80fe13 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -331,20 +331,6 @@ TEST(VectorComposite, unary_Minus)
ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
}
-TEST(VectorComposite, size)
-{
- VectorComposite x;
-
- x.emplace("P", Vector2d(1,1));
- x.emplace("O", Vector3d(2,2,2));
- x.emplace("V", Vector4d(3,3,3,3));
-
- ASSERT_EQ(x.size("PO"), 5);
- ASSERT_EQ(x.size("VO"), 7);
- ASSERT_EQ(x.size("PVO"), 9);
- ASSERT_EQ(x.size("OPV"), 9);
-}
-
TEST(VectorComposite, stateVector)
{
VectorComposite x;