diff --git a/.gitignore b/.gitignore
index 2bc23797c67e5dbc5ec8d8b0bd8b543f1883f26b..dc88af0b4496bafe08038782bd6fbd2a28902403 100644
--- a/.gitignore
+++ b/.gitignore
@@ -22,3 +22,7 @@ src/examples/map_polyline_example_write.yaml
*.graffle
/Default/
+
+src/CMakeCache.txt
+
+src/CMakeFiles/cmake.check_cache
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4e986387fc47fbc42585f8818d4fe86adc03e7fc..def5cd35fe89743bce48370c67f900afcd0ba95e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -447,9 +447,9 @@ IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
-IF (Suitesparse_FOUND)
- ADD_SUBDIRECTORY(solver)
-ENDIF(Suitesparse_FOUND)
+#IF (Suitesparse_FOUND)
+# ADD_SUBDIRECTORY(solver)
+#ENDIF(Suitesparse_FOUND)
# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
IF(YAMLCPP_FOUND)
@@ -550,8 +550,8 @@ INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/processors)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/solver)
+#INSTALL(FILES ${HDRS_SOLVER}
+# DESTINATION include/iri-algorithms/wolf/solver)
INSTALL(FILES ${HDRS_SERIALIZATION}
DESTINATION include/iri-algorithms/wolf/serialization)
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/create_numeric_diff_cost_function.h b/src/ceres_wrapper/create_numeric_diff_cost_function.h
index 30c1c89c6d31093d5d0d2534bba039aa153af95e..d6650b20397a11bec5b1e57f707e95b51c2dc21d 100644
--- a/src/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/src/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -32,20 +32,20 @@ std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSiz
inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr)
{
- switch (_ctr_ptr->getTypeId())
- {
- // just for testing
- case CTR_ODOM_2D:
- return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
-
- /* For adding a new constraint, add the #include and a case:
- case CTR_ENUM:
- return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
- */
-
- default:
+// switch (_ctr_ptr->getTypeId())
+// {
+// // just for testing
+// case CTR_ODOM_2D:
+// return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
+//
+// /* For adding a new constraint, add the #include and a case:
+// case CTR_ENUM:
+// return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
+// */
+//
+// default:
throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
- }
+// }
}
} // 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 a63810f6bdba3220340560f50f9d3cac6e22b020..73c84a9570db2bf4a7b2a5fe7e0742801f05006c 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -65,7 +65,7 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>(CTR_AHP,
+ ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>("AHP",
_landmark_ptr->getAnchorFrame(),
nullptr,
nullptr,
@@ -82,8 +82,6 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
anchor_sensor_extrinsics_o_(_ftr_ptr->getCapturePtr()->getSensorOPtr()->getState()),
intrinsic_(_ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr()->getState())
{
- setType("AHP");
-
// obtain some intrinsics from provided sensor
distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getDistortionVector();
}
@@ -185,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_2D.h b/src/constraint_GPS_2D.h
index d8b5efda4a7f8bb2a43325cef3179821354f2897..5feb77fb97c19d71d48ad740bf387712ab8e169d 100644
--- a/src/constraint_GPS_2D.h
+++ b/src/constraint_GPS_2D.h
@@ -16,9 +16,9 @@ class ConstraintGPS2D : public ConstraintAutodiff<ConstraintGPS2D, 2, 2>
public:
ConstraintGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPS2D, 2, 2>(CTR_GPS_FIX_2D, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
+ ConstraintAutodiff<ConstraintGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
{
- setType("GPS FIX 2D");
+ //
}
virtual ~ConstraintGPS2D() = default;
diff --git a/src/constraint_GPS_pseudorange_2D.h b/src/constraint_GPS_pseudorange_2D.h
index e0d8d2a9ae21eb868640350e1a5aaac564f51c44..6bda014c58944abeff03bf04621f6723f234b1e6 100644
--- a/src/constraint_GPS_pseudorange_2D.h
+++ b/src/constraint_GPS_pseudorange_2D.h
@@ -30,18 +30,22 @@ class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudo
ConstraintGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>(CTR_GPS_PR_2D,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to the initial pos frame
- _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame
- _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to the vehicle frame
- // orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapPPtr()), // map position with respect to ecef
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapOPtr())) // map orientation with respect to ecef
+ ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to the initial pos frame
+ _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame
+ _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to the vehicle frame
+ // orientation of antenna is not needed, because omnidirectional
+ _ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr(), //intrinsic parameter = receiver time bias
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapPPtr()), // map position with respect to ecef
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapOPtr())) // map orientation with respect to ecef
{
- setType("GPS PR 2D");
sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
//std::cout << "ConstraintGPSPseudorange2D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
diff --git a/src/constraint_GPS_pseudorange_3D.h b/src/constraint_GPS_pseudorange_3D.h
index 97fb326c08ba76178248935fbd96164cd5266f12..1de907155e5fbb44cb14d1fa6afa06081d308820 100644
--- a/src/constraint_GPS_pseudorange_3D.h
+++ b/src/constraint_GPS_pseudorange_3D.h
@@ -29,7 +29,8 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>(CTR_GPS_PR_3D, nullptr, nullptr, nullptr, _apply_loss_function, _status,
+ ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status,
_ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
_ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
_ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame
@@ -38,7 +39,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
(std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getMapPPtr(), // initial vehicle position wrt ecef frame
(std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getMapOPtr()) // initial vehicle orientation wrt ecef frame
{
- setType("GPS PR 3D");
sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
@@ -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 625e7fd85acc09503ecc699dce8c97657db02d9e..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
*
@@ -148,7 +148,7 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
- CTR_IMU,
+ "IMU",
_cap_origin_ptr->getFramePtr(),
_cap_origin_ptr,
nullptr,
@@ -180,7 +180,7 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
sqrt_A_r_dt_inv((Eigen::Matrix3s::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
{
- setType("IMU");
+ //
}
@@ -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_analytic.cpp b/src/constraint_analytic.cpp
index be273d6fc80be1cd851ddbdd6b5bc56725b1812d..c2b0268e3f819162050a7404346a9a70ce388642 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -3,7 +3,7 @@
namespace wolf {
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp,
+ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
bool _apply_loss_function, ConstraintStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
@@ -14,7 +14,7 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp,
resizeVectors();
}
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp,
+ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -23,7 +23,7 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp,
bool _apply_loss_function, ConstraintStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index ad490237c526e8f9fbc40bfe9bd8306aa71cbbec..9384afd71f23b46773dc59d1166830ce6a6d0272 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -23,7 +23,9 @@ class ConstraintAnalytic: public ConstraintBase
* Constructor of category CTR_ABSOLUTE
*
**/
- ConstraintAnalytic(ConstraintType _tp, bool _apply_loss_function, ConstraintStatus _status,
+ ConstraintAnalytic(const std::string& _tp,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr = nullptr,
StateBlockPtr _state2Ptr = nullptr,
@@ -36,7 +38,7 @@ class ConstraintAnalytic: public ConstraintBase
StateBlockPtr _state9Ptr = nullptr ) ;
- ConstraintAnalytic(ConstraintType _tp,
+ ConstraintAnalytic(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
diff --git a/src/constraint_autodiff.h b/src/constraint_autodiff.h
index 109f73623dd0b674afb8a8a57b80e98b5672631c..7e6f227a44da539cb80a2d4a97f1e114372b93c5 100644
--- a/src/constraint_autodiff.h
+++ b/src/constraint_autodiff.h
@@ -58,7 +58,7 @@ class ConstraintAutodiff : public ConstraintBase
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -364,7 +364,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -629,7 +629,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -882,7 +882,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1123,7 +1123,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1347,7 +1347,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1559,7 +1559,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1763,7 +1763,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1955,7 +1955,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2135,7 +2135,7 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
public:
- ConstraintAutodiff(ConstraintType _tp,
+ ConstraintAutodiff(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index 30e1db1531de9c8aec3e48ac13cefd33e2328a51..ca54791bb95766f72ab7d9e67eb7d717cb8dd081 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -6,12 +6,13 @@ namespace wolf {
unsigned int ConstraintBase::constraint_id_count_ = 0;
-ConstraintBase::ConstraintBase(ConstraintType _tp, bool _apply_loss_function, ConstraintStatus _status) :
- NodeBase("CONSTRAINT", "Base"),
+ConstraintBase::ConstraintBase(const std::string& _tp,
+ bool _apply_loss_function,
+ ConstraintStatus _status) :
+ NodeBase("CONSTRAINT", _tp),
feature_ptr_(), // nullptr
is_removing_(false),
constraint_id_(++constraint_id_count_),
- type_id_(_tp),
status_(_status),
apply_loss_function_(_apply_loss_function),
frame_other_ptr_(), // nullptr
@@ -22,18 +23,17 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, bool _apply_loss_function, Co
// std::cout << "constructed +c" << id() << std::endl;
}
-ConstraintBase::ConstraintBase(ConstraintType _tp,
+ConstraintBase::ConstraintBase(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, ConstraintStatus _status) :
- NodeBase("CONSTRAINT", "Base"),
+ NodeBase("CONSTRAINT", _tp),
feature_ptr_(),
is_removing_(false),
constraint_id_(++constraint_id_count_),
- type_id_(_tp),
status_(_status),
apply_loss_function_(_apply_loss_function),
frame_other_ptr_(_frame_other_ptr),
@@ -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 534cf8537c54b3ec3b5d92e3435fa3c2532d9ada..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>
@@ -26,7 +46,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
protected:
unsigned int constraint_id_;
- ConstraintType type_id_; ///< type of constraint (types defined at wolf.h)
ConstraintStatus status_; ///< status of constraint (types defined at wolf.h)
bool apply_loss_function_; ///< flag for applying loss function to this constraint
FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category CTR_FRAME)
@@ -39,13 +58,13 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
/** \brief Constructor of category CTR_ABSOLUTE
**/
- ConstraintBase(ConstraintType _tp,
+ ConstraintBase(const std::string& _tp,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE);
/** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
**/
- ConstraintBase(ConstraintType _tp,
+ ConstraintBase(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -60,10 +79,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
unsigned int id() const;
- /** \brief Returns the constraint type
- **/
- ConstraintType getTypeId() const;
-
/** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
@@ -161,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
};
@@ -181,45 +196,35 @@ 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
{
return constraint_id_;
}
-inline ConstraintType ConstraintBase::getTypeId() const
-{
- return type_id_;
-}
-
inline FeatureBasePtr ConstraintBase::getFeaturePtr() const
{
return feature_ptr_.lock();
}
-//inline ConstraintCategory ConstraintBase::getCategory() const
-//{
-// return category_;
-//}
-
inline ConstraintStatus ConstraintBase::getStatus() const
{
return status_;
@@ -230,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_block_absolute.h b/src/constraint_block_absolute.h
index a02fcc237b0eccfa2f32f44b7de2440dfedb5bb2..fe8eec7a9f01095838c0572d4517e4493d85fa3a 100644
--- a/src/constraint_block_absolute.h
+++ b/src/constraint_block_absolute.h
@@ -23,9 +23,10 @@ class ConstraintBlockAbsolute: public ConstraintAutodiff<ConstraintBlockAbsolute
public:
ConstraintBlockAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintBlockAbsolute,3,3>(CTR_BLOCK_ABS, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
+ ConstraintAutodiff<ConstraintBlockAbsolute,3,3>("BLOCK ABS",
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
{
- setType("FIX SB");
+ //
}
virtual ~ConstraintBlockAbsolute() = default;
diff --git a/src/constraint_container.h b/src/constraint_container.h
index e4c9e66c0ebdfd6d7b2f555fd1544fda4ea8a194..c796d1e326f11b1fcb78d17341a1bb6f782211dd 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -23,12 +23,12 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
const ProcessorBasePtr& _processor_ptr,
const unsigned int _corner,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintContainer,3,2,1,2,1>(CTR_CONTAINER, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
+ ConstraintAutodiff<ConstraintContainer,3,2,1,2,1>("CONTAINER",
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
assert(/*_corner >= 0 &&*/ _corner <= 3 && "Wrong corner id in constraint container constructor");
- setType("CONTAINER");
std::cout << "new constraint container: corner idx = " << corner_ << std::endl;
}
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index 922d17d5aca43eba5e360426041e674d7f2827bf..96afcfd509c35f24fe27d01f149532019cea427f 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -18,9 +18,10 @@ class ConstraintCorner2D: public ConstraintAutodiff<ConstraintCorner2D, 3,2,1,2,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintCorner2D,3,2,1,2,1>(CTR_CORNER_2D, nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
+ ConstraintAutodiff<ConstraintCorner2D,3,2,1,2,1>("CORNER 2D",
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
{
- setType("CORNER 2D");
+ //
}
virtual ~ConstraintCorner2D() = default;
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 948a162be4fcd08d443272d46d470b99a76ae904..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);
@@ -58,12 +58,12 @@ class ConstraintEpipolar : public ConstraintBase
inline ConstraintEpipolar::ConstraintEpipolar(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, ConstraintStatus _status) :
- ConstraintBase(CTR_EPIPOLAR, nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+ ConstraintBase("EPIPOLAR", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
{
- setType("EPIPOLAR");
+ //
}
-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_fix_bias.h b/src/constraint_fix_bias.h
index de1480919e5a1aa37d24c4a95b4114196f7796ba..bc9807dd8635c37deffdef99b9a9e6d54dae28b6 100644
--- a/src/constraint_fix_bias.h
+++ b/src/constraint_fix_bias.h
@@ -21,10 +21,10 @@ class ConstraintFixBias: public ConstraintAutodiff<ConstraintFixBias,6,3,3>
{
public:
ConstraintFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintFixBias, 6, 3, 3>(CTR_FIX_BIAS, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getAccBiasPtr(),
+ ConstraintAutodiff<ConstraintFixBias, 6, 3, 3>("FIX BIAS",
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getAccBiasPtr(),
std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getGyroBiasPtr())
{
- setType("FIX BIAS");
// std::cout << "created ConstraintFixBias " << std::endl;
}
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index decbf0e1f3222b536758b360cd02865abd64afd9..11333302fc5f3fa12a01354e5f6b08cc60fc9065 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -19,7 +19,7 @@ class ConstraintOdom2D : public ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2, 1>(CTR_ODOM_2D,
+ ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
@@ -28,7 +28,7 @@ class ConstraintOdom2D : public ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2,
_ftr_ptr->getFramePtr()->getPPtr(),
_ftr_ptr->getFramePtr()->getOPtr())
{
- setType("ODOM 2D");
+ //
}
virtual ~ConstraintOdom2D() = default;
@@ -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 dd35bfec72dd5dd239437afa6586e9856e62a3bb..d69655e2714928729b610ba8162075db642c68b3 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -18,9 +18,10 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintRelative2DAnalytic(_ftr_ptr, CTR_ODOM_2D, _frame_ptr, _processor_ptr, _apply_loss_function, _status)
+ ConstraintRelative2DAnalytic("ODOM_2D", _ftr_ptr,
+ _frame_ptr, _processor_ptr, _apply_loss_function, _status)
{
- setType("ODOM 2D ANALYTIC");
+ //
}
virtual ~ConstraintOdom2DAnalytic() = default;
@@ -94,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 d5d820e2c4f1a800142ae22f85ad9aa474b082bc..a3d4b7af0ae3419d0a198591750aa174aeb26769 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -77,7 +77,7 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintOdom3D, 6, 3, 4, 3, 4>(CTR_ODOM_3D, // type
+ ConstraintAutodiff<ConstraintOdom3D, 6, 3, 4, 3, 4>("ODOM 3D", // type
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
@@ -90,8 +90,6 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
_frame_past_ptr->getPPtr(), // past frame P
_frame_past_ptr->getOPtr()) // past frame Q
{
- setType("ODOM 3D");
-
// WOLF_TRACE("Constr ODOM 3D (f", _ftr_current_ptr->id(),
// " F", _ftr_current_ptr->getCapturePtr()->getFramePtr()->id(),
// ") (Fo", _frame_past_ptr->id(), ")");
@@ -103,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);
@@ -129,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();
@@ -152,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/constraint_point_2D.h b/src/constraint_point_2D.h
index c001fb553363e162ca9c445d88ac2788583dac34..50d190409986f90bb0828a24c973e8c9f2508e19 100644
--- a/src/constraint_point_2D.h
+++ b/src/constraint_point_2D.h
@@ -29,12 +29,12 @@ class ConstraintPoint2D: public ConstraintAutodiff<ConstraintPoint2D, 2,2,1,2,1,
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPoint2D,2,2,1,2,1,2>(CTR_POINT_2D, nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
+ ConstraintAutodiff<ConstraintPoint2D,2,2,1,2,1,2>("POINT 2D",
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl;
- setType("POINT TO POINT 2D");
Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
diff --git a/src/constraint_point_to_line_2D.h b/src/constraint_point_to_line_2D.h
index be69caa7e60aa8ef00e0162563bad298775e3548..324ae24d600cc646472340e60fd92732f833d003 100644
--- a/src/constraint_point_to_line_2D.h
+++ b/src/constraint_point_to_line_2D.h
@@ -30,11 +30,11 @@ class ConstraintPointToLine2D: public ConstraintAutodiff<ConstraintPointToLine2D
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPointToLine2D, 1,2,1,2,1,2,2>(CTR_POINT_TO_LINE_2D, nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
+ ConstraintAutodiff<ConstraintPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "ConstraintPointToLine2D" << std::endl;
- setType("POINT TO LINE 2D");
Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
diff --git a/src/constraint_pose_2D.h b/src/constraint_pose_2D.h
index 7ddc7478c6d5b40d51a239c467063bd14d9fb3d1..652c1c3777887c8fe37b1c9d057a52b3b3d09471 100644
--- a/src/constraint_pose_2D.h
+++ b/src/constraint_pose_2D.h
@@ -19,9 +19,8 @@ class ConstraintPose2D: public ConstraintAutodiff<ConstraintPose2D,3,2,1>
{
public:
ConstraintPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPose2D, 3, 2, 1>(CTR_POSE_2D, nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintAutodiff<ConstraintPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
- setType("POSE 2D");
// std::cout << "created ConstraintPose2D " << std::endl;
}
diff --git a/src/constraint_pose_3D.h b/src/constraint_pose_3D.h
index 31221af3e35a047a73808c35a724a0ce526dc2a1..8823eefb76f4ed40277b652632ed086b262d3074 100644
--- a/src/constraint_pose_3D.h
+++ b/src/constraint_pose_3D.h
@@ -18,9 +18,9 @@ class ConstraintPose3D: public ConstraintAutodiff<ConstraintPose3D,6,3,4>
public:
ConstraintPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPose3D,6,3,4>(CTR_POSE_3D, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintAutodiff<ConstraintPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
- setType("FIX3D");
+ //
}
virtual ~ConstraintPose3D() = default;
diff --git a/src/constraint_quaternion_absolute.h b/src/constraint_quaternion_absolute.h
index 8848154618d1b5d24ecfccbee63713743994d901..1b147a03df6a951507a71c8405f1e495d57db76c 100644
--- a/src/constraint_quaternion_absolute.h
+++ b/src/constraint_quaternion_absolute.h
@@ -25,9 +25,10 @@ class ConstraintQuaternionAbsolute: public ConstraintAutodiff<ConstraintQuaterni
public:
ConstraintQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintQuaternionAbsolute,3,4>(CTR_BLOCK_ABS, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
+ ConstraintAutodiff<ConstraintQuaternionAbsolute,3,4>("QUATERNION ABS",
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
{
- setType("FIX Q");
+ //
}
virtual ~ConstraintQuaternionAbsolute() = default;
diff --git a/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h
index 5d2aadb7e3b22a13056588c0540ad9d89d97ea8d..32b8b30593a73717be3fccc4459652da17dce616 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/constraint_relative_2D_analytic.h
@@ -17,8 +17,8 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/** \brief Constructor of category CTR_FRAME
**/
- ConstraintRelative2DAnalytic(const FeatureBasePtr& _ftr_ptr,
- const ConstraintType& _tp,
+ ConstraintRelative2DAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
@@ -30,8 +30,8 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/** \brief Constructor of category CTR_FEATURE
**/
- ConstraintRelative2DAnalytic(const FeatureBasePtr& _ftr_ptr,
- const ConstraintType& _tp,
+ ConstraintRelative2DAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
@@ -43,8 +43,8 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/** \brief Constructor of category CTR_LANDMARK
**/
- ConstraintRelative2DAnalytic(const FeatureBasePtr& _ftr_ptr,
- const ConstraintType& _tp,
+ ConstraintRelative2DAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
diff --git a/src/constraints/constraint_autodiff_distance_3D.h b/src/constraints/constraint_autodiff_distance_3D.h
index 2b627b8d009024d1df1d90e1882406bff666b1df..a77e3880fa5d7a85a4a18e74b1b0a2e05aaed2dd 100644
--- a/src/constraints/constraint_autodiff_distance_3D.h
+++ b/src/constraints/constraint_autodiff_distance_3D.h
@@ -23,7 +23,7 @@ class ConstraintAutodiffDistance3D : public ConstraintAutodiff<ConstraintAutodif
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
- ConstraintAutodiff(CTR_DISTANCE_3D,
+ ConstraintAutodiff("DISTANCE 3D",
_frm_other,
nullptr,
nullptr,
@@ -34,7 +34,6 @@ class ConstraintAutodiffDistance3D : public ConstraintAutodiff<ConstraintAutodif
_feat->getFramePtr()->getPPtr(),
_frm_other->getPPtr())
{
- setType("DISTANCE 3D");
setFeaturePtr(_feat);
}
@@ -55,16 +54,7 @@ class ConstraintAutodiffDistance3D : public ConstraintAutodiff<ConstraintAutodif
Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
-// WOLF_DEBUG("pos1: ", pos1);
-// WOLF_DEBUG("pos2: ", pos2);
-// WOLF_DEBUG("vect: ", pos2-pos1);
-// WOLF_DEBUG("exp dist: ", dist_exp);
-// WOLF_DEBUG("meas dist: ", dist_meas);
-// WOLF_DEBUG("error: ", dist_meas - dist_exp);
-// WOLF_DEBUG("sqrt info: ", sqrt_info_upper);
-
res = sqrt_info_upper * (dist_meas - dist_exp);
-// WOLF_DEBUG("residual: ", res);
return true;
}
diff --git a/src/constraints/constraint_autodiff_trifocal.h b/src/constraints/constraint_autodiff_trifocal.h
index 26f6aca038d1a90a99e4106f46ffcb9c89217cc3..405066042531bd8f9b508830d42141371470ad4c 100644
--- a/src/constraints/constraint_autodiff_trifocal.h
+++ b/src/constraints/constraint_autodiff_trifocal.h
@@ -135,7 +135,7 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
const FeatureBasePtr& _feature_last_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- ConstraintStatus _status) : ConstraintAutodiff( CTR_TRIFOCAL_PLP,
+ ConstraintStatus _status) : ConstraintAutodiff( "TRIFOCAL PLP",
nullptr,
nullptr,
_feature_origin_ptr,
@@ -155,7 +155,6 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensorPtr())),
sqrt_information_upper(Matrix3s::Zero())
{
- setType("AUTODIFF TRIFOCAL");
setFeaturePtr(_feature_last_ptr);
Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
pixel_canonical_prev_ = K_inv * Vector3s(_feature_prev_ptr ->getMeasurement(0), _feature_prev_ptr ->getMeasurement(1), 1.0);
diff --git a/src/constraints/constraint_diff_drive.h b/src/constraints/constraint_diff_drive.h
index d67e22c79d8c6d33a48b0bce6c3d76f4a41fff4b..8d6a898a04faf8e8c2fd22481d4e7308a1513255 100644
--- a/src/constraints/constraint_diff_drive.h
+++ b/src/constraints/constraint_diff_drive.h
@@ -44,7 +44,7 @@ public:
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
const ConstraintStatus _status = CTR_ACTIVE) :
- Base(CTR_DIFF_DRIVE, _capture_origin_ptr->getFramePtr(), _capture_origin_ptr,
+ Base("DIFF DRIVE", _capture_origin_ptr->getFramePtr(), _capture_origin_ptr,
nullptr, nullptr, _processor_ptr,
_apply_loss_function, _status,
_frame_ptr->getPPtr(), _frame_ptr->getOPtr(),
@@ -58,7 +58,7 @@ public:
_ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
J_delta_calib_(_ftr_ptr->getJacobianFactor())
{
- setType("DIFF DRIVE");
+ //
}
/**
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/constraint_bearing.h b/src/hello_wolf/constraint_bearing.h
index 3b34b6e1afe353f04e5630e910bd5de7c8ebd018..1ce1c7d783d84333fa8ae68894a86ab1aa565d42 100644
--- a/src/hello_wolf/constraint_bearing.h
+++ b/src/hello_wolf/constraint_bearing.h
@@ -21,14 +21,13 @@ class ConstraintBearing : public ConstraintAutodiff<ConstraintBearing, 1, 2, 1,
ConstraintBearing(const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintBearing, 1, 2, 1, 2>(CTR_BEARING_2D, nullptr, nullptr, nullptr,
+ ConstraintAutodiff<ConstraintBearing, 1, 2, 1, 2>("BEARING", nullptr, nullptr, nullptr,
_landmark_other_ptr, _processor_ptr,
_apply_loss_function, _status,
getCapturePtr()->getFramePtr()->getPPtr(),
getCapturePtr()->getFramePtr()->getOPtr(),
_landmark_other_ptr->getPPtr())
{
- setType("BEARING");
//
}
diff --git a/src/hello_wolf/constraint_range_bearing.h b/src/hello_wolf/constraint_range_bearing.h
index cb79d2ee7a6a097393d124de6f3c60a58b5d6f56..89b5137862da7ffe4e12fac7b7aa5942bda2968c 100644
--- a/src/hello_wolf/constraint_range_bearing.h
+++ b/src/hello_wolf/constraint_range_bearing.h
@@ -26,7 +26,7 @@ class ConstraintRangeBearing : public ConstraintAutodiff<ConstraintRangeBearing,
bool _apply_loss_function, // apply loss function to residual?
ConstraintStatus _status) : // active constraint?
ConstraintAutodiff<ConstraintRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
- CTR_BEARING_2D, // constraint type enum (see wolf.h)
+ "RANGE BEARING", // constraint type enum (see wolf.h)
nullptr, // other frame's pointer
nullptr, // other capture's pointer
nullptr, // other feature's pointer
@@ -40,7 +40,7 @@ class ConstraintRangeBearing : public ConstraintAutodiff<ConstraintRangeBearing,
_capture_own->getSensorPtr()->getOPtr(), // sensor orientation state block
_landmark_other_ptr->getPPtr()) // landmark position state block
{
- setType("RANGE BEARING"); // constraint type text (for eventual ConstraintFactory and visualization)
+ //
}
virtual ~ConstraintRangeBearing()
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/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp
index e0f2a6cfbf12b13af7bfe291f08d7a5848dfd60d..bc1bcc71abedb342e2003498844593db8e538b36 100644
--- a/src/landmark_polyline_2D.cpp
+++ b/src/landmark_polyline_2D.cpp
@@ -240,9 +240,12 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
ConstraintBasePtr new_ctr_ptr = nullptr;
for (auto ctr_ptr : old_constraints_list)
{
- if (ctr_ptr->getTypeId() == CTR_POINT_2D)
+ ConstraintPoint2DPtr ctr_point_ptr;
+ ConstraintPointToLine2DPtr ctr_point_line_ptr;
+ if ( (ctr_point_ptr = std::dynamic_pointer_cast<ConstraintPoint2D>(ctr_ptr)))
+// if (ctr_ptr->getTypeId() == CTR_POINT_2D)
{
- ConstraintPoint2DPtr ctr_point_ptr = std::static_pointer_cast<ConstraintPoint2D>(ctr_ptr);
+// ConstraintPoint2DPtr ctr_point_ptr = std::static_pointer_cast<ConstraintPoint2D>(ctr_ptr);
// If landmark point constrained -> new constraint
if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
@@ -254,30 +257,31 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
ctr_point_ptr->getApplyLossFunction(),
ctr_point_ptr->getStatus());
}
- else if (ctr_ptr->getTypeId() == CTR_POINT_TO_LINE_2D)
+ else if ((ctr_point_line_ptr = std::dynamic_pointer_cast<ConstraintPointToLine2D>(ctr_ptr)))
+// else if (ctr_ptr->getTypeId() == CTR_POINT_TO_LINE_2D)
{
- ConstraintPointToLine2DPtr ctr_point_ptr = std::static_pointer_cast<ConstraintPointToLine2D>(ctr_ptr);
+// ConstraintPointToLine2DPtr ctr_point_line_ptr = std::static_pointer_cast<ConstraintPointToLine2D>(ctr_ptr);
// If landmark point constrained -> new constraint
- if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
+ if (ctr_point_line_ptr->getLandmarkPointId() == _remove_id)
new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_ptr->getProcessor(),
- ctr_point_ptr->getFeaturePointId(),
+ ctr_point_line_ptr->getProcessor(),
+ ctr_point_line_ptr->getFeaturePointId(),
_remain_id,
- ctr_point_ptr->getLandmarkPointAuxId(),
+ ctr_point_line_ptr->getLandmarkPointAuxId(),
ctr_point_ptr->getApplyLossFunction(),
- ctr_point_ptr->getStatus());
+ ctr_point_line_ptr->getStatus());
// If landmark point is aux point -> new constraint
- else if (ctr_point_ptr->getLandmarkPointAuxId() == _remove_id)
+ else if (ctr_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_ptr->getProcessor(),
- ctr_point_ptr->getFeaturePointId(),
- ctr_point_ptr->getLandmarkPointId(),
+ ctr_point_line_ptr->getProcessor(),
+ ctr_point_line_ptr->getFeaturePointId(),
+ ctr_point_line_ptr->getLandmarkPointId(),
_remain_id,
- ctr_point_ptr->getApplyLossFunction(),
- ctr_point_ptr->getStatus());
+ ctr_point_line_ptr->getApplyLossFunction(),
+ ctr_point_line_ptr->getStatus());
}
else
throw std::runtime_error ("polyline constraint of unknown type");
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 97a76cfdd0c682eda3403cf7d816f5ce51d5e43c..6fd47ad622d9a5754e5525041ebcd138c6229241 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -195,67 +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 all possible constraints
- *
- * You may add items to this list as needed. Be concise with names, and document your entries.
- */
-typedef enum
-{
- CTR_GPS_FIX_2D = 1, ///< 2D GPS Fix constraint.
- CTR_GPS_PR_2D, ///< 2D GPS Pseudorange constraint.
- CTR_GPS_PR_3D, ///< 3D GPS Pseudorange constraint.
- CTR_POSE_2D, ///< Pose constraint (for priors) in 2D.
- CTR_POSE_3D, ///< Pose constraint (for priors) in 3D.
- CTR_FIX_BIAS, ///< Fix constraint (for priors) on bias.
- CTR_ODOM_2D, ///< 2D Odometry constraint .
- CTR_ODOM_3D, ///< 3D Odometry constraint .
- CTR_CORNER_2D, ///< 2D corner constraint .
- CTR_POINT_2D, ///< 2D point constraint .
- CTR_POINT_TO_LINE_2D, ///< 2D point constraint .
- CTR_CONTAINER, ///< 2D container constraint .
- CTR_IMG_PNT_TO_EP, ///< constraint from a image point to a Euclidean 3D point landmark (EP). See https://hal.archives-ouvertes.fr/hal-00451778/document
- CTR_IMG_PNT_TO_HP, ///< constraint from a image point to a Homogeneous 3D point landmark (HP). See https://hal.archives-ouvertes.fr/hal-00451778/document
- CTR_EPIPOLAR, ///< Epipolar constraint
- CTR_AHP, ///< Anchored Homogeneous Point constraint
- CTR_AHP_NL, ///< Anchored Homogeneous Point constraint (temporal, to be removed)
- CTR_IMU, ///< IMU constraint
- CTR_DIFF_DRIVE, ///< Diff-drive constraint
- CTR_BEARING_2D, ///< 2D bearing
- CTR_BLOCK_ABS, ///< absolute constraint to Poisition or Velocity depending on argument StateBlockPtr (for priors)
- CTR_TRIFOCAL_PLP, ///< Trifocal tensor constraint of the type point-line-point
- CTR_DISTANCE_3D ///< Distance between two 3D frames
-} ConstraintType;
-
-/** \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
/////////////////////////////////////////////////////////////////////////