diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 25d8c530677fc6b3052b77e5c5a0cedad44a0925..6b8524bdc8f6d293803f7a6371f09c0b39656e72 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -77,7 +77,9 @@ SET(HDRS
frame_base.h
landmark_base.h
landmark_corner_2D.h
+ landmark_container.h
map_base.h
+ map_containers.h
node_base.h
node_terminus.h
node_linked.h
@@ -117,7 +119,9 @@ SET(SRCS
frame_base.cpp
landmark_base.cpp
landmark_corner_2D.cpp
+ landmark_container.cpp
map_base.cpp
+ map_containers.cpp
node_base.cpp
node_terminus.cpp
sensor_base.cpp
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 2d6629d3ab6373792e5468a2501315b51d2ff5c5..27d1f68cb468ef17b462d25fd30f9a0958fa029f 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -127,7 +127,7 @@ void CaptureLaser2D::establishConstraints()
WolfScalar min_distance_sq = max_distance_matching_sq;
//Find the closest landmark to the feature
- LandmarkCorner2D* correspondent_landmark = nullptr;
+ LandmarkBase* correspondent_landmark = nullptr;
const Eigen::Vector2s& feature_position = (*feature_it)->getMeasurement().head(2);
const WolfScalar& feature_orientation = (*feature_it)->getMeasurement()(2);
const WolfScalar& feature_aperture = (*feature_it)->getMeasurement()(3);
@@ -173,7 +173,7 @@ void CaptureLaser2D::establishConstraints()
// std::cout << " OK!" << std::endl;
// std::cout << "Closer landmark found (satisfying orientation and aperture)" << std::endl;
- correspondent_landmark = (LandmarkCorner2D*) (*landmark_it);
+ correspondent_landmark = (*landmark_it);
min_distance_sq = distance_sq;
}
// else
@@ -190,18 +190,18 @@ void CaptureLaser2D::establishConstraints()
// std::cout << "+++++ No landmark found. Creating a new one..." << std::endl;
StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr());
getTop()->addState(new_landmark_state_position, feature_global_position);
- StateBase* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
+ StateOrientation* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
getTop()->addState(new_landmark_state_orientation, Eigen::Map < Eigen::Vector1s > (&feature_global_orientation, 1));
- correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture);
- LandmarkBase* corr_landmark(correspondent_landmark);
- getTop()->getMapPtr()->addLandmark(corr_landmark);
+ correspondent_landmark = (LandmarkBase*)new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture);
+ getTop()->getMapPtr()->addLandmark(correspondent_landmark);
// std::cout << "Landmark created: " <<
// getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl <<
// feature_global_position.transpose() <<
// "\t" << feature_global_orientation <<
// "\t" << feature_aperture << std::endl;
+
}
else
{
@@ -212,7 +212,7 @@ void CaptureLaser2D::establishConstraints()
// << (*feature_it)->nodeId() << std::endl;
// Add constraint to the correspondent landmark
- (*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it, correspondent_landmark, getFramePtr()->getPPtr(), //_robotPPtr,
+ (*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it, (LandmarkCorner2D*)correspondent_landmark, getFramePtr()->getPPtr(), //_robotPPtr,
getFramePtr()->getOPtr(), //_robotOPtr,
correspondent_landmark->getPPtr(), //_landmarkPPtr,
correspondent_landmark->getOPtr())); //_landmarkOPtr,
@@ -229,7 +229,7 @@ void CaptureLaser2D::establishConstraintsMHTree()
std::vector<bool> associated_mask;
Eigen::Vector2s feature_global_position;
WolfScalar feature_global_orientation;
- LandmarkCorner2D* correspondent_landmark = nullptr;
+ LandmarkBase* correspondent_landmark = nullptr;
// Global transformation TODO: Change by a function
Eigen::Vector2s t_robot = getFramePtr()->getPPtr()->getVector();
@@ -251,17 +251,26 @@ void CaptureLaser2D::establishConstraintsMHTree()
jj = 0;
for (auto j_it = getTop()->getMapPtr()->getLandmarkListPtr()->begin(); j_it != getTop()->getMapPtr()->getLandmarkListPtr()->end(); j_it++, jj++)
{
- //If aperture difference is small enough, proceed with Mahalanobis distance. Otherwise Set prob to 0 to force unassociation
- apert_diff = fabs( (*i_it)->getMeasurement(3) - (*j_it)->getDescriptor(0) );
- if (apert_diff < MAX_ACCEPTED_APERTURE_DIFF)
+ if ((*j_it)->getType() == LANDMARK_CORNER)
+ {
+ //If aperture difference is small enough, proceed with Mahalanobis distance. Otherwise Set prob to 0 to force unassociation
+ apert_diff = fabs( (*i_it)->getMeasurement(3) - (*j_it)->getDescriptor(0) );
+ if (apert_diff < MAX_ACCEPTED_APERTURE_DIFF)
+ {
+ dm2 = computeMahalanobisDistance(*i_it, *j_it);//Mahalanobis squared
+ if (dm2 < 5*5)
+ prob = erfc( sqrt(dm2)/1.4142136 );// sqrt(2) = 1.4142136
+ else
+ prob = 0;
+ tree.setScore(ii,jj,prob);
+ }
+ else
+ tree.setScore(ii,jj,0.);//prob to 0
+ }
+ else if ((*j_it)->getType() == LANDMARK_CONTAINER)
{
- dm2 = computeMahalanobisDistance(*i_it, *j_it);//Mahalanobis squared
- if (dm2 < 5*5) prob = erfc( sqrt(dm2)/1.4142136 );// sqrt(2) = 1.4142136
- else prob = 0;
- tree.setScore(ii,jj,prob);
+ //TODO
}
- else
- tree.setScore(ii,jj,0.);//prob to 0
}
}
@@ -295,13 +304,15 @@ void CaptureLaser2D::establishConstraintsMHTree()
//create new landmark at global coordinates
StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr());
getTop()->addState(new_landmark_state_position, feature_global_position);
- StateBase* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
+ StateOrientation* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
getTop()->addState(new_landmark_state_orientation, Eigen::Map < Eigen::Vector1s > (&feature_global_orientation, 1));
//add it to the slam map as a new landmark
correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture);
- LandmarkBase* corr_landmark(correspondent_landmark);
- getTop()->getMapPtr()->addLandmark(corr_landmark);
+ getTop()->getMapPtr()->addLandmark(correspondent_landmark);
+
+ // Try to build a container
+ tryContainer((LandmarkCorner2D*)correspondent_landmark);
}
else //feature-landmark association case
{
@@ -311,7 +322,7 @@ void CaptureLaser2D::establishConstraintsMHTree()
{
if ( jj == ft_lk_pairs[kk].second )
{
- correspondent_landmark = (LandmarkCorner2D*) (*j_it);
+ correspondent_landmark = (*j_it);
correspondent_landmark->hit();
break;
}
@@ -324,7 +335,7 @@ void CaptureLaser2D::establishConstraintsMHTree()
// Add constraint to the correspondent landmark
(*i_it)->addConstraint(new ConstraintCorner2DTheta(
*i_it, //feature pointer
- correspondent_landmark, //landmark pointer
+ (LandmarkCorner2D*)correspondent_landmark, //landmark pointer
getFramePtr()->getPPtr(), //_robotPPtr,
getFramePtr()->getOPtr(), //_robotOPtr,
correspondent_landmark->getPPtr(), //_landmarkPPtr,
@@ -426,3 +437,49 @@ WolfScalar CaptureLaser2D::computeMahalanobisDistance(const FeatureBase* _featur
return 0;
}
}
+
+void CaptureLaser2D::tryContainer(const LandmarkCorner2D* _corner_ptr)
+{
+ LandmarkContainer* container_ptr = nullptr;
+ Eigen::Matrix2s R_corner = (_corner_ptr->getOPtr())->getRotationMatrix().topLeftCorner<2,2>();
+
+ // It has to be 90º corner
+ if (std::fabs(_corner_ptr->getAperture() - M_PI / 2) < MAX_ACCEPTED_APERTURE_DIFF)
+ {
+ // Check all existing corners searching a container
+ for (auto landmark_it = getTop()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != getTop()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ // should be a 90º corner
+ if ((*landmark_it)->getType() == LANDMARK_CORNER && std::fabs(((LandmarkCorner2D*)(*landmark_it))->getAperture() - M_PI / 2) < MAX_ACCEPTED_APERTURE_DIFF)
+ {
+ if (container_ptr == nullptr)
+ {
+ Eigen::Vector2s difference = _corner_ptr->getPPtr()->getVector() - (*landmark_it)->getPPtr()->getVector();
+
+ // Short side
+ if (std::fabs(difference.norm() - 2.44) < 0.15)
+ {
+ std::cout << "Container detected by the short size!" << std::endl;
+ }
+ // Long side
+ else if (std::fabs(difference.norm() - 12.2) < 0.15)
+ {
+ std::cout << "Container detected by the long size!" << std::endl;
+ }
+ // Diagonal
+ else if (std::fabs(difference.norm() - 12.44) < 0.15)
+ {
+ std::cout << "Container detected by the diagonal!" << std::endl;
+ }
+ }
+ else
+ {
+ container_ptr->tryCorner((LandmarkCorner2D*)(*landmark_it));
+ }
+ }
+ }
+ }
+
+ if (container_ptr != nullptr)
+ getTop()->getMapPtr()->addLandmark((LandmarkBase*)(container_ptr));
+}
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index cab6248f17389418b5a22d1ca57414f1758deef1..62464b70bbd9c28e7a7469ca5efc1d9f7b5e641f 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -23,6 +23,7 @@
#include "sensor_laser_2D.h"
#include "feature_corner_2D.h"
#include "landmark_corner_2D.h"
+#include "landmark_container.h"
#include "state_point.h"
#include "state_orientation.h"
#include "state_theta.h"
@@ -126,5 +127,7 @@ class CaptureLaser2D : public CaptureBase
virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
WolfScalar computeMahalanobisDistance(const FeatureBase* _feature, const LandmarkBase* _landmark);
+
+ void tryContainer(const LandmarkCorner2D* _corner_ptr);
};
#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index e6ce37ed46cae2620f9fdb7bc34e527db912b8fe..7d48f39ffb0b7e6ed4302ffc7d98bbcef8359c17 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -1,17 +1,13 @@
#include "capture_odom_2D.h"
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr) :
- CaptureRelative(_init_ts, _sensor_ptr),
- initial_time_stamp_(_init_ts),
- final_time_stamp_(_final_ts)
+ CaptureRelative(_init_ts, _final_ts, _sensor_ptr)
{
//
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector3s& _data) :
- CaptureRelative(_init_ts, _sensor_ptr, _data),
- initial_time_stamp_(_init_ts),
- final_time_stamp_(_final_ts)
+ CaptureRelative(_init_ts, _final_ts, _sensor_ptr, _data)
{
data_covariance_ = Eigen::Matrix3s::Zero();
data_covariance_(0, 0) = std::max(1e-10, _data(0) * _data(0) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor() * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
@@ -21,16 +17,14 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::Vector3s& _data, const Eigen::Matrix3s& _data_covariance) :
- CaptureRelative(_init_ts, _sensor_ptr, _data, _data_covariance),
- initial_time_stamp_(_init_ts),
- final_time_stamp_(_final_ts)
+ CaptureRelative(_init_ts, _final_ts, _sensor_ptr, _data, _data_covariance)
{
//
}
CaptureOdom2D::~CaptureOdom2D()
{
- //std::cout << "Destroying GPS fix capture...\n";
+ //std::cout << "Destroying CaptureOdom2D capture...\n";
}
inline void CaptureOdom2D::processCapture()
@@ -110,7 +104,7 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
// TODO Jacobians!
data_covariance_ += _new_capture->getDataCovariance();
- final_time_stamp_ = _new_capture->final_time_stamp_;
+ this->final_time_stamp_ = _new_capture->getFinalTimeStamp();
getFeatureListPtr()->front()->setMeasurement(data_);
getFeatureListPtr()->front()->setMeasurementCovariance(data_covariance_);
@@ -119,7 +113,7 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
CaptureOdom2D* CaptureOdom2D::interpolateCapture(const TimeStamp& _ts)
{
- WolfScalar ratio = (_ts.get() - initial_time_stamp_.get()) / (final_time_stamp_.get() - initial_time_stamp_.get());
+ WolfScalar ratio = (_ts.get() - this->time_stamp_.get()) / (this->final_time_stamp_.get() - this->time_stamp_.get());
// Second part
CaptureOdom2D* second_odom_ptr = new CaptureOdom2D(_ts, final_time_stamp_, sensor_ptr_, data_ * (1-ratio), data_covariance_ * (1-ratio));
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index d8c671d2fd55c26ee8a4f89c14fdd07a2e68470d..26f71aa48847763144c0642f732eea149fa97a81 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -13,9 +13,6 @@
//class CaptureGPSFix
class CaptureOdom2D : public CaptureRelative
{
- protected:
- TimeStamp initial_time_stamp_; ///< Initial Time stamp
- TimeStamp final_time_stamp_; ///< Final Time stamp
public:
CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr);
@@ -28,7 +25,7 @@ class CaptureOdom2D : public CaptureRelative
virtual void processCapture();
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
+ virtual Eigen::VectorXs computePrior(const TimeStamp& _now = 0) const;
virtual void addConstraints();
diff --git a/src/capture_relative.cpp b/src/capture_relative.cpp
index 4036d5f1d9da36540ec9b79e17ca5e7517aebb47..54e0c8b0d504052d79a8686bf3da1874a4dd3e05 100644
--- a/src/capture_relative.cpp
+++ b/src/capture_relative.cpp
@@ -1,19 +1,22 @@
#include "capture_relative.h"
-CaptureRelative::CaptureRelative(const TimeStamp& _ts, SensorBase* _sensor_ptr) :
- CaptureBase(_ts, _sensor_ptr)
+CaptureRelative::CaptureRelative(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr) :
+ CaptureBase(_init_ts, _sensor_ptr),
+ final_time_stamp_(_final_ts)
{
//
}
-CaptureRelative::CaptureRelative(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data) :
- CaptureBase(_ts, _sensor_ptr, _data)
+CaptureRelative::CaptureRelative(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data) :
+ CaptureBase(_init_ts, _sensor_ptr, _data),
+ final_time_stamp_(_final_ts)
{
//
}
-CaptureRelative::CaptureRelative(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase(_ts, _sensor_ptr, _data, _data_covariance)
+CaptureRelative::CaptureRelative(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+ CaptureBase(_init_ts, _sensor_ptr, _data, _data_covariance),
+ final_time_stamp_(_final_ts)
{
//
}
@@ -21,3 +24,23 @@ CaptureRelative::~CaptureRelative()
{
//
}
+
+TimeStamp CaptureRelative::getInitTimeStamp() const
+{
+ return this->getTimeStamp();
+}
+
+TimeStamp CaptureRelative::getFinalTimeStamp() const
+{
+ return final_time_stamp_;
+}
+
+void CaptureRelative::setInitTimeStamp(const TimeStamp & _ts)
+{
+ this->setTimeStamp(_ts);
+}
+
+void CaptureRelative::setFinalTimeStamp(const TimeStamp & _ts)
+{
+ final_time_stamp_ = _ts;
+}
diff --git a/src/capture_relative.h b/src/capture_relative.h
index 4c4707aea46dbfb8ef96972b3be20613b41c6cf4..6fb665e830950f2c936ba193b3b584faac0bf6ca 100644
--- a/src/capture_relative.h
+++ b/src/capture_relative.h
@@ -12,17 +12,28 @@
//class CaptureBase
class CaptureRelative : public CaptureBase
{
+ protected:
+ TimeStamp final_time_stamp_; ///< Final Time stamp
+
public:
- CaptureRelative(const TimeStamp& _ts, SensorBase* _sensor_ptr);
+ CaptureRelative(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr);
- CaptureRelative(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
+ CaptureRelative(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
- CaptureRelative(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CaptureRelative(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureRelative();
virtual void integrateCapture(CaptureRelative* _new_capture) = 0;
virtual CaptureRelative* interpolateCapture(const TimeStamp& _ts) = 0;
+
+ TimeStamp getInitTimeStamp() const;
+
+ TimeStamp getFinalTimeStamp() const;
+
+ void setInitTimeStamp(const TimeStamp & _ts);
+
+ void setFinalTimeStamp(const TimeStamp & _ts);
};
#endif
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 2f38937a2b963f50169261cdbe86f4acc77d7df4..dfc720d395c7c130ca14c10481bb5b008032222a 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -36,7 +36,7 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
return ceres_summary_;
}
-void CeresManager::computeCovariances(RadarOdom+* _problem_ptr)
+void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
{
//std::cout << "computing covariances..." << std::endl;
@@ -128,7 +128,7 @@ void CeresManager::computeCovariances(RadarOdom+* _problem_ptr)
}
}
-void CeresManager::update(RadarOdom+* _problem_ptr)
+void CeresManager::update(WolfProblem* _problem_ptr)
{
// IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
if (_problem_ptr->isReallocated())
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index f156a31780cc8486c2e3acc052b4fe949ae6d514..da9cdca74a9ec35983578eed338ef6a7b654a4fd 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -39,7 +39,7 @@ class CeresManager
ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
- void computeCovariances(RadarOdom+* _problem_ptr);
+ void computeCovariances(WolfProblem* _problem_ptr);
void update(const WolfProblemPtr _problem_ptr);
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index 47e4babe0095f76a46c6b87003e1bc2ab6cf3f62..62fd1e45b999b32f775797d7cc538f432491b6d6 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -21,7 +21,7 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
// //
// }
- ConstraintCorner2DTheta(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, StateBase* _robotPPtr, StateBase* _robotOPtr, StateBase* _landmarkPPtr, StateBase* _landmarkOPtr) :
+ ConstraintCorner2DTheta(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, StateBase* _robotPPtr, StateOrientation* _robotOPtr, StateBase* _landmarkPPtr, StateOrientation* _landmarkOPtr) :
ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D_THETA, _robotPPtr, _robotOPtr,_landmarkPPtr, _landmarkOPtr),
lmk_ptr_(_lmk_ptr)
{
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h
index 3cc76af752fcaf3f2c6b6b427088879cec46e83a..167ac61f87b856396b56e561d040802b934d7157 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D_theta.h
@@ -16,7 +16,7 @@ class ConstraintOdom2DTheta : public ConstraintSparse<3, 2, 1, 2, 1>
// //
// }
- ConstraintOdom2DTheta(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
+ ConstraintOdom2DTheta(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateOrientation* _state1Ptr, StateBase* _state2Ptr, StateOrientation* _state3Ptr) :
ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr)
{
//
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 5ec7d26e1eef8e298a0623d9afe56eb84ece33c4..0c69241e989a7fa44d6e12afa5a485914f3b161b 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -235,7 +235,7 @@ int main(int argc, char** argv)
std::cout << "ADD CAPTURES..." << std::endl;
t1 = clock();
// adding new sensor captures
- wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+ wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
// wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
// wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/test_iQR_wolf2.cpp
index 6aee8612fe645561573cf894d9ebc8d7d1634ab7..f99aeb028750d6fbc2b5c210b86f860ebfe504a3 100644
--- a/src/examples/test_iQR_wolf2.cpp
+++ b/src/examples/test_iQR_wolf2.cpp
@@ -244,9 +244,9 @@ int main(int argc, char *argv[])
// ADD CAPTURES ---------------------------
//std::cout << "ADD CAPTURES..." << std::endl;
// adding new sensor captures
- wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+ wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
- wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+ wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
diff --git a/src/examples/test_node_linked.cpp b/src/examples/test_node_linked.cpp
index 3326040dba103c297522e6caf037cfcac3919271..3a1f1bec7383bd258ac61324a00491297d091936 100644
--- a/src/examples/test_node_linked.cpp
+++ b/src/examples/test_node_linked.cpp
@@ -13,10 +13,10 @@ using namespace std;
class TrajectoryN;
class FrameN;
class MeasurementN;
-class RadarOdom+;
+class WolfProblem;
//class TrajectoryN
-class TrajectoryN : public NodeLinked<RadarOdom+,FrameN>
+class TrajectoryN : public NodeLinked<WolfProblem,FrameN>
{
protected:
unsigned int length_; //just something to play
@@ -99,7 +99,7 @@ int main()
cout << "========================================================" << endl;
cout << endl << "TEST 1. Constructors" << endl;
- RadarOdom+* problem_(new RadarOdom+());
+ WolfProblem* problem_(new WolfProblem());
TrajectoryN* trajectory_(new TrajectoryN(2));
FrameN* frame_1_(new FrameN(1.011));
FrameN* frame_2_(new FrameN(2.022));
@@ -136,7 +136,7 @@ int main()
cout << "========================================================" << endl;
cout << endl << "TEST 5. getTop()" << endl;
- RadarOdom+* nb_ptr = sensor_data_radar_->getTop();
+ WolfProblem* nb_ptr = sensor_data_radar_->getTop();
//shared_ptr<TrajectoryN> nb_shptr((TrajectoryN*)nb_ptr);
cout << "TOP node is: " << nb_ptr->nodeId() << endl;
//cout << "nb_shptr.use_count(): " << nb_shptr.use_count() << "; value: " << nb_shptr.get() << endl;
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index c0a40563e634ba1b2aa2c79351dfe6a262c48ae9..5f09e480c762d2b6f6165e9c5b4204f3e341ac01 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -1,7 +1,7 @@
#include "frame_base.h"
-FrameBase::FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+FrameBase::FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateOrientation* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
//NodeLinked(MID, "FRAME", _traj_ptr),
NodeLinked(MID, "FRAME"),
type_(REGULAR_FRAME),
@@ -15,7 +15,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr,
//
}
-FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateOrientation* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
//NodeLinked(MID, "FRAME", _traj_ptr),
NodeLinked(MID, "FRAME"),
type_(_tp),
@@ -203,7 +203,7 @@ StateBase* FrameBase::getPPtr() const
return p_ptr_;
}
-StateBase* FrameBase::getOPtr() const
+StateOrientation* FrameBase::getOPtr() const
{
return o_ptr_;
}
diff --git a/src/frame_base.h b/src/frame_base.h
index fec673a1237ed3f720d838de1a8c7be337362f02..51c3742027641ce7984cc28910ab2b7f08e0d2a7 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -28,7 +28,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
TimeStamp time_stamp_; //frame time stamp
StateStatus status_; // status of the estimation of the frame state
StateBase* p_ptr_; // Position state unit pointer
- StateBase* o_ptr_; // Orientation state unit pointer
+ StateOrientation* o_ptr_; // Orientation state unit pointer
StateBase* v_ptr_; // Velocity state unit pointer
StateBase* w_ptr_; // Angular velocity state unit pointer
//TODO: accelerations?
@@ -45,7 +45,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
* \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
*
**/
- FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr); //ACM: Either remove all pointer arguments from this header, or merge both constructors in a single one
+ FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateOrientation* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr); //ACM: Either remove all pointer arguments from this header, or merge both constructors in a single one
/** \brief Constructor with type, time stamp and state pointer
*
@@ -59,7 +59,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
* \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
*
**/
- FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateOrientation* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
/** \brief Destructor
*
@@ -111,7 +111,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
StateBase* getPPtr() const;
- StateBase* getOPtr() const;
+ StateOrientation* getOPtr() const;
StateBase* getVPtr() const;
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 3a235217ba14a3a4c0e7837dfa8f3090382cb167..e80a0f9892234b3428b08672a3d50acfcbb42f01 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -1,7 +1,7 @@
#include "landmark_base.h"
-LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
NodeLinked(MID, "LANDMARK"),
type_(_tp),
status_(LANDMARK_CANDIDATE),
@@ -96,7 +96,7 @@ StateBase* LandmarkBase::getPPtr() const
return p_ptr_;
}
-StateBase* LandmarkBase::getOPtr() const
+StateOrientation* LandmarkBase::getOPtr() const
{
return o_ptr_;
}
@@ -126,6 +126,11 @@ WolfScalar LandmarkBase::getDescriptor(unsigned int _ii) const
return descriptor_(_ii);
}
+const LandmarkType LandmarkBase::getType() const
+{
+ return type_;
+}
+
//const StateBasePtr LandmarkBase::getStatePtr() const
//{
// return st_list_.front();
diff --git a/src/landmark_base.h b/src/landmark_base.h
index b7052c50e13bb37a4200d1e6b90d2e079694b28d..4ccdc187af375119a81c7466d79305f5000b6454 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -16,6 +16,7 @@ class NodeTerminus;
#include "time_stamp.h"
#include "node_linked.h"
#include "map_base.h"
+#include "state_orientation.h"
// why v, w and a ?
// add descriptor as a StateBase -> Could be estimated or not. Aperture could be one case of "descriptor"that can be estimated or not
@@ -33,7 +34,7 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
TimeStamp stamp_; // stamp of the creation of the landmark (and stamp of destruction when status is LANDMARK_OLD)
//StateBaseList st_list_; //List of pointers to the state corresponding to the landmark estimation
StateBase* p_ptr_; // Position state unit pointer
- StateBase* o_ptr_; // Orientation state unit pointer
+ StateOrientation* o_ptr_; // Orientation state unit pointer
StateBase* v_ptr_; // Velocity state unit pointer
StateBase* w_ptr_; // Angular velocity state unit pointer
//TODO: accelerations?
@@ -45,12 +46,12 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
* Constructor with type, and state pointer
* \param _tp indicates landmark type.(types defined at wolf.h)
* \param _p_ptr StateBase pointer to the position
- * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
+ * \param _o_ptr StateOrientation pointer to the orientation (default: nullptr)
* \param _v_ptr StateBase pointer to the velocity (default: nullptr)
* \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
*
**/
- LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
/** \brief Constructor with type, time stamp and state list
*
@@ -81,7 +82,7 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
StateBase* getPPtr() const;
- StateBase* getOPtr() const;
+ StateOrientation* getOPtr() const;
StateBase* getVPtr() const;
@@ -99,6 +100,8 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
**/
WolfScalar getDescriptor(unsigned int _ii) const;
+ const LandmarkType getType() const;
+
//const StateBase* getStatePtr() const;
//const StateBaseList* getStateListPtr() const;
diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp
index 176effa1820cfb32c61f20a432e6910981322c93..d0d35e3732f92b166f8ad337490b3989f5062605 100644
--- a/src/landmark_corner_2D.cpp
+++ b/src/landmark_corner_2D.cpp
@@ -1,7 +1,7 @@
#include "landmark_corner_2D.h"
-LandmarkCorner2D::LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture) :
+LandmarkCorner2D::LandmarkCorner2D(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _aperture) :
LandmarkBase(LANDMARK_CORNER, _p_ptr, _o_ptr)
{
setDescriptor(Eigen::VectorXs::Constant(1,_aperture));
diff --git a/src/landmark_corner_2D.h b/src/landmark_corner_2D.h
index cb6645840316f6aa803928c5cb8987a637d2836b..bd9afb65d13ca211dcfe5fae5f596f15fda7c489 100644
--- a/src/landmark_corner_2D.h
+++ b/src/landmark_corner_2D.h
@@ -28,7 +28,7 @@ class LandmarkCorner2D : public LandmarkBase
* \param _aperture descriptor of the landmark: aperture of the corner
*
**/
- LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture=0);
+ LandmarkCorner2D(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _aperture=0);
/** \brief Destructor
*
diff --git a/src/map_base.h b/src/map_base.h
index 7be3110e732b0a68fd14cff7a3f8cf009b10b798..e33edb0c902d4a767ae03c29218cbe503ce7f502 100644
--- a/src/map_base.h
+++ b/src/map_base.h
@@ -2,7 +2,7 @@
#ifndef MAP_BASE_H_
#define MAP_BASE_H_
-class RadarOdom+;
+class WolfProblem;
class LandmarkBase;
//std includes
@@ -20,7 +20,7 @@ class LandmarkBase;
#include "landmark_base.h"
//class MapBase
-class MapBase : public NodeLinked<RadarOdom+,LandmarkBase>
+class MapBase : public NodeLinked<WolfProblem,LandmarkBase>
{
public:
/** \brief Constructor
@@ -42,7 +42,7 @@ class MapBase : public NodeLinked<RadarOdom+,LandmarkBase>
* Adds a landmark
*
**/
- void addLandmark(LandmarkBase* _landmark_ptr);
+ virtual void addLandmark(LandmarkBase* _landmark_ptr);
/** \brief Removes a landmark
*
diff --git a/src/node_base.h b/src/node_base.h
index 93733cd226bac5a9c73d6ff759131ff96cb062e6..0716fff5118be42392977308365732eb047714fa 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -4,7 +4,7 @@
#include <iostream>
#include "wolf.h"
-class RadarOdom+;
+class WolfProblem;
/** \brief Base class for Nodes
*
diff --git a/src/node_linked.h b/src/node_linked.h
index 841267fbb45edc834b475bda808c537f9d23040a..776e3c01887cf33986732c85bdb15dceee4c6a32 100644
--- a/src/node_linked.h
+++ b/src/node_linked.h
@@ -12,7 +12,7 @@
#ifndef NODE_LINKED_H_
#define NODE_LINKED_H_
-class RadarOdom+;
+class WolfProblem;
//wolf includes
#include "node_base.h"
@@ -172,7 +172,7 @@ class NodeLinked : public NodeBase
* TODO: Review if it could return a pointer to a derived class instead of NodeBase JVN: I tried to do so...
*
**/
- virtual RadarOdom+* getTop();
+ virtual WolfProblem* getTop();
/** \brief Prints node information
*
@@ -349,7 +349,7 @@ inline void NodeLinked<UpperType, LowerType>::removeDownNode(const LowerNodeIter
//TODO: confirm this change by the others :)
template<class UpperType, class LowerType>
-RadarOdom+* NodeLinked<UpperType, LowerType>::getTop()
+WolfProblem* NodeLinked<UpperType, LowerType>::getTop()
{
return up_node_ptr_->getTop();
}
diff --git a/src/node_terminus.cpp b/src/node_terminus.cpp
index 91f6817f26881f4ec3371018efd39cff2238b077..633626558fbf422d6214bd40c2599d821435e6cf 100644
--- a/src/node_terminus.cpp
+++ b/src/node_terminus.cpp
@@ -12,7 +12,7 @@ NodeTerminus::~NodeTerminus()
//
}
-RadarOdom+* NodeTerminus::getTop()
+WolfProblem* NodeTerminus::getTop()
{
return nullptr;
}
diff --git a/src/node_terminus.h b/src/node_terminus.h
index 26319773fa4f9a6a7cd234f2c4246a1396beab87..b4d5a98a11e12b174bb896cc4a5fca6a341f86d9 100644
--- a/src/node_terminus.h
+++ b/src/node_terminus.h
@@ -24,7 +24,7 @@ class NodeTerminus : public NodeBase
virtual ~NodeTerminus();
- RadarOdom+* getTop();
+ WolfProblem* getTop();
};
#endif /* NODE_TERMINUS_H_ */
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index d459f645836d58ab4c7c984089ca1e2695551c11..bd44b91bd7b4a87291f2f2484212afb95c7731dd 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -73,7 +73,7 @@ class SolverQR
}
- void update(RadarOdom+* _problem_ptr)
+ void update(WolfProblem* _problem_ptr)
{
// ADD/UPDATE STATE UNITS
for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 9d8bb2cb700bd47008b64cde0209581044a50548..4ae845e2dcdee7d8874fdbae74f7ca819d78815c 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -19,7 +19,7 @@ void SolverManager::solve()
//{
//}
-void SolverManager::update(RadarOdom+* _problem_ptr)
+void SolverManager::update(const WolfProblemPtr _problem_ptr)
{
// IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
if (_problem_ptr->isReallocated())
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index bb2909845bef2c5108f4e7b95ca276674ae6c7cf..709c164c996d5eba3f96f98fc21306288d7752ab 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -2,7 +2,7 @@
#ifndef TRAJECTORY_BASE_H_
#define TRAJECTORY_BASE_H_
-class RadarOdom+;
+class WolfProblem;
class FrameBase;
//std includes
@@ -21,7 +21,7 @@ class FrameBase;
#include "state_base.h"
//class TrajectoryBase
-class TrajectoryBase : public NodeLinked<RadarOdom+,FrameBase>
+class TrajectoryBase : public NodeLinked<WolfProblem,FrameBase>
{
protected:
// TODO: JVN: No seria millor que això ho tingui el wolf_problem o el wolf_manager?
diff --git a/src/wolf.h b/src/wolf.h
index 2e98f0cdeac3dcc493ff6b64eb50e67248f214c6..701c710182e2875bbed37c1e6f85308b1e79ad7e 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -182,7 +182,7 @@ typedef enum
{
LANDMARK_POINT, ///< A point landmark, either 3D or 2D
LANDMARK_CORNER, ///< A corner landmark (2D)
- LANDMARK_CONTAINER ///< A container landmark
+ LANDMARK_CONTAINER ///< A container landmark (2D)
} LandmarkType;
typedef enum
@@ -215,7 +215,7 @@ typedef enum
//class VehicleBase;
class NodeTerminus;
-class RadarOdom+;
+class WolfProblem;
class MapBase;
class LandmarkBase;
class LandmarkCorner2D;
@@ -246,7 +246,7 @@ class PinHole;
//Problem
//typedef std::shared_ptr<WolfProblem> WolfProblemShPtr;
-typedef RadarOdom+* WolfProblemPtr;
+typedef WolfProblem* WolfProblemPtr;
//Map
typedef std::list<MapBase*> MapBaseList;
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 54fe4c158364fd76be547ff11d282f52d7b0d701..ab8c68ffcb19a4f5a57d40c92c149bffaaa72cf2 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -45,7 +45,7 @@ class WolfManager
{
protected:
//sets the problem
- RadarOdom+* problem_;
+ WolfProblem* problem_;
//pointer to a sensor providing predictions
SensorBase* sensor_prior_;
@@ -84,9 +84,9 @@ class WolfManager
void update();
- Eigen::VectorXs getVehiclePose();
+ Eigen::VectorXs getVehiclePose(const TimeStamp& _now = 0);
- RadarOdom+* getProblemPtr();
+ WolfProblem* getProblemPtr();
void setWindowSize(const unsigned int& _size);
@@ -105,7 +105,7 @@ WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::Wol
const unsigned int& _w_size,
const WolfScalar& _new_frame_elapsed_time) :
- problem_(new RadarOdom+(_state_length)),
+ problem_(new WolfProblem(_state_length)),
sensor_prior_(_sensor_prior),
current_frame_(nullptr),
last_frame_(nullptr),
@@ -129,7 +129,7 @@ WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::Wol
first_window_frame_++;
// Initial covariance (fake ODOM 2D capture from fake frame to initial frame)
- CaptureRelative* initial_covariance = new CaptureOdom2D(TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov);
+ CaptureRelative* initial_covariance = new CaptureOdom2D(TimeStamp(0), TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov);
last_frame_->addCapture(initial_covariance);
initial_covariance->processCapture();
last_capture_relative_ = initial_covariance;
@@ -149,10 +149,23 @@ WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::~Wo
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
{
- std::cout << "creating new frame..." << std::endl;
+ //std::cout << "creating new frame..." << std::endl;
assert(_frame_state.size() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE + StateVelocityT::BLOCK_SIZE + StateOmegaT::BLOCK_SIZE && "Wrong frame vector when creating a new frame");
+ // Process current frame non-odometry captures
+ if (current_frame_ != nullptr)
+ {
+ //std::cout << "Processing last frame non-odometry captures " << current_frame_->getCaptureListPtr()->size() << std::endl;
+ for (auto capture_it = current_frame_->getCaptureListPtr()->begin(); capture_it != current_frame_->getCaptureListPtr()->end(); capture_it++)
+ if ((*capture_it)->getSensorPtr() != sensor_prior_)
+ {
+ //std::cout << "processing capture " << (*capture_it)->nodeId() << std::endl;
+ (*capture_it)->processCapture();
+ }
+ }
+ //std::cout << "Last frame non-odometry captures processed" << std::endl;
+
// Store last frame
last_frame_ = current_frame_;
@@ -192,13 +205,13 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
// Fixing or removing old frames
manageWindow();
- std::cout << "new frame created" << std::endl;
+ //std::cout << "new frame created" << std::endl;
}
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::createFrame(const TimeStamp& _time_stamp)
{
- std::cout << "creating new frame from prior..." << std::endl;
+ //std::cout << "creating new frame from prior..." << std::endl;
createFrame(last_capture_relative_->computePrior(_time_stamp), _time_stamp);
}
@@ -206,15 +219,15 @@ template <class StatePositionT, class StateOrientationT, class StateVelocityT, c
void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::addCapture(CaptureBase* _capture)
{
new_captures_.push(_capture);
- std::cout << "added new capture: " << _capture->nodeId() << " stamp: ";
- _capture->getTimeStamp().print();
- std::cout << std::endl;
+ //std::cout << "added new capture: " << _capture->nodeId() << " stamp: ";
+ //_capture->getTimeStamp().print();
+ //std::cout << std::endl;
}
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::manageWindow()
{
- std::cout << "managing window..." << std::endl;
+ //std::cout << "managing window..." << std::endl;
// WINDOW of FRAMES (remove or fix old frames)
if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_+1)
{
@@ -223,22 +236,22 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
(*first_window_frame_)->fix();
first_window_frame_++;
}
- std::cout << "window managed" << std::endl;
+ //std::cout << "window managed" << std::endl;
}
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
bool WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::checkNewFrame(CaptureBase* new_capture)
{
- std::cout << "checking if new frame..." << std::endl;
+ //std::cout << "checking if new frame..." << std::endl;
// TODO: not only time, depending on the sensor...
- std::cout << new_capture->getTimeStamp().get() - last_frame_->getTimeStamp().get() << std::endl;
+ //std::cout << new_capture->getTimeStamp().get() - last_frame_->getTimeStamp().get() << std::endl;
return new_capture->getTimeStamp().get() - last_frame_->getTimeStamp().get() > new_frame_elapsed_time_;
}
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::update()
{
- std::cout << "updating..." << std::endl;
+ //std::cout << "updating..." << std::endl;
while (!new_captures_.empty())
{
// EXTRACT NEW CAPTURE
@@ -255,7 +268,7 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
// ODOMETRY SENSOR
if (new_capture->getSensorPtr() == sensor_prior_)
{
- std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl;
+ //std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl;
// NEW KEY FRAME ?
if (checkNewFrame(new_capture))
createFrame(new_capture->getTimeStamp());
@@ -274,41 +287,42 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
}
else
{
- std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl;
+ //std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl;
// NEW KEY FRAME ?
if (checkNewFrame(new_capture))
createFrame(new_capture->getTimeStamp());
// ADD CAPTURE TO THE CURRENT FRAME (or substitute the same sensor previous capture)
- std::cout << "searching repeated capture..." << new_capture->nodeId() << std::endl;
+ //std::cout << "searching repeated capture..." << new_capture->nodeId() << std::endl;
CaptureBaseIter repeated_capture_it = current_frame_->hasCaptureOf(new_capture->getSensorPtr());
if (repeated_capture_it != current_frame_->getCaptureListPtr()->end()) // repeated capture
{
- std::cout << "repeated capture, keeping old capture" << new_capture->nodeId() << std::endl;
- //current_frame_->removeCapture(repeated_capture_it);
+ //std::cout << "repeated capture, keeping new capture" << new_capture->nodeId() << std::endl;
+ current_frame_->removeCapture(repeated_capture_it);
+ current_frame_->addCapture(new_capture);
}
else
{
- std::cout << "not repeated, adding capture..." << new_capture->nodeId() << std::endl;
- last_frame_->addCapture(new_capture);
- std::cout << "processing capture..." << new_capture->nodeId() << std::endl;
- new_capture->processCapture();
- std::cout << "processed" << std::endl;
+ //std::cout << "not repeated, adding capture..." << new_capture->nodeId() << std::endl;
+ current_frame_->addCapture(new_capture);
}
}
}
- std::cout << "updated" << std::endl;
+ //std::cout << "updated" << std::endl;
}
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
Eigen::VectorXs WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::getVehiclePose(const TimeStamp& _now)
{
- return last_capture_relative_->computePrior(_now);
+ if (last_capture_relative_ == nullptr)
+ return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr());
+ else
+ return last_capture_relative_->computePrior(_now);
}
template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-RadarOdom+* WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::getProblemPtr()
+WolfProblem* WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::getProblemPtr()
{
return problem_;
}
diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp
index 625fc62f635389a4c9438f22930a294b78ee45fd..3b794498e5c4a517d90678002f790dc8f36da059 100644
--- a/src/wolf_problem.cpp
+++ b/src/wolf_problem.cpp
@@ -1,6 +1,6 @@
#include "wolf_problem.h"
-RadarOdom+::WolfProblem(unsigned int _size) :
+WolfProblem::WolfProblem(unsigned int _size) :
NodeBase("WOLF_PROBLEM"), //
state_(_size),
covariance_(_size,_size),
@@ -16,7 +16,7 @@ RadarOdom+::WolfProblem(unsigned int _size) :
trajectory_ptr_->linkToUpperNode( this );
}
-RadarOdom+::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsigned int _size) :
+WolfProblem::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsigned int _size) :
NodeBase("WOLF_PROBLEM"), //
state_(_size),
covariance_(_size,_size),
@@ -30,13 +30,13 @@ RadarOdom+::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsi
trajectory_ptr_->linkToUpperNode( this );
}
-RadarOdom+::~RadarOdom+()
+WolfProblem::~WolfProblem()
{
delete trajectory_ptr_;
delete map_ptr_;
}
-bool RadarOdom+::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _new_state_values)
+bool WolfProblem::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _new_state_values)
{
// Check if resize should be done
if (state_idx_last_+_new_state_ptr->getStateSize() > state_.size())
@@ -73,7 +73,7 @@ bool RadarOdom+::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _new
return reallocated_;
}
-void RadarOdom+::addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov)
+void WolfProblem::addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov)
{
assert(_state1 != nullptr);
assert(_state1->getPtr() != nullptr);
@@ -101,7 +101,7 @@ void RadarOdom+::addCovarianceBlock(StateBase* _state1, StateBase* _state2, cons
covariance_.coeffRef(i+row,j+col) = (flip ? _cov(j,i) : _cov(i,j));
}
-void RadarOdom+::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const
+void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const
{
assert(_state1 != nullptr);
assert(_state1->getPtr() != nullptr);
@@ -126,7 +126,7 @@ void RadarOdom+::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eige
_cov_block = (flip ? Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)) : Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)).transpose() );
}
-void RadarOdom+::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::Map<Eigen::MatrixXs>& _cov_block) const
+void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::Map<Eigen::MatrixXs>& _cov_block) const
{
assert(_state1 != nullptr);
assert(_state1->getPtr() != nullptr);
@@ -151,7 +151,7 @@ void RadarOdom+::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eige
_cov_block = (flip ? Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)).transpose() : Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)) );
}
-void RadarOdom+::removeState(StateBase* _state_ptr)
+void WolfProblem::removeState(StateBase* _state_ptr)
{
// TODO: Reordering? Mandatory for filtering...
state_list_.remove(_state_ptr);
@@ -159,93 +159,93 @@ void RadarOdom+::removeState(StateBase* _state_ptr)
delete _state_ptr;
}
-WolfScalar* RadarOdom+::getStatePtr()
+WolfScalar* WolfProblem::getStatePtr()
{
return state_.data();
}
-WolfScalar* RadarOdom+::getNewStatePtr()
+WolfScalar* WolfProblem::getNewStatePtr()
{
return state_.data()+state_idx_last_;
}
-const unsigned int RadarOdom+::getStateSize() const
+const unsigned int WolfProblem::getStateSize() const
{
return state_idx_last_;
}
-void RadarOdom+::addMap(MapBase* _map_ptr)
+void WolfProblem::addMap(MapBase* _map_ptr)
{
map_ptr_ = _map_ptr;
map_ptr_->linkToUpperNode( this );
}
-void RadarOdom+::addTrajectory(TrajectoryBase* _trajectory_ptr)
+void WolfProblem::addTrajectory(TrajectoryBase* _trajectory_ptr)
{
trajectory_ptr_ = _trajectory_ptr;
trajectory_ptr_->linkToUpperNode( this );
}
-MapBase* RadarOdom+::getMapPtr()
+MapBase* WolfProblem::getMapPtr()
{
return map_ptr_;
}
-TrajectoryBase* RadarOdom+::getTrajectoryPtr()
+TrajectoryBase* WolfProblem::getTrajectoryPtr()
{
return trajectory_ptr_;
}
-FrameBase* RadarOdom+::getLastFramePtr()
+FrameBase* WolfProblem::getLastFramePtr()
{
return trajectory_ptr_->getLastFramePtr();
}
-StateBaseList* RadarOdom+::getStateListPtr()
+StateBaseList* WolfProblem::getStateListPtr()
{
return &state_list_;
}
-std::list<WolfScalar*>* RadarOdom+::getRemovedStateListPtr()
+std::list<WolfScalar*>* WolfProblem::getRemovedStateListPtr()
{
return &removed_state_ptr_list_;
}
-void RadarOdom+::print(unsigned int _ntabs, std::ostream& _ost) const
+void WolfProblem::print(unsigned int _ntabs, std::ostream& _ost) const
{
printSelf(_ntabs, _ost); //one line
printLower(_ntabs, _ost);
}
-void RadarOdom+::printSelf(unsigned int _ntabs, std::ostream& _ost) const
+void WolfProblem::printSelf(unsigned int _ntabs, std::ostream& _ost) const
{
printTabs(_ntabs);
_ost << nodeLabel() << " " << nodeId() << " : ";
_ost << "TOP" << std::endl;
}
-const Eigen::VectorXs RadarOdom+::getState() const
+const Eigen::VectorXs WolfProblem::getState() const
{
return state_;
}
-bool RadarOdom+::isReallocated() const
+bool WolfProblem::isReallocated() const
{
return reallocated_;
}
-void RadarOdom+::reallocationDone()
+void WolfProblem::reallocationDone()
{
reallocated_ = false;
}
-RadarOdom+* RadarOdom+::getTop()
+WolfProblem* WolfProblem::getTop()
{
return this;
}
-void RadarOdom+::printLower(unsigned int _ntabs, std::ostream& _ost) const
+void WolfProblem::printLower(unsigned int _ntabs, std::ostream& _ost) const
{
printTabs(_ntabs);
_ost << "\tLower Nodes ==> [ ";
diff --git a/src/wolf_problem.h b/src/wolf_problem.h
index f6b57b59a8043c7ed723f8f1876fbf8d40ccd7bd..e8bbe5f8c3df416a474a57e95d1f28f217bc4c43 100644
--- a/src/wolf_problem.h
+++ b/src/wolf_problem.h
@@ -17,7 +17,7 @@
* - up_node_: A regular pointer to a derived node object, specified by the template parameter UpperType.
*
*/
-class RadarOdom+: public NodeBase
+class WolfProblem: public NodeBase
{
protected:
Eigen::VectorXs state_;
@@ -52,7 +52,7 @@ class RadarOdom+: public NodeBase
* Default destructor
*
*/
- virtual ~RadarOdom+();
+ virtual ~WolfProblem();
/** \brief Adds a new state unit to the state
*
@@ -200,7 +200,7 @@ class RadarOdom+: public NodeBase
* Returns a pointer to this
*
*/
- virtual RadarOdom+* getTop();
+ virtual WolfProblem* getTop();
/** \brief Prints node information
*