diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a0e58c7b29b46d0e16eef36afe9ca569bb745a42..b27d30c1778425c86670b85478cbc49d0cbdbdb3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -61,7 +61,7 @@ SET(HDRS_BASE
SET(HDRS
capture_base.h
- capture_relative.h
+ capture_motion.h
capture_fix.h
capture_gps_fix.h
capture_odom_2D.h
@@ -69,9 +69,8 @@ SET(HDRS
constraint_sparse.h
constraint_gps_2D.h
constraint_fix.h
- constraint_odom_2D_theta.h
- constraint_odom_2D_complex_angle.h
- constraint_corner_2D_theta.h
+ constraint_odom_2D.h
+ constraint_corner_2D.h
constraint_container.h
feature_base.h
feature_corner_2D.h
@@ -91,11 +90,6 @@ SET(HDRS
sensor_odom_2D.h
sensor_gps_fix.h
state_base.h
- state_point.h
- state_orientation.h
- state_quaternion.h
- state_theta.h
- state_complex_angle.h
time_stamp.h
trajectory_base.h
wolf_problem.h
@@ -111,7 +105,7 @@ SET(HDRS_DTASSC
#sources
SET(SRCS
capture_base.cpp
- capture_relative.cpp
+ capture_motion.cpp
capture_gps_fix.cpp
capture_fix.cpp
capture_odom_2D.cpp
@@ -132,10 +126,6 @@ SET(SRCS
sensor_odom_2D.cpp
sensor_gps_fix.cpp
state_base.cpp
- state_orientation.cpp
- state_quaternion.cpp
- state_theta.cpp
- state_complex_angle.cpp
time_stamp.cpp
trajectory_base.cpp
wolf_problem.cpp
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index a1c7f6759aaacfabefdac8f01aaeb02449b5db00..7f3323aaafeb689e0f2c44047592ff05e928450b 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -18,7 +18,7 @@ void CaptureFix::processCapture()
addFeature(new FeatureFix(data_,data_covariance_));
// ADD CONSTRAINT
- getFeatureListPtr()->front()->addConstraint(new ConstraintFix(getFeatureListPtr()->front(), getFramePtr()->getPPtr(), getFramePtr()->getOPtr()));
+ getFeatureListPtr()->front()->addConstraint(new ConstraintFix(getFeatureListPtr()->front(), getFramePtr()));
}
Eigen::VectorXs CaptureFix::computePrior(const TimeStamp& _now) const
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index a0c6bcfd470b14783204d2ca5d4349261d3feef9..55b1cbd98f80a74f4c33cda677023aa0902a1045 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -29,7 +29,7 @@ void CaptureGPSFix::processCapture()
addFeature(new FeatureGPSFix(data_,data_covariance_));
// ADD CONSTRAINT
- getFeatureListPtr()->front()->addConstraint(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()->getPPtr()));
+ getFeatureListPtr()->front()->addConstraint(new ConstraintGPS2D(getFeatureListPtr()->front(), getFramePtr()));
}
Eigen::VectorXs CaptureGPSFix::computePrior(const TimeStamp& _now) const
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 30fbd0b0a64bdb055ab070e0b46fa155caa685e6..0fefc48009589873581cee074e3e3411abe197f8 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -260,21 +260,13 @@ void CaptureLaser2D::establishConstraintsMHTree()
unsigned int associed_landmark_index = landmarks_index_map[ft_lk_pairs[ii]];
if (associed_landmark->getType() == LANDMARK_CORNER)
- associed_feature->addConstraint(new ConstraintCorner2DTheta(associed_feature, //feature pointer
- (LandmarkCorner2D*)(associed_landmark), //landmark pointer
- getFramePtr()->getPPtr(), //_robotPPtr,
- getFramePtr()->getOPtr(), //_robotOPtr,
- associed_landmark->getPPtr(), //_landmarkPPtr,
- associed_landmark->getOPtr())); //_landmarkOPtr,
+ associed_feature->addConstraint(new ConstraintCorner2D(associed_feature, // feature pointer
+ (LandmarkCorner2D*)(associed_landmark))); // landmark pointer
else if (associed_landmark->getType() == LANDMARK_CONTAINER)
associed_feature->addConstraint(new ConstraintContainer(associed_feature, //feature pointer
(LandmarkContainer*)(associed_landmark), //landmark pointer
- associed_landmark_index, // corner index
- getFramePtr()->getPPtr(), //_robotPPtr,
- getFramePtr()->getOPtr(), //_robotOPtr,
- associed_landmark->getPPtr(), //_landmarkPPtr,
- associed_landmark->getOPtr())); //_landmarkOPtr,
+ associed_landmark_index )); // corner index
}
}
@@ -570,9 +562,9 @@ bool CaptureLaser2D::fitNewContainer(FeatureCorner2D* _corner_ptr, LandmarkCorne
void CaptureLaser2D::createCornerLandmark(FeatureCorner2D* _corner_ptr, const Eigen::Vector3s& _feature_global_pose)
{
//create new landmark at global coordinates
- StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr());
+ StateBase* new_landmark_state_position = new StateBase(getTop()->getNewStatePtr(), 2);
getTop()->addState(new_landmark_state_position, _feature_global_pose.head(2));
- StateOrientation* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
+ StateBase* new_landmark_state_orientation = new StateBase(getTop()->getNewStatePtr(), 1);
getTop()->addState(new_landmark_state_orientation, _feature_global_pose.tail(1));
// Initialize landmark covariance
@@ -591,7 +583,7 @@ void CaptureLaser2D::createCornerLandmark(FeatureCorner2D* _corner_ptr, const Ei
//add it to the slam map as a new landmark
LandmarkCorner2D* new_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, _corner_ptr->getMeasurement()(3));
- _corner_ptr->addConstraint(new ConstraintCorner2DTheta(_corner_ptr, //feature pointer
+ _corner_ptr->addConstraint(new ConstraintCorner2D(_corner_ptr, //feature pointer
new_landmark, //landmark pointer
getFramePtr()->getPPtr(), //_robotPPtr,
getFramePtr()->getOPtr(), //_robotOPtr,
@@ -606,9 +598,9 @@ void CaptureLaser2D::createContainerLandmark(FeatureCorner2D* _corner_ptr, const
// CREATING LANDMARK CONTAINER
// create new landmark state units
- StateBase* new_container_position = new StatePoint2D(getTop()->getNewStatePtr());
+ StateBase* new_container_position = new StateBase(getTop()->getNewStatePtr());
getTop()->addState(new_container_position, Eigen::Vector2s::Zero());
- StateOrientation* new_container_orientation = new StateTheta(getTop()->getNewStatePtr());
+ StateBase* new_container_orientation = new StateBase(getTop()->getNewStatePtr());
getTop()->addState(new_container_orientation, Eigen::Vector1s::Zero());
// create new landmark
@@ -651,7 +643,7 @@ void CaptureLaser2D::createContainerLandmark(FeatureCorner2D* _corner_ptr, const
new_landmark, //landmark pointer
_corner_idx, //corner idx
(*ctr_it)->getStatePtrVector().at(0), //_robotPPtr,
- (StateOrientation*)(*ctr_it)->getStatePtrVector().at(1), //_robotOPtr,
+ (StateBase*)(*ctr_it)->getStatePtrVector().at(1), //_robotOPtr,
new_landmark->getPPtr(), //_landmarkPPtr,
new_landmark->getOPtr())); //_landmarkOPtr,
}
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 6b63ec509a68848b0a994dd8534d8cfa520284c5..612617140d62c866f1a0a9ec12409707dc7711bf 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -19,6 +19,7 @@
#include "laser_scan_utils/corner_detector.h"
//wolf includes
+#include "state_base.h"
#include "constraint_corner_2D_theta.h"
#include "constraint_container.h"
#include "capture_base.h"
@@ -26,9 +27,6 @@
#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"
#include "data_association/association_tree.h"
//wolf forward declarations
diff --git a/src/capture_relative.cpp b/src/capture_motion.cpp
similarity index 97%
rename from src/capture_relative.cpp
rename to src/capture_motion.cpp
index 08b0f2f3a3c69eec3555870f5595c3a04f8389ff..5d83b34a0b6732fbdafc77356c04ba04bab5329b 100644
--- a/src/capture_relative.cpp
+++ b/src/capture_motion.cpp
@@ -1,4 +1,4 @@
-#include "capture_relative.h"
+#include "capture_motion.h"
CaptureMotion::CaptureMotion(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr) :
CaptureBase(_init_ts, _sensor_ptr),
diff --git a/src/capture_relative.h b/src/capture_motion.h
similarity index 100%
rename from src/capture_relative.h
rename to src/capture_motion.h
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index 7d48f39ffb0b7e6ed4302ffc7d98bbcef8359c17..d49c0f3526bfefefa7128c825f6447bf9a36fb30 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -1,13 +1,13 @@
#include "capture_odom_2D.h"
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorBase* _sensor_ptr) :
- CaptureRelative(_init_ts, _final_ts, _sensor_ptr)
+ CaptureMotion(_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, _final_ts, _sensor_ptr, _data)
+ CaptureMotion(_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());
@@ -17,7 +17,7 @@ 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, _final_ts, _sensor_ptr, _data, _data_covariance)
+ CaptureMotion(_init_ts, _final_ts, _sensor_ptr, _data, _data_covariance)
{
//
}
@@ -40,33 +40,16 @@ Eigen::VectorXs CaptureOdom2D::computePrior(const TimeStamp& _now) const
{
assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
- if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- Eigen::Vector4s prior;
- Eigen::Map<Eigen::Vector4s> initial_pose(getFramePtr()->getPPtr()->getPtr());
- //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
- prior(0) = initial_pose(0) + data_(0) * initial_pose(2) - data_(1) * initial_pose(3);
- prior(1) = initial_pose(1) + data_(0) * initial_pose(3) + data_(1) * initial_pose(2);
- prior(2) = initial_pose(2) * cos(data_(2)) - initial_pose(3) * sin(data_(2));
- prior(3) = initial_pose(2) * sin(data_(2)) + initial_pose(3) * cos(data_(2));
- //std::cout << "data_: " << data_.transpose() << std::endl;
- //std::cout << "prior: " << prior.transpose() << std::endl;
-
- return prior;
- }
- else
- {
- Eigen::Vector3s prior;
- Eigen::Map<Eigen::Vector3s> initial_pose(getFramePtr()->getPPtr()->getPtr());
- //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
- prior(0) = initial_pose(0) + data_(0) * cos(initial_pose(2)) - data_(1) * sin(initial_pose(2));
- prior(1) = initial_pose(1) + data_(0) * sin(initial_pose(2)) + data_(1) * cos(initial_pose(2));
- prior(2) = initial_pose(2) + data_(2);
- //std::cout << "data_: " << data_.transpose() << std::endl;
- //std::cout << "prior: " << prior.transpose() << std::endl;
-
- return prior;
- }
+ Eigen::Vector3s prior;
+ Eigen::Map<Eigen::Vector3s> initial_pose(getFramePtr()->getPPtr()->getPtr());
+ //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
+ prior(0) = initial_pose(0) + data_(0) * cos(initial_pose(2)) - data_(1) * sin(initial_pose(2));
+ prior(1) = initial_pose(1) + data_(0) * sin(initial_pose(2)) + data_(1) * cos(initial_pose(2));
+ prior(2) = initial_pose(2) + data_(2);
+ //std::cout << "data_: " << data_.transpose() << std::endl;
+ //std::cout << "prior: " << prior.transpose() << std::endl;
+
+ return prior;
}
@@ -74,25 +57,11 @@ void CaptureOdom2D::addConstraints()
{
assert(getFramePtr()->getNextFrame() != nullptr && "Trying to add odometry constraint in the last frame (there is no next frame)");
- if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DComplexAngle(getFeatureListPtr()->front(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr(),
- getFramePtr()->getNextFrame()->getPPtr(),
- getFramePtr()->getNextFrame()->getOPtr()));
- }
- else
- {
- getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DTheta(getFeatureListPtr()->front(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr(),
- getFramePtr()->getNextFrame()->getPPtr(),
- getFramePtr()->getNextFrame()->getOPtr()));
- }
+ getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2D(getFeatureListPtr()->front(),
+ getFramePtr()->getPreviousFrame()));
}
-void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
+void CaptureOdom2D::integrateCapture(CaptureMotion* _new_capture)
{
assert(dynamic_cast<CaptureOdom2D*>(_new_capture) && "Trying to integrate with a CaptureOdom2D a CaptureRelativePtr which is not CaptureOdom2D");
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index 26f71aa48847763144c0642f732eea149fa97a81..d53d8677ed9eeb0b18f825efa66d51aeec04931b 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -6,12 +6,12 @@
//
//Wolf includes
-#include "capture_relative.h"
+#include "capture_motion.h"
#include "feature_odom_2D.h"
#include "sensor_odom_2D.h"
//class CaptureGPSFix
-class CaptureOdom2D : public CaptureRelative
+class CaptureOdom2D : public CaptureMotion
{
public:
@@ -29,7 +29,7 @@ class CaptureOdom2D : public CaptureRelative
virtual void addConstraints();
- virtual void integrateCapture(CaptureRelative* _new_capture);
+ virtual void integrateCapture(CaptureMotion* _new_capture);
virtual CaptureOdom2D* interpolateCapture(const TimeStamp& _ts);
diff --git a/src/capture_twist_2D.cpp b/src/capture_twist_2D.cpp
deleted file mode 100644
index c9f19c3f0086c3f88827039ac81a14de1183d1ed..0000000000000000000000000000000000000000
--- a/src/capture_twist_2D.cpp
+++ /dev/null
@@ -1,141 +0,0 @@
-#include "capture_twist_2D.h"
-
-CaptureTwist2D::CaptureTwist2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorTwist2D* _sensor_ptr) :
- CaptureRelative(_init_ts, _sensor_ptr)
-{
- //
-}
-
-CaptureTwist2D::CaptureTwist2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorTwist2D* _sensor_ptr, const Eigen::Vector3s& _data) :
- CaptureRelative(_init_ts, _sensor_ptr, _data)
-{
- data_covariance_ = Eigen::Matrix3s::Zero();
- data_covariance_(0, 0) = std::max(1e-10, _sensor_ptr->getLinealNoise());
- data_covariance_(1, 1) = std::max(1e-10, _sensor_ptr->getLinealNoise());
- data_covariance_(2, 2) = std::max(1e-10, _sensor_ptr->getAngularNoise());
-// std::cout << data_covariance_ << std::endl;
-}
-
-CaptureTwist2D::CaptureTwist2D(const TimeStamp& _init_ts, const TimeStamp& _final_ts, SensorTwist2D* _sensor_ptr, const Eigen::Vector3s& _data, const Eigen::Matrix3s& _data_covariance) :
- CaptureRelative(_init_ts, _sensor_ptr, _data, _data_covariance)
-{
- //
-}
-
-CaptureTwist2D::~CaptureTwist2D()
-{
- //std::cout << "Destroying GPS fix capture...\n";
-}
-
-inline void CaptureTwist2D::processCapture()
-{
- // ADD FEATURE
- addFeature(new FeatureTwist2D(data_, data_covariance_));
-
- // ADD CONSTRAINT
- addConstraints();
-}
-
-Eigen::VectorXs CaptureTwist2D::computePrior(const TimeStamp& _now) const
-{
- assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
-
- WolfScalar dt = (_now - this->time_stamp_).get();
- Eigen::Vector3s disp = dt * data_;
-
- if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- Eigen::Vector4s prior;
- Eigen::Map<Eigen::Vector4s> initial_pose(getFramePtr()->getPPtr()->getPtr());
- //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
- prior(0) = initial_pose(0) + disp(0) * initial_pose(2) - disp(1) * initial_pose(3);
- prior(1) = initial_pose(1) + disp(0) * initial_pose(3) + disp(1) * initial_pose(2);
- prior(2) = initial_pose(2) * cos(disp(2)) - initial_pose(3) * sin(disp(2));
- prior(3) = initial_pose(2) * sin(disp(2)) + initial_pose(3) * cos(disp(2));
- //std::cout << "data_: " << data_.transpose() << std::endl;
- //std::cout << "prior: " << prior.transpose() << std::endl;
-
- return prior;
- }
- else
- {
- Eigen::Vector3s prior;
- Eigen::Map<Eigen::Vector3s> initial_pose(getFramePtr()->getPPtr()->getPtr());
- //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
- prior(0) = initial_pose(0) + disp(0) * cos(initial_pose(2)) - disp(1) * sin(initial_pose(2));
- prior(1) = initial_pose(1) + disp(0) * sin(initial_pose(2)) + disp(1) * cos(initial_pose(2));
- prior(2) = initial_pose(2) + disp(2);
- //std::cout << "data_: " << data_.transpose() << std::endl;
- //std::cout << "prior: " << prior.transpose() << std::endl;
-
- return prior;
- }
-
-}
-
-void CaptureTwist2D::addConstraints()
-{
- assert(getFramePtr()->getNextFrame() != nullptr && "Trying to add odometry constraint in the last frame (there is no next frame)");
-
- getFramePtr()->getNextFrame()
-
- if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DComplexAngle(getFeatureListPtr()->front(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr(),
- getFramePtr()->getNextFrame()->getPPtr(),
- getFramePtr()->getNextFrame()->getOPtr()));
- }
- else
- {
- getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DTheta(getFeatureListPtr()->front(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr(),
- getFramePtr()->getNextFrame()->getPPtr(),
- getFramePtr()->getNextFrame()->getOPtr()));
- }
-}
-
-void CaptureTwist2D::integrateCapture(CaptureRelative* _new_capture)
-{
- assert(dynamic_cast<CaptureOdom2D*>(_new_capture) && "Trying to integrate with a CaptureOdom2D a CaptureRelativePtr which is not CaptureOdom2D");
-
- //std::cout << "Integrate odoms: " << std::endl << data_.transpose() << std::endl << _new_capture->getData().transpose() << std::endl;
- data_(0) += (_new_capture->getData()(0) * cos(data_(2)) - _new_capture->getData()(1) * sin(data_(2)));
- data_(1) += (_new_capture->getData()(0) * sin(data_(2)) + _new_capture->getData()(1) * cos(data_(2)));
- data_(2) += _new_capture->getData()(2);
-
- // TODO Jacobians!
- data_covariance_ += _new_capture->getDataCovariance();
-
- final_time_stamp_ = _new_capture->final_time_stamp_;
-
- getFeatureListPtr()->front()->setMeasurement(data_);
- getFeatureListPtr()->front()->setMeasurementCovariance(data_covariance_);
- //std::cout << "Integrated odoms: " << std::endl << data_.transpose() << std::endl << data_covariance_ << std::endl;
-}
-
-CaptureOdom2D* CaptureTwist2D::interpolateCapture(const TimeStamp& _ts)
-{
- WolfScalar ratio = (_ts.get() - initial_time_stamp_.get()) / (final_time_stamp_.get() - initial_time_stamp_.get());
-
- // Second part
- CaptureOdom2D* second_odom_ptr = new CaptureOdom2D(_ts, final_time_stamp_, sensor_ptr_, data_ * (1-ratio), data_covariance_ * (1-ratio));
-
- // First part (stored in this)
- data_ *= ratio;
- data_covariance_*= ratio;
- final_time_stamp_ = _ts;
-
- return second_odom_ptr;
-}
-
-//void CaptureTwist2D::printSelf(unsigned int _ntabs, std::ostream & _ost) const
-//{
-// NodeLinked::printSelf(_ntabs, _ost);
-// //printTabs(_ntabs);
-// //_ost << "\tSensor pose : ( " << sensor_ptr_->pose().x().transpose() << " )" << std::endl;
-// //printNTabs(_ntabs);
-// //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
-//}
diff --git a/src/capture_twist_2D.h b/src/capture_twist_2D.h
deleted file mode 100644
index 3a6ac931e47ed9c515d22e4f92fa35a3a4fd7a8f..0000000000000000000000000000000000000000
--- a/src/capture_twist_2D.h
+++ /dev/null
@@ -1,36 +0,0 @@
-
-#ifndef CAPTURE_TWIST_2D_H_
-#define CAPTURE_TWIST_2D_H_
-
-//std includes
-//
-
-//Wolf includes
-#include "capture_relative.h"
-#include "feature_twist_2D.h"
-#include "sensor_twist_2D.h"
-
-class CaptureTwist2D : public CaptureRelative
-{
- public:
- CaptureTwist2D(const TimeStamp& _init_ts, SensorTwist2D* _sensor_ptr);
-
- CaptureTwist2D(const TimeStamp& _init_ts, SensorTwist2D* _sensor_ptr, const Eigen::Vector3s& _data);
-
- CaptureTwist2D(const TimeStamp& _init_ts, SensorTwist2D* _sensor_ptr, const Eigen::Vector3s& _data, const Eigen::Matrix3s& _data_covariance);
-
- virtual ~CaptureTwist2D();
-
- virtual void processCapture();
-
- virtual Eigen::VectorXs computePrior(const TimeStamp& _now) const;
-
- virtual void addConstraints();
-
- virtual void integrateCapture(CaptureRelative* _new_capture);
-
- virtual CaptureTwist2D* interpolateCapture(const TimeStamp& _ts);
-
- //virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
-};
-#endif
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 30e62febacc8ee1bdb3488265cabf1d363bf4130..18f17bef7cf3994cecdb2d13657d438531de3ba1 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -206,45 +206,20 @@ void CeresManager::addStateUnit(StateBase* _st_ptr)
switch (_st_ptr->getStateType())
{
- case ST_COMPLEX_ANGLE:
- {
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
-// case PARAM_QUATERNION:
-// {
-// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
-// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
-// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
-// break;
-// }
- case ST_THETA:
+ case ST_VECTOR:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateTheta*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getStateSize(), nullptr);
break;
}
- case ST_POINT_1D:
+ case ST_QUATERNION:
{
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_2D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ //TODO: change nullptr below by quaternion parametrization following method in complex_angle_parametrization.cpp
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), _st_ptr->getStateSize(), nullptr);
break;
}
default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
+ std::cout << "Unknown state type!" << std::endl;
}
if (_st_ptr->getStateStatus() != ST_ESTIMATED)
updateStateUnitStatus(_st_ptr);
@@ -315,27 +290,10 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D_COMPLEX_ANGLE:
- {
- ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2DComplexAngle,
- specific_ptr->measurementSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CTR_ODOM_2D_THETA:
+ case CTR_ODOM_2D:
{
- ConstraintOdom2DTheta* specific_ptr = (ConstraintOdom2DTheta*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2DTheta,
+ ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintOdom2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -349,10 +307,10 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D_THETA:
+ case CTR_CORNER_2D:
{
- ConstraintCorner2DTheta* specific_ptr = (ConstraintCorner2DTheta*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintCorner2DTheta,
+ ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintCorner2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 2d5375c54059b53444b688bcb0988775516b9e07..da1095d0eff72490d025a0dce41c0bf7d23ebaf9 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -9,15 +9,11 @@
//wof includes
#include "../wolf.h"
#include "../state_base.h"
-#include "../state_point.h"
-#include "../state_complex_angle.h"
-#include "../state_theta.h"
#include "../constraint_sparse.h"
#include "../constraint_fix.h"
#include "../constraint_gps_2D.h"
-#include "../constraint_odom_2D_theta.h"
-#include "../constraint_odom_2D_complex_angle.h"
-#include "../constraint_corner_2D_theta.h"
+#include "../constraint_odom_2D.h"
+#include "../constraint_corner_2D.h"
#include "../constraint_container.h"
// ceres wrapper includes
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index 050b4d8ac7558f89e551a1be3ffb855374ca534c..babb3de7d75bafc93aad8891163924368185f1a3 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -4,7 +4,8 @@ ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp) :
NodeLinked(BOTTOM, "CONSTRAINT"),
type_(_tp),
measurement_(_ftr_ptr->getMeasurement()),
- measurement_covariance_(_ftr_ptr->getMeasurementCovariance())
+ measurement_covariance_(_ftr_ptr->getMeasurementCovariance()),
+ pending_status_(ADD_PENDING)
{
//
}
@@ -33,3 +34,13 @@ CaptureBase* ConstraintBase::getCapturePtr() const
{
return upperNodePtr()->upperNodePtr();
}
+
+PendingStatus ConstraintBase::getPendingStatus() const
+{
+ return pending_status_;
+}
+
+void ConstraintBase::setPendingStatus(PendingStatus _pending)
+{
+ pending_status_ = _pending;
+}
diff --git a/src/constraint_base.h b/src/constraint_base.h
index fc12182e0ea31f2021af5712b4b7f9ee27a67813..721d339c1d3fbf4b2d91da79135b70bf739f7c41 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -19,9 +19,11 @@ class NodeTerminus;
class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
{
protected:
- ConstraintType type_; //type of constraint (types defined at wolf.h)
- const Eigen::VectorXs& measurement_; // TODO:TBD: pointer, map or copy of the feature measurement?
- const Eigen::MatrixXs& measurement_covariance_; // TODO:TBD: pointer, map or copy of the feature measurement covariance?
+ ConstraintType type_; ///< type of constraint (types defined at wolf.h)
+ const Eigen::VectorXs& measurement_; ///< Direct access to the measurement
+ const Eigen::MatrixXs& measurement_covariance_; ///< Direct access to the measurement's covariance
+ PendingStatus pending_status_; ///< Pending status
+
public:
/** \brief Constructor
@@ -87,5 +89,20 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
**/
virtual unsigned int getSize() const = 0;
+ /** \brief Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ * Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ */
+ PendingStatus getPendingStatus() const;
+
+ /** \brief Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ * Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ */
+ void setPendingStatus(PendingStatus _pending);
+
+
};
#endif
diff --git a/src/constraint_container.h b/src/constraint_container.h
index 1af63e1c0eccf914d71dcedca702800a00f7a346..481a3b3a663ac7aae0340b38d4c7dce85156d6e1 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -15,12 +15,12 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintContainer(FeatureBase* _ftr_ptr, LandmarkContainer* _lmk_ptr, const unsigned int _corner, StateBase* _robotPPtr, StateOrientation* _robotOPtr, StateBase* _landmarkPPtr, StateOrientation* _landmarkOPtr) :
- ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CONTAINER, _robotPPtr, _robotOPtr,_landmarkPPtr, _landmarkOPtr),
+ ConstraintContainer(FeatureBase* _ftr_ptr, LandmarkContainer* _lmk_ptr, const unsigned int _corner) :
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CONTAINER, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
- assert(_corner >= 0 && _corner <= 4 && "wrong corner id in constraint container constructor");
+ assert(_corner >= 0 && _corner <= 3 && "Wrong corner id in constraint container constructor");
lmk_ptr_->hit(this);
}
@@ -51,7 +51,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
// sensor transformation
Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2,2>().transpose()).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getRotationMatrix2D().transpose()).cast<T>();
Eigen::Matrix<T,2,2> inverse_R_robot;
inverse_R_robot << cos(*_robotO), sin(*_robotO),
@@ -64,7 +64,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
// Expected measurement
Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position - robot_position + R_landmark * corner_position) - sensor_position);
- T expected_measurement_orientation = (*_landmarkO) - (*_robotO) - T(getCapturePtr()->getSensorPtr()->getOPtr()->getYaw()) + T(lmk_ptr_->getCorner(corner_)(2));
+ T expected_measurement_orientation = (*_landmarkO) - (*_robotO) - T( *(getCapturePtr()->getSensorPtr()->getOPtr()->getPtr()) ) + T(lmk_ptr_->getCorner(corner_)(2));
// Residuals
_residuals[0] = (expected_measurement_position(0) - T(measurement_(0))) / T(sqrt(measurement_covariance_(0,0)));
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D.h
similarity index 81%
rename from src/constraint_corner_2D_theta.h
rename to src/constraint_corner_2D.h
index 4546a036de345b9d36d9cf41de425214eb9048ac..d651fcb9b55c184bbc326751efbdafed6f2adbdf 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D.h
@@ -6,7 +6,7 @@
#include "constraint_sparse.h"
#include "landmark_corner_2D.h"
-class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
+class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
{
protected:
LandmarkCorner2D* lmk_ptr_;
@@ -14,23 +14,16 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
public:
static const unsigned int N_BLOCKS = 4;
-// ConstraintCorner2DTheta(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, WolfScalar* _robotPPtr, WolfScalar* _robotOPtr, WolfScalar* _landmarkPPtr, WolfScalar* _landmarkOPtr) :
-// ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D_THETA, _robotPPtr, _robotOPtr, _landmarkPPtr, _landmarkOPtr),
-// lmk_ptr_(_lmk_ptr)
-// {
-// //
-// }
-
- 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),
+ ConstraintCorner2D(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr) :
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
lmk_ptr_(_lmk_ptr)
{
lmk_ptr_->hit(this);
}
- virtual ~ConstraintCorner2DTheta()
+ virtual ~ConstraintCorner2D()
{
- //std::cout << "deleting ConstraintCorner2DTheta " << nodeId() << std::endl;
+ //std::cout << "deleting ConstraintCorner2D " << nodeId() << std::endl;
lmk_ptr_->unhit(this);
}
@@ -55,7 +48,7 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
// sensor transformation
Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2,2>().transpose()).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getRotationMatrix2D().transpose()).cast<T>();
Eigen::Matrix<T,2,2> inverse_R_robot;
inverse_R_robot << cos(*_robotO), sin(*_robotO),
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index be8516cb61ec2a3386d9f96c5b9e60e6eaeb7bce..cabe1420891b34a9d681edfc322ebeb5c8e0678f 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -11,8 +11,8 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
public:
static const unsigned int N_BLOCKS = 2;
- ConstraintFix(FeatureBase* _ftr_ptr, StateBase* _pPtr, StateBase* _oPtr):
- ConstraintSparse<3,2,1>(_ftr_ptr, CTR_FIX, _pPtr, _oPtr)
+ ConstraintFix(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr):
+ ConstraintSparse<3,2,1>(_ftr_ptr, CTR_FIX, _frame_ptr->getPPtr(), _frame_ptr->getOPtr())
{
//
}
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index ad57d9ce45ddf71094839f73ddadbe7a764498f5..11ef68a9ea16ecb9d6d029ade6489aaa9a2af8e4 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -11,14 +11,8 @@ class ConstraintGPS2D: public ConstraintSparse<2,2>
public:
static const unsigned int N_BLOCKS = 1;
-// ConstraintGPS2D(FeatureBase* _ftr_ptr, WolfScalar* _statePtr) :
-// ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _statePtr)
-// {
-// //
-// }
-
- ConstraintGPS2D(FeatureBase* _ftr_ptr, StateBase* _statePtr):
- ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _statePtr)
+ ConstraintGPS2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr):
+ ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _frame_ptr->getPPtr())
{
//
}
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D.h
similarity index 82%
rename from src/constraint_odom_2D_theta.h
rename to src/constraint_odom_2D.h
index 2feb124d5ca469757e76880208fd89fb8d232b8b..eaefa6d5ba5b4f62c98d1e001105068fcc6e3da2 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D.h
@@ -5,24 +5,18 @@
#include "wolf.h"
#include "constraint_sparse.h"
-class ConstraintOdom2DTheta : public ConstraintSparse<3, 2, 1, 2, 1>
+class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
{
public:
static const unsigned int N_BLOCKS = 4;
-// ConstraintOdom2DTheta(FeatureBase*_ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
-// ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
-// {
-// //
-// }
-
- 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)
+ ConstraintOdom2D(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr) :
+ ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
//
}
- virtual ~ConstraintOdom2DTheta()
+ virtual ~ConstraintOdom2D()
{
//
}
diff --git a/src/constraint_odom_2D_complex_angle.h b/src/constraint_odom_2D_complex_angle.h
deleted file mode 100644
index 3778524ec9bb9ec11fba50f78494a43f084f2cfb..0000000000000000000000000000000000000000
--- a/src/constraint_odom_2D_complex_angle.h
+++ /dev/null
@@ -1,61 +0,0 @@
-#ifndef CONSTRAINT_ODOM_2D_COMPLEX_ANGLE_H_
-#define CONSTRAINT_ODOM_2D_COMPLEX_ANGLE_H_
-
-//Wolf includes
-#include "wolf.h"
-#include "constraint_sparse.h"
-
-class ConstraintOdom2DComplexAngle : public ConstraintSparse<3, 2, 2, 2, 2>
-{
- public:
- static const unsigned int N_BLOCKS = 4;
-
-// ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
-// ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
-// {
-// //
-// }
-
- ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
- ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr)
- {
- //
- }
-
- virtual ~ConstraintOdom2DComplexAngle()
- {
- //
- }
-
- template<typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- // rotar per menys l'angle de primer _o1
- T expected_longitudinal = _o1[0] * (_p2[0] - _p1[0]) + _o1[1] * (_p2[1] - _p1[1]); // cos(-o1)(x2-x1) - sin(-o1)(y2-y1)
- T expected_lateral = -_o1[1] * (_p2[0] - _p1[0]) + _o1[0] * (_p2[1] - _p1[1]); // sin(-o1)(x2-x1) + cos(-o1)(y2-y1)
- T expected_rotation = atan2(_o2[1] * _o1[0] - _o2[0] * _o1[1], _o1[0] * _o2[0] + _o1[1] * _o2[1]);
-
- // Residuals
- _residuals[0] = (expected_longitudinal - T(measurement_(0))) / T(sqrt(measurement_covariance_(0, 0)));
- _residuals[1] = (expected_lateral - T(measurement_(1))) / T(sqrt(measurement_covariance_(1, 1)));
- _residuals[2] = expected_rotation - T(measurement_(2));
-
- while (_residuals[2] > T(M_PI))
- _residuals[2] = _residuals[2] - T(2*M_PI);
- while (_residuals[2] <= T(-M_PI))
- _residuals[2] = _residuals[2] + T(2*M_PI);
-
- _residuals[2] = _residuals[2] / T(sqrt(measurement_covariance_(2, 2)));
-
-// T expected_longitudinal = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
-// T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
-
- // Residuals
-// _residuals[0] = (expected_longitudinal - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
-// _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
-
- return true;
- }
-};
-#endif
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index 57a46bac35bc107e007cb1a0267eb64df45d24c4..8823716cf7c57954ff424145c395c9181bdfccc1 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -6,12 +6,6 @@
#include "wolf.h"
#include "constraint_base.h"
-//TODO:
-// - public static const may be are not necessary, since sizes are already kept in ConstraintBase::state_block_sizes_vector_
-// JVN: Yes, they are necessary for the ceres cost function constructor. Maybe, the state_block_sizes_vector_ is not necessary (it can be useful in filtering...)
-// - measurement_ptr can be set from FeatureBase::measurement_, once this constraint is up-linked to a feature.
-// May be a simple get is enough to access this data.
-// -
//template class ConstraintBase
template <const unsigned int MEASUREMENT_SIZE,
diff --git a/src/constraint_twist_2D_theta.h b/src/constraint_twist_2D_theta.h
deleted file mode 100644
index da4c13f5a5b6b39994e17a7106433cec828bbbc9..0000000000000000000000000000000000000000
--- a/src/constraint_twist_2D_theta.h
+++ /dev/null
@@ -1,74 +0,0 @@
-#ifndef CONSTRAINT_TWIST_2D_THETA_H_
-#define CONSTRAINT_TWIST_2D_THETA_H_
-
-//Wolf includes
-#include "wolf.h"
-#include "constraint_sparse.h"
-
-class ConstraintTwist2DTheta : public ConstraintSparse<3, 2, 1, 2, 1, 2, 1>
-{
- public:
- static const unsigned int N_BLOCKS = 6;
-
- ConstraintTwist2DTheta(FeatureBase* _ftr_ptr, StatePoint2D* _p1_ptr, StateTheta* _o1_ptr, StateVelocity2D* _v1_ptr, StateOmega2D* _w1_ptr, StatePoint2D* _p2_ptr, StateTheta* _o2_ptr) :
- ConstraintSparse<3, 2, 1, 2, 1, 2, 1>(_ftr_ptr, CTR_TWIST_2D_THETA, _p1_ptr, _o1_ptr,_v1_ptr, _w1_ptr, _p2_ptr, _o2_ptr)
- {
- //
- }
-
- virtual ~ConstraintTwist2DTheta()
- {
- //
- }
-
- template<typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _v1, const T* const _w1,const T* const _p2, const T* const _o2, T* _residuals) const
- {
-// std::cout << "_p1: ";
-// for (int i=0; i < 2; i++)
-// std::cout << "\n\t" << _p1[i];
-// std::cout << std::endl;
-// std::cout << "_o1: ";
-// for (int i=0; i < 1; i++)
-// std::cout << "\n\t" << _o1[i];
-// std::cout << std::endl;
-// std::cout << "_p2: ";
-// for (int i=0; i < 2; i++)
-// std::cout << "\n\t" << _p2[i];
-// std::cout << std::endl;
-// std::cout << "_o2: ";
-// for (int i=0; i < 1; i++)
-// std::cout << "\n\t" << _o2[i];
-// std::cout << std::endl;
-// std::cout << "measurement_: ";
-// for (int i=0; i < 3; i++)
-// std::cout << "\n\t" << measurement_(i);
-// std::cout << std::endl;
-// std::cout << "measurement_covariance_: ";
-// for (int i=0; i < 3; i++)
-// std::cout << "\n\t" << measurement_covariance_(i,i);
-// std::cout << std::endl;
-
- // Expected measurement
- // rotar per menys l'angle de primer _o1
- T expected_longitudinal = cos(_o1[0]) * (_p2[0] - _p1[0]) + sin(_o1[0]) * (_p2[1] - _p1[1]); // cos(-o1)(x2-x1) - sin(-o1)(y2-y1)
- T expected_lateral = -sin(_o1[0]) * (_p2[0] - _p1[0]) + cos(_o1[0]) * (_p2[1] - _p1[1]); // sin(-o1)(x2-x1) + cos(-o1)(y2-y1)
- T expected_rotation = _o2[0] - _o1[0];
-
- // Residuals
- _residuals[0] = (expected_longitudinal - T(measurement_(0))) / T(sqrt(std::max(measurement_covariance_(0, 0),1e-6)));
- _residuals[1] = (expected_lateral - T(measurement_(1))) / T(sqrt(std::max(measurement_covariance_(1, 1),1e-6)));
- _residuals[2] = (expected_rotation - T(measurement_(2))) / T(sqrt(std::max(measurement_covariance_(2, 2),1e-6)));
-
- // Expected measurement
-// T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
-// T expected_rotation = _o2[0]-_o1[0];
-//
-// // Residuals
-// _residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
-// _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
-
- return true;
- }
-};
-#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 2e5cab0b90942af4c140435ab5b9a954b7ced633..f5c2600bd0917e63bb914707c5e182f5c6a15ce7 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -96,7 +96,4 @@ IF(faramotics_FOUND)
ENDIF (laser_scan_utils_FOUND)
ENDIF(faramotics_FOUND)
-# Testing a eigen quaternion
-ADD_EXECUTABLE(test_state_quaternion test_state_quaternion.cpp)
-TARGET_LINK_LIBRARIES(test_state_quaternion ${PROJECT_NAME})
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 0c69241e989a7fa44d6e12afa5a485914f3b161b..7e94ea9a624d9f6d50da99b578e3dee4e2b1fdc4 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -168,17 +168,17 @@ int main(int argc, char** argv)
Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(new StatePoint3D(odom_pose.data()), new StateTheta(&odom_pose(2)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(new StatePoint3D(gps_pose.data()), new StateTheta(&gps_pose(2)), gps_std);
- SensorLaser2D laser_1_sensor(new StatePoint3D(laser_1_pose.data()), new StateTheta(&laser_1_pose(3)));
- SensorLaser2D laser_2_sensor(new StatePoint3D(laser_2_pose.data()), new StateTheta(&laser_2_pose(3)));
+ SensorOdom2D odom_sensor(new StateBase(odom_pose.data()), new StateBase(&odom_pose(2)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StateBase(gps_pose.data()), new StateBase(&gps_pose(2)), gps_std);
+ SensorLaser2D laser_1_sensor(new StateBase(laser_1_pose.data()), new StateBase(&laser_1_pose(3)));
+ SensorLaser2D laser_2_sensor(new StateBase(laser_2_pose.data()), new StateBase(&laser_2_pose(3)));
// Initial pose
pose_odom << 2, 8, 0;
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager<StatePoint2D, StateTheta>* wolf_manager = new WolfManager<StatePoint2D, StateTheta>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
+ WolfManager<StateBase, StateBase>* wolf_manager = new WolfManager<StateBase, StateBase>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
//std::cout << "START TRAJECTORY..." << std::endl;
// START TRAJECTORY ============================================================================================
diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp
index e757949e8821a2e291f3b384e208353c5fe9a650..8bef1334bf70ba384467f0d6a821e2fde72d681b 100644
--- a/src/examples/test_ceres_laser.cpp
+++ b/src/examples/test_ceres_laser.cpp
@@ -140,14 +140,14 @@ class WolfManager
void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
{
// Create frame and add it to the trajectory
- StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr());
+ StateBase* new_position = new StateBase(problem_->getNewStatePtr());
problem_->addState(new_position, _frame_state.head(2));
StateBase* new_orientation;
if (use_complex_angles_)
new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
else
- new_orientation = new StateTheta(problem_->getNewStatePtr());
+ new_orientation = new StateBase(problem_->getNewStatePtr());
problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
@@ -178,20 +178,20 @@ class WolfManager
if (new_capture->getSensorPtr() == sensor_prior_)
{
// FIND PREVIOUS RELATIVE CAPTURE
- CaptureRelative* previous_relative_capture = nullptr;
+ CaptureMotion* previous_relative_capture = nullptr;
for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
{
//std::cout << "capture: " << (*capture_it)->nodeId() << std::endl;
if ((*capture_it)->getSensorPtr() == sensor_prior_)
{
- previous_relative_capture = (CaptureRelative*)(*capture_it);
+ previous_relative_capture = (CaptureMotion*)(*capture_it);
break;
}
}
// INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
- previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture));
+ previous_relative_capture->integrateCapture((CaptureMotion*)(new_capture));
Eigen::VectorXs prior = previous_relative_capture->computePrior();
// NEW KEY FRAME (if enough time from last frame)
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
index fbe9ee76cb256bece3168c23890d3e316a375124..20560388261908152a303ccf57bee98006abbca6 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -264,7 +264,7 @@ class Constraint2DOdometryTheta : public ConstraintSparse<2,2,1,2,1>
virtual ConstraintType getType() const
{
- return CTR_ODOM_2D_THETA;
+ return CTR_ODOM_2D;
}
};
@@ -355,19 +355,19 @@ class WolfManager
// Create frame
if (use_complex_angles_)
// frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
-// StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
+// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)),
// StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)))));
frames_.push_back(new FrameBase(_time_stamp,
- new StatePoint2D(state_.data()+first_empty_state_),
+ new StateBase(state_.data()+first_empty_state_),
new StateComplexAngle(state_.data()+first_empty_state_+2)));
else
// frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp,
-// StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
-// StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2)))));
+// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)),
+// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_+2)))));
frames_.push_back(new FrameBase(_time_stamp,
- new StatePoint2D(state_.data()+first_empty_state_),
- new StateTheta(state_.data()+first_empty_state_+2)));
+ new StateBase(state_.data()+first_empty_state_),
+ new StateBase(state_.data()+first_empty_state_+2)));
// Update first free state location index
first_empty_state_ += use_complex_angles_ ? 4 : 3;
}
@@ -605,13 +605,13 @@ class CeresManager
case ST_POINT_2D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
@@ -657,7 +657,7 @@ class CeresManager
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D_THETA:
+ case CTR_ODOM_2D:
{
Constraint2DOdometryTheta* specific_ptr = (Constraint2DOdometryTheta*)(_corrPtr);
return new ceres::AutoDiffCostFunction<Constraint2DOdometryTheta,
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
index 57c31d85b0635516c61b021a9474f73c9baa3993..d33a4bf4b9e870b13e6d303047147442860b0abd 100644
--- a/src/examples/test_ceres_manager_tree.cpp
+++ b/src/examples/test_ceres_manager_tree.cpp
@@ -86,15 +86,15 @@ class WolfManager
if (use_complex_angles_)
{
FrameBase* new_frame = new FrameBase(_time_stamp,
- new StatePoint2D(state_.data()+first_empty_state_),
+ new StateBase(state_.data()+first_empty_state_),
new StateComplexAngle(state_.data()+first_empty_state_+2));
trajectory_->addFrame(new_frame);
}
else
{
FrameBase* new_frame = new FrameBase(_time_stamp,
- new StatePoint2D(state_.data()+first_empty_state_),
- new StateTheta(state_.data()+first_empty_state_+2));
+ new StateBase(state_.data()+first_empty_state_),
+ new StateBase(state_.data()+first_empty_state_+2));
trajectory_->addFrame(new_frame);
}
diff --git a/src/examples/test_ceres_wrapper_jet.cpp b/src/examples/test_ceres_wrapper_jet.cpp
index c071b848ad86ebb9baea83fc3d3c91f64a630bc1..cf8dd52d54696496dc4e45d39df8d0af1f40ec2c 100644
--- a/src/examples/test_ceres_wrapper_jet.cpp
+++ b/src/examples/test_ceres_wrapper_jet.cpp
@@ -20,8 +20,6 @@
//Wolf includes
#include "sensor_base.h"
#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
#include "capture_base.h"
#include "state_base.h"
#include "wolf.h"
diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/test_iQR_wolf2.cpp
index f99aeb028750d6fbc2b5c210b86f860ebfe504a3..05ecf64df6c9989c6385546a2d6f4ef233d6472f 100644
--- a/src/examples/test_iQR_wolf2.cpp
+++ b/src/examples/test_iQR_wolf2.cpp
@@ -176,18 +176,18 @@ int main(int argc, char *argv[])
Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(new StatePoint3D(odom_pose.data()), new StateTheta(&odom_pose(2)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(new StatePoint3D(gps_pose.data()), new StateTheta(&gps_pose(2)), gps_std);
- SensorLaser2D laser_1_sensor(new StatePoint3D(laser_1_pose.data()), new StateTheta(&laser_1_pose(3)));
- SensorLaser2D laser_2_sensor(new StatePoint3D(laser_2_pose.data()), new StateTheta(&laser_2_pose(3)));
+ SensorOdom2D odom_sensor(new StateBase(odom_pose.data()), new StateBase(&odom_pose(2)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StateBase(gps_pose.data()), new StateBase(&gps_pose(2)), gps_std);
+ SensorLaser2D laser_1_sensor(new StateBase(laser_1_pose.data()), new StateBase(&laser_1_pose(3)));
+ SensorLaser2D laser_2_sensor(new StateBase(laser_2_pose.data()), new StateBase(&laser_2_pose(3)));
// Initial pose
pose_odom << 2, 8, 0;
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager<StatePoint2D, StateTheta>* wolf_manager_QR = new WolfManager<StatePoint2D, StateTheta>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
- WolfManager<StatePoint2D, StateTheta>* wolf_manager_ceres = new WolfManager<StatePoint2D, StateTheta>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager<StateBase, StateBase>* wolf_manager_QR = new WolfManager<StateBase, StateBase>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager<StateBase, StateBase>* wolf_manager_ceres = new WolfManager<StateBase, StateBase>(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
deleted file mode 100644
index be5ac860ad21b7bee7145ad69687351cd30d9d03..0000000000000000000000000000000000000000
--- a/src/examples/test_state_quaternion.cpp
+++ /dev/null
@@ -1,47 +0,0 @@
-
-//std
-#include <iostream>
-
-//Eigen
-#include <eigen3/Eigen/Geometry>
-
-//Wolf
-#include "wolf.h"
-#include "state_quaternion.h"
-
-int main()
-{
- std::cout << std::endl << "Eigen Quatenrnion test" << std::endl;
-
- WolfScalar q1[4];
- Eigen::Map<Eigen::Quaternions> q1_map(q1);
-
- //try to find out how eigen sorts storage (real part tail or head ? )
- std::cout << std::endl << "************************** TEST #1 ***************************" << std::endl;
- q1_map.w() = -1;
- q1_map.x() = 2;
- q1_map.y() = 5;
- q1_map.z() = 9;
- std::cout << "q1[0]=" << q1[0] << "; q1_map.x()=" << q1_map.x() << std::endl;
- std::cout << "q1[1]=" << q1[1] << "; q1_map.y()=" << q1_map.y() << std::endl;
- std::cout << "q1[2]=" << q1[2] << "; q1_map.z()=" << q1_map.z() << std::endl;
- std::cout << "q1[3]=" << q1[3] << "; q1_map.w()=" << q1_map.w() << std::endl;
- std::cout << std::endl << "RESULT: Eigen stores REAL part in the LAST memory position of the quaternion." << std::endl;
-
- //rot matrix
- std::cout << std::endl << "************************** TEST #2 ***************************" << std::endl;
- StateQuaternion sq(q1);
- Eigen::Matrix3s RM;
- sq.normalize();
- std::cout << "Rot matrix through StateQuaternion class" << std::endl;
- RM = sq.getRotationMatrix();
- std::cout << RM << std::endl;
-
- std::cout << "Rot matrix through Eigen::Map class" << std::endl;
- RM = q1_map.toRotationMatrix();
- std::cout << RM << std::endl;
-
- std::cout << std::endl << "End of Eigen Quatenrnion tests" << std::endl;
- return 0;
-}
-
diff --git a/src/feature_odom_2D.h b/src/feature_odom_2D.h
index 2c52bf8b29cb6e11ed6031edf90924c33806656d..b220c8b3c751527766e54073a242bc555c1faa0a 100644
--- a/src/feature_odom_2D.h
+++ b/src/feature_odom_2D.h
@@ -5,8 +5,7 @@
//Wolf includes
#include "feature_base.h"
-#include "constraint_odom_2D_theta.h"
-#include "constraint_odom_2D_complex_angle.h"
+#include "constraint_odom_2D.h"
//class FeatureOdom2D
class FeatureOdom2D : public FeatureBase
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 56d595130a948eaf950b52f5e90ac498f2e75d82..30be6810a386522b4d74981cad6c36500391ef27 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, StateOrientation* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+FrameBase::FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _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, StateOrientation*
//
}
-FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateOrientation* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
//NodeLinked(MID, "FRAME", _traj_ptr),
NodeLinked(MID, "FRAME"),
type_(_tp),
@@ -180,7 +180,7 @@ FrameBase* FrameBase::getPreviousFrame() const
FrameBase* FrameBase::getNextFrame() const
{
- //std::cout << "finding previous frame of " << this->node_id_ << std::endl;
+ //std::cout << "finding next frame of " << this->node_id_ << std::endl;
auto f_it = getTrajectoryPtr()->getFrameListPtr()->rbegin();
f_it++; //starting from second last frame
@@ -203,7 +203,7 @@ StateBase* FrameBase::getPPtr() const
return p_ptr_;
}
-StateOrientation* FrameBase::getOPtr() const
+StateBase* FrameBase::getOPtr() const
{
return o_ptr_;
}
diff --git a/src/frame_base.h b/src/frame_base.h
index 51c3742027641ce7984cc28910ab2b7f08e0d2a7..fec673a1237ed3f720d838de1a8c7be337362f02 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
- StateOrientation* o_ptr_; // Orientation state unit pointer
+ StateBase* 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, 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
+ 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
/** \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, StateOrientation* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _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;
- StateOrientation* getOPtr() const;
+ StateBase* getOPtr() const;
StateBase* getVPtr() const;
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 7ffddffd7f502b3c61f0ea18b74137bc07a5f657..54608d9a3a90d7d83ec6d98e6aa049891e5e5145 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, StateOrientation* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
+LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, StateBase* _v_ptr, StateBase* _w_ptr) :
NodeLinked(MID, "LANDMARK"),
type_(_tp),
status_(LANDMARK_CANDIDATE),
@@ -103,7 +103,7 @@ StateBase* LandmarkBase::getPPtr() const
return p_ptr_;
}
-StateOrientation* LandmarkBase::getOPtr() const
+StateBase* LandmarkBase::getOPtr() const
{
return o_ptr_;
}
@@ -123,7 +123,7 @@ void LandmarkBase::setPPtr(StateBase* _st_ptr)
p_ptr_ = _st_ptr;
}
-void LandmarkBase::setOPtr(StateOrientation* _st_ptr)
+void LandmarkBase::setOPtr(StateBase* _st_ptr)
{
o_ptr_ = _st_ptr;
}
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 6dfcad540700e1f8850778dba58912cceee928de..39c7ead8d9ac93db92b2ae892da6a3f80f6f1862 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -16,7 +16,7 @@ class NodeTerminus;
#include "time_stamp.h"
#include "node_linked.h"
#include "map_base.h"
-#include "state_orientation.h"
+#include "state_base.h"
#include "constraint_base.h"
// why v, w and a ?
@@ -34,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
- StateOrientation* o_ptr_; // Orientation state unit pointer
+ StateBase* o_ptr_; // Orientation state unit pointer
StateBase* v_ptr_; // Velocity state unit pointer
StateBase* w_ptr_; // Angular velocity state unit pointer
//TODO: accelerations?
@@ -47,12 +47,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 StateOrientation pointer to the orientation (default: nullptr)
+ * \param _o_ptr StateBase 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, StateOrientation* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
/** \brief Constructor with type, time stamp and state list
*
@@ -85,7 +85,7 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
StateBase* getPPtr() const;
- StateOrientation* getOPtr() const;
+ StateBase* getOPtr() const;
StateBase* getVPtr() const;
@@ -93,7 +93,7 @@ class LandmarkBase : public NodeLinked<MapBase, NodeTerminus>
void setPPtr(StateBase* _st_ptr);
- void setOPtr(StateOrientation* _st_ptr);
+ void setOPtr(StateBase* _st_ptr);
void setVPtr(StateBase* _st_ptr);
diff --git a/src/landmark_container.cpp b/src/landmark_container.cpp
index eb6a68f1290d23fbc56a4b6b4c162d6002692f63..d484500012449e2e15ebfdce5dd749c767aa6e68 100644
--- a/src/landmark_container.cpp
+++ b/src/landmark_container.cpp
@@ -1,7 +1,7 @@
#include "landmark_container.h"
-LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
corners_(3,4)
{
@@ -14,7 +14,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
0, M_PI/2, M_PI, -M_PI/2;
}
-LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh, const WolfScalar& _length) :
LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
corners_(3,4)
{
@@ -41,13 +41,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
Eigen::Vector2s perpendicularAB;
perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AB / 2 + perpendicularAB * _witdh / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(AB(1),AB(0));
- else
- {
- container_orientation(0) = AB(0)/AB.norm();
- container_orientation(1) = AB(1)/AB.norm();
- }
+ container_orientation(0) = atan2(AB(1),AB(0));
}
// Short side detected (B & C)
@@ -58,13 +52,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
Eigen::Vector2s perpendicularBC;
perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BC / 2 + perpendicularBC * _length / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(BC(1),BC(0));
- else
- {
- container_orientation(0) = BC(0)/BC.norm();
- container_orientation(1) = BC(1)/BC.norm();
- }
+ container_orientation(0) = atan2(BC(1),BC(0));
}
// Diagonal detected
@@ -74,13 +62,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
std::cout << "diagonal AC detected" << std::endl;
Eigen::Vector2s AC = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AC / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
- else
- {
- container_orientation(0) = AC(0)/AC.norm() -_length/descriptor.norm();
- container_orientation(1) = AC(1)/AC.norm() -_witdh/descriptor.norm();
- }
+ container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
}
// B & D
else if ( (_corner_1_idx == 1 && _corner_2_idx == 3) || (_corner_1_idx == 3 && _corner_2_idx == 1) )
@@ -88,18 +70,12 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
std::cout << "diagonal BD detected" << std::endl;
Eigen::Vector2s BD = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BD / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
- else
- {
- container_orientation(0) = BD(0)/BD.norm() +_length/descriptor.norm();
- container_orientation(1) = BD(1)/BD.norm() +_witdh/descriptor.norm();
- }
+ container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
}
std::cout << "Container center pose... " << container_position.transpose() << " " << container_orientation.transpose() << std::endl;
}
-LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
+LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh, const WolfScalar& _length) :
LandmarkBase(LANDMARK_CONTAINER, _p_ptr, _o_ptr),
corners_(3,4)
{
@@ -128,13 +104,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
Eigen::Vector2s perpendicularAB;
perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) + AB / 2 + perpendicularAB * _witdh / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(AB(1),AB(0));
- else
- {
- container_orientation(0) = AB(0)/AB.norm();
- container_orientation(1) = AB(1)/AB.norm();
- }
+ container_orientation(0) = atan2(AB(1),AB(0));
}
// C & D
else if (_corner_C_ptr != nullptr && _corner_D_ptr != nullptr)
@@ -143,13 +113,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
Eigen::Vector2s perpendicularCD;
perpendicularCD << -CD(1)/CD.norm(), CD(0)/CD.norm();
container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) + CD / 2 + perpendicularCD * _witdh / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(-CD(1),-CD(0));
- else
- {
- container_orientation(0) = -CD(0)/CD.norm();
- container_orientation(1) = -CD(1)/CD.norm();
- }
+ container_orientation(0) = atan2(-CD(1),-CD(0));
}
// Short side detected
// B & C
@@ -159,13 +123,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
Eigen::Vector2s perpendicularBC;
perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) + BC / 2 + perpendicularBC * _length / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(BC(1),BC(0));
- else
- {
- container_orientation(0) = BC(0)/BC.norm();
- container_orientation(1) = BC(1)/BC.norm();
- }
+ container_orientation(0) = atan2(BC(1),BC(0));
}
// D & A
else if (_corner_D_ptr != nullptr && _corner_A_ptr != nullptr)
@@ -174,13 +132,7 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
Eigen::Vector2s perpendicularDA;
perpendicularDA << -DA(1)/DA.norm(), DA(0)/DA.norm();
container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) + DA / 2 + perpendicularDA * _length / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(-DA(1),-DA(0));
- else
- {
- container_orientation(0) = -DA(0)/DA.norm();
- container_orientation(1) = -DA(1)/DA.norm();
- }
+ container_orientation(0) = atan2(-DA(1),-DA(0));
}
// Diagonal detected
// A & C
@@ -188,26 +140,14 @@ LandmarkContainer::LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr
{
Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr());
container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) + AC / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
- else
- {
- container_orientation(0) = AC(0)/AC.norm() -_length/descriptor.norm();
- container_orientation(1) = AC(1)/AC.norm() -_witdh/descriptor.norm();
- }
+ container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
}
// B & D
else if (_corner_B_ptr != nullptr && _corner_D_ptr != nullptr)
{
Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr());
container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) + BD / 2;
- if (_o_ptr->getStateType() == ST_THETA)
- container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
- else
- {
- container_orientation(0) = BD(0)/BD.norm() +_length/descriptor.norm();
- container_orientation(1) = BD(1)/BD.norm() +_witdh/descriptor.norm();
- }
+ container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
}
}
diff --git a/src/landmark_container.h b/src/landmark_container.h
index 3726276b4236b787b2c3ec166c219c1644eeb63e..09c5cc448211c2adfb98413a634283c3733c4fd2 100644
--- a/src/landmark_container.h
+++ b/src/landmark_container.h
@@ -25,18 +25,18 @@ class LandmarkContainer : public LandmarkBase
*
* Constructor with type, and state pointer
* \param _p_ptr StateBase pointer to the position
- * \param _o_ptr StateOrientation pointer to the orientation
+ * \param _o_ptr StateBase pointer to the orientation
* \param _witdh descriptor of the landmark: container width
* \param _length descriptor of the landmark: container length
*
**/
- LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+ LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
/** \brief Constructor with type, time stamp and the position state pointer
*
* Constructor with type, and state pointer
* \param _p_ptr StateBase pointer to the position
- * \param _o_ptr StateOrientation pointer to the orientation
+ * \param _o_ptr StateBase pointer to the orientation
* \param _corner_1_pose pose of corner 1
* \param _corner_2_pose pose of corner 2
* \param _corner_1_idx index of corner 1 (A = 0, B = 1, C = 2, D = 3)
@@ -45,20 +45,20 @@ class LandmarkContainer : public LandmarkBase
* \param _length descriptor of the landmark: container length
*
**/
- LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+ LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
/** \brief Constructor with type, time stamp and the position state pointer
*
* Constructor with type, and state pointer
* \param _p_ptr StateBase pointer to the position
- * \param _o_ptr StateOrientation pointer to the orientation
+ * \param _o_ptr StateBase pointer to the orientation
* \param _corner_1_ptr LandmarkCorner2D pointer to one of its corners
* \param _corner_2_ptr LandmarkCorner2D pointer to one of its corners
* \param _witdh descriptor of the landmark: container width
* \param _length descriptor of the landmark: container length
*
**/
- LandmarkContainer(StateBase* _p_ptr, StateOrientation* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
+ LandmarkContainer(StateBase* _p_ptr, StateBase* _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const WolfScalar& _witdh=2.44, const WolfScalar& _length=12.2);
/** \brief Destructor
*
diff --git a/src/landmark_corner_2D.cpp b/src/landmark_corner_2D.cpp
index d0d35e3732f92b166f8ad337490b3989f5062605..176effa1820cfb32c61f20a432e6910981322c93 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, StateOrientation* _o_ptr, const WolfScalar& _aperture) :
+LandmarkCorner2D::LandmarkCorner2D(StateBase* _p_ptr, StateBase* _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 bd9afb65d13ca211dcfe5fae5f596f15fda7c489..cb6645840316f6aa803928c5cb8987a637d2836b 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, StateOrientation* _o_ptr, const WolfScalar& _aperture=0);
+ LandmarkCorner2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _aperture=0);
/** \brief Destructor
*
diff --git a/src/node_base.cpp b/src/node_base.cpp
index 57bcc414c424c5d4fa0980879515acb95209fadb..685d67bebbaf5d8cefc749ac758534b8dd875b3d 100644
--- a/src/node_base.cpp
+++ b/src/node_base.cpp
@@ -27,16 +27,6 @@ std::string NodeBase::nodeLabel() const
return label_;
}
-PendingStatus NodeBase::getPendingStatus() const
-{
- return node_pending_;
-}
-
-void NodeBase::setPendingStatus(PendingStatus _pending)
-{
- node_pending_ = _pending;
-}
-
void NodeBase::print(unsigned int _ntabs, std::ostream& _ost) const
{
_ost << label_ << " " << node_id_ << std::endl;
diff --git a/src/node_base.h b/src/node_base.h
index 0716fff5118be42392977308365732eb047714fa..fbdfe2ab4e90a018a950eecc0f61369f39138238 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -51,20 +51,6 @@ class NodeBase
*/
std::string nodeLabel() const;
- /** \brief Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- * Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- */
- PendingStatus getPendingStatus() const;
-
- /** \brief Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- * Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
- *
- */
- void setPendingStatus(PendingStatus _pending);
-
/** \brief Print node information
*
* Prints node information. Inine function.
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 63beea954634e097b7ac097cbb4d41c831e9e9ae..f186de66204ad9a71e4706533f1c129363ed4838 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -1,6 +1,6 @@
#include "sensor_base.h"
-SensorBase::SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params) :
+SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::VectorXs & _params) :
NodeBase("SENSOR"),
type_(_tp),
p_ptr_(_p_ptr),
@@ -10,7 +10,7 @@ SensorBase::SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrient
params_ = _params;
}
-SensorBase::SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size) :
+SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, unsigned int _params_size) :
NodeBase("SENSOR"),
type_(_tp),
p_ptr_(_p_ptr),
@@ -30,12 +30,31 @@ const SensorType SensorBase::getSensorType() const
return type_;
}
-StatePoint3D* SensorBase::getPPtr() const
+StateBase* SensorBase::getPPtr() const
{
return p_ptr_;
}
-StateOrientation* SensorBase::getOPtr() const
+StateBase* SensorBase::getOPtr() const
{
return o_ptr_;
}
+
+Eigen::Matrix2s SensorBase::getRotationMatrix2D() {
+ // TODO: move this code somewhere else and do a real get()
+ assert ( o_ptr_->getStateType() != ST_QUATERNION && "2D rot matrix not defined for quaternions." );
+ return Eigen::Rotation2D<WolfScalar>(*(o_ptr_->getPtr())).matrix();
+}
+
+Eigen::Matrix3s SensorBase::getRotationMatrix3D() {
+ // TODO: move this code somewhere else and do a real get()
+ Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
+
+ if ( o_ptr_->getStateType() == ST_QUATERNION )
+ R.block<2,2>(0,0) = Eigen::Rotation2D<WolfScalar>(*(o_ptr_->getPtr())).matrix();
+
+ else
+ R = Eigen::Map<Eigen::Quaternions>(o_ptr_->getPtr()).toRotationMatrix();
+
+ return R;
+}
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 58512587c058cc2e78867f8a6653e6ac8a5fa69f..d16c82608a2c5d9a69b1920f7c81a7c97a1fc628 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -7,15 +7,14 @@
//Wolf includes
#include "wolf.h"
#include "node_base.h"
-#include "state_point.h"
-#include "state_orientation.h"
+#include "state_base.h"
class SensorBase : public NodeBase
{
protected:
SensorType type_; //indicates sensor type. Enum defined at wolf.h
- StatePoint3D* p_ptr_; // sensor position state unit pointer
- StateOrientation* o_ptr_; // sensor orientation state unit pointer
+ StateBase* p_ptr_; // sensor position state unit pointer
+ StateBase* o_ptr_; // sensor orientation state unit pointer
Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ...
public:
@@ -32,30 +31,35 @@ class SensorBase : public NodeBase
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
* \param _p_ptr StateBase pointer to the sensor position
- * \param _o_ptr StateOrientation pointer to the sensor orientation
+ * \param _o_ptr StateBase pointer to the sensor orientation
* \param _params Vector containing the sensor parameters
*
**/
- SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params);
+ SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, const Eigen::VectorXs & _params);
/** \brief Constructor with parameter size
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
* \param _p_ptr StateBase pointer to the sensor position
- * \param _o_ptr StateOrientation pointer to the sensor orientation
+ * \param _o_ptr StateBase pointer to the sensor orientation
* \param _params_size size of the vector containing the sensor parameters
*
**/
- SensorBase(const SensorType & _tp, StatePoint3D* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size);
+ SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateBase* _o_ptr, unsigned int _params_size);
~SensorBase();
const SensorType getSensorType() const;
- StatePoint3D* getPPtr() const;
+ StateBase* getPPtr() const;
+
+ StateBase* getOPtr() const;
+
+ Eigen::Matrix2s getRotationMatrix2D();
+
+ Eigen::Matrix3s getRotationMatrix3D();
- StateOrientation* getOPtr() const;
};
#endif
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index 9af26aa724a236e7a6c3acc86aac53423e6a2bcb..fffa2d51304c4edfc3d78955835888e143a2662b 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -1,6 +1,6 @@
#include "sensor_gps_fix.h"
-SensorGPSFix::SensorGPSFix(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const double& _noise) :
+SensorGPSFix::SensorGPSFix(StateBase* _p_ptr, StateBase* _o_ptr, const double& _noise) :
SensorBase(GPS_FIX, _p_ptr, _o_ptr, Eigen::VectorXs::Constant(1,_noise))
{
//
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
index ec85cac55f9885d581d4fd3e85ceaf1634c45cd6..5eedff65bbdbbcd64624ce7004e05ef1f0c1b5b1 100644
--- a/src/sensor_gps_fix.h
+++ b/src/sensor_gps_fix.h
@@ -12,11 +12,11 @@ class SensorGPSFix : public SensorBase
*
* Constructor with arguments
* \param _p_ptr StateBase pointer to the sensor position
- * \param _o_ptr StateOrientation pointer to the sensor orientation
+ * \param _o_ptr StateBase pointer to the sensor orientation
* \param _noise noise standard deviation
*
**/
- SensorGPSFix(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const double& _noise);
+ SensorGPSFix(StateBase* _p_ptr, StateBase* _o_ptr, const double& _noise);
/** \brief Destructor
*
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 4c1c48af34ad05ea8d0746dcb55ddb8c51174cc7..b3f2b9820d99bbb861809f3cde146c40ed54b95c 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -7,7 +7,7 @@
// params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time;
// }
-SensorLaser2D::SensorLaser2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr) :
+SensorLaser2D::SensorLaser2D(StateBase* _p_ptr, StateBase* _o_ptr) :
SensorBase(LIDAR, _p_ptr, _o_ptr, 8)
{
setDefaultScanParams();
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index e500434a6bd6711a8a48e872cccace9df8f2c31b..c8a3ce0c5f3f009ad5bcd84998440b05e92b5ed2 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -25,11 +25,11 @@ class SensorLaser2D : public SensorBase
*
* Constructor with arguments
* \param _p_ptr StateBase pointer to the sensor position
- * \param _o_ptr StateOrientation pointer to the sensor orientation
+ * \param _o_ptr StateBase pointer to the sensor orientation
* \param _params struct with scan parameters. See laser_scan_utils library API for reference
*
**/
- SensorLaser2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr);
+ SensorLaser2D(StateBase* _p_ptr, StateBase* _o_ptr);
//SensorLaser2D(const Eigen::VectorXs & _sp, const laserscanutils::ScanParams & _params);
/** \brief Destructor
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index 19ac3eae28d1ccb4c450f31289d7acbd57c8bf0a..6359f6236b29b15a8adad67c08cdb6ee46fbf5ba 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -1,6 +1,6 @@
#include "sensor_odom_2D.h"
-SensorOdom2D::SensorOdom2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
+SensorOdom2D::SensorOdom2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
SensorBase(ODOM_2D, _p_ptr, _o_ptr, 2)
{
params_ << _disp_noise_factor, _rot_noise_factor;
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 675edd79d7c09e6f90963beccdb56a9723e24535..24e67daff68173b8a620bcb754f12f4db98d4e60 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -12,12 +12,12 @@ class SensorOdom2D : public SensorBase
*
* Constructor with arguments
* \param _p_ptr StateBase pointer to the sensor position wrt vehicle base
- * \param _o_ptr StateOrientation pointer to the sensor orientation wrt vehicle base
+ * \param _o_ptr StateBase pointer to the sensor orientation wrt vehicle base
* \param _disp_noise_factor displacement noise factor
* \param _rot_noise_factor rotation noise factor
*
**/
- SensorOdom2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
+ SensorOdom2D(StateBase* _p_ptr, StateBase* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
/** \brief Destructor
*
diff --git a/src/sensor_twist_2D.cpp b/src/sensor_twist_2D.cpp
deleted file mode 100644
index b20dfd95f0011d953f9e4e1d38f44d8caf13208e..0000000000000000000000000000000000000000
--- a/src/sensor_twist_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "sensor_twist_2D.h"
-
-SensorTwist2D::SensorTwist2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _lineal_noise, const WolfScalar& _angular_noise) :
- SensorBase(TWIST_2D, _p_ptr, _o_ptr, 2)
-{
- params_ << _lineal_noise, _angular_noise;
-}
-
-SensorTwist2D::~SensorTwist2D()
-{
- //
-}
-
-WolfScalar SensorTwist2D::getLinealNoise() const
-{
- return params_(0);
-}
-
-WolfScalar SensorTwist2D::getAngularNoise() const
-{
- return params_(1);
-}
diff --git a/src/sensor_twist_2D.h b/src/sensor_twist_2D.h
deleted file mode 100644
index a28ee2d511c76f891bafffb28757592fb898dffc..0000000000000000000000000000000000000000
--- a/src/sensor_twist_2D.h
+++ /dev/null
@@ -1,44 +0,0 @@
-
-#ifndef SENSOR_TWIST_2D_H_
-#define SENSOR_TWIST_2D_H_
-
-//wolf includes
-#include "sensor_base.h"
-
-class SensorTwist2D : public SensorBase
-{
- public:
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _p_ptr StateBase pointer to the sensor position wrt vehicle base
- * \param _o_ptr StateOrientation pointer to the sensor orientation wrt vehicle base
- * \param _lineal_noise lineal velocity noise
- * \param _angular_noise angular velocity noise
- *
- **/
- SensorTwist2D(StatePoint3D* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _lineal_noise, const WolfScalar& _angular_noise);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~SensorTwist2D();
-
- /** \brief Returns displacement noise factor
- *
- * Returns displacement noise factor
- *
- **/
- double getLinealNoise() const;
-
- /** \brief Returns rotation noise factor
- *
- * Returns rotation noise factor
- *
- **/
- double getAngularNoise() const;
-
-};
-#endif
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index bd44b91bd7b4a87291f2f2484212afb95c7731dd..b32be207a5fd633ea7fb013e06054f5f5440bbe6 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -546,10 +546,10 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D_THETA:
+ case CTR_ODOM_2D:
{
- ConstraintOdom2DTheta* specific_ptr = (ConstraintOdom2DTheta*)(_corrPtr);
- return (CostFunctionBase*)new CostFunctionSparse<ConstraintOdom2DTheta,
+ ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
+ return (CostFunctionBase*)new CostFunctionSparse<ConstraintOdom2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -563,10 +563,10 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D_THETA:
+ case CTR_CORNER_2D:
{
- ConstraintCorner2DTheta* specific_ptr = (ConstraintCorner2DTheta*)(_corrPtr);
- return (CostFunctionBase*)new CostFunctionSparse<ConstraintCorner2DTheta,
+ ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
+ return (CostFunctionBase*)new CostFunctionSparse<ConstraintCorner2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 4ae845e2dcdee7d8874fdbae74f7ca819d78815c..baf5fad764697c1daeb1cf8a83a7344e2f1aa28f 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -91,7 +91,7 @@ void SolverManager::addStateUnit(StateBase* _st_ptr)
case ST_THETA:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateTheta*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_1D:
@@ -100,16 +100,16 @@ void SolverManager::addStateUnit(StateBase* _st_ptr)
ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
- case ST_POINT_2D:
+ case ST_VECTOR:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBase*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
@@ -186,10 +186,10 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBase* _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D_THETA:
+ case CTR_ODOM_2D:
{
- ConstraintOdom2DTheta* specific_ptr = (ConstraintOdom2DTheta*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2DTheta,
+ ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintOdom2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -203,10 +203,10 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBase* _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D_THETA:
+ case CTR_CORNER_2D:
{
- ConstraintCorner2DTheta* specific_ptr = (ConstraintCorner2DTheta*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintCorner2DTheta,
+ ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintCorner2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
diff --git a/src/state_base.cpp b/src/state_base.cpp
index 915a78c9d25da948b5b439e883a0c2b55f250a39..fd48d87b58f4c49d223c0c741326d17910f591d0 100644
--- a/src/state_base.cpp
+++ b/src/state_base.cpp
@@ -1,22 +1,16 @@
#include "state_base.h"
-StateBase::StateBase(WolfScalar* _st_ptr) :
- NodeBase("STATE"),
+StateBase::StateBase(WolfScalar* _st_ptr, unsigned int _size, StateType _type, StateStatus _status) :
+ type_(_type),
state_ptr_(_st_ptr),
- status_(ST_ESTIMATED)
+ size_(_size),
+ status_(_status),
+ pending_status_(ADD_PENDING)
{
//
}
-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;
@@ -43,3 +37,14 @@ void StateBase::setStateStatus(const StateStatus& _status)
if (getPendingStatus() != ADD_PENDING)
setPendingStatus(UPDATE_PENDING);
}
+
+PendingStatus StateBase::getPendingStatus() const
+{
+ return pending_status_;
+}
+
+void StateBase::setPendingStatus(PendingStatus _pending)
+{
+ pending_status_ = _pending;
+}
+
diff --git a/src/state_base.h b/src/state_base.h
index 68ead00e655c67164632baae5b5724c6f7c973ab..066fc05a0e09639d2dbc2ac9adadb3b1232a2129 100644
--- a/src/state_base.h
+++ b/src/state_base.h
@@ -12,11 +12,14 @@
#include "node_base.h"
//class StateBase
-class StateBase : public NodeBase
+class StateBase //: public NodeBase
{
protected:
+ StateType type_;
WolfScalar* state_ptr_;
+ unsigned int size_;
StateStatus status_;
+ PendingStatus pending_status_;
public:
/** \brief Constructor with scalar pointer
@@ -25,16 +28,7 @@ class StateBase : public NodeBase
* \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
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx);
+ StateBase(WolfScalar* _st_ptr, unsigned int _size, StateType _type = ST_VECTOR, StateStatus _status = ST_ESTIMATED);
/** \brief Destructor
*
@@ -91,6 +85,21 @@ class StateBase : public NodeBase
*
**/
virtual unsigned int getStateSize() const = 0;
+
+ /** \brief Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ * Gets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ */
+ PendingStatus getPendingStatus() const;
+
+ /** \brief Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ * Sets the node pending status (pending or not to be added/updated in the filter or optimizer)
+ *
+ */
+ void setPendingStatus(PendingStatus _pending);
+
/** \brief Prints all the elements of the state unit
*
diff --git a/src/state_complex_angle.cpp b/src/state_complex_angle.cpp
deleted file mode 100644
index 7801214ff739572d35ee8a7da51db745920cd5f2..0000000000000000000000000000000000000000
--- a/src/state_complex_angle.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-
-#include "state_complex_angle.h"
-
-StateComplexAngle::StateComplexAngle(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateOrientation(_st_remote, _idx)
-{
- //
-}
-
-StateComplexAngle::StateComplexAngle(WolfScalar* _st_ptr) :
- StateOrientation(_st_ptr)
-{
- //
-}
-
-StateComplexAngle::~StateComplexAngle()
-{
- //
-}
-
-StateType StateComplexAngle::getStateType() const
-{
- return ST_COMPLEX_ANGLE;
-}
-
-unsigned int StateComplexAngle::getStateSize() const
-{
- return BLOCK_SIZE;
-}
-
-void StateComplexAngle::rotationMatrix(Eigen::Matrix3s& R) const
-{
- R = Eigen::Matrix3s::Identity();
-
- R(0,0) = *state_ptr_;
- R(1,0) = *(state_ptr_+1);
-
- R(0,1) = -*(state_ptr_+1);
- R(1,1) = *state_ptr_;
-}
-
-Eigen::Map<const Eigen::VectorXs> StateComplexAngle::getVector() const
-{
- return Eigen::Map<const Eigen::VectorXs>(state_ptr_, BLOCK_SIZE);
-}
-
-WolfScalar StateComplexAngle::getYaw() const
-{
- return atan2(*(state_ptr_+1), *state_ptr_);
-}
-
-void StateComplexAngle::print(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << nodeLabel() << " " << nodeId() << std::endl;
- printTabs(_ntabs);
- _ost << *state_ptr_<< " " << *(state_ptr_+1) << std::endl;
-}
diff --git a/src/state_complex_angle.h b/src/state_complex_angle.h
deleted file mode 100644
index 6f356144ad4985c49d10416fc4d5dbfb3677e82d..0000000000000000000000000000000000000000
--- a/src/state_complex_angle.h
+++ /dev/null
@@ -1,87 +0,0 @@
-
-#ifndef STATE_COMPLEX_ANGLE_H_
-#define STATE_COMPLEX_ANGLE_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "state_orientation.h"
-
-//class StateComplexAngle
-class StateComplexAngle : public StateOrientation
-{
- public:
- static const unsigned int BLOCK_SIZE = 2;
-
- /** \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
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateComplexAngle(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
- *
- **/
- StateComplexAngle(WolfScalar* _st_ptr);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateComplexAngle();
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual StateType getStateType() const;
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const;
-
- /** \brief Returns the 3x3 rotation matrix of the orientation
- *
- * Returns the 3x3 rotation matrix of the orientation
- *
- **/
- //virtual Eigen::Matrix3s getRotationMatrix() const;
- void rotationMatrix(Eigen::Matrix3s& R) const;
-
- /** \brief Returns the yaw angle
- *
- * Returns the yaw angle
- *
- **/
- virtual WolfScalar getYaw() const;
-
- /** \brief Returns a (mapped) vector of the state unit
- *
- * Returns a (mapped) vector of the state unit
- *
- **/
- virtual Eigen::Map<const Eigen::VectorXs> getVector() const;
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-#endif
diff --git a/src/state_orientation.cpp b/src/state_orientation.cpp
deleted file mode 100644
index 58ba497e5b02cc63f8023571fde21c01eeed786b..0000000000000000000000000000000000000000
--- a/src/state_orientation.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-
-#include "state_orientation.h"
-
-StateOrientation::StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
-{
- //
-}
-
-
-StateOrientation::StateOrientation(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
-{
- //
-}
-
-StateOrientation::~StateOrientation()
-{
- //
-}
-
-Eigen::Matrix3s StateOrientation::getRotationMatrix() const
-{
- Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
- rotationMatrix(R);
- return R;
-}
diff --git a/src/state_orientation.h b/src/state_orientation.h
deleted file mode 100644
index 4ffc5413ab9a285240345cbc756fc0b0f25ded1d..0000000000000000000000000000000000000000
--- a/src/state_orientation.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef STATE_ORIENTATION_H_
-#define STATE_ORIENTATION_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "state_base.h"
-
-//class StateOrientation
-class StateOrientation : public StateBase
-{
- public:
-
- /** \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
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateOrientation(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
- *
- **/
- StateOrientation(WolfScalar* _st_ptr);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateOrientation();
-
- /** \brief Returns the 3x3 rotation matrix of the orientation
- *
- * Returns the 3x3 rotation matrix of the orientation
- *
- **/
- Eigen::Matrix3s getRotationMatrix() const;
- virtual void rotationMatrix(Eigen::Matrix3s& R) const = 0;
-
- /** \brief Returns the yaw angle
- *
- * Returns the yaw angle
- *
- **/
- virtual WolfScalar getYaw() const = 0;
-
- /** \brief Returns a (mapped) vector of the state unit
- *
- * Returns a (mapped) vector of the state unit
- *
- **/
- virtual Eigen::Map<const Eigen::VectorXs> getVector() const = 0;
-};
-#endif
diff --git a/src/state_point.h b/src/state_point.h
deleted file mode 100644
index b59f33f179e4bb7083ea53ffb5505c8fb79f3305..0000000000000000000000000000000000000000
--- a/src/state_point.h
+++ /dev/null
@@ -1,109 +0,0 @@
-
-#ifndef STATE_POINT_H_
-#define STATE_POINT_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "state_base.h"
-
-//class StateBase
-template <unsigned int SIZE>
-class StatePoint : public StateBase
-{
- public:
- static const unsigned int BLOCK_SIZE = SIZE;
-
- /** \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
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StatePoint(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateBase(_st_remote, _idx)
- {
- //
- }
-
- /** \brief Constructor with scalar pointer
- *
- * Constructor with scalar pointer
- * \param _st_ptr is the pointer of the first element of the state unit
- *
- **/
- StatePoint(WolfScalar* _st_ptr) :
- StateBase(_st_ptr)
- {
- //
- }
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StatePoint()
- {
- //
- }
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual StateType getStateType() const
- {
- switch (BLOCK_SIZE)
- {
- case 1:
- return ST_POINT_1D;
- case 2:
- return ST_POINT_2D;
- case 3:
- return ST_POINT_3D;
- }
- }
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const
- {
- return BLOCK_SIZE;
- }
-
- /** \brief Returns the point in a (mapped) vector
- *
- * Returns the point in a (mapped) vector
- *
- **/
- virtual Eigen::Map<const Eigen::VectorXs> getVector() const
- {
- return Eigen::Map<const Eigen::VectorXs>(state_ptr_, BLOCK_SIZE);
- }
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const
- {
- printTabs(_ntabs);
- _ost << nodeLabel() << " " << nodeId() << std::endl;
- printTabs(_ntabs);
- for (unsigned int i = 0; i < BLOCK_SIZE; i++)
- _ost << *(this->state_ptr_+i) << " ";
- _ost << std::endl;
- }
-};
-#endif
diff --git a/src/state_quaternion.cpp b/src/state_quaternion.cpp
deleted file mode 100644
index 76b0ade4b665e8114338e0cc2334849b2dafb6d9..0000000000000000000000000000000000000000
--- a/src/state_quaternion.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-#include "state_quaternion.h"
-
-StateQuaternion::StateQuaternion(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateOrientation(_st_remote, _idx)
-{
- //
-}
-
-StateQuaternion::StateQuaternion(WolfScalar* _st_ptr) :
- StateOrientation(_st_ptr)
-{
- //
-}
-
-StateQuaternion::~StateQuaternion()
-{
- //
-}
-
-StateType StateQuaternion::getStateType() const
-{
- return ST_QUATERNION;
-}
-
-unsigned int StateQuaternion::getStateSize() const
-{
- return BLOCK_SIZE;
-}
-
-// Eigen::Matrix3s StateQuaternion::getRotationMatrix() const
-// {
-// Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
-// this->getRotationMatrix(R);
-// return R;
-// }
-
-void StateQuaternion::rotationMatrix(Eigen::Matrix3s& R) const
-{
-// WolfScalar qi,qj,qk,qr;
-// qi = *state_ptr_;
-// qj = *(state_ptr_+1);
-// qk = *(state_ptr_+2);
-// qr = *(state_ptr_+3);
-//
-// R(0,0) = 1 - 2*qj*qj - 2*qk*qk;
-// R(1,0) = 2*(qi*qj + qk*qr);
-// R(2,0) = 2*(qi*qk - qj*qr);
-//
-// R(0,1) = 2*(qi*qj - qk*qr);
-// R(1,1) = 1 - 2*qi*qi - 2*qk*qk;
-// R(2,1) = 2*(qi*qr + qj*qk);
-//
-// R(0,2) = 2*(qi*qk + qj*qr);
-// R(1,2) = 2*(qj*qk - qi*qr);
-// R(2,2) = 1 - 2*qi*qi - 2*qj*qj;
-
- R = Eigen::Map<Eigen::Quaternions>(state_ptr_).toRotationMatrix();
-
- //std::cout << "StateQuaternion::getRotationMatrix()" << R << std::endl;
-}
-
-WolfScalar StateQuaternion::getYaw() const
-{
- //return atan2(2.0*(qj*qk + qr*qi), qr*qr - qi*qi - qj*qj + qk*qk);
- //return atan2(2.0*(qi*qr + qj*qk), 1 - 2 * (qk*qk + qr*qr));
- //return angles(2);
- return Eigen::Map<Eigen::Quaternions>(state_ptr_).toRotationMatrix().eulerAngles(0, 1, 2)(2);
-}
-
-
-Eigen::Map<const Eigen::VectorXs> StateQuaternion::getVector() const
-{
- return Eigen::Map<const Eigen::VectorXs>(state_ptr_, 4);
-}
-
-Eigen::Map<const Eigen::Quaternions> StateQuaternion::getQuaternion() const
-{
- return Eigen::Map<const Eigen::Quaternion<WolfScalar> >(state_ptr_);
-}
-
-void StateQuaternion::normalize()
-{
- Eigen::Map<Eigen::Quaternions> q_map(state_ptr_);
- q_map.normalize();
-}
-
-void StateQuaternion::print(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << nodeLabel() << " " << nodeId() << std::endl;
- printTabs(_ntabs);
- for (unsigned int i = 0; i < BLOCK_SIZE; i++)
- _ost << *(this->state_ptr_ + i) << " ";
- _ost << std::endl;
-}
diff --git a/src/state_quaternion.h b/src/state_quaternion.h
deleted file mode 100644
index 95972e52875727f4a36dc645415c167402b91cf8..0000000000000000000000000000000000000000
--- a/src/state_quaternion.h
+++ /dev/null
@@ -1,103 +0,0 @@
-#ifndef STATE_QUATERNION_H_
-#define STATE_QUATERNION_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "state_orientation.h"
-
-//class StateQuaternion
-//Last component is the real part.
-class StateQuaternion : public StateOrientation
-{
- public:
- static const unsigned int BLOCK_SIZE = 4;
-
- public:
-
- /** \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
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateQuaternion(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
- *
- **/
- StateQuaternion(WolfScalar* _st_ptr);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateQuaternion();
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual StateType getStateType() const;
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const;
-
- /** \brief Returns the 3x3 rotation matrix of the orientation
- *
- * Returns the 3x3 rotation matrix of the orientation
- *
- **/
- //virtual Eigen::Matrix3s getRotationMatrix() const;
- virtual void rotationMatrix(Eigen::Matrix3s& R) const;
-
- /** \brief Returns the yaw angle
- *
- * Returns the yaw angle
- *
- **/
- virtual WolfScalar getYaw() const;
-
- /** \brief Returns a (mapped) vector of the state unit
- *
- * Returns a (mapped) vector of the state unit
- *
- **/
- virtual Eigen::Map<const Eigen::VectorXs> getVector() const;
-
- /** \brief Returns a (mapped) quaternion of the state unit
- *
- * Returns a (mapped) quaternion of the state unit
- *
- **/
- virtual Eigen::Map<const Eigen::Quaternions> getQuaternion() const;
-
- /** \brief Normalizes the quaternion
- *
- * Normalizes the quaternion
- *
- **/
- void normalize();
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-#endif
diff --git a/src/state_theta.cpp b/src/state_theta.cpp
deleted file mode 100644
index 3953ca1660035ecf99198d04c6fc308e3ecc7bd1..0000000000000000000000000000000000000000
--- a/src/state_theta.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-#include "state_theta.h"
-
-StateTheta::StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateOrientation(_st_remote, _idx)
-{
- //
-}
-
-StateTheta::StateTheta(WolfScalar* _st_ptr) :
- StateOrientation(_st_ptr)
-{
- //
-}
-
-StateTheta::~StateTheta()
-{
- //
-}
-
-StateType StateTheta::getStateType() const
-{
- return ST_THETA;
-}
-
-unsigned int StateTheta::getStateSize() const
-{
- return BLOCK_SIZE;
-}
-
-// Eigen::Matrix3s StateTheta::getRotationMatrix() const
-// {
-// Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
-// this->getRotationMatrix(R);
-// return R;
-// }
-
-void StateTheta::rotationMatrix(Eigen::Matrix3s& R) const
-{
- R = Eigen::Matrix3s::Identity();
- R.block<2,2>(0,0) = Eigen::Rotation2D<WolfScalar>(*state_ptr_).matrix();
-// R(0, 0) = cos(*state_ptr_);
-// R(1, 1) = cos(*state_ptr_);
-// R(0, 1) = -sin(*state_ptr_);
-// R(1, 0) = sin(*state_ptr_);
- //std::cout << "StateTheta::getRotationMatrix()" << R << std::endl;
-}
-
-WolfScalar StateTheta::getYaw() const
-{
- return *state_ptr_;
-}
-
-Eigen::Map<const Eigen::VectorXs> StateTheta::getVector() const
-{
- return Eigen::Map<const Eigen::VectorXs>(state_ptr_, 1);
-}
-
-void StateTheta::print(unsigned int _ntabs, std::ostream& _ost) const
-{
- printTabs(_ntabs);
- _ost << nodeLabel() << " " << nodeId() << std::endl;
- printTabs(_ntabs);
- for (unsigned int i = 0; i < BLOCK_SIZE; i++)
- _ost << *(this->state_ptr_ + i) << " ";
- _ost << std::endl;
-}
diff --git a/src/state_theta.h b/src/state_theta.h
deleted file mode 100644
index cf359ca4a38ff4c000dc4494933f2bc226f1dd6d..0000000000000000000000000000000000000000
--- a/src/state_theta.h
+++ /dev/null
@@ -1,88 +0,0 @@
-#ifndef STATE_THETA_H_
-#define STATE_THETA_H_
-
-//std includes
-#include <iostream>
-#include <vector>
-#include <cmath>
-
-//Wolf includes
-#include "wolf.h"
-#include "state_orientation.h"
-
-//class StateTheta
-class StateTheta : public StateOrientation
-{
- public:
- static const unsigned int BLOCK_SIZE = 1;
-
- public:
-
- /** \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
- * \param _st_remote is the whole state vector
- * \param _idx is the index of the first element of the state in the whole state vector
- *
- **/
- StateTheta(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
- *
- **/
- StateTheta(WolfScalar* _st_ptr);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~StateTheta();
-
- /** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual StateType getStateType() const;
-
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const;
-
- /** \brief Returns the 3x3 rotation matrix of the orientation
- *
- * Returns the 3x3 rotation matrix of the orientation
- *
- **/
- //virtual Eigen::Matrix3s getRotationMatrix() const;
- void rotationMatrix(Eigen::Matrix3s& R) const;
-
- /** \brief Returns the yaw angle
- *
- * Returns the yaw angle
- *
- **/
- virtual WolfScalar getYaw() const;
-
- /** \brief Returns a (mapped) vector of the state unit
- *
- * Returns a (mapped) vector of the state unit
- *
- **/
- virtual Eigen::Map<const Eigen::VectorXs> getVector() const;
-
- /** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
-};
-#endif
diff --git a/src/wolf.h b/src/wolf.h
index a1e3cd488a5a7be16fdc3a75176a180a6826731f..d972a671451e452c759c9a728397715a3d5e40bf 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -115,9 +115,9 @@ typedef enum
CTR_GPS_FIX_2D, ///< marks a 2D GPS Fix constraint.
CTR_FIX, ///< marks a Fix constraint (for priors).
CTR_ODOM_2D_COMPLEX_ANGLE, ///< marks a 2D Odometry constraint using complex angles.
- CTR_ODOM_2D_THETA, ///< marks a 2D Odometry constraint using theta angles.
+ CTR_ODOM_2D, ///< marks a 2D Odometry constraint using theta angles.
CTR_TWIST_2D_THETA, ///< marks a 2D Twist constraint using theta angles.
- CTR_CORNER_2D_THETA, ///< marks a 2D corner constraint using theta angles.
+ CTR_CORNER_2D, ///< marks a 2D corner constraint using theta angles.
CTR_CONTAINER ///< marks a 2D container constraint using theta angles.
} ConstraintType;
@@ -131,11 +131,7 @@ typedef enum
*/
typedef enum
{
- ST_POINT_1D, ///< A 1D point. No parametrization.
- ST_POINT_2D, ///< A 2D point. No parametrization.
- ST_POINT_3D, ///< A 3D point. No parametrization.
- ST_THETA, ///< A 2D orientation represented by a single angle. No parametrization.
- ST_COMPLEX_ANGLE, ///< A 2D orientation represented by a complex number.
+ ST_VECTOR, ///< A vector. No local parametrization.
ST_QUATERNION ///< A 3D orientation represented by a quaternion.
} StateType;
@@ -224,7 +220,7 @@ class LandmarkCorner2D;
class TrajectoryBase;
class FrameBase;
class CaptureBase;
-class CaptureRelative;
+class CaptureMotion;
class CaptureLaser2D;
class FeatureBase;
class FeatureCorner2D;
@@ -275,7 +271,7 @@ typedef std::list<CaptureBase*> CaptureBaseList;
typedef CaptureBaseList::iterator CaptureBaseIter;
// - Capture Relative
-typedef std::list<CaptureRelative*> CaptureRelativeList;
+typedef std::list<CaptureMotion*> CaptureRelativeList;
typedef CaptureRelativeList::iterator CaptureRelativeIter;
// - Feature
@@ -302,13 +298,6 @@ typedef TransSensorMap::iterator TransSensorIter;
typedef std::list<StateBase*> StateBaseList;
typedef StateBaseList::iterator StateBaseIter;
-typedef StatePoint<1> StatePoint1D;
-typedef StatePoint<2> StatePoint2D;
-typedef StatePoint<3> StatePoint3D;
-typedef StatePoint<2> StateVelocity2D;
-typedef StatePoint<1> StateOmega2D;
-typedef StatePoint<0> NoState;
-
// - Pin hole
///** \brief Enumeration of all possible feature types
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 90e2933448722f190cb45e9a92c36f8d4a869a49..8478304d0dadff0ca5e0ec0c7f2c8cf7c12e6f5b 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -41,7 +41,7 @@
#include "wolf_problem.h"
#include "state_quaternion.h"
-template <class StatePositionT, class StateOrientationT, class StateVelocityT=NoState, class StateOmegaT=NoState>
+template <class StatePositionT, class StateBaseT, class StateVelocityT=NoState, class StateOmegaT=NoState>
class WolfManager
{
protected:
@@ -55,8 +55,8 @@ class WolfManager
FrameBaseIter first_window_frame_;
FrameBase* current_frame_;
FrameBase* previous_frame_;
- CaptureRelative* last_capture_relative_;
- CaptureRelative* second_last_capture_relative_;
+ CaptureMotion* last_capture_relative_;
+ CaptureMotion* second_last_capture_relative_;
std::queue<CaptureBase*> new_captures_;
//Manager parameters
@@ -98,8 +98,8 @@ class WolfManager
//////////////////////////////////////////
// IMPLEMENTATION
//////////////////////////////////////////
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::WolfManager(const unsigned int& _state_length,
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::WolfManager(const unsigned int& _state_length,
SensorBase* _sensor_prior,
const Eigen::VectorXs& _init_frame,
const Eigen::MatrixXs& _init_frame_cov,
@@ -114,9 +114,9 @@ WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::Wol
window_size_(_w_size),
new_frame_elapsed_time_(_new_frame_elapsed_time)
{
- assert( _init_frame.size() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
- _init_frame_cov.cols() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
- _init_frame_cov.rows() == StatePositionT::BLOCK_SIZE + StateOrientationT::BLOCK_SIZE &&
+ assert( _init_frame.size() == StatePositionT::BLOCK_SIZE + StateBaseT::BLOCK_SIZE &&
+ _init_frame_cov.cols() == StatePositionT::BLOCK_SIZE + StateBaseT::BLOCK_SIZE &&
+ _init_frame_cov.rows() == StatePositionT::BLOCK_SIZE + StateBaseT::BLOCK_SIZE &&
"Wrong init_frame state vector or covariance matrix size");
std::cout << "initializing wolfmanager" << std::endl;
@@ -136,18 +136,18 @@ WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::Wol
std::cout << " wolfmanager initialized" << std::endl;
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::~WolfManager()
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::~WolfManager()
{
delete problem_;
}
-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)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
{
//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");
+ assert(_frame_state.size() == StatePositionT::BLOCK_SIZE + StateBaseT::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)
@@ -170,9 +170,9 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
problem_->addState(new_position, _frame_state.head<StatePositionT::BLOCK_SIZE>());
//std::cout << "StatePosition created" << std::endl;
- StateOrientationT* new_orientation = new StateOrientationT(problem_->getNewStatePtr());
- problem_->addState(new_orientation, _frame_state.segment<StateOrientationT::BLOCK_SIZE>(new_position->getStateSize()));
- //std::cout << "StateOrientation created" << std::endl;
+ StateBaseT* new_orientation = new StateBaseT(problem_->getNewStatePtr());
+ problem_->addState(new_orientation, _frame_state.segment<StateBaseT::BLOCK_SIZE>(new_position->getStateSize()));
+ //std::cout << "StateBase created" << std::endl;
StateVelocityT* new_velocity = nullptr;
if (StateVelocityT::BLOCK_SIZE != 0)
@@ -198,7 +198,7 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
// empty last capture relative
if (previous_frame_ != nullptr)
{
- CaptureRelative* empty_odom = new CaptureOdom2D(_time_stamp, _time_stamp, sensor_prior_, Eigen::Vector3s::Zero());
+ CaptureMotion* empty_odom = new CaptureOdom2D(_time_stamp, _time_stamp, sensor_prior_, Eigen::Vector3s::Zero());
previous_frame_->addCapture(empty_odom);
empty_odom->processCapture();
last_capture_relative_ = empty_odom;
@@ -210,15 +210,15 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
//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)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::createFrame(const TimeStamp& _time_stamp)
{
//std::cout << "creating new frame from prior..." << std::endl;
createFrame(last_capture_relative_->computePrior(_time_stamp), _time_stamp);
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::addCapture(CaptureBase* _capture)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::addCapture(CaptureBase* _capture)
{
new_captures_.push(_capture);
//std::cout << "added new capture: " << _capture->nodeId() << " stamp: ";
@@ -226,8 +226,8 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
//std::cout << std::endl;
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::manageWindow()
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::manageWindow()
{
//std::cout << "managing window..." << std::endl;
// WINDOW of FRAMES (remove or fix old frames)
@@ -241,8 +241,8 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
//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)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+bool WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::checkNewFrame(CaptureBase* new_capture)
{
//std::cout << "checking if new frame..." << std::endl;
// TODO: not only time, depending on the sensor...
@@ -250,8 +250,8 @@ bool WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
return new_capture->getTimeStamp().get() - previous_frame_->getTimeStamp().get() > new_frame_elapsed_time_;
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::update()
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::update()
{
//std::cout << "updating..." << std::endl;
while (!new_captures_.empty())
@@ -276,7 +276,7 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
createFrame(new_capture->getTimeStamp());
// ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
- last_capture_relative_->integrateCapture((CaptureRelative*) (new_capture));
+ last_capture_relative_->integrateCapture((CaptureMotion*) (new_capture));
current_frame_->setState(last_capture_relative_->computePrior(new_capture->getTimeStamp()));
current_frame_->setTimeStamp(new_capture->getTimeStamp());
}
@@ -307,8 +307,8 @@ void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>
//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)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+Eigen::VectorXs WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::getVehiclePose(const TimeStamp& _now)
{
if (last_capture_relative_ == nullptr)
return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr());
@@ -316,20 +316,20 @@ Eigen::VectorXs WolfManager<StatePositionT, StateOrientationT, StateVelocityT, S
return last_capture_relative_->computePrior(_now);
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-WolfProblem* WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::getProblemPtr()
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+WolfProblem* WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::getProblemPtr()
{
return problem_;
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::setWindowSize(const unsigned int& _size)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::setWindowSize(const unsigned int& _size)
{
window_size_ = _size;
}
-template <class StatePositionT, class StateOrientationT, class StateVelocityT, class StateOmegaT>
-void WolfManager<StatePositionT, StateOrientationT, StateVelocityT, StateOmegaT>::setNewFrameElapsedTime(const WolfScalar& _dt)
+template <class StatePositionT, class StateBaseT, class StateVelocityT, class StateOmegaT>
+void WolfManager<StatePositionT, StateBaseT, StateVelocityT, StateOmegaT>::setNewFrameElapsedTime(const WolfScalar& _dt)
{
new_frame_elapsed_time_ = _dt;
}