diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 8aca178a01e8bdca1c0bc6212d36a707e2681488..c34f025208a5c4086d13d6d83c125f7b5fdaaf5b 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -178,8 +178,8 @@ int main(int argc, char** argv)
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e3, pose_odom, Eigen::Matrix3s::Identity() * 0.01, 0.3, window_size);
-
+ WolfManager* wolf_manager = new WolfManager(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3, complex_angle);
+
//std::cout << "START TRAJECTORY..." << std::endl;
// START TRAJECTORY ============================================================================================
for (unsigned int step = 1; step < n_execution; step++)
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 0524f1d1c2dd6849b5853133a90db69fc96a9a8f..9ab7161b36739c2093abfcf87b72654bae388557 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -18,7 +18,15 @@ class SensorBase : public NodeBase
StateOrientation* o_ptr_; // sensor orientation state unit pointer
Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ...
- public:
+ public:
+ /** \brief Constructor without parameters
+ *
+ * Constructor without parameters.
+ * Allows allocation of containers of StateBase, such as std::vector<SensorBase>
+ *
+ **/
+ //SensorBase();
+
/** \brief Constructor with parameter vector
*
* Constructor with parameter vector
diff --git a/src/state_base.cpp b/src/state_base.cpp
index 1b2f5c6cd3a8ec520bd4c105c960fae4cf380cd3..915a78c9d25da948b5b439e883a0c2b55f250a39 100644
--- a/src/state_base.cpp
+++ b/src/state_base.cpp
@@ -1,23 +1,22 @@
#include "state_base.h"
-StateBase::StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+StateBase::StateBase(WolfScalar* _st_ptr) :
NodeBase("STATE"),
- state_ptr_(_st_remote.data() + _idx),
+ state_ptr_(_st_ptr),
status_(ST_ESTIMATED)
{
//
}
-
-StateBase::StateBase(WolfScalar* _st_ptr) :
- NodeBase("STATE"),
- state_ptr_(_st_ptr),
- status_(ST_ESTIMATED)
+StateBase::StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+ NodeBase("STATE"),
+ state_ptr_(_st_remote.data() + _idx),
+ status_(ST_ESTIMATED)
{
- //
+ //
}
-
+
StateBase::~StateBase()
{
//std::cout << "deleting StateBase " << nodeId() << std::endl;
diff --git a/src/state_base.h b/src/state_base.h
index a31dd401f038f6b89dbd30fe27090ed157abd8dd..68ead00e655c67164632baae5b5724c6f7c973ab 100644
--- a/src/state_base.h
+++ b/src/state_base.h
@@ -19,6 +19,14 @@ class StateBase : public NodeBase
StateStatus status_;
public:
+ /** \brief Constructor with scalar pointer
+ *
+ * Constructor with scalar pointer
+ * \param _st_ptr is the pointer of the first element of the state unit
+ *
+ **/
+ StateBase(WolfScalar* _st_ptr);
+
/** \brief Constructor with whole state storage and index where starts the state unit
*
* Constructor with whole state storage and index where starts the state unit
@@ -27,14 +35,6 @@ class StateBase : public NodeBase
*
**/
StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx);
-
- /** \brief Constructor with scalar pointer
- *
- * Constructor with scalar pointer
- * \param _st_ptr is the pointer of the first element of the state unit
- *
- **/
- StateBase(WolfScalar* _st_ptr);
/** \brief Destructor
*
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 218c6969bbe9ea9c6671013597fa4c22e1e4a5e5..4a853600cfdaa0b13eb96f6e0cca8c5be65288d6 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -42,26 +42,39 @@
class WolfManager
{
protected:
- bool use_complex_angles_;
+ //sets the problem
WolfProblem* problem_;
- std::queue<CaptureBase*> new_captures_;
+
+ //pointer to a sensor providing predictions
SensorBase* sensor_prior_;
- unsigned int window_size_;
+
+ //auxiliar/temporary iterators, frames and captures
FrameBaseIter first_window_frame_;
CaptureRelative* last_capture_relative_;
CaptureRelative* second_last_capture_relative_;
+ std::queue<CaptureBase*> new_captures_;
+
+ //Manager parameters
+ unsigned int window_size_;
WolfScalar new_frame_elapsed_time_;
+ bool use_complex_angles_;
public:
- WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const Eigen::VectorXs& _init_frame,
- const Eigen::MatrixXs& _init_frame_cov, const WolfScalar& _new_frame_elapsed_time = 0.1, const unsigned int& _w_size = 10) :
- use_complex_angles_(_complex_angle),
+ WolfManager(const unsigned int& _state_length,
+ SensorBase* _sensor_prior,
+ const Eigen::VectorXs& _init_frame,
+ const Eigen::MatrixXs& _init_frame_cov,
+ const unsigned int& _w_size = 10,
+ const WolfScalar& _new_frame_elapsed_time = 0.1,
+ const bool _complex_angle = false) :
+
problem_(new WolfProblem(_state_length)),
sensor_prior_(_sensor_prior),
+ last_capture_relative_(nullptr),
+ second_last_capture_relative_(nullptr),
window_size_(_w_size),
new_frame_elapsed_time_(_new_frame_elapsed_time),
- last_capture_relative_(nullptr),
- second_last_capture_relative_(nullptr)
+ use_complex_angles_(_complex_angle)
{
assert( ((!_complex_angle && _init_frame.size() == 3 && _init_frame_cov.cols() == 3 && _init_frame_cov.rows() == 3) ||
(_complex_angle && _init_frame.size() == 4 && _init_frame_cov.cols() == 4 && _init_frame_cov.rows() == 4))
@@ -71,10 +84,7 @@ class WolfManager
// Set initial covariance with a fake ODOM 2D capture to a fix frame
createFrame(_init_frame, TimeStamp(0));
problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
- last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0),
- nullptr,
- Eigen::Vector3s::Zero(),
- _init_frame_cov)));
+ last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov)));
createFrame(_init_frame, TimeStamp(0));
second_last_capture_relative_->processCapture();
}