diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f9dee8f5ac4a46b5e74281ee196113241994f730..abd78202957f9aa05b9d98c7f68ddd0938bbe0ba 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -51,9 +51,6 @@ SET(SRCS
capture_gps_fix.cpp
capture_odom_2D.cpp
correspondence_base.cpp
- correspondence_gps_2D.cpp
- correspondence_odom_2D_theta.cpp
- correspondence_odom_2D_complex_angle.cpp
feature_base.cpp
feature_gps_fix.cpp
feature_odom_2D.cpp
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index aca486e4bf550dddac238f11804cf8496c667461..de303e1b79467dd8b57757565046f8734729c665 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -32,7 +32,7 @@ CaptureBase::~CaptureBase()
//std::cout << "Destroying capture...\n";
}
-inline void CaptureBase::linkToFrame(const FrameBaseShPtr& _frm_ptr)
+void CaptureBase::linkToFrame(const FrameBaseShPtr& _frm_ptr)
{
linkToUpperNode(_frm_ptr.get());
}
@@ -43,43 +43,43 @@ void CaptureBase::addFeature(FeatureBaseShPtr & _ft_ptr)
addDownNode(_ft_ptr);
}
-inline const FrameBasePtr CaptureBase::getFramePtr() const
+const FrameBasePtr CaptureBase::getFramePtr() const
{
return upperNodePtr();
}
-// inline FeatureBaseList & CaptureBase::getFeatureList() const
+// FeatureBaseList & CaptureBase::getFeatureList() const
// {
// return downNodeList();
// }
-inline FeatureBaseList* CaptureBase::getFeatureListPtr()
+FeatureBaseList* CaptureBase::getFeatureListPtr()
{
return getDownNodeListPtr();
}
-inline TimeStamp CaptureBase::getTimeStamp() const
+TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
}
-inline SensorBasePtr CaptureBase::getSensorPtr() const
+SensorBasePtr CaptureBase::getSensorPtr() const
{
return sensor_ptr_;
}
-inline SensorType CaptureBase::getSensorType() const
+SensorType CaptureBase::getSensorType() const
{
return sensor_ptr_->getSensorType();
}
-inline void CaptureBase::setTimeStamp(const TimeStamp & _ts)
+void CaptureBase::setTimeStamp(const TimeStamp & _ts)
{
time_stamp_ = _ts;
}
-inline void CaptureBase::setTimeStampToNow()
+void CaptureBase::setTimeStampToNow()
{
time_stamp_.setToNow();
}
@@ -100,7 +100,7 @@ void CaptureBase::setDataCovariance(const Eigen::MatrixXs& _data_cov)
data_covariance_ = _data_cov;
}
-inline void CaptureBase::processCapture()
+void CaptureBase::processCapture()
{
std::cout << "... processing capture" << std::endl;
}
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index 9ded05c8f0dd83ccafd828ce35483f8779f626a1..a624cbfe70d839ad381cb966fd46555738ec0476 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -35,6 +35,11 @@ Eigen::VectorXs CaptureGPSFix::computePrior() const
return data_;
}
+void CaptureGPSFix::findCorrespondences()
+{
+
+}
+
//void CaptureGPSFix::printSelf(unsigned int _ntabs, std::ostream & _ost) const
//{
// NodeLinked::printSelf(_ntabs, _ost);
diff --git a/src/capture_gps_fix.h b/src/capture_gps_fix.h
index b236f3b860c4d8fa439905f2c82d97a0061b6160..3b4b5de5e2c4bb43fe37bdbf120d76bae4d68e2a 100644
--- a/src/capture_gps_fix.h
+++ b/src/capture_gps_fix.h
@@ -23,7 +23,9 @@ class CaptureGPSFix : public CaptureBase
virtual void processCapture();
- virtual Eigen::VectorXs computePrior() const = 0;
+ virtual Eigen::VectorXs computePrior() const;
+
+ virtual void findCorrespondences();
//virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index e9ab5354e0d34b378a28634be5bfa5926ee36175..13961d671e2de9f4796fa40b98fb5c1eec7a87a8 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -30,8 +30,9 @@ inline void CaptureOdom2D::processCapture()
addFeature(new_feature);
}
-Eigen::VectorXs CaptureOdom2D::computePrior(const FrameBaseShPtr& _previous_frame) const
+Eigen::VectorXs CaptureOdom2D::computePrior() const
{
+ FrameBasePtr _previous_frame = getFramePtr()->getPreviousFrame();
if (_previous_frame->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
{
@@ -59,6 +60,11 @@ Eigen::VectorXs CaptureOdom2D::computePrior(const FrameBaseShPtr& _previous_fram
}
}
+void CaptureOdom2D::findCorrespondences()
+{
+ //
+}
+
//void CaptureOdom2D::printSelf(unsigned int _ntabs, std::ostream & _ost) const
//{
// NodeLinked::printSelf(_ntabs, _ost);
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index 85a0ec1870b1d7a0666bd27618979e09b9350ba8..b3d3c4b9d8a1878fff6c9353a408462c4ef23ffb 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -23,7 +23,9 @@ class CaptureOdom2D : public CaptureRelative
virtual void processCapture();
- virtual Eigen::VectorXs computePrior(const FrameBaseShPtr& _previous_frame) const;
+ virtual Eigen::VectorXs computePrior() const;
+
+ virtual void findCorrespondences();
//virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
diff --git a/src/capture_relative.cpp b/src/capture_relative.cpp
index 8a1151a826a308f46fd334c641f66ecad72fdf0a..ea41a4103e046875cc3ee788903ee880ff7a3d72 100644
--- a/src/capture_relative.cpp
+++ b/src/capture_relative.cpp
@@ -17,8 +17,7 @@ CaptureRelative::CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sen
{
//
}
-
CaptureRelative::~CaptureRelative()
{
- //std::cout << "Destroying relative capture...\n";
+ //
}
diff --git a/src/capture_relative.h b/src/capture_relative.h
index 0e8d006f1b7298c1c07a040b300ef37394abeac5..7f6fed65231918d159cce060bd4960587d24f916 100644
--- a/src/capture_relative.h
+++ b/src/capture_relative.h
@@ -20,12 +20,5 @@ class CaptureRelative : public CaptureBase
CaptureRelative(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureRelative();
-
- /** \brief Get a prior from adding the relative motion capture to a previous frame
- *
- * Get a prior from adding the relative motion capture to a previous frame
- *
- **/
- virtual Eigen::VectorXs computePrior(const FrameBaseShPtr& _previous_frame) const = 0;
};
#endif
diff --git a/src/correspondence_base.cpp b/src/correspondence_base.cpp
index 599821c223a3af476783b0e9e9153df4526a6272..87345a4360b09638967ae40ebb486014e481f8ae 100644
--- a/src/correspondence_base.cpp
+++ b/src/correspondence_base.cpp
@@ -14,7 +14,7 @@ CorrespondenceBase::~CorrespondenceBase()
//
}
-inline CorrespondenceType CorrespondenceBase::getType() const
+CorrespondenceType CorrespondenceBase::getCorrespondenceType() const
{
return type_;
}
diff --git a/src/correspondence_base.h b/src/correspondence_base.h
index 69b97eeb0228ebba4ad05da4cd9471596cfc669d..b6eea58c73294599e36b7108e7e6264f0b740d8d 100644
--- a/src/correspondence_base.h
+++ b/src/correspondence_base.h
@@ -21,8 +21,8 @@ class CorrespondenceBase : public NodeLinked<FeatureBase,NodeTerminus>
{
protected:
CorrespondenceType type_; //type of correspondence (types defined at wolf.h)
- const Eigen::VectorXs * measurement_ptr_; // TBD: pointer, map or copy of the feature measurement?
- const Eigen::MatrixXs * measurement_covariance_ptr_; // TBD: pointer, map or copy of the feature measurement covariance?
+ Eigen::VectorXs * measurement_ptr_; // TBD: pointer, map or copy of the feature measurement?
+ Eigen::MatrixXs * measurement_covariance_ptr_; // TBD: pointer, map or copy of the feature measurement covariance?
public:
/** \brief Constructor
@@ -44,14 +44,14 @@ class CorrespondenceBase : public NodeLinked<FeatureBase,NodeTerminus>
* Returns the correspondence type
*
**/
- CorrespondenceType getType() const;
+ CorrespondenceType getCorrespondenceType() const;
/** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the correspondence
*
* Returns a vector of scalar pointers to the first element of all state blocks involved in the correspondence.
*
**/
- virtual const std::vector<WolfScalar*> getBlockPtrVector() = 0;
+ virtual const std::vector<WolfScalar*> getStateBlockPtrVector() = 0;
/** \brief Returns a pointer to the feature measurement
*
diff --git a/src/correspondence_gps_2D.h b/src/correspondence_gps_2D.h
index 50aafce9310280379a6b00a3095b2a6e25718133..4e3a195f7895205c18c312ec70e2649437fbc6bd 100644
--- a/src/correspondence_gps_2D.h
+++ b/src/correspondence_gps_2D.h
@@ -11,13 +11,29 @@ class CorrespondenceGPS2D: public CorrespondenceSparse<2,2>
public:
static const unsigned int N_BLOCKS = 1;
- CorrespondenceGPS2D(const FeatureBasePtr& _ftr_ptr, WolfScalar* _statePtr);
+ CorrespondenceGPS2D(const FeatureBasePtr& _ftr_ptr, WolfScalar* _statePtr) :
+ CorrespondenceSparse<2,2>(_ftr_ptr,CORR_GPS_FIX_2D, _statePtr)
+ {
+ //
+ }
- CorrespondenceGPS2D(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _statePtr);
+ CorrespondenceGPS2D(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _statePtr):
+ CorrespondenceSparse<2,2>(_ftr_ptr,CORR_GPS_FIX_2D, _statePtr->getPtr())
+ {
+ //
+ }
- virtual ~CorrespondenceGPS2D();
+ virtual ~CorrespondenceGPS2D()
+ {
+ //
+ }
template <typename T>
- bool operator()(const T* const _x, T* _residuals) const;
+ bool operator()(const T* const _x, T* _residuals) const
+ {
+ _residuals[0] = (T((*measurement_ptr_)(0)) - _x[0]) / T((*measurement_covariance_ptr_)(0,0));
+ _residuals[1] = (T((*measurement_ptr_)(1)) - _x[1]) / T((*measurement_covariance_ptr_)(1,1));
+ return true;
+ }
};
#endif
diff --git a/src/correspondence_odom_2D_complex_angle.h b/src/correspondence_odom_2D_complex_angle.h
index 6719d9a2bbba37943eafdb2c7d37da07c3921ade..a1bb15dab5f58c8b1ce73e44864c48cdf858ca65 100644
--- a/src/correspondence_odom_2D_complex_angle.h
+++ b/src/correspondence_odom_2D_complex_angle.h
@@ -11,13 +11,35 @@ class CorrespondenceOdom2DComplexAngle: public CorrespondenceSparse<2,2,2,2,2>
public:
static const unsigned int N_BLOCKS = 2;
- CorrespondenceOdom2DComplexAngle(const FeatureBasePtr& _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr);
+ CorrespondenceOdom2DComplexAngle(const FeatureBasePtr& _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
+ CorrespondenceSparse<2,2,2,2,2>(_ftr_ptr,CORR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
+ {
+ //
+ }
- CorrespondenceOdom2DComplexAngle(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr);
+ CorrespondenceOdom2DComplexAngle(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
+ CorrespondenceSparse<2,2,2,2,2>(_ftr_ptr,CORR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
+ {
+ //
+ }
- virtual ~CorrespondenceOdom2DComplexAngle();
+ virtual ~CorrespondenceOdom2DComplexAngle()
+ {
+ //
+ }
template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const;
+ bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
+ {
+ // 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 = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
+
+ // Residuals
+ _residuals[0] = (expected_range - T((*measurement_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
+ _residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
+
+ return true;
+ }
};
#endif
diff --git a/src/correspondence_odom_2D_theta.h b/src/correspondence_odom_2D_theta.h
index baf84f8ac6463e401ea364eb49d4ad68468f6671..957abb5844e8f9137f32ce894f3c553cef861a34 100644
--- a/src/correspondence_odom_2D_theta.h
+++ b/src/correspondence_odom_2D_theta.h
@@ -11,13 +11,35 @@ class CorrespondenceOdom2DTheta: public CorrespondenceSparse<2,2,1,2,1>
public:
static const unsigned int N_BLOCKS = 2;
- CorrespondenceOdom2DTheta(const FeatureBasePtr& _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr);
+ CorrespondenceOdom2DTheta(const FeatureBasePtr& _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
+ CorrespondenceSparse<2,2,1,2,1>(_ftr_ptr,CORR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
+ {
+ //
+ }
- CorrespondenceOdom2DTheta(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr);
+ CorrespondenceOdom2DTheta(const FeatureBasePtr& _ftr_ptr, const StateBaseShPtr& _state0Ptr, const StateBaseShPtr& _state1Ptr, const StateBaseShPtr& _state2Ptr, const StateBaseShPtr& _state3Ptr) :
+ CorrespondenceSparse<2,2,1,2,1>(_ftr_ptr,CORR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
+ {
+ //
+ }
- virtual ~CorrespondenceOdom2DTheta();
+ virtual ~CorrespondenceOdom2DTheta()
+ {
+ //
+ }
template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const;
+ bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
+ {
+ // 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_ptr_)(0))*T((*measurement_ptr_)(0))) / T((*measurement_covariance_ptr_)(0,0));
+ _residuals[1] = (expected_rotation - T((*measurement_ptr_)(1))) / T((*measurement_covariance_ptr_)(1,1));
+
+ return true;
+ }
};
#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index e91fc06a8a7bc12420f4b0f9770dcf57e2a1516a..319877efb874d526c6b3d439a58a92f5f4f26829 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -23,8 +23,8 @@ TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_2_sensors ${PROJECT_NAME})
# TARGET_LINK_LIBRARIES(test_ceres_wrapper_state_units ${PROJECT_NAME})
# jet test manager
-# ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
+ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
+TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
ADD_EXECUTABLE(test_ceres_odom_batch test_ceres_odom_batch.cpp)
@@ -39,5 +39,5 @@ TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
# test ceres manager, wolf manager and wolf tree
-# # ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME})
\ No newline at end of file
+ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp)
+TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME})
\ No newline at end of file
diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp
index 73793787abe2085a0b86e5cd21e5cdeae797a42a..8cdb8f6591ecd5769db02a8a3ef78414d5c2718f 100644
--- a/src/examples/test_ceres_manager.cpp
+++ b/src/examples/test_ceres_manager.cpp
@@ -41,45 +41,6 @@ class CorrespondenceXBase;
typedef std::shared_ptr<CorrespondenceXBase> CorrespondenceXShPtr;
typedef std::shared_ptr<CaptureXBase> CaptureXShPtr;
-//class ComplexAngleParameterization : public ceres::LocalParameterization
-//{
-// public:
-// virtual ~ComplexAngleParameterization()
-// {
-// }
-//
-// virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
-// {
-// x_plus_delta_raw[0] = x_raw[0] * cos(delta_raw[0]) - x_raw[1] * sin(delta_raw[0]);
-// x_plus_delta_raw[1] = x_raw[1] * cos(delta_raw[0]) + x_raw[0] * sin(delta_raw[0]);
-//
-// //normalize
-// //double norm = sqrt(x_plus_delta_raw[0] * x_plus_delta_raw[0] + x_plus_delta_raw[1] * x_plus_delta_raw[1]);
-// //std::cout << "(before normalization) norm = " << norm << std::endl;
-// //x_plus_delta_raw[0] /= norm;
-// //x_plus_delta_raw[1] /= norm;
-//
-// return true;
-// }
-//
-// virtual bool ComputeJacobian(const double* x, double* jacobian) const
-// {
-// jacobian[0] = -x[1];
-// jacobian[1] = x[0];
-// return true;
-// }
-//
-// virtual int GlobalSize() const
-// {
-// return 2;
-// }
-//
-// virtual int LocalSize() const
-// {
-// return 1;
-// }
-//};
-
class CorrespondenceXBase
{
protected:
@@ -749,8 +710,8 @@ int main(int argc, char** argv)
Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings
std::queue<StateBaseShPtr> new_state_units; // new state units in wolf that must be added to ceres
std::queue<CorrespondenceXShPtr> new_correspondences; // new correspondences in wolf that must be added to ceres
- SensorBaseShPtr odom_sensor = SensorBaseShPtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1)));
- SensorBaseShPtr gps_sensor = SensorBaseShPtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1)));
+ SensorBaseShPtr odom_sensor = SensorBaseShPtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1),0));
+ SensorBaseShPtr gps_sensor = SensorBaseShPtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0));
// Initial pose
pose_true << 0,0,0;
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
index ece8175ea514489ac987f4d8348436c14257c5a8..a544cccff72f33403e8e97ea546480caf9863d9e 100644
--- a/src/examples/test_ceres_manager_tree.cpp
+++ b/src/examples/test_ceres_manager_tree.cpp
@@ -18,20 +18,23 @@
#include "glog/logging.h"
//Wolf includes
+#include "wolf.h"
#include "sensor_base.h"
#include "frame_base.h"
#include "state_point.h"
#include "state_complex_angle.h"
#include "capture_base.h"
#include "capture_relative.h"
+#include "capture_odom_2D.h"
+#include "capture_gps_fix.h"
#include "state_base.h"
-#include "wolf.h"
+#include "correspondence_sparse.h"
#include "correspondence_gps_2D.h"
#include "correspondence_odom_2D_theta.h"
#include "correspondence_odom_2D_complex_angle.h"
// ceres wrapper includes
-//#include "ceres_wrapper/complex_angle_parametrization.h"
+#include "ceres_wrapper/complex_angle_parametrization.h"
/**
* This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
@@ -40,45 +43,6 @@
using namespace Eigen;
-class ComplexAngleParameterization : public ceres::LocalParameterization
-{
- public:
- virtual ~ComplexAngleParameterization()
- {
- }
-
- virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
- {
- x_plus_delta_raw[0] = x_raw[0] * cos(delta_raw[0]) - x_raw[1] * sin(delta_raw[0]);
- x_plus_delta_raw[1] = x_raw[1] * cos(delta_raw[0]) + x_raw[0] * sin(delta_raw[0]);
-
- //normalize
- //double norm = sqrt(x_plus_delta_raw[0] * x_plus_delta_raw[0] + x_plus_delta_raw[1] * x_plus_delta_raw[1]);
- //std::cout << "(before normalization) norm = " << norm << std::endl;
- //x_plus_delta_raw[0] /= norm;
- //x_plus_delta_raw[1] /= norm;
-
- return true;
- }
-
- virtual bool ComputeJacobian(const double* x, double* jacobian) const
- {
- jacobian[0] = -x[1];
- jacobian[1] = x[0];
- return true;
- }
-
- virtual int GlobalSize() const
- {
- return 2;
- }
-
- virtual int LocalSize() const
- {
- return 1;
- }
-};
-
class WolfManager
{
protected:
@@ -104,6 +68,8 @@ class WolfManager
else
init_frame << 0, 0, 0;
createFrame(init_frame, 0);
+
+ std::cout << "first frame created\n";
}
virtual ~WolfManager()
@@ -127,19 +93,23 @@ class WolfManager
// Store in state_
state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state;
- // Create frame
+ // Create frame and add it to the trajectory
if (use_complex_angles_)
- new FrameBase(trajectory_,
- _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2)));
-
+ {
+ FrameBaseShPtr new_frame(new FrameBase(trajectory_,
+ _time_stamp,
+ StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
+ StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2))));
+ trajectory_->addFrame(new_frame);
+ }
else
- new FrameBase(trajectory_,
- _time_stamp,
- StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
- StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2)));
-
+ {
+ FrameBaseShPtr new_frame(new FrameBase(trajectory_,
+ _time_stamp,
+ StateBaseShPtr(new StatePoint2D(state_.data()+first_empty_state_)),
+ StateBaseShPtr(new StateTheta(state_.data()+first_empty_state_+2))));
+ trajectory_->addFrame(new_frame);
+ }
// Update first free state location index
first_empty_state_ += use_complex_angles_ ? 4 : 3;
}
@@ -167,20 +137,20 @@ class WolfManager
// TODO: Change by something like...
//new_state_units.insert(new_state_units.end(), trajectory_.getFrameList.back()->getStateList().begin(), trajectory_.getFrameList.back()->getStateList().end());
- new_state_units.push_back(trajectory_->getFrameList().back()->getPPtr().get());
- new_state_units.push_back(trajectory_->getFrameList().back()->getOPtr().get());
+ new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr().get());
+ new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr().get());
}
// COMPUTE CAPTURE (features, correspondences)
new_capture->processCapture();
// LINK CAPTURE TO ITS FRAME
- new_capture->linkToFrame(trajectory_->getFrameList().back());
+ new_capture->linkToFrame(trajectory_->getFrameListPtr()->back());
// ADD CORRESPONDENCES TO THE new_correspondences OUTPUT PARAM
- for (FeatureBaseIter feature_list_iter=new_capture->getFeatureList().begin(); feature_list_iter!=new_capture->getFeatureList().end(); feature_list_iter++)
+ for (FeatureBaseIter feature_list_iter=new_capture->getFeatureListPtr()->begin(); feature_list_iter!=new_capture->getFeatureListPtr()->end(); feature_list_iter++)
{
- for (CorrespondenceBaseIter correspondence_list_iter=(*feature_list_iter)->getCorrespondenceList().begin(); correspondence_list_iter!=(*feature_list_iter)->getCorrespondenceList().end(); correspondence_list_iter++)
+ for (CorrespondenceBaseIter correspondence_list_iter=(*feature_list_iter)->getCorrespondenceListPtr()->begin(); correspondence_list_iter!=(*feature_list_iter)->getCorrespondenceListPtr()->end(); correspondence_list_iter++)
{
new_correspondences.push_back((*correspondence_list_iter).get());
}
@@ -195,9 +165,12 @@ class WolfManager
std::list<StateBasePtr> getStateList()
{
+
+ std::cout << "getStateList...\n";
+
std::list<StateBasePtr> st_list;
- for (FrameBaseIter frame_list_iter=trajectory_->getFrameList().begin(); frame_list_iter!=trajectory_->getFrameList().end(); frame_list_iter++)
+ for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
{
//st_list.insert(st_list.end(), (*frame_list_iter)->getStateList().begin(), (*frame_list_iter)->getStateList().end());
st_list.push_back((*frame_list_iter)->getPPtr().get());
@@ -211,13 +184,13 @@ class WolfManager
{
std::list<CorrespondenceBaseShPtr> corr_list;
- for (FrameBaseIter frame_list_iter=trajectory_->getFrameList().begin(); frame_list_iter!=trajectory_->getFrameList().end(); frame_list_iter++)
+ for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
{
- for (CaptureBaseIter capture_list_iter=(*frame_list_iter)->getCaptureList().begin(); capture_list_iter!=(*frame_list_iter)->getCaptureList().end(); capture_list_iter++)
+ for (CaptureBaseIter capture_list_iter=(*frame_list_iter)->getCaptureListPtr()->begin(); capture_list_iter!=(*frame_list_iter)->getCaptureListPtr()->end(); capture_list_iter++)
{
- for (FeatureBaseIter feature_list_iter=(*capture_list_iter)->getFeatureList().begin(); feature_list_iter!=(*capture_list_iter)->getFeatureList().end(); feature_list_iter++)
+ for (FeatureBaseIter feature_list_iter=(*capture_list_iter)->getFeatureListPtr()->begin(); feature_list_iter!=(*capture_list_iter)->getFeatureListPtr()->end(); feature_list_iter++)
{
- corr_list.insert(corr_list.end(),(*feature_list_iter)->getCorrespondenceList().begin(), (*feature_list_iter)->getCorrespondenceList().end());
+ corr_list.insert(corr_list.end(),(*feature_list_iter)->getCorrespondenceListPtr()->begin(), (*feature_list_iter)->getCorrespondenceListPtr()->end());
}
}
}
@@ -263,13 +236,13 @@ class CeresManager
return ceres_summary_;
}
- void addCorrespondences(std::queue<CorrespondenceBasePtr>& _new_correspondences)
+ void addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences)
{
//std::cout << _new_correspondences.size() << " new correspondences\n";
while (!_new_correspondences.empty())
{
addCorrespondence(_new_correspondences.front());
- _new_correspondences.pop();
+ _new_correspondences.pop_front();
}
}
@@ -285,7 +258,7 @@ class CeresManager
void addCorrespondence(const CorrespondenceBasePtr& _corr_ptr)
{
- ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getBlockPtrVector());
+ ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>(blockIdx,_corr_ptr));
}
@@ -350,7 +323,7 @@ class CeresManager
ceres::CostFunction* createCostFunction(const CorrespondenceBasePtr& _corrPtr)
{
- switch (_corrPtr->getType())
+ switch (_corrPtr->getCorrespondenceType())
{
case CORR_GPS_FIX_2D:
{
@@ -466,8 +439,8 @@ int main(int argc, char** argv)
std::list<CorrespondenceBasePtr> new_correspondences; // new correspondences in wolf that must be added to ceres
// Wolf manager initialization
- SensorBasePtr odom_sensor = SensorBasePtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1)));
- SensorBasePtr gps_sensor = SensorBasePtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1)));
+ SensorBasePtr odom_sensor = SensorBasePtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1),0));
+ SensorBasePtr gps_sensor = SensorBasePtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0));
WolfManager* wolf_manager = new WolfManager(odom_sensor, n_execution * (complex_angle ? 4 : 3), complex_angle);
// Initial pose
@@ -510,18 +483,20 @@ int main(int argc, char** argv)
new_state_units = wolf_manager->getStateList(); // First pose to be added in ceres
for (uint step=1; step < n_execution; step++)
{
- // adding sensor captures
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureOdom2D(odom_readings.segment(step*2,2), TimeStamp(step_time), odom_sensor)));
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureGPSFix(gps_fix_readings.segment(step*3,3), TimeStamp(step_time), gps_sensor)));
-
+ std::cout << "adding new sensor captures...\n";
+ // adding new sensor captures
+ wolf_manager->addCapture(CaptureBaseShPtr(new CaptureOdom2D(TimeStamp(step*0.01), odom_sensor, odom_readings.segment(step*2,2))));
+ wolf_manager->addCapture(CaptureBaseShPtr(new CaptureGPSFix(TimeStamp(step*0.01), gps_sensor, gps_fix_readings.segment(step*3,3))));
+ std::cout << "updating problem...\n";
// updating problem
wolf_manager->update(new_state_units, new_correspondences);
-
+ std::cout << "sadding new state units and correspondences to ceres...\n";
// adding new state units and correspondences to ceres
ceres_manager->addStateUnits(new_state_units);
ceres_manager->addCorrespondences(new_correspondences);
}
+ std::cout << "solving...\n";
// SOLVE OPTIMIZATION ============================================================================================
ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
t2=clock();
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 69ea8bd7a308c41b62e3e3f6a8de7cc4d7f73bfe..cd3380d6b7bda878de748a2cd09b8aea132693ef 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -57,12 +57,12 @@ CorrespondenceBaseList* FeatureBase::getCorrespondenceListPtr()
return getDownNodeListPtr();
}
-const Eigen::VectorXs * FeatureBase::getMeasurementPtr()
+Eigen::VectorXs * FeatureBase::getMeasurementPtr()
{
return &measurement_;
}
-const Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr()
+Eigen::MatrixXs * FeatureBase::getMeasurementCovariancePtr()
{
return &measurement_covariance_;
}
diff --git a/src/feature_base.h b/src/feature_base.h
index c062e05b8b7a38d39a5175efc964402db0a31d2f..9dd15aecfebeec77b20dfc416cc5c35fd1cf5fa8 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -60,9 +60,9 @@ class FeatureBase : public NodeLinked<CaptureBase,CorrespondenceBase>
CorrespondenceBaseList* getCorrespondenceListPtr();
- const Eigen::VectorXs * getMeasurementPtr();
+ Eigen::VectorXs * getMeasurementPtr();
- const Eigen::MatrixXs * getMeasurementCovariancePtr();
+ Eigen::MatrixXs * getMeasurementCovariancePtr();
void setMeasurement(const Eigen::VectorXs & _meas);
diff --git a/src/feature_gps_fix.cpp b/src/feature_gps_fix.cpp
index 0a9181337e4d9a93eb220be51d8a47d8f0d561cd..0efd143734b3fd6eb85157d8f04599c065f9359a 100644
--- a/src/feature_gps_fix.cpp
+++ b/src/feature_gps_fix.cpp
@@ -25,5 +25,6 @@ FeatureGPSFix::~FeatureGPSFix()
void FeatureGPSFix::findCorrespondences()
{
- //addCorrespondence(CorrespondenceBaseShPtr(new CorrespondenceGPSFix(upperNode()->upperNode()->getPPtr())));
+ CorrespondenceBaseShPtr gps_correspondence(new CorrespondenceGPS2D(this, getCapturePtr()->getFramePtr()->getPPtr()));
+ addCorrespondence(gps_correspondence);
}
diff --git a/src/feature_gps_fix.h b/src/feature_gps_fix.h
index 4f5bd1da7de9c53ce273a204abd1f4f8b8ae6d2c..5263d4a4f1b6255a8197a3b28ae2836b93dd8245 100644
--- a/src/feature_gps_fix.h
+++ b/src/feature_gps_fix.h
@@ -5,6 +5,7 @@
//Wolf includes
#include "feature_base.h"
+#include "correspondence_gps_2D.h"
//class FeatureGPSFix
class FeatureGPSFix : public FeatureBase
diff --git a/src/feature_odom_2D.cpp b/src/feature_odom_2D.cpp
index 471bd9ae4145f2266d256c87b89852ce0461eec9..6bf05e7788388f2cb172e6a7fe8c9945c3ef3ef3 100644
--- a/src/feature_odom_2D.cpp
+++ b/src/feature_odom_2D.cpp
@@ -25,5 +25,22 @@ FeatureOdom2D::~FeatureOdom2D()
void FeatureOdom2D::findCorrespondences()
{
- //TODO: find previous frame
+ if (getCapturePtr()->getFramePtr()->getOPtr()->getStateType() == ST_THETA)
+ {
+ CorrespondenceBaseShPtr odom_correspondence(new CorrespondenceOdom2DTheta(this,
+ getCapturePtr()->getFramePtr()->getPreviousFrame()->getPPtr(),
+ getCapturePtr()->getFramePtr()->getPreviousFrame()->getOPtr(),
+ getCapturePtr()->getFramePtr()->getPPtr(),
+ getCapturePtr()->getFramePtr()->getOPtr()));
+ addCorrespondence(odom_correspondence);
+ }
+ else
+ {
+ CorrespondenceBaseShPtr odom_correspondence(new CorrespondenceOdom2DComplexAngle(this,
+ getCapturePtr()->getFramePtr()->getPreviousFrame()->getPPtr(),
+ getCapturePtr()->getFramePtr()->getPreviousFrame()->getOPtr(),
+ getCapturePtr()->getFramePtr()->getPPtr(),
+ getCapturePtr()->getFramePtr()->getOPtr()));
+ addCorrespondence(odom_correspondence);
+ }
}
diff --git a/src/feature_odom_2D.h b/src/feature_odom_2D.h
index 6ed8079855fcd57a1b55571f56481b5fc63f1353..d6d66321b5195603a0c4faf41ea9bc6e55ee02ce 100644
--- a/src/feature_odom_2D.h
+++ b/src/feature_odom_2D.h
@@ -5,6 +5,8 @@
//Wolf includes
#include "feature_base.h"
+#include "correspondence_odom_2D_theta.h"
+#include "correspondence_odom_2D_complex_angle.h"
//class FeatureOdom2D
class FeatureOdom2D : public FeatureBase
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index e6810e2535eefa5004ea691aa84da8d026daeef8..039f31fa3f560e68646e3e38f7cc67a13132eb56 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -10,7 +10,7 @@ FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const TimeStamp& _ts,
v_ptr_(_v_ptr),
w_ptr_(_w_ptr)
{
- //
+ //
}
FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const FrameType & _tp, const TimeStamp& _ts, const StateBaseShPtr& _p_ptr, const StateBaseShPtr& _o_ptr, const StateBaseShPtr& _v_ptr, const StateBaseShPtr& _w_ptr) :
diff --git a/src/frame_base.h b/src/frame_base.h
index 70727b6943c07e065e6280fd7116bae631379621..9360ab35dff0b9726961e211a1056c31ca1476df 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -91,7 +91,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
CaptureBaseList* getCaptureListPtr();
- FrameBase* getPreviousFrame() const;
+ FrameBasePtr getPreviousFrame() const;
//const Eigen::Vector3s & state() const;
diff --git a/src/node_linked.h b/src/node_linked.h
index 21527113c963a1c82978cee7ba8ab97303d0d5ed..e601f523b3711d4b45d467909016b8c57e25ae5a 100644
--- a/src/node_linked.h
+++ b/src/node_linked.h
@@ -35,11 +35,11 @@ class NodeLinked : public NodeBase
{
public:
typedef UpperType* UpperNodePtr;
+ typedef LowerType* LowerNodePtr; // JVN: era protected
+ typedef std::shared_ptr<LowerType> LowerNodeShPtr; // JVN: era protected
protected:
- typedef LowerType* LowerNodePtr;
//typedef std::shared_ptr<UpperType> UpperNodeShPtr;
- typedef std::shared_ptr<LowerType> LowerNodeShPtr;
typedef std::list<LowerNodeShPtr> LowerNodeList;
typedef typename LowerNodeList::iterator LowerNodeIter;
@@ -262,7 +262,9 @@ inline void NodeLinked<UpperType, LowerType>::linkToUpperNode(UpperNodePtr _pptr
if (isTop())
up_node_ptr_ = nullptr;
else
+ {
up_node_ptr_ = _pptr;
+ }
}
template<class UpperType, class LowerType>
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 63d0141eef96e831a4b68d596ca431d0ffa9e2e5..0b81337c754053975c378f2c850ff86458e97b09 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -11,6 +11,11 @@ TrajectoryBase::~TrajectoryBase()
//
}
+void TrajectoryBase::addFrame(FrameBaseShPtr& _frame_ptr)
+{
+ addDownNode(_frame_ptr);
+}
+
FrameBaseList* TrajectoryBase::getFrameListPtr()
{
return getDownNodeListPtr();
diff --git a/src/trajectory_base.h b/src/trajectory_base.h
index 44ba4cdb0d6a9f9fdd02d1699489fc811bc94e71..e803be9bf6665ad798f6052bf0fe5b29f07cebc0 100644
--- a/src/trajectory_base.h
+++ b/src/trajectory_base.h
@@ -41,6 +41,13 @@ class TrajectoryBase : public NodeLinked<NodeTerminus,FrameBase>
**/
~TrajectoryBase();
+ /** \brief Add a frame to the trajectory
+ *
+ * Add a frame to the trajectory
+ *
+ **/
+ void addFrame(FrameBaseShPtr& _frame_ptr);
+
/** \brief Returns a pointer to Frame list
*
* Returns a pointer to Frame list