diff --git a/src/IMU_tools.h b/src/IMU_tools.h
index ebdf64df1a37dfcc945996c93368657abf11dd47..4f6ecda67b6695723b11ddecc56403da57a931a0 100644
--- a/src/IMU_tools.h
+++ b/src/IMU_tools.h
@@ -67,7 +67,7 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q, MatrixBase<D3>& v)
v << T3(0), T3(0), T3(0);
}
-template<typename T = wolf::Scalar>
+template<typename T = Scalar>
inline Matrix<T, 10, 1> identity()
{
Matrix<T, 10, 1> ret;
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index b4271a1a09bc6b4085acba5995706c7826fdcdf8..b0d1f0982da56ec28b0bced2de743310bfc6f2bd 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -117,6 +117,19 @@ void CaptureBase::addFeatureList(FeatureBaseList& _new_ft_list)
feature_list_.splice(feature_list_.end(), _new_ft_list);
}
+void CaptureBase::getConstraintList(ConstraintBaseList& _ctr_list)
+{
+ for (auto f_ptr : getFeatureList())
+ f_ptr->getConstraintList(_ctr_list);
+}
+
+ConstraintBasePtr CaptureBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
+{
+ constrained_by_list_.push_back(_ctr_ptr);
+ _ctr_ptr->setCaptureOtherPtr(shared_from_this());
+ return _ctr_ptr;
+}
+
StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const
{
if (getSensorPtr())
@@ -264,7 +277,7 @@ void CaptureBase::registerNewStateBlocks()
}
}
-wolf::Size CaptureBase::computeCalibSize() const
+Size CaptureBase::computeCalibSize() const
{
Size sz = 0;
for (Size i = 0; i < state_block_vec_.size(); i++)
diff --git a/src/capture_base.h b/src/capture_base.h
index e7626e1d5b0f7fbb12a45912afcb25e696e5f464..48d3f64e690d22217222f8c4eb67aac1802a287c 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -104,7 +104,15 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void updateCalibSize();
};
-inline wolf::Size CaptureBase::getCalibSize() const
+}
+
+#include "sensor_base.h"
+#include "frame_base.h"
+#include "feature_base.h"
+
+namespace wolf{
+
+inline Size CaptureBase::getCalibSize() const
{
return calib_size_;
}
@@ -114,14 +122,6 @@ inline void CaptureBase::updateCalibSize()
calib_size_ = computeCalibSize();
}
-}
-
-#include "sensor_base.h"
-#include "frame_base.h"
-#include "feature_base.h"
-
-namespace wolf{
-
inline const std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec() const
{
return state_block_vec_;
@@ -174,19 +174,6 @@ inline FeatureBaseList& CaptureBase::getFeatureList()
return feature_list_;
}
-inline void CaptureBase::getConstraintList(ConstraintBaseList& _ctr_list)
-{
- for (auto f_ptr : getFeatureList())
- f_ptr->getConstraintList(_ctr_list);
-}
-
-inline ConstraintBasePtr CaptureBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
-{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setCaptureOtherPtr( shared_from_this() );
- return _ctr_ptr;
-}
-
inline unsigned int CaptureBase::getHits() const
{
return constrained_by_list_.size();
diff --git a/src/capture_motion.h b/src/capture_motion.h
index 65b7fd4f33553ab89492ba1d8429fc585f1c62a4..6cc4f1732280192431e9e37b9b485fca82e30498 100644
--- a/src/capture_motion.h
+++ b/src/capture_motion.h
@@ -126,12 +126,12 @@ inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXs& _data_cov)
data_cov_ = _data_cov;
}
-inline const wolf::MotionBuffer& CaptureMotion::getBuffer() const
+inline const MotionBuffer& CaptureMotion::getBuffer() const
{
return buffer_;
}
-inline wolf::MotionBuffer& CaptureMotion::getBuffer()
+inline MotionBuffer& CaptureMotion::getBuffer()
{
return buffer_;
}
@@ -152,7 +152,7 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const
return _delta + _delta_error;
}
-inline wolf::FrameBasePtr CaptureMotion::getOriginFramePtr()
+inline FrameBasePtr CaptureMotion::getOriginFramePtr()
{
return origin_frame_ptr_;
}
diff --git a/src/captures/capture_wheel_joint_position.h b/src/captures/capture_wheel_joint_position.h
index 3db21c9c98df85fe9c618158591f45e044900d8d..02829cd1a89414652cd7f1cff05b768d51ee252f 100644
--- a/src/captures/capture_wheel_joint_position.h
+++ b/src/captures/capture_wheel_joint_position.h
@@ -176,7 +176,7 @@ protected:
// //SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
//};
-//using CaptureDiffDriveWheelJointPosition = wolf::CaptureWheelJointPosition<wolf::DiffDriveController>;
+//using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>;
} // namespace wolf
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index 5b495d52fb23056c14cf308f8ceb8d69eade01f9..2a5d8efec9aacf1fd3276a0ea749b079577e2d17 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -84,7 +84,7 @@ void SolverManager::update()
assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
}
-wolf::ProblemPtr SolverManager::getProblemPtr()
+ProblemPtr SolverManager::getProblemPtr()
{
return wolf_problem_;
}
diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index b73113cd8f791ee360d4a017285ba61a9f460ec5..73c84a9570db2bf4a7b2a5fe7e0742801f05006c 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -183,7 +183,7 @@ inline bool ConstraintAHP::operator ()(const T* const _current_frame_p,
return true;
}
-inline wolf::ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr,
+inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr,
const LandmarkAHPPtr& _lmk_ahp_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
diff --git a/src/constraint_GPS_pseudorange_3D.h b/src/constraint_GPS_pseudorange_3D.h
index ef9bc5ef09616410282f53b6f0eeb35a38476af8..1de907155e5fbb44cb14d1fa6afa06081d308820 100644
--- a/src/constraint_GPS_pseudorange_3D.h
+++ b/src/constraint_GPS_pseudorange_3D.h
@@ -60,7 +60,7 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+ static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
NodeBasePtr _correspondant_ptr = nullptr)
{
return std::make_shared<ConstraintGPSPseudorange3D>(_feature_ptr);
diff --git a/src/constraint_IMU.h b/src/constraint_IMU.h
index a47992f9e500655bbb571ba1fd67b5b59cb50817..9ccf54eebc59b1ef1f33ab1514fed028cafce056 100644
--- a/src/constraint_IMU.h
+++ b/src/constraint_IMU.h
@@ -57,7 +57,7 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3
* Vector3s _p2 : position in current frame
* Vector4s _q2 : orientation quaternion in current frame
* Vector3s _v2 : velocity in current frame
- * Matrix<9,1, wolf::Scalar> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
+ * Matrix<9,1, Scalar> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
*/
template<typename D1, typename D2, typename D3>
bool residual(const Eigen::MatrixBase<D1> & _p1,
@@ -120,8 +120,8 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3
Eigen::Matrix3s dDq_dwb_;
/// Metrics
- const wolf::Scalar dt_; ///< delta-time and delta-time-squared between keyframes
- const wolf::Scalar ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
+ const Scalar dt_; ///< delta-time and delta-time-squared between keyframes
+ const Scalar ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
/* bias covariance and bias residuals
*
@@ -420,10 +420,10 @@ inline Eigen::VectorXs ConstraintIMU::expectation() const
const Eigen::Vector3s frame_previous_vel = (frm_previous->getVPtr()->getState());
// Define results vector and Map bits over it
- Eigen::Matrix<wolf::Scalar, 10, 1> exp;
- Eigen::Map<Eigen::Matrix<wolf::Scalar, 3, 1> > pe(exp.data() );
- Eigen::Map<Eigen::Quaternion<wolf::Scalar> > qe(exp.data() + 3);
- Eigen::Map<Eigen::Matrix<wolf::Scalar, 3, 1> > ve(exp.data() + 7);
+ Eigen::Matrix<Scalar, 10, 1> exp;
+ Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > pe(exp.data() );
+ Eigen::Map<Eigen::Quaternion<Scalar> > qe(exp.data() + 3);
+ Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > ve(exp.data() + 7);
expectation(frame_previous_pos, frame_previous_ori, frame_previous_vel,
frame_current_pos, frame_current_ori, frame_current_vel,
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index fd0f08899f59171ab0073713408b991a54e065e4..ca54791bb95766f72ab7d9e67eb7d717cb8dd081 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -133,4 +133,19 @@ void ConstraintBase::setStatus(ConstraintStatus _status)
status_ = _status;
}
+void ConstraintBase::setApplyLossFunction(const bool _apply)
+{
+ if (apply_loss_function_ != _apply)
+ {
+ if (getProblem() == nullptr)
+ std::cout << "constraint not linked with Problem, apply loss function change not notified" << std::endl;
+ else
+ {
+ ConstraintBasePtr this_c = shared_from_this();
+ getProblem()->removeConstraintPtr(this_c);
+ getProblem()->addConstraintPtr(this_c);
+ }
+ }
+}
+
} // namespace wolf
diff --git a/src/constraint_base.h b/src/constraint_base.h
index bb33d208134a7241f1378f1630f4a3c5998fd62c..c5b2a756480a8f2a75ac3c55fcb49971d3a4ae9b 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -14,6 +14,26 @@ class FeatureBase;
namespace wolf {
+/** \brief Enumeration of constraint status
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ */
+typedef enum
+{
+ CTR_INACTIVE = 0, ///< Constraint established with a frame (odometry).
+ CTR_ACTIVE = 1 ///< Constraint established with absolute reference.
+} ConstraintStatus;
+
+/** \brief Enumeration of jacobian computation method
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ */
+typedef enum
+{
+ JAC_AUTO = 1, ///< Auto differentiation (AutoDiffCostFunctionWrapper or ceres::NumericDiffCostFunction).
+ JAC_NUMERIC, ///< Numeric differentiation (ceres::NumericDiffCostFunction).
+ JAC_ANALYTIC ///< Analytic jacobians.
+} JacobianMethod;
//class ConstraintBase
class ConstraintBase : public NodeBase, public std::enable_shared_from_this<ConstraintBase>
@@ -156,11 +176,11 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
*/
void setProcessor(const ProcessorBasePtr& _processor_ptr);
- protected:
- template<typename D>
- void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
- template<int R, int C>
- void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar
+// protected:
+// template<typename D>
+// void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
+// template<int R, int C>
+// void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar
};
@@ -176,24 +196,24 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
namespace wolf{
-template<typename D>
-inline void ConstraintBase::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {} // Do nothing if input Scalar type is ceres::Jet
-template<int R, int C>
-inline void ConstraintBase::print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const // Normal print if Scalar type is wolf::Scalar
-{
- if (mat.cols() == 1)
- {
- WOLF_TRACE(name, ": ", mat.transpose());
- }
- else if (mat.rows() == 1)
- {
- WOLF_TRACE(name, ": ", mat);
- }
- else
- {
- WOLF_TRACE(name, ":\n", mat);
- }
-}
+//template<typename D>
+//inline void ConstraintBase::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {} // Do nothing if input Scalar type is ceres::Jet
+//template<int R, int C>
+//inline void ConstraintBase::print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const // Normal print if Scalar type is wolf::Scalar
+//{
+// if (mat.cols() == 1)
+// {
+// WOLF_TRACE(name, ": ", mat.transpose());
+// }
+// else if (mat.rows() == 1)
+// {
+// WOLF_TRACE(name, ": ", mat);
+// }
+// else
+// {
+// WOLF_TRACE(name, ":\n", mat);
+// }
+//}
inline unsigned int ConstraintBase::id() const
{
@@ -215,21 +235,6 @@ inline bool ConstraintBase::getApplyLossFunction()
return apply_loss_function_;
}
-inline void ConstraintBase::setApplyLossFunction(const bool _apply)
-{
- if (apply_loss_function_ != _apply)
- {
- if (getProblem() == nullptr)
- std::cout << "constraint not linked with Problem, apply loss function change not notified" << std::endl;
- else
- {
- ConstraintBasePtr this_c = shared_from_this();
- getProblem()->removeConstraintPtr(this_c);
- getProblem()->addConstraintPtr(this_c);
- }
- }
-}
-
inline ProcessorBasePtr ConstraintBase::getProcessor() const
{
return processor_ptr_.lock();
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 0f048c7ac841979456f330caa878424581f640ef..2d7d2a14214be26a6ee19fd858c86a72ea269a6e 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -49,7 +49,7 @@ class ConstraintEpipolar : public ConstraintBase
virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
public:
- static wolf::ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
+ static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr);
@@ -63,7 +63,7 @@ inline ConstraintEpipolar::ConstraintEpipolar(const FeatureBasePtr& /*_feature_p
//
}
-inline wolf::ConstraintBasePtr ConstraintEpipolar::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+inline ConstraintBasePtr ConstraintEpipolar::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr)
{
return std::make_shared<ConstraintEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 401c5d4d8c75beb9431058f176952c191ca4d20f..11333302fc5f3fa12a01354e5f6b08cc60fc9065 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -38,7 +38,7 @@ class ConstraintOdom2D : public ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2,
T* _residuals) const;
public:
- static wolf::ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+ static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
{
return std::make_shared<ConstraintOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
}
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index bfd6aaf387670dc2aa10a1b8526a313b216d628c..d69655e2714928729b610ba8162075db642c68b3 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -95,7 +95,7 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
public:
- static wolf::ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
+ static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr)
{
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index b5fce1bf8f3ca5403272c87143f160cdd2a5b0b3..a3d4b7af0ae3419d0a198591750aa174aeb26769 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -101,7 +101,7 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
}
template<typename T>
-inline bool wolf::ConstraintOdom3D::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
+inline bool ConstraintOdom3D::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
@@ -127,7 +127,7 @@ inline bool wolf::ConstraintOdom3D::expectation(const T* const _p_current, const
return true;
}
-inline Eigen::VectorXs wolf::ConstraintOdom3D::expectation() const
+inline Eigen::VectorXs ConstraintOdom3D::expectation() const
{
Eigen::VectorXs exp(7);
FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
@@ -150,7 +150,7 @@ inline Eigen::VectorXs wolf::ConstraintOdom3D::expectation() const
}
template<typename T>
-inline bool wolf::ConstraintOdom3D::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
+inline bool ConstraintOdom3D::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
const T* const _q_past, T* _residuals) const
{
diff --git a/src/factory.h b/src/factory.h
index 00d2263c2657c183bd6ac1204198ece06ce4ea9e..c63d6df8d04f62f167610eace6ec8c5d61ca500e 100644
--- a/src/factory.h
+++ b/src/factory.h
@@ -347,8 +347,10 @@ inline std::string LandmarkFactory::getClass()
}
// Frames
-class FrameBase;
class TimeStamp;
+} // namespace wolf
+#include "frame_base.h"
+namespace wolf{
typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory;
template<>
inline std::string FrameFactory::getClass()
diff --git a/src/feature_IMU.cpp b/src/feature_IMU.cpp
index c76b4378f139d9e1ef57cb0654ec449285a0223a..a532b4c25ee21bbea911dfccd8f2825aa56d7eab 100644
--- a/src/feature_IMU.cpp
+++ b/src/feature_IMU.cpp
@@ -5,7 +5,7 @@ namespace wolf {
FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
const Eigen::MatrixXs& _delta_preintegrated_covariance,
const Eigen::Vector6s& _bias,
- const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians,
+ const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
CaptureMotionPtr _cap_imu_ptr) :
FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
acc_bias_preint_(_bias.head<3>()),
diff --git a/src/feature_IMU.h b/src/feature_IMU.h
index 5e12d0b404f4e5e6873d2bcc3c741549b7215e82..37e6884db6a5415647ad3e32e305483202fe973d 100644
--- a/src/feature_IMU.h
+++ b/src/feature_IMU.h
@@ -31,7 +31,7 @@ class FeatureIMU : public FeatureBase
FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
const Eigen::MatrixXs& _delta_preintegrated_covariance,
const Eigen::Vector6s& _bias,
- const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians,
+ const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
CaptureMotionPtr _cap_imu_ptr = nullptr);
/** \brief Constructor from capture pointer
diff --git a/src/feature_base.h b/src/feature_base.h
index b56c650f6757b843a036438ec51c53a8e7b11be1..495d7d58f26d3409a22b5781bed478ad66ebea6b 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -105,7 +105,7 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
-inline wolf::ConstraintBaseList& FeatureBase::getConstrainedByList()
+inline ConstraintBaseList& FeatureBase::getConstrainedByList()
{
return constrained_by_list_;
}
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 92467b61d78ac4c681aa1aafef06cbe63f79baf5..a1a806bee07eec7387b19b99364f06e4ce72e88d 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -98,6 +98,13 @@ void FrameBase::remove()
}
}
+void FrameBase::setTimeStamp(const TimeStamp& _ts)
+{
+ time_stamp_ = _ts;
+ if (isKey() && getTrajectoryPtr() != nullptr)
+ getTrajectoryPtr()->sortFrame(shared_from_this());
+}
+
void FrameBase::registerNewStateBlocks()
{
if (getProblem() != nullptr)
@@ -332,6 +339,12 @@ CaptureBaseList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
return captures;
}
+void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
+{
+ _cap_ptr->unlinkFromFrame();
+ capture_list_.remove(_cap_ptr);
+}
+
ConstraintBasePtr FrameBase::getConstraintOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
{
for (const ConstraintBasePtr& constaint_ptr : getConstrainedByList())
@@ -348,6 +361,12 @@ ConstraintBasePtr FrameBase::getConstraintOf(const ProcessorBasePtr _processor_p
return nullptr;
}
+void FrameBase::getConstraintList(ConstraintBaseList& _ctr_list)
+{
+ for (auto c_ptr : getCaptureList())
+ c_ptr->getConstraintList(_ctr_list);
+}
+
ConstraintBasePtr FrameBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.push_back(_ctr_ptr);
diff --git a/src/frame_base.h b/src/frame_base.h
index 7160385638de70b282887e294e539a831dcd1e88..0ac6d3ca3ebe8bd7db28318204d0fad3d1cb62f5 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -18,6 +18,14 @@ class StateBlock;
namespace wolf {
+/** \brief Enumeration of frame types: key-frame or non-key-frame
+ */
+typedef enum
+{
+ NON_KEY_FRAME = 0, ///< Regular frame. It does not play at optimization.
+ KEY_FRAME = 1 ///< key frame. It plays at optimizations.
+} FrameType;
+
//class FrameBase
class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
@@ -169,13 +177,6 @@ inline bool FrameBase::isKey() const
return (type_ == KEY_FRAME);
}
-inline void FrameBase::setTimeStamp(const TimeStamp& _ts)
-{
- time_stamp_ = _ts;
- if (isKey() && getTrajectoryPtr() != nullptr)
- getTrajectoryPtr()->sortFrame(shared_from_this());
-}
-
inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
{
_ts = time_stamp_;
@@ -252,18 +253,6 @@ inline void FrameBase::resizeStateBlockVec(int _size)
state_block_vec_.resize(_size);
}
-inline void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
-{
- _cap_ptr->unlinkFromFrame();
- capture_list_.remove(_cap_ptr);
-}
-
-inline void FrameBase::getConstraintList(ConstraintBaseList& _ctr_list)
-{
- for (auto c_ptr : getCaptureList())
- c_ptr->getConstraintList(_ctr_list);
-}
-
inline unsigned int FrameBase::getHits() const
{
return constrained_by_list_.size();
diff --git a/src/hello_wolf/capture_range_bearing.h b/src/hello_wolf/capture_range_bearing.h
index 1c83cdb3971c1d321ffd581c5fa40689a05ffd9f..6e18876cb415a91c1c0dcddf1e9047e563ed5f80 100644
--- a/src/hello_wolf/capture_range_bearing.h
+++ b/src/hello_wolf/capture_range_bearing.h
@@ -27,8 +27,8 @@ class CaptureRangeBearing : public CaptureBase
const int& getId (int _i) const;
const Eigen::VectorXs& getRanges () const;
const Eigen::VectorXs& getBearings () const;
- const wolf::Scalar& getRange (int _i) const;
- const wolf::Scalar& getBearing (int _i) const;
+ const Scalar& getRange (int _i) const;
+ const Scalar& getBearing (int _i) const;
Eigen::Vector2s getRangeBearing (int _i) const;
Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
@@ -58,12 +58,12 @@ inline const Eigen::VectorXs& CaptureRangeBearing::getBearings() const
return bearings_;
}
-inline const wolf::Scalar& CaptureRangeBearing::getRange(int _i) const
+inline const Scalar& CaptureRangeBearing::getRange(int _i) const
{
return ranges_(_i);
}
-inline const wolf::Scalar& CaptureRangeBearing::getBearing(int _i) const
+inline const Scalar& CaptureRangeBearing::getBearing(int _i) const
{
return bearings_(_i);
}
diff --git a/src/hello_wolf/feature_range_bearing.h b/src/hello_wolf/feature_range_bearing.h
index 93779b6d4b9433ec125b528552edcc9e185a45b1..e3d30bba2b08c6bba72a8a83808dcdba2b1a4631 100644
--- a/src/hello_wolf/feature_range_bearing.h
+++ b/src/hello_wolf/feature_range_bearing.h
@@ -15,7 +15,7 @@ namespace wolf
WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
-class FeatureRangeBearing : public wolf::FeatureBase
+class FeatureRangeBearing : public FeatureBase
{
public:
FeatureRangeBearing(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
diff --git a/src/landmark_AHP.cpp b/src/landmark_AHP.cpp
index 2355c3f896b3302dbe79fac7bea460d83c464214..78b2bdb32264fd5e371bb9be25bd3342c1150fe4 100644
--- a/src/landmark_AHP.cpp
+++ b/src/landmark_AHP.cpp
@@ -55,7 +55,7 @@ Eigen::Vector3s LandmarkAHP::point() const
return point_hmg.head<3>()/point_hmg(3);
}
-wolf::LandmarkBasePtr LandmarkAHP::create(const YAML::Node& _node)
+LandmarkBasePtr LandmarkAHP::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
Eigen::VectorXs pos_homog = _node["position"] .as< Eigen::VectorXs >();
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 6e6cf8b037f413734f6eb88a57a5a36a526b999d..1cb8380a5b77b0d2429153c502dec23a74bedf7c 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -102,6 +102,16 @@ void LandmarkBase::registerNewStateBlocks()
}
}
+Eigen::MatrixXs LandmarkBase::getCovariance() const
+{
+ return getProblem()->getLandmarkCovariance(shared_from_this());
+}
+
+bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
+{
+ return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
+}
+
void LandmarkBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 5223a94994de802719e132c196bd182312128c97..50a5608f97adcb35208f0dd82e79848a8c3bfe91 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -105,16 +105,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
namespace wolf{
-inline bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
-{
- return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
-}
-
-inline Eigen::MatrixXs LandmarkBase::getCovariance() const
-{
- return getProblem()->getLandmarkCovariance(shared_from_this());
-}
-
inline MapBasePtr LandmarkBase::getMapPtr()
{
return map_ptr_.lock();
diff --git a/src/local_parametrization_base.h b/src/local_parametrization_base.h
index f6b447dd9e2df06be03d4d08dc739f4bdd2f1a3f..acb9ec683416fb46da4ba9dee60e48f3a2752d0a 100644
--- a/src/local_parametrization_base.h
+++ b/src/local_parametrization_base.h
@@ -13,6 +13,8 @@
namespace wolf {
+//WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
+
class LocalParametrizationBase{
protected:
unsigned int global_size_;
diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp
index 20e38eccf6874efe4fa4097583d4bdd956700aea..46e9ca9f8ba3b49155a7a913d1df4f0f515ce5a3 100644
--- a/src/local_parametrization_quaternion.cpp
+++ b/src/local_parametrization_quaternion.cpp
@@ -8,7 +8,7 @@ namespace wolf {
////////// LOCAL PERTURBATION //////////////
template <>
-bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
+bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
Eigen::Map<const Eigen::VectorXs>& _delta_theta,
Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
{
@@ -27,7 +27,7 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::plus(Eigen::Map<const Eigen
}
template <>
-bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
+bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
Eigen::Map<Eigen::MatrixXs>& _jacobian) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -43,7 +43,7 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::computeJacobian(Eigen::Map<
}
template <>
-bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
+bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
Eigen::Map<const Eigen::VectorXs>& _q2,
Eigen::Map<Eigen::VectorXs>& _q2_minus_q1)
{
@@ -61,7 +61,7 @@ bool LocalParametrizationQuaternion<wolf::DQ_LOCAL>::minus(Eigen::Map<const Eige
////////// GLOBAL PERTURBATION //////////////
template <>
-bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
Eigen::Map<const Eigen::VectorXs>& _delta_theta,
Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
{
@@ -80,7 +80,7 @@ bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::plus(Eigen::Map<const Eige
}
template <>
-bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
Eigen::Map<Eigen::MatrixXs>& _jacobian) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -96,7 +96,7 @@ bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::computeJacobian(Eigen::Map
}
template <>
-bool LocalParametrizationQuaternion<wolf::DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
Eigen::Map<const Eigen::VectorXs>& _q2,
Eigen::Map<Eigen::VectorXs>& _q2_minus_q1)
{
diff --git a/src/problem.cpp b/src/problem.cpp
index f1f2558ef2bd0723ce1000607ff203afc1f19f51..d7bdec581973a55a58b7fe0658934096fe8374e5 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -1,21 +1,25 @@
-#include <sensor_GPS.h>
+// wolf includes
#include "problem.h"
-#include "constraint_base.h"
-#include "state_block.h"
-#include "state_quaternion.h"
#include "hardware_base.h"
#include "trajectory_base.h"
#include "map_base.h"
-#include "processor_motion.h"
-#include "processor_tracker.h"
#include "sensor_base.h"
#include "factory.h"
#include "sensor_factory.h"
#include "processor_factory.h"
+#include "constraint_base.h"
+#include "state_block.h"
+#include "processor_motion.h"
-#include <algorithm>
+#include "processor_tracker.h"
#include "capture_pose.h"
+// IRI libs includes
+#include <sensor_GPS.h>
+
+// C++ includes
+#include <algorithm>
+
namespace wolf
{
@@ -149,7 +153,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
}
}
-wolf::SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
+SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
{
auto sen_it = std::find_if(getHardwarePtr()->getSensorList().begin(),
getHardwarePtr()->getSensorList().end(),
@@ -942,7 +946,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl;
}
-FrameBasePtr wolf::Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
+FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
{
return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
}
diff --git a/src/problem.h b/src/problem.h
index adaa9c823bd21f59a710dbf613b081f0c6fbf7c8..53ee4c59b9b4be14213d3a2a3f75ee51ae29cb9e 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -15,6 +15,7 @@ struct ProcessorParamsBase;
//wolf includes
#include "wolf.h"
+#include "frame_base.h"
// std includes
@@ -325,7 +326,7 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
-inline wolf::ProcessorMotionPtr& Problem::getProcessorMotionPtr()
+inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
{
return processor_motion_ptr_;
}
diff --git a/src/processor_GPS.cpp b/src/processor_GPS.cpp
index 74f20e48cc56598179f6d0c8af170fa7c97400a4..a72f47119b5b67d48058e8da557ce8be824b461b 100644
--- a/src/processor_GPS.cpp
+++ b/src/processor_GPS.cpp
@@ -54,14 +54,14 @@ bool ProcessorGPS::voteForKeyFrame()
return false;
}
-bool ProcessorGPS::keyFrameCallback(wolf::FrameBasePtr, const Scalar& _time_tol)
+void ProcessorGPS::keyFrameCallback(FrameBasePtr, const Scalar& _time_tol)
{
- return false;
+ //
}
-wolf::ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
+ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
{
- ProcessorGPSPtr prc_ptr = std::make_shared<ProcessorGPS>();
+ ProcessorGPSPtr prc_ptr = std::make_shared<ProcessorGPS>(_params->time_tolerance);
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor_GPS.h b/src/processor_GPS.h
index 5eae46f5afca8cfc141fb7533f9d84c8dbf700f1..0941b22e5227a797f18f7393b6ab4ba18788e131 100644
--- a/src/processor_GPS.h
+++ b/src/processor_GPS.h
@@ -24,13 +24,13 @@ class ProcessorGPS : public ProcessorBase
Scalar gps_covariance_;
public:
- ProcessorGPS();
- virtual ~ProcessorGPS(Scalar time_tolerance_);
+ ProcessorGPS(Scalar time_tolerance_);
+ virtual ~ProcessorGPS();
virtual void configure(SensorBasePtr _sensor) { };
virtual void init(CaptureBasePtr _capture_ptr);
virtual void process(CaptureBasePtr _capture_ptr);
virtual bool voteForKeyFrame();
- virtual bool keyFrameCallback(wolf::FrameBasePtr, const Scalar& _time_tol);
+ virtual void keyFrameCallback(FrameBasePtr, const Scalar& _time_tol);
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
diff --git a/src/processor_base.h b/src/processor_base.h
index e3990f51f895f257bd154f91ee726f0109bfc397..73fdc721ebbadc8630f36772282c44db9b45afd6 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -10,6 +10,7 @@ class SensorBase;
#include "wolf.h"
#include "node_base.h"
#include "time_stamp.h"
+#include "frame_base.h"
// std
#include <memory>
diff --git a/src/processor_frame_nearest_neighbor_filter.h b/src/processor_frame_nearest_neighbor_filter.h
index dfc2c2208a21b3068202d866611530725aebdc2c..b4eb2e7fb79cfde1325dbb8bc5ca764e1095a2e3 100644
--- a/src/processor_frame_nearest_neighbor_filter.h
+++ b/src/processor_frame_nearest_neighbor_filter.h
@@ -53,7 +53,7 @@ private:
// area of the computed covariance ellipse.
// depends on how many percent of data should be considered.
- wolf::Scalar area_;
+ Scalar area_;
ProcessorParamsFrameNearestNeighborFilter params_;
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 11e299f3089bcccc40ecd3793a6c16a6f7eaaced..df3471855ac4b030db679cfa7cd0027afc9328b9 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -141,7 +141,7 @@ class ProcessorMotion : public ProcessorBase
* \return the state vector
*/
Eigen::VectorXs getCurrentState();
- wolf::TimeStamp getCurrentTimeStamp();
+ TimeStamp getCurrentTimeStamp();
/** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp
@@ -462,7 +462,7 @@ inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts)
return x;
}
-inline wolf::TimeStamp ProcessorMotion::getCurrentTimeStamp()
+inline TimeStamp ProcessorMotion::getCurrentTimeStamp()
{
return getBuffer().get().back().ts_;
}
diff --git a/src/processors/processor_tracker_feature_trifocal.h b/src/processors/processor_tracker_feature_trifocal.h
index 2bd8b29b2653b3add58e2c2f0186edc01d33fc74..e7a648308ddec03bb6a20b00c7f94386d779875b 100644
--- a/src/processors/processor_tracker_feature_trifocal.h
+++ b/src/processors/processor_tracker_feature_trifocal.h
@@ -136,7 +136,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
const SensorBasePtr _sensor_ptr);
};
-inline wolf::CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOriginPtr()
+inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOriginPtr()
{
return prev_origin_ptr_;
}
diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h
index 0b34016e29025f9ddc43a07582924988095e01a1..2f5d9baa576f697772800359a46a239170f39144 100644
--- a/src/sensor_IMU.h
+++ b/src/sensor_IMU.h
@@ -15,12 +15,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
struct IntrinsicsIMU : public IntrinsicsBase
{
//noise std dev
- wolf::Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
- wolf::Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+ Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+ Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
//Initial biases std dev
- wolf::Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec
- wolf::Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec
+ Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec
+ Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec
// bias rate of change std dev
Scalar ab_rate_stdev = 0.00001;
@@ -35,14 +35,14 @@ class SensorIMU : public SensorBase
{
protected:
- wolf::Scalar a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
- wolf::Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
+ Scalar a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
+ Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
//This is a trial to constraint how much can the bias change in 1 sec at most
- wolf::Scalar ab_initial_stdev; //accelerometer micro_g/sec
- wolf::Scalar wb_initial_stdev; //gyroscope rad/sec
- wolf::Scalar ab_rate_stdev; //accelerometer micro_g/sec
- wolf::Scalar wb_rate_stdev; //gyroscope rad/sec
+ Scalar ab_initial_stdev; //accelerometer micro_g/sec
+ Scalar wb_initial_stdev; //gyroscope rad/sec
+ Scalar ab_rate_stdev; //accelerometer micro_g/sec
+ Scalar wb_rate_stdev; //gyroscope rad/sec
public:
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 48b7b3da4c0fb1f0fb26c5583713cc250809164c..ce30ddf8084ec0598e76fc4aa2781628d1d761f7 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -283,7 +283,7 @@ StateBlockPtr SensorBase::getIntrinsicPtr()
return getStateBlockPtrDynamic(2);
}
-wolf::Size SensorBase::computeCalibSize() const
+Size SensorBase::computeCalibSize() const
{
Size sz = 0;
for (Size i = 0; i < state_block_vec_.size(); i++)
diff --git a/src/sensor_base.h b/src/sensor_base.h
index a1befcf792bcb81cca58d972ff1ea33bf4e2b9df..cfacfbc7d23bd17eb34b0ef3bff6fa3cc95ae207 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -250,7 +250,7 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr;
}
-inline wolf::Size SensorBase::getCalibSize() const
+inline Size SensorBase::getCalibSize() const
{
return calib_size_;
}
diff --git a/src/sensor_factory.h b/src/sensor_factory.h
index abdba1e604a199b81b8a32c765c98197bdd87ff3..3e7e1aabc300d286fbf1c37373de0ba7614f75f1 100644
--- a/src/sensor_factory.h
+++ b/src/sensor_factory.h
@@ -220,7 +220,7 @@ inline std::string SensorFactory::getClass()
#define WOLF_REGISTER_SENSOR(SensorType, SensorName) \
namespace{ const bool WOLF_UNUSED SensorName##Registered = \
- wolf::SensorFactory::get().registerCreator(SensorType, SensorName::create); }\
+ SensorFactory::get().registerCreator(SensorType, SensorName::create); }\
} /* namespace wolf */
diff --git a/src/state_block.h b/src/state_block.h
index fe11dce9e8272ed58d807cba6227065386198057..7903ceb748e1700495c83f6bae02efc869dfe2e1 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -10,6 +10,7 @@ class LocalParametrizationBase;
//Wolf includes
#include "wolf.h"
+#include "local_parametrization_base.h"
//std includes
#include <iostream>
@@ -107,6 +108,7 @@ class StateBlock
// IMPLEMENTATION
#include "local_parametrization_base.h"
+
namespace wolf {
inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
diff --git a/src/three_D_tools.h b/src/three_D_tools.h
index 28a15f29757116ffc1c9bc10f9507df58f9a5452..ea3386b30aed3011d055dd77875db7ace06b52e4 100644
--- a/src/three_D_tools.h
+++ b/src/three_D_tools.h
@@ -56,7 +56,7 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
q << T2(0), T2(0), T2(0), T2(1);
}
-template<typename T = wolf::Scalar>
+template<typename T = Scalar>
inline Matrix<T, 7, 1> identity()
{
Matrix<T, 7, 1> ret;
diff --git a/src/time_stamp.h b/src/time_stamp.h
index 40c6a532511066b13ad57590220e815b3317852f..d3902e82f2732bfc63c23a3b223a721435bdadb1 100644
--- a/src/time_stamp.h
+++ b/src/time_stamp.h
@@ -5,10 +5,8 @@
//wolf includes
#include "wolf.h"
-#include <sys/time.h>
-
-
//C, std
+#include <sys/time.h>
#include <iostream>
diff --git a/src/wolf.h b/src/wolf.h
index 424cd92953c71dc44f8d0fea3c19a1b2ccbf1cc8..6fd47ad622d9a5754e5525041ebcd138c6229241 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -195,36 +195,6 @@ struct MatrixSizeCheck
// End of check matrix sizes /////////////////////////////////////////////////
-/** \brief Enumeration of frame types: key-frame or non-key-frame
- */
-typedef enum
-{
- NON_KEY_FRAME = 0, ///< regular frame. It does play at optimizations but it will be discarded from the window once a newer frame arrives.
- KEY_FRAME = 1 ///< key frame. It will stay in the frames window and play at optimizations.
-} FrameType;
-
-/** \brief Enumeration of constraint status
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- */
-typedef enum
-{
- CTR_INACTIVE = 0, ///< Constraint established with a frame (odometry).
- CTR_ACTIVE = 1 ///< Constraint established with absolute reference.
-} ConstraintStatus;
-
-/** \brief Enumeration of jacobian computation method
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- */
-typedef enum
-{
- JAC_AUTO = 1, ///< Auto differentiation (AutoDiffCostFunctionWrapper or ceres::NumericDiffCostFunction).
- JAC_NUMERIC, ///< Numeric differentiation (ceres::NumericDiffCostFunction).
- JAC_ANALYTIC ///< Analytic jacobians.
-} JacobianMethod;
-
-
/////////////////////////////////////////////////////////////////////////
// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
/////////////////////////////////////////////////////////////////////////