From a5b3b1e8be27b99245a4e6a261083431919b4889 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Wed, 16 Dec 2015 18:01:38 +0100
Subject: [PATCH] computePrior() renamed to computeFramePose(). Still in
CaptureBase.
---
src/capture_base.h | 9 ++++-----
src/capture_fix.cpp | 2 +-
src/capture_fix.h | 2 +-
src/capture_gps.cpp | 2 +-
src/capture_gps.h | 2 +-
src/capture_gps_fix.cpp | 2 +-
src/capture_gps_fix.h | 2 +-
src/capture_laser_2D.cpp | 2 +-
src/capture_laser_2D.h | 2 +-
src/capture_odom_2D.cpp | 2 +-
src/capture_odom_2D.h | 2 +-
src/capture_void.cpp | 2 +-
src/capture_void.h | 2 +-
src/processor_base.h | 28 ++++++++++++++++++----------
src/wolf_manager.cpp | 6 +++---
src/wolf_manager_gps.cpp | 4 ++--
16 files changed, 39 insertions(+), 32 deletions(-)
diff --git a/src/capture_base.h b/src/capture_base.h
index 5915cf94e..155fd298c 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 d665f7aeb..c1994bb85 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 eb65845e1..187406114 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 b4fb3019a..f2d1a24a4 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 697ed5800..e1c7ed974 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 50960a1d9..ec7790a3c 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 71a0a010c..8cf7bc4b6 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 0b4dd5812..7ea74c2b4 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 03511c65c..1e36f87da 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 dde778ccf..8d9d51549 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 ad535c270..d6f36c193 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 ad7ae05c2..06966eed5 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 1de4522e5..03cf49d94 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 6dbae45d5..50976b148 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 5fa139d08..6ab9fa3ba 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 626a9a413..e09fe07da 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);
}
--
GitLab