diff --git a/src/capture_base.h b/src/capture_base.h
index 5915cf94e0fccaf038e9c76a975a692e73164155..155fd298c10d2f809c12c28ecea6e8833238b3d0 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -85,14 +85,13 @@ class CaptureBase : public NodeLinked<FrameBase, FeatureBase>
void setTimeStampToNow();
- // TODO rename to process()
virtual void process();
- // TODO Rename to computeFrameInitialGuess() ... for instance
+ // TODO Move it to ProcessorX class()
+ // Rename to computeFrameInitialGuess() ... for instance
// Another name could be provideFrameInitialGuess();
- // Move it to ProcessorX class()
- // Should be virtual in ProcessorBase with an empty/error message
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const = 0;
+ //Should be virtual in ProcessorBase with an empty/error message
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp& _now) const = 0;
virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index d665f7aebe1d7a77914c03effdab28450e3793e4..c1994bb8544a958a30942f0760dd49d68d6c08a5 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -25,7 +25,7 @@ void CaptureFix::process()
//std::cout << "ConstraintFix added " << std::endl;
}
-Eigen::VectorXs CaptureFix::computePrior(const TimeStamp& _now) const
+Eigen::VectorXs CaptureFix::computeFramePose(const TimeStamp& _now) const
{
return data_;
}
diff --git a/src/capture_fix.h b/src/capture_fix.h
index eb65845e1fd4658203921e59563dd4e75f78120c..18740611450fffa0d8c234415d51904f8796c766 100644
--- a/src/capture_fix.h
+++ b/src/capture_fix.h
@@ -29,6 +29,6 @@ class CaptureFix : public CaptureBase
virtual void process();
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp& _now) const;
};
#endif
diff --git a/src/capture_gps.cpp b/src/capture_gps.cpp
index b4fb3019aa9840f7c1a482bcde7be57cddc1d76b..f2d1a24a4395d8d35beb2643bbc2f7d7f7bf9dba 100644
--- a/src/capture_gps.cpp
+++ b/src/capture_gps.cpp
@@ -47,7 +47,7 @@ void CaptureGPS::process()
/*
* Dummy implementation of the method, only because it's pure virtual
*/
-Eigen::VectorXs CaptureGPS::computePrior(const TimeStamp &_now) const
+Eigen::VectorXs CaptureGPS::computeFramePose(const TimeStamp &_now) const
{
return Eigen::Vector3s(0, 0, 0);
}
diff --git a/src/capture_gps.h b/src/capture_gps.h
index 697ed58006124c06777841e8ed1205ae04f6ac0b..e1c7ed974757280f73e65a6e07a36035bb074863 100644
--- a/src/capture_gps.h
+++ b/src/capture_gps.h
@@ -32,7 +32,7 @@ public:
/*
* Dummy implementation of the method, only because it's pure virtual
*/
- virtual Eigen::VectorXs computePrior(const TimeStamp &_now) const;
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp &_now) const;
};
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index 50960a1d9de91fe63b958d2fe6c01eef7195e7b4..ec7790a3cdcfff0024d5c4dccb00509839279829 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -29,7 +29,7 @@ void CaptureGPSFix::process()
getFeatureListPtr()->front()->addConstraintFrom(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()));
}
-Eigen::VectorXs CaptureGPSFix::computePrior(const TimeStamp& _now) const
+Eigen::VectorXs CaptureGPSFix::computeFramePose(const TimeStamp& _now) const
{
return data_;
}
diff --git a/src/capture_gps_fix.h b/src/capture_gps_fix.h
index 71a0a010ca0ca8460f50f9e1a5ffa58d9304e65b..8cf7bc4b64ad68cff0edae3aa823d2fd36710424 100644
--- a/src/capture_gps_fix.h
+++ b/src/capture_gps_fix.h
@@ -29,7 +29,7 @@ class CaptureGPSFix : public CaptureBase
virtual void process();
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp& _now) const;
//virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 0b4dd581236d54af937dbaad9d85d28d1a739618..7ea74c2b489f7834939b2497e1778e5a5f43d583 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -51,7 +51,7 @@ CaptureLaser2D::~CaptureLaser2D()
}
-Eigen::VectorXs CaptureLaser2D::computePrior(const TimeStamp& _now) const
+Eigen::VectorXs CaptureLaser2D::computeFramePose(const TimeStamp& _now) const
{
return Eigen::Vector3s(1, 2, 3);
}
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 03511c65cb3d14b92ea70619400aceef593d1ed0..1e36f87da5e54aa5e687a2bdf5d88e0c4b7489af 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -100,7 +100,7 @@ class CaptureLaser2D : public CaptureBase
//
// void computeExpectedFeature(LandmarkBase* _landmark_ptr, Eigen::Vector4s& expected_feature_, Eigen::Matrix3s& expected_feature_cov_);
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp& _now) const;
// Eigen::VectorXs computeSquaredMahalanobisDistances(const FeatureBase* _feature_ptr, const LandmarkBase* _landmark_ptr, const Eigen::MatrixXs& _mu);
// Eigen::VectorXs computeSquaredMahalanobisDistances(const FeatureBase* _feature_ptr, const Eigen::Vector4s& _expected_feature, const Eigen::Matrix3s& _expected_feature_cov, const Eigen::MatrixXs& _mu);
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index dde778ccfaf858e952439ddd831224d0da63e429..8d9d51549ccd730d199366e5afbb42b7adade80c 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -33,7 +33,7 @@ inline void CaptureOdom2D::process()
addConstraints();
}
-Eigen::VectorXs CaptureOdom2D::computePrior(const TimeStamp& _now) const
+Eigen::VectorXs CaptureOdom2D::computeFramePose(const TimeStamp& _now) const
{
assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index ad535c270a7988e780cd3c0de743f2c310a3b4fd..d6f36c193b0bc671c617ec829547bcf1e7f63a58 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -34,7 +34,7 @@ class CaptureOdom2D : public CaptureMotion
virtual void process();
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now = 0) const;
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp& _now = 0) const;
virtual void addConstraints();
diff --git a/src/capture_void.cpp b/src/capture_void.cpp
index ad7ae05c2426090f5d99c596dc8bee79d24632ad..06966eed5eaa59d9586c9a38a7678d7a6e50ea8b 100644
--- a/src/capture_void.cpp
+++ b/src/capture_void.cpp
@@ -11,7 +11,7 @@ CaptureVoid::~CaptureVoid()
//std::cout << "deleting CaptureVoid " << nodeId() << std::endl;
}
-Eigen::VectorXs CaptureVoid::computePrior(const TimeStamp& _now) const
+Eigen::VectorXs CaptureVoid::computeFramePose(const TimeStamp& _now) const
{
return Eigen::VectorXs::Zero(3);
}
diff --git a/src/capture_void.h b/src/capture_void.h
index 1de4522e5871a09d7e55342284f099627e799441..03cf49d94d5c7cc47f46f97b15685d347f3ab4b9 100644
--- a/src/capture_void.h
+++ b/src/capture_void.h
@@ -17,6 +17,6 @@ class CaptureVoid : public CaptureBase
**/
virtual ~CaptureVoid();
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
+ virtual Eigen::VectorXs computeFramePose(const TimeStamp& _now) const;
};
#endif
diff --git a/src/processor_base.h b/src/processor_base.h
index 6dbae45d5919c58f869f222a4af2a00111d7c9db..50976b1485fe2fc4b057bd6c2ea990079def476f 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -5,13 +5,14 @@
//Wolf includes
#include "wolf.h"
+#include "time_stamp.h"
#include "wolf_problem.h"
#include "node_linked.h"
#include "node_terminus.h"
#include "sensor_base.h"
//class ProcessorBase
-class ProcessorBase : public NodeLinked<SensorBase,NodeTerminus>
+class ProcessorBase : public NodeLinked<SensorBase, NodeTerminus>
{
public:
/** \brief Constructor
@@ -25,23 +26,30 @@ class ProcessorBase : public NodeLinked<SensorBase,NodeTerminus>
*
* Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
*
- **/
+ **/
virtual ~ProcessorBase();
SensorBase* getSensorPtr();
/** \brief Extract Features
- *
- * Extract Features from a given capture
- *
- **/
+ *
+ * Extract Features from a given capture
+ *
+ **/
virtual void extractFeatures(CaptureBase* _capture_ptr) = 0;
/** \brief Establish Constraints
- *
- * Establish Constraints for all features of a given capture
- *
- **/
+ *
+ * Establish Constraints for all features of a given capture
+ *
+ **/
virtual void establishConstraints(CaptureBase* _capture_ptr) = 0;
+
+ virtual Eigen::VectorXs provideFrameInitialGuess(const TimeStamp& _now) const
+ {
+ return Eigen::VectorXs(1);
+ }
+
+
};
#endif
diff --git a/src/wolf_manager.cpp b/src/wolf_manager.cpp
index 5fa139d0889fe97b9a97c6a6001520240166a588..6ab9fa3ba0b6bcfcd24f7d9df0bed933ce5dba79 100644
--- a/src/wolf_manager.cpp
+++ b/src/wolf_manager.cpp
@@ -129,7 +129,7 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta
void WolfManager::createFrame(const TimeStamp& _time_stamp)
{
//std::cout << "creating new frame from prior..." << std::endl;
- createFrame(last_capture_relative_->computePrior(_time_stamp), _time_stamp);
+ createFrame(last_capture_relative_->computeFramePose(_time_stamp), _time_stamp);
}
void WolfManager::addSensor(SensorBase* _sensor_ptr)
@@ -199,7 +199,7 @@ void WolfManager::update()
// ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
last_capture_relative_->integrateCapture((CaptureMotion*) (new_capture));
- current_frame_->setState(last_capture_relative_->computePrior(new_capture->getTimeStamp()));
+ current_frame_->setState(last_capture_relative_->computeFramePose(new_capture->getTimeStamp()));
current_frame_->setTimeStamp(new_capture->getTimeStamp());
delete new_capture;
}
@@ -233,7 +233,7 @@ Eigen::VectorXs WolfManager::getVehiclePose(const TimeStamp& _now)
if (last_capture_relative_ == nullptr)
return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr());
else
- return last_capture_relative_->computePrior(_now);
+ return last_capture_relative_->computeFramePose(_now);
}
diff --git a/src/wolf_manager_gps.cpp b/src/wolf_manager_gps.cpp
index 626a9a413201fb4face2bb09797c101e88bd4c33..e09fe07daf15652d5542b80c7b4376f2df89c593 100644
--- a/src/wolf_manager_gps.cpp
+++ b/src/wolf_manager_gps.cpp
@@ -171,7 +171,7 @@ void WolfManagerGPS::update()
// ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
last_capture_relative_->integrateCapture((CaptureMotion*) (new_capture));
- current_frame_->setState(last_capture_relative_->computePrior(new_capture->getTimeStamp()));
+ current_frame_->setState(last_capture_relative_->computeFramePose(new_capture->getTimeStamp()));
current_frame_->setTimeStamp(new_capture->getTimeStamp());
delete new_capture;
}
@@ -205,7 +205,7 @@ Eigen::VectorXs WolfManagerGPS::getVehiclePose(const TimeStamp& _now)
if (last_capture_relative_ == nullptr)
return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr());
else
- return last_capture_relative_->computePrior(_now);
+ return last_capture_relative_->computeFramePose(_now);
}