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/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/constraint_AHP.h b/src/constraint_AHP.h
index a63810f6bdba3220340560f50f9d3cac6e22b020..b73113cd8f791ee360d4a017285ba61a9f460ec5 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();
}
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..ef9bc5ef09616410282f53b6f0eeb35a38476af8 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();
diff --git a/src/constraint_IMU.h b/src/constraint_IMU.h
index 625e7fd85acc09503ecc699dce8c97657db02d9e..a47992f9e500655bbb571ba1fd67b5b59cb50817 100644
--- a/src/constraint_IMU.h
+++ b/src/constraint_IMU.h
@@ -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");
+ //
}
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..fd0f08899f59171ab0073713408b991a54e065e4 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),
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 534cf8537c54b3ec3b5d92e3435fa3c2532d9ada..bb33d208134a7241f1378f1630f4a3c5998fd62c 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -26,7 +26,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 +38,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 +59,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;
@@ -205,21 +200,11 @@ 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_;
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..0f048c7ac841979456f330caa878424581f640ef 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -58,9 +58,9 @@ 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,
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..401c5d4d8c75beb9431058f176952c191ca4d20f 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;
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index dd35bfec72dd5dd239437afa6586e9856e62a3bb..bfd6aaf387670dc2aa10a1b8526a313b216d628c 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;
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index d5d820e2c4f1a800142ae22f85ad9aa474b082bc..b5fce1bf8f3ca5403272c87143f160cdd2a5b0b3 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(), ")");
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/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/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/wolf.h b/src/wolf.h
index 97a76cfdd0c682eda3403cf7d816f5ce51d5e43c..424cd92953c71dc44f8d0fea3c19a1b2ccbf1cc8 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -203,37 +203,6 @@ typedef enum
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.