diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index ce6bb49fe75e5200c1824f7f496e6f96d9c55131..5c958ba175327d9ae30061ec71605996561505f4 100644
--- a/hello_wolf/factor_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -5,8 +5,8 @@
* Author: jsola
*/
-#ifndef HELLO_WOLF_CONSTRAINT_BEARING_H_
-#define HELLO_WOLF_CONSTRAINT_BEARING_H_
+#ifndef HELLO_WOLF_FACTOR_BEARING_H_
+#define HELLO_WOLF_FACTOR_BEARING_H_
#include "base/factor/factor_autodiff.h"
@@ -88,4 +88,4 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
} // namespace wolf
-#endif /* HELLO_WOLF_CONSTRAINT_BEARING_H_ */
+#endif /* HELLO_WOLF_FACTOR_BEARING_H_ */
diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h
index 727e75a4e07ef3b696bb4075f016677d307320ba..1414d61a20252cd76402af416e4a484812d57c8a 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -5,8 +5,8 @@
* \author: jsola
*/
-#ifndef HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
-#define HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
+#ifndef HELLO_WOLF_FACTOR_RANGE_BEARING_H_
+#define HELLO_WOLF_FACTOR_RANGE_BEARING_H_
#include "base/factor/factor_autodiff.h"
@@ -132,4 +132,4 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi
}
} // namespace wolf
-#endif /* HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_ */
+#endif /* HELLO_WOLF_FACTOR_RANGE_BEARING_H_ */
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 3dcf4bf7fd79014729137b1d75865892662a4e24..1bfd9684616393396492819f07ed8cf7aad8c907 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -89,7 +89,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
lmk,
prc,
false,
- CTR_ACTIVE);
+ FAC_ACTIVE);
ftr->addFactor(ctr);
lmk->addConstrainedBy(ctr);
}
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index f9e4abe9e1fff5b6852e12bbb5acaf196b6ca11f..7643f243ea4b8a05e0acdad82eb4454a8e87a6c4 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -66,13 +66,13 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
FeatureBasePtrList& getFeatureList();
void addFeatureList(FeatureBasePtrList& _new_ft_list);
- void getFactorList(FactorBasePtrList& _ctr_list);
+ void getFactorList(FactorBasePtrList& _fac_list);
SensorBasePtr getSensor() const;
virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
// constrained by
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _ctr_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index 5cdd70066f2ebae6791d6d92dc146601985066f8..c15355fbd0c2cdaae9ea1c39c7032bb311b3b3e9 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -28,8 +28,8 @@ class CeresManager : public SolverManager
{
protected:
- std::map<FactorBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
- std::map<FactorBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
+ std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
+ std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
@@ -71,9 +71,9 @@ class CeresManager : public SolverManager
std::string solveImpl(const ReportVerbosity report_level) override;
- void addFactor(const FactorBasePtr& ctr_ptr) override;
+ void addFactor(const FactorBasePtr& fac_ptr) override;
- void removeFactor(const FactorBasePtr& ctr_ptr) override;
+ void removeFactor(const FactorBasePtr& fac_ptr) override;
void addStateBlock(const StateBlockPtr& state_ptr) override;
@@ -83,7 +83,7 @@ class CeresManager : public SolverManager
void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) override;
- ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _ctr_ptr);
+ ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
};
inline ceres::Solver::Summary CeresManager::getSummary()
diff --git a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
index 967cf1d5ea73fd4692ffe603372868857cf915c7..fafa62aae9d0fae3f0a26e4bc08816dc33b9e73b 100644
--- a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -28,17 +28,17 @@ std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSiz
T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_factor_ptr).get());
};
-inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _ctr_ptr)
+inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _fac_ptr)
{
-// switch (_ctr_ptr->getTypeId())
+// switch (_fac_ptr->getTypeId())
// {
// // just for testing
-// case CTR_ODOM_2D:
-// return createNumericDiffCostFunctionCeres<FactorOdom2D>(_ctr_ptr);
+// case FAC_ODOM_2D:
+// return createNumericDiffCostFunctionCeres<FactorOdom2D>(_fac_ptr);
//
// /* For adding a new factor, add the #include and a case:
-// case CTR_ENUM:
-// return createNumericDiffCostFunctionCeres<FactorType>(_ctr_ptr);
+// case FAC_ENUM:
+// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
// */
//
// default:
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
index 663cc5417167708648fb82ea929681c367fbbbe2..d4945e066a372d5ec95578a87552e4369eec1b2e 100644
--- a/include/base/ceres_wrapper/qr_manager.h
+++ b/include/base/ceres_wrapper/qr_manager.h
@@ -22,7 +22,7 @@ class QRManager : public SolverManager
Eigen::SparseMatrixs A_;
Eigen::VectorXs b_;
std::map<StateBlockPtr, unsigned int> sb_2_col_;
- std::map<FactorBasePtr, unsigned int> ctr_2_row_;
+ std::map<FactorBasePtr, unsigned int> fac_2_row_;
bool any_state_block_removed_;
unsigned int new_state_blocks_;
unsigned int N_batch_;
@@ -44,9 +44,9 @@ class QRManager : public SolverManager
bool computeDecomposition();
- virtual void addFactor(FactorBasePtr _ctr_ptr);
+ virtual void addFactor(FactorBasePtr _fac_ptr);
- virtual void removeFactor(FactorBasePtr _ctr_ptr);
+ virtual void removeFactor(FactorBasePtr _fac_ptr);
virtual void addStateBlock(StateBlockPtr _st_ptr);
@@ -54,7 +54,7 @@ class QRManager : public SolverManager
virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
- void relinearizeFactor(FactorBasePtr _ctr_ptr);
+ void relinearizeFactor(FactorBasePtr _fac_ptr);
};
} /* namespace wolf */
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
index 10a97fb6ef0624ec93cc8599a96709fc90877a43..7252c409d2bf0c503e06b6fe74dec895e3754150 100644
--- a/include/base/ceres_wrapper/solver_manager.h
+++ b/include/base/ceres_wrapper/solver_manager.h
@@ -48,9 +48,9 @@ class SolverManager
private:
- virtual void addFactor(FactorBasePtr _ctr_ptr) = 0;
+ virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
- virtual void removeFactor(FactorBasePtr _ctr_ptr) = 0;
+ virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
diff --git a/include/base/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
index 33a1de8e2e4a16a565f323be6e815f8a0ab25a0b..a6448b06458658949bff014aa93da139f74f0e83 100644
--- a/include/base/factor/factor_AHP.h
+++ b/include/base/factor/factor_AHP.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_AHP_H
-#define CONSTRAINT_AHP_H
+#ifndef FACTOR_AHP_H
+#define FACTOR_AHP_H
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -29,7 +29,7 @@ class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
const LandmarkAHPPtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorAHP() = default;
@@ -56,7 +56,7 @@ class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
const LandmarkAHPPtr& _lmk_ahp_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
};
@@ -190,11 +190,11 @@ inline FactorAHPPtr FactorAHP::create(const FeatureBasePtr& _ftr_ptr,
FactorStatus _status)
{
// construct factor
- FactorAHPPtr ctr_ahp = std::make_shared<FactorAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
+ FactorAHPPtr fac_ahp = std::make_shared<FactorAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
- return ctr_ahp;
+ return fac_ahp;
}
} // namespace wolf
-#endif // CONSTRAINT_AHP_H
+#endif // FACTOR_AHP_H
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
index b34e7c06ebf81628ca7423357495fbb69fa11b21..a3ce0e60979b6241fbf2d4f871543764430c8015 100644
--- a/include/base/factor/factor_GPS_2D.h
+++ b/include/base/factor/factor_GPS_2D.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_GPS_2D_H_
-#define CONSTRAINT_GPS_2D_H_
+#ifndef FACTOR_GPS_2D_H_
+#define FACTOR_GPS_2D_H_
//Wolf includes
#include "base/wolf.h"
@@ -15,7 +15,7 @@ class FactorGPS2D : public FactorAutodiff<FactorGPS2D, 2, 2>
{
public:
- FactorGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ FactorGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP())
{
//
diff --git a/include/base/factor/factor_GPS_pseudorange_2D.h b/include/base/factor/factor_GPS_pseudorange_2D.h
index 40b8a5115392bcfbce961f45e6abde8dbb92417e..82b186fc65e43f124bf1fd9e4f905b4d14928379 100644
--- a/include/base/factor/factor_GPS_pseudorange_2D.h
+++ b/include/base/factor/factor_GPS_pseudorange_2D.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_GPS_PSEUDORANGE_2D_H_
-#define CONSTRAINT_GPS_PSEUDORANGE_2D_H_
+#ifndef FACTOR_GPS_PSEUDORANGE_2D_H_
+#define FACTOR_GPS_PSEUDORANGE_2D_H_
#define LIGHT_SPEED_ 299792458
@@ -29,7 +29,7 @@ class FactorGPSPseudorange2D : public FactorAutodiff<FactorGPSPseudorange2D, 1,
public:
FactorGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
nullptr,
nullptr,
@@ -238,4 +238,4 @@ inline bool FactorGPSPseudorange2D::operator ()(const T* const _vehicle_p, const
} // namespace wolf
-#endif //CONSTRAINT_GPS_PSEUDORANGE_2D_H_
+#endif //FACTOR_GPS_PSEUDORANGE_2D_H_
diff --git a/include/base/factor/factor_GPS_pseudorange_3D.h b/include/base/factor/factor_GPS_pseudorange_3D.h
index d7a33681c7fde08bea6e2d086aa30364149eba2e..72e44d4f62f55358e33cf2ebb14254ca532f1bd3 100644
--- a/include/base/factor/factor_GPS_pseudorange_3D.h
+++ b/include/base/factor/factor_GPS_pseudorange_3D.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_GPS_PSEUDORANGE_3D_H_
-#define CONSTRAINT_GPS_PSEUDORANGE_3D_H_
+#ifndef FACTOR_GPS_PSEUDORANGE_3D_H_
+#define FACTOR_GPS_PSEUDORANGE_3D_H_
#define LIGHT_SPEED 299792458
@@ -28,7 +28,7 @@ class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3
FactorGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
_ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to map frame
@@ -138,4 +138,4 @@ inline bool FactorGPSPseudorange3D::operator ()(const T* const _vehicle_p, const
} // namespace wolf
-#endif //CONSTRAINT_GPS_PSEUDORANGE_3D_H_
+#endif //FACTOR_GPS_PSEUDORANGE_3D_H_
diff --git a/include/base/factor/factor_IMU.h b/include/base/factor/factor_IMU.h
index c1b9ba65a9026bed9b9f4ca1a32365b02bbcac0a..fbb29d1642e91d9fe90f55da673f3b8a87b6c335 100644
--- a/include/base/factor/factor_IMU.h
+++ b/include/base/factor/factor_IMU.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_IMU_THETA_H_
-#define CONSTRAINT_IMU_THETA_H_
+#ifndef FACTOR_IMU_THETA_H_
+#define FACTOR_IMU_THETA_H_
//Wolf includes
#include "base/feature/feature_IMU.h"
@@ -21,7 +21,7 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
const CaptureIMUPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorIMU() = default;
diff --git a/include/base/factor/factor_analytic.h b/include/base/factor/factor_analytic.h
index 0404b2a97f122cd66d1dbbc6fb04e5221a342d81..14604bdb21ff2d0ed8d13b0d5fcf55055a637bf7 100644
--- a/include/base/factor/factor_analytic.h
+++ b/include/base/factor/factor_analytic.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_ANALYTIC_H_
-#define CONSTRAINT_ANALYTIC_H_
+#ifndef FACTOR_ANALYTIC_H_
+#define FACTOR_ANALYTIC_H_
//Wolf includes
#include "base/factor/factor_base.h"
@@ -18,9 +18,9 @@ class FactorAnalytic: public FactorBase
public:
- /** \brief Constructor of category CTR_ABSOLUTE
+ /** \brief Constructor of category FAC_ABSOLUTE
*
- * Constructor of category CTR_ABSOLUTE
+ * Constructor of category FAC_ABSOLUTE
*
**/
FactorAnalytic(const std::string& _tp,
diff --git a/include/base/factor/factor_autodiff.h b/include/base/factor/factor_autodiff.h
index f46be9e4ffd7840802ddefe5f4b14bdb7ff97c34..be35c772aacb9c7fce4e780d413fa0df675c1655 100644
--- a/include/base/factor/factor_autodiff.h
+++ b/include/base/factor/factor_autodiff.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_AUTODIFF_H_
-#define CONSTRAINT_AUTODIFF_H_
+#ifndef FACTOR_AUTODIFF_H_
+#define FACTOR_AUTODIFF_H_
//Wolf includes
#include "base/factor/factor_base.h"
@@ -15,7 +15,7 @@
namespace wolf {
//template class FactorAutodiff
-template <class CtrT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
+template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
class FactorAutodiff : public FactorBase
{
public:
@@ -59,23 +59,23 @@ class FactorAutodiff : public FactorBase
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -144,7 +144,7 @@ class FactorAutodiff : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -165,7 +165,7 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -241,7 +241,7 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -301,8 +301,8 @@ class FactorAutodiff : public FactorBase
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
{
public:
@@ -344,22 +344,22 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -420,7 +420,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -440,7 +440,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -507,7 +507,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -553,8 +553,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
{
public:
@@ -595,21 +595,21 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -666,7 +666,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -685,7 +685,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -749,7 +749,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -794,8 +794,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
{
public:
@@ -835,20 +835,20 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -901,7 +901,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -919,7 +919,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -980,7 +980,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1024,8 +1024,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
{
public:
@@ -1064,19 +1064,19 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -1125,7 +1125,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -1142,7 +1142,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1200,7 +1200,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1239,8 +1239,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
{
public:
@@ -1277,18 +1277,18 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -1333,7 +1333,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -1349,7 +1349,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1404,7 +1404,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1442,8 +1442,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1479,17 +1479,17 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -1530,7 +1530,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -1545,7 +1545,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1597,7 +1597,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1638,8 +1638,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1674,16 +1674,16 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -1720,7 +1720,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
residuals);
@@ -1734,7 +1734,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
residuals_jets_->data());
@@ -1783,7 +1783,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
residuals_jets_->data());
@@ -1823,8 +1823,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
-class FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1858,15 +1858,15 @@ class FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -1899,7 +1899,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
residuals);
}
@@ -1912,7 +1912,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
residuals_jets_->data());
@@ -1958,7 +1958,7 @@ class FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
residuals_jets_->data());
@@ -1997,8 +1997,8 @@ class FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0>
-class FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
+template <class FacT,unsigned int RES,unsigned int B0>
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -2031,14 +2031,14 @@ class FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(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,
- FactorStatus _status,
- StateBlockPtr _state0Ptr) :
+ 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,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
@@ -2067,7 +2067,7 @@ class FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
residuals);
}
// also compute jacobians
@@ -2079,7 +2079,7 @@ class FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
residuals_jets_->data());
// fill the residual array
@@ -2122,7 +2122,7 @@ class FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
residuals_jets_->data());
// fill the residual vector
@@ -2163,112 +2163,112 @@ class FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
// 10 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
// 9 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
// 8 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
// 7 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
// 6 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
// 5 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
// 4 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
// 3 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
// 2 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
// 1 BLOCK
-template <class CtrT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
+template <class FacT,unsigned int RES,unsigned int B0>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
// 10 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
-// 9 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
-// 8 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
-// 7 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
-// 6 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
B0,
B0+B1,
B0+B1+B2,
B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
-// 5 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
+// 9 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
B0,
B0+B1,
B0+B1+B2,
- B0+B1+B2+B3};
-// 4 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
+// 8 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
B0,
B0+B1,
- B0+B1+B2};
-// 3 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
+// 7 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
B0,
- B0+B1};
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
+// 6 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
+// 5 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
+// 4 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
+// 3 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
// 2 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
// 1 BLOCK
-template <class CtrT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+template <class FacT,unsigned int RES,unsigned int B0>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
index bc957f3cfc303d10b2ba4d53df1bbdd79d35ff82..0eb1fc7570494b4731656034c9fc56025516c9af 100644
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -5,8 +5,8 @@
* \author: jsola
*/
-#ifndef CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
-#define CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
+#ifndef FACTOR_AUTODIFF_DISTANCE_3D_H_
+#define FACTOR_AUTODIFF_DISTANCE_3D_H_
#include "base/factor/factor_autodiff.h"
@@ -19,20 +19,20 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
{
public:
FactorAutodiffDistance3D(const FeatureBasePtr& _feat,
- const FrameBasePtr& _frm_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("DISTANCE 3D",
- _frm_other,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getP(),
- _frm_other->getP())
+ const FrameBasePtr& _frm_other,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff("DISTANCE 3D",
+ _frm_other,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feat->getFrame()->getP(),
+ _frm_other->getP())
{
setFeaturePtr(_feat);
}
@@ -41,8 +41,8 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
template<typename T>
bool operator () (const T* const _pos1,
- const T* const _pos2,
- T* _residual) const
+ const T* const _pos2,
+ T* _residual) const
{
using namespace Eigen;
@@ -62,4 +62,4 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
} /* namespace wolf */
-#endif /* CONSTRAINT_AUTODIFF_DISTANCE_3D_H_ */
+#endif /* FACTOR_AUTODIFF_DISTANCE_3D_H_ */
diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
index 5e8c59e2bed453630a2d3ac6c42034412a2c6906..b7e56e0ac17b15d857f05522f42f911de3668291 100644
--- a/include/base/factor/factor_autodiff_trifocal.h
+++ b/include/base/factor/factor_autodiff_trifocal.h
@@ -1,5 +1,5 @@
-#ifndef _CONSTRAINT_AUTODIFF_TRIFOCAL_H_
-#define _CONSTRAINT_AUTODIFF_TRIFOCAL_H_
+#ifndef _FACTOR_AUTODIFF_TRIFOCAL_H_
+#define _FACTOR_AUTODIFF_TRIFOCAL_H_
//Wolf includes
//#include "base/wolf.h"
@@ -23,11 +23,11 @@ class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3,
/** \brief Class constructor
*/
FactorAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
- const FeatureBasePtr& _feature_origin_ptr,
- const FeatureBasePtr& _feature_last_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status);
+ const FeatureBasePtr& _feature_origin_ptr,
+ const FeatureBasePtr& _feature_last_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status);
/** \brief Class Destructor
*/
@@ -126,31 +126,31 @@ namespace wolf
using namespace Eigen;
// Constructor
-FactorAutodiffTrifocal::FactorAutodiffTrifocal(
- const FeatureBasePtr& _feature_prev_ptr,
- const FeatureBasePtr& _feature_origin_ptr,
- const FeatureBasePtr& _feature_last_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) : FactorAutodiff( "TRIFOCAL PLP",
- nullptr,
- nullptr,
- _feature_origin_ptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feature_prev_ptr->getFrame()->getP(),
- _feature_prev_ptr->getFrame()->getO(),
- _feature_origin_ptr->getFrame()->getP(),
- _feature_origin_ptr->getFrame()->getO(),
- _feature_last_ptr->getFrame()->getP(),
- _feature_last_ptr->getFrame()->getO(),
- _feature_last_ptr->getCapture()->getSensorP(),
- _feature_last_ptr->getCapture()->getSensorO() ),
- feature_prev_ptr_(_feature_prev_ptr),
- camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
- sqrt_information_upper(Matrix3s::Zero())
+FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
+ const FeatureBasePtr& _feature_origin_ptr,
+ const FeatureBasePtr& _feature_last_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff( "TRIFOCAL PLP",
+ nullptr,
+ nullptr,
+ _feature_origin_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feature_prev_ptr->getFrame()->getP(),
+ _feature_prev_ptr->getFrame()->getO(),
+ _feature_origin_ptr->getFrame()->getP(),
+ _feature_origin_ptr->getFrame()->getO(),
+ _feature_last_ptr->getFrame()->getP(),
+ _feature_last_ptr->getFrame()->getO(),
+ _feature_last_ptr->getCapture()->getSensorP(),
+ _feature_last_ptr->getCapture()->getSensorO() ),
+ feature_prev_ptr_(_feature_prev_ptr),
+ camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
+ sqrt_information_upper(Matrix3s::Zero())
{
setFeaturePtr(_feature_last_ptr);
Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
@@ -216,14 +216,14 @@ inline FeatureBasePtr FactorAutodiffTrifocal::getFeaturePrev()
template<typename T>
bool FactorAutodiffTrifocal::operator ()( const T* const _prev_pos,
- const T* const _prev_quat,
- const T* const _origin_pos,
- const T* const _origin_quat,
- const T* const _last_pos,
- const T* const _last_quat,
- const T* const _sen_pos,
- const T* const _sen_quat,
- T* _residuals) const
+ const T* const _prev_quat,
+ const T* const _origin_pos,
+ const T* const _origin_quat,
+ const T* const _last_pos,
+ const T* const _last_quat,
+ const T* const _sen_pos,
+ const T* const _sen_quat,
+ T* _residuals) const
{
// MAPS
@@ -246,15 +246,15 @@ bool FactorAutodiffTrifocal::operator ()( const T* const _prev_pos,
template<typename D1, typename D2, class T, typename D3>
inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
- const QuaternionBase<D2>& _wqr1,
- const MatrixBase<D1>& _wtr2,
- const QuaternionBase<D2>& _wqr2,
- const MatrixBase<D1>& _wtr3,
- const QuaternionBase<D2>& _wqr3,
- const MatrixBase<D1>& _rtc,
- const QuaternionBase<D2>& _rqc,
- vision_utils::TrifocalTensorBase<T>& _tensor,
- MatrixBase<D3>& _c2Ec1) const
+ const QuaternionBase<D2>& _wqr1,
+ const MatrixBase<D1>& _wtr2,
+ const QuaternionBase<D2>& _wqr2,
+ const MatrixBase<D1>& _wtr3,
+ const QuaternionBase<D2>& _wqr3,
+ const MatrixBase<D1>& _rtc,
+ const QuaternionBase<D2>& _rqc,
+ vision_utils::TrifocalTensorBase<T>& _tensor,
+ MatrixBase<D3>& _c2Ec1) const
{
typedef Translation<T, 3> TranslationType;
@@ -317,7 +317,7 @@ inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
template<typename T, typename D1>
inline Matrix<T, 3, 1> FactorAutodiffTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1) const
+ const MatrixBase<D1>& _c2Ec1) const
{
// 1. COMMON COMPUTATIONS
@@ -347,10 +347,10 @@ inline Matrix<T, 3, 1> FactorAutodiffTrifocal::residual(const vision_utils::Trif
// Helper functions to be used by the above
template<class T, typename D1, typename D2, typename D3, typename D4>
inline Matrix<T, 3, 1> FactorAutodiffTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1,
- MatrixBase<D2>& _J_e_m1,
- MatrixBase<D3>& _J_e_m2,
- MatrixBase<D4>& _J_e_m3)
+ const MatrixBase<D1>& _c2Ec1,
+ MatrixBase<D2>& _J_e_m1,
+ MatrixBase<D3>& _J_e_m2,
+ MatrixBase<D4>& _J_e_m3)
{
// 1. COMMON COMPUTATIONS
@@ -394,4 +394,4 @@ inline Matrix<T, 3, 1> FactorAutodiffTrifocal::error_jacobians(const vision_util
} // namespace wolf
-#endif /* _CONSTRAINT_AUTODIFF_TRIFOCAL_H_ */
+#endif /* _FACTOR_AUTODIFF_TRIFOCAL_H_ */
diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h
index 6f91a034ab15d6b0cb4ff4da24d6bcdd8b9e977e..ab5e362e8efb4d50e198a324770aaf19d9518f7c 100644
--- a/include/base/factor/factor_base.h
+++ b/include/base/factor/factor_base.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_BASE_H_
-#define CONSTRAINT_BASE_H_
+#ifndef FACTOR_BASE_H_
+#define FACTOR_BASE_H_
// Forward declarations for node templates
namespace wolf{
@@ -20,8 +20,8 @@ namespace wolf {
*/
typedef enum
{
- CTR_INACTIVE = 0, ///< Factor established with a frame (odometry).
- CTR_ACTIVE = 1 ///< Factor established with absolute reference.
+ FAC_INACTIVE = 0, ///< Factor established with a frame (odometry).
+ FAC_ACTIVE = 1 ///< Factor established with absolute reference.
} FactorStatus;
/** \brief Enumeration of jacobian computation method
@@ -47,19 +47,19 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
unsigned int factor_id_;
FactorStatus status_; ///< status of factor (types defined at wolf.h)
bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category CTR_FRAME)
+ FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category FAC_FRAME)
CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer (for category CTR_FEATURE)
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer (for category CTR_LANDMARK)
+ FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer (for category FAC_FEATURE)
+ LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer (for category FAC_LANDMARK)
ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
public:
- /** \brief Constructor of category CTR_ABSOLUTE
+ /** \brief Constructor of category FAC_ABSOLUTE
**/
FactorBase(const std::string& _tp,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
/** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
**/
@@ -70,7 +70,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorBase() = default;
diff --git a/include/base/factor/factor_block_absolute.h b/include/base/factor/factor_block_absolute.h
index 2246bc58ea5d0e9de4dccd1f095b392fd096ee3b..4e0da9d764f50d5f55c14c596696b138c9b76035 100644
--- a/include/base/factor/factor_block_absolute.h
+++ b/include/base/factor/factor_block_absolute.h
@@ -5,8 +5,8 @@
* \author: AtDinesh
*/
-#ifndef CONSTRAINT_BLOCK_ABSOLUTE_H_
-#define CONSTRAINT_BLOCK_ABSOLUTE_H_
+#ifndef FACTOR_BLOCK_ABSOLUTE_H_
+#define FACTOR_BLOCK_ABSOLUTE_H_
//Wolf includes
#include "base/factor/factor_analytic.h"
@@ -36,14 +36,14 @@ class FactorBlockAbsolute : public FactorAnalytic
*
*/
FactorBlockAbsolute(StateBlockPtr _sb_ptr,
- unsigned int _start_idx = 0,
- int _size = -1,
- bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ unsigned int _start_idx = 0,
+ int _size = -1,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("BLOCK ABS",
- _apply_loss_function,
- _status,
- _sb_ptr),
+ _apply_loss_function,
+ _status,
+ _sb_ptr),
sb_size_(_sb_ptr->getSize()),
sb_constrained_start_(_start_idx),
sb_constrained_size_(_size == -1 ? sb_size_ : _size)
diff --git a/include/base/factor/factor_container.h b/include/base/factor/factor_container.h
index 1298cd99a7fa7759656cb2ca7f35d20136b74c97..93d455a8e7e36ce85e1688c5c70a91b020d963c9 100644
--- a/include/base/factor/factor_container.h
+++ b/include/base/factor/factor_container.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_CONTAINER_H_
-#define CONSTRAINT_CONTAINER_H_
+#ifndef FACTOR_CONTAINER_H_
+#define FACTOR_CONTAINER_H_
//Wolf includes
#include "base/wolf.h"
@@ -22,9 +22,19 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
const LandmarkContainerPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
const unsigned int _corner,
- bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorContainer,3,2,1,2,1>("CONTAINER",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(),_ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO()),
+ nullptr,
+ nullptr,
+ nullptr,
+ _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
@@ -79,7 +89,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
// Residuals
residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
- //std::cout << "\nCONSTRAINT: " << id() << std::endl;
+ //std::cout << "\nFACTOR: " << id() << std::endl;
//std::cout << "Feature: " << getFeature()->id() << std::endl;
//std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
//std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
@@ -118,12 +128,12 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr)
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr)
{
- unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
+ unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
- return std::make_shared<FactorContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
+ return std::make_shared<FactorContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
}
};
diff --git a/include/base/factor/factor_corner_2D.h b/include/base/factor/factor_corner_2D.h
index 712b722c08b935519774c639832a238a3a140a85..2bcacaac7600e55cdbcfe672e6c1d4c88697a18e 100644
--- a/include/base/factor/factor_corner_2D.h
+++ b/include/base/factor/factor_corner_2D.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_CORNER_2D_THETA_H_
-#define CONSTRAINT_CORNER_2D_THETA_H_
+#ifndef FACTOR_CORNER_2D_THETA_H_
+#define FACTOR_CORNER_2D_THETA_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -14,12 +14,22 @@ class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
public:
FactorCorner2D(const FeatureBasePtr _ftr_ptr,
- const LandmarkCorner2DPtr _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ const LandmarkCorner2DPtr _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorCorner2D,3,2,1,2,1>("CORNER 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(),_ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO())
+ nullptr,
+ nullptr,
+ nullptr,
+ _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO())
{
//
}
@@ -28,7 +38,7 @@ class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
LandmarkCorner2DPtr getLandmark()
{
- return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
+ return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
}
template <typename T>
@@ -37,14 +47,14 @@ class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
{
- return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
}
};
template<typename T>
inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const
+ const T* const _landmarkO, T* _residuals) const
{
// Mapping
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
@@ -80,7 +90,7 @@ inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _
// Residuals
residuals_map = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>().cast<T>() * residuals_map;
- //std::cout << "\nCONSTRAINT: " << id() << std::endl;
+ //std::cout << "\nFACTOR: " << id() << std::endl;
//std::cout << "Feature: " << getFeature()->id() << std::endl;
//std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
//std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h
index 89a54a3c111bc779d8560cbd2df32d538c4bdf98..b495859c7409031153025fe507c0f10113c92a54 100644
--- a/include/base/factor/factor_diff_drive.h
+++ b/include/base/factor/factor_diff_drive.h
@@ -5,8 +5,8 @@
* \author: Jeremie Deray
*/
-#ifndef WOLF_CONSTRAINT_DIFF_DRIVE_H_
-#define WOLF_CONSTRAINT_DIFF_DRIVE_H_
+#ifndef WOLF_FACTOR_DIFF_DRIVE_H_
+#define WOLF_FACTOR_DIFF_DRIVE_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -43,7 +43,7 @@ public:
const CaptureWheelJointPositionPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
- const FactorStatus _status = CTR_ACTIVE) :
+ const FactorStatus _status = FAC_ACTIVE) :
Base("DIFF DRIVE", _capture_origin_ptr->getFrame(), _capture_origin_ptr,
nullptr, nullptr, _processor_ptr,
_apply_loss_function, _status,
@@ -136,4 +136,4 @@ FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* co
} // namespace wolf
-#endif /* WOLF_CONSTRAINT_DIFF_DRIVE_H_ */
+#endif /* WOLF_FACTOR_DIFF_DRIVE_H_ */
diff --git a/include/base/factor/factor_epipolar.h b/include/base/factor/factor_epipolar.h
index 075fa9203d8140be27d10322fe1251fd4ff2c80f..42f0e855894c6f247ef30f15c77508157824fb68 100644
--- a/include/base/factor/factor_epipolar.h
+++ b/include/base/factor/factor_epipolar.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_EPIPOLAR_H
-#define CONSTRAINT_EPIPOLAR_H
+#ifndef FACTOR_EPIPOLAR_H
+#define FACTOR_EPIPOLAR_H
#include "base/factor/factor_base.h"
@@ -15,7 +15,7 @@ class FactorEpipolar : public FactorBase
const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorEpipolar() = default;
@@ -66,4 +66,4 @@ inline FactorBasePtr FactorEpipolar::create(const FeatureBasePtr& _feature_ptr,
} // namespace wolf
-#endif // CONSTRAINT_EPIPOLAR_H
+#endif // FACTOR_EPIPOLAR_H
diff --git a/include/base/factor/factor_fix_bias.h b/include/base/factor/factor_fix_bias.h
index c34ff5bd0b56b885b7512a9a3e26d91f2f25b49e..6c1f321dc0f7fcaf965476dc802080e3d3bb23b5 100644
--- a/include/base/factor/factor_fix_bias.h
+++ b/include/base/factor/factor_fix_bias.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_FIX_BIAS_H_
-#define CONSTRAINT_FIX_BIAS_H_
+#ifndef FACTOR_FIX_BIAS_H_
+#define FACTOR_FIX_BIAS_H_
//Wolf includes
#include "base/capture/capture_IMU.h"
@@ -19,7 +19,7 @@ WOLF_PTR_TYPEDEFS(FactorFixBias);
class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
{
public:
- FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS",
nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h
index 3413791a5663994ad79122e11a19b02e97174f7f..e45570d79a8debe284310998cc87f48032521a10 100644
--- a/include/base/factor/factor_odom_2D.h
+++ b/include/base/factor/factor_odom_2D.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_ODOM_2D_THETA_H_
-#define CONSTRAINT_ODOM_2D_THETA_H_
+#ifndef FACTOR_ODOM_2D_THETA_H_
+#define FACTOR_ODOM_2D_THETA_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -18,7 +18,7 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
diff --git a/include/base/factor/factor_odom_2D_analytic.h b/include/base/factor/factor_odom_2D_analytic.h
index b19757ce59c8abe0206335d0cb5ff64161084e86..2eb3296240c9b28766919748875eaea9720d87e0 100644
--- a/include/base/factor/factor_odom_2D_analytic.h
+++ b/include/base/factor/factor_odom_2D_analytic.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_ODOM_2D_ANALYTIC_H_
-#define CONSTRAINT_ODOM_2D_ANALYTIC_H_
+#ifndef FACTOR_ODOM_2D_ANALYTIC_H_
+#define FACTOR_ODOM_2D_ANALYTIC_H_
//Wolf includes
#include "base/factor/factor_relative_2D_analytic.h"
@@ -17,7 +17,7 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ FactorStatus _status = FAC_ACTIVE) :
FactorRelative2DAnalytic("ODOM_2D", _ftr_ptr,
_frame_ptr, _processor_ptr, _apply_loss_function, _status)
{
diff --git a/include/base/factor/factor_odom_3D.h b/include/base/factor/factor_odom_3D.h
index ec14e38065c8bdc2f5d041a47395b86ddc70bbb1..ce3456ef0e8c286c7b082a79a41fa268e4608a8e 100644
--- a/include/base/factor/factor_odom_3D.h
+++ b/include/base/factor/factor_odom_3D.h
@@ -5,8 +5,8 @@
* Author: jsola
*/
-#ifndef CONSTRAINT_ODOM_3D_H_
-#define CONSTRAINT_ODOM_3D_H_
+#ifndef FACTOR_ODOM_3D_H_
+#define FACTOR_ODOM_3D_H_
#include "base/factor/factor_autodiff.h"
#include "base/rotations.h"
@@ -24,7 +24,7 @@ class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorOdom3D() = default;
@@ -228,4 +228,4 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
} /* namespace wolf */
-#endif /* CONSTRAINT_ODOM_3D_H_ */
+#endif /* FACTOR_ODOM_3D_H_ */
diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h
index 16ed524719d7fe234333285517f015cb321f1991..2034260de9483f592e54de2a1e3f125a12c19879 100644
--- a/include/base/factor/factor_point_2D.h
+++ b/include/base/factor/factor_point_2D.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_POINT_2D_THETA_H_
-#define CONSTRAINT_POINT_2D_THETA_H_
+#ifndef FACTOR_POINT_2D_THETA_H_
+#define FACTOR_POINT_2D_THETA_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -28,7 +28,7 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _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))
diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
index 239fdc62b91330da88f6c04915eaed24b7de1641..e60c8d3c7ef2677637cb06d86a832064fdaa44ce 100644
--- a/include/base/factor/factor_point_to_line_2D.h
+++ b/include/base/factor/factor_point_to_line_2D.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_POINT_TO_LINE_2D_H_
-#define CONSTRAINT_POINT_TO_LINE_2D_H_
+#ifndef FACTOR_POINT_TO_LINE_2D_H_
+#define FACTOR_POINT_TO_LINE_2D_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -29,7 +29,7 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
- bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _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))
diff --git a/include/base/factor/factor_pose_2D.h b/include/base/factor/factor_pose_2D.h
index 93397ae4a3858dc6b246215b4cb87e5387083e62..306b5e15ebf9e2e67eb41c8e5072aa050f7d6b32 100644
--- a/include/base/factor/factor_pose_2D.h
+++ b/include/base/factor/factor_pose_2D.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_POSE_2D_H_
-#define CONSTRAINT_POSE_2D_H_
+#ifndef FACTOR_POSE_2D_H_
+#define FACTOR_POSE_2D_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(FactorPose2D);
class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
{
public:
- FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
// std::cout << "created FactorPose2D " << std::endl;
diff --git a/include/base/factor/factor_pose_3D.h b/include/base/factor/factor_pose_3D.h
index e53137aa039b27c6157b7e049c9ddca5b2286a60..b9c4da39516235081a4ec91334fa06958478264c 100644
--- a/include/base/factor/factor_pose_3D.h
+++ b/include/base/factor/factor_pose_3D.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_POSE_3D_H_
-#define CONSTRAINT_POSE_3D_H_
+#ifndef FACTOR_POSE_3D_H_
+#define FACTOR_POSE_3D_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -16,7 +16,7 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
{
public:
- FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
diff --git a/include/base/factor/factor_quaternion_absolute.h b/include/base/factor/factor_quaternion_absolute.h
index 89fc55e72c1ff0ae04a3a872371a83abf718fad9..1864a6c8eede4bd07190c4da737a7e1f25da117f 100644
--- a/include/base/factor/factor_quaternion_absolute.h
+++ b/include/base/factor/factor_quaternion_absolute.h
@@ -5,8 +5,8 @@
* \author: AtDinesh
*/
-#ifndef CONSTRAINT_QUATERNION_ABSOLUTE_H_
-#define CONSTRAINT_QUATERNION_ABSOLUTE_H_
+#ifndef FACTOR_QUATERNION_ABSOLUTE_H_
+#define FACTOR_QUATERNION_ABSOLUTE_H_
//Wolf includes
#include "base/factor/factor_autodiff.h"
@@ -23,7 +23,7 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
{
public:
- FactorQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+ FactorQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
{
diff --git a/include/base/factor/factor_relative_2D_analytic.h b/include/base/factor/factor_relative_2D_analytic.h
index 63ead17ad1ac5a8b55205ce81112ef472ef522d3..5c3aec2333bb85999c6892c6ce365e0de3c5ac2e 100644
--- a/include/base/factor/factor_relative_2D_analytic.h
+++ b/include/base/factor/factor_relative_2D_analytic.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_RELATIVE_2D_ANALYTIC_H_
-#define CONSTRAINT_RELATIVE_2D_ANALYTIC_H_
+#ifndef FACTOR_RELATIVE_2D_ANALYTIC_H_
+#define FACTOR_RELATIVE_2D_ANALYTIC_H_
//Wolf includes
#include "base/factor/factor_analytic.h"
@@ -15,40 +15,40 @@ class FactorRelative2DAnalytic : public FactorAnalytic
{
public:
- /** \brief Constructor of category CTR_FRAME
+ /** \brief Constructor of category FAC_FRAME
**/
FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
- /** \brief Constructor of category CTR_FEATURE
+ /** \brief Constructor of category FAC_FEATURE
**/
FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
{
//
}
- /** \brief Constructor of category CTR_LANDMARK
+ /** \brief Constructor of category FAC_LANDMARK
**/
FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- FactorStatus _status = CTR_ACTIVE) :
+ FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
{
//
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index ac42e22030e61094ad2bb0c8e077c2a3abb6743c..df014203d7f5bb530f192347b4f19136c822de0b 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -81,12 +81,12 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
FactorBasePtrList& getFactorList();
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _ctr_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
// all factors
- void getFactorList(FactorBasePtrList & _ctr_list);
+ void getFactorList(FactorBasePtrList & _fac_list);
private:
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index 90775781f0dde3c8d018e519dce54279d5b28fe3..e6e4c14db28b5e5c897d851773b1f2ff4534ec84 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -135,8 +135,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
- void getFactorList(FactorBasePtrList& _ctr_list);
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _ctr_ptr);
+ void getFactorList(FactorBasePtrList& _fac_list);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 25bd5b630549a36a0f6867297161c714592bff5e..f12da14aabfd3dd33c03be8f2bc22fd5de6c2ced 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -86,7 +86,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Navigate wolf tree
virtual void setProblem(ProblemPtr _problem) final;
- FactorBasePtr addConstrainedBy(FactorBasePtr _ctr_ptr);
+ FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
diff --git a/include/base/node_base.h b/include/base/node_base.h
index 2426f53a53357005f9e0fcab1ed0af72b7494b7a..ebbc640825d3d6775da395b2fe1d02c03abf8073 100644
--- a/include/base/node_base.h
+++ b/include/base/node_base.h
@@ -22,7 +22,7 @@ namespace wolf {
* - "FRAME"
* - "CAPTURE"
* - "FEATURE"
- * - "CONSTRAINT"
+ * - "FACTOR"
* - "MAP"
* - "LANDMARK"
*
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 47c45714a9381301d3edb0ac3eace52b8447a189..f64b3571970400704e9ea680e291b705362cdebc 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -76,9 +76,9 @@ protected:
virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
- virtual void addFactor(const FactorBasePtr& ctr_ptr) = 0;
+ virtual void addFactor(const FactorBasePtr& fac_ptr) = 0;
- virtual void removeFactor(const FactorBasePtr& ctr_ptr) = 0;
+ virtual void removeFactor(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h
index 8e576b317980318f1d57f4bd40a896cfeb2c0627..154b78639d03c71368b65bbaf65ee778421ba313 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -99,7 +99,7 @@ class SolverQR
}
problem_ptr_->getStateBlockNotificationList().pop_front();
}
- // UPDATE CONSTRAINTS
+ // UPDATE FACTORS
while (!problem_ptr_->getFactorNotificationList().empty())
{
switch (problem_ptr_->getFactorNotificationList().front().notification_)
@@ -528,16 +528,16 @@ class SolverQR
return nodes_.size();
}
- CostFunctionBasePtr createCostFunction(FactorBasePtr _corrPtr)
+ CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr)
{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
+ //std::cout << "adding fac " << _fac_ptr->id() << std::endl;
+ //_fac_ptr->print();
- switch (_corrPtr->getTypeId())
+ switch (_fac_ptr->getTypeId())
{
- case CTR_GPS_FIX_2D:
+ case FAC_GPS_FIX_2D:
{
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
+ FactorGPS2D* specific_ptr = (FactorGPS2D*)(_fac_ptr);
return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2D, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
@@ -545,9 +545,9 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr));
break;
}
- case CTR_ODOM_2D:
+ case FAC_ODOM_2D:
{
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
+ FactorOdom2D* specific_ptr = (FactorOdom2D*)(_fac_ptr);
return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2D, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
@@ -555,9 +555,9 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D:
+ case FAC_CORNER_2D:
{
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
+ FactorCorner2D* specific_ptr = (FactorCorner2D*)(_fac_ptr);
return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2D, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
index ad7a0cc5e84f97b1c42067febbb8d41368089b51..09c0abe7f8a83cde800e76968da2a5a990e0007c 100644
--- a/include/base/solver_suitesparse/solver_manager.h
+++ b/include/base/solver_suitesparse/solver_manager.h
@@ -32,9 +32,9 @@ class SolverManager
void update(const WolfProblemPtr _problem_ptr);
- void addFactor(FactorBasePtr _corr_ptr);
+ void addFactor(FactorBasePtr _fac_ptr);
- void removeFactor(const unsigned int& _corr_idx);
+ void removeFactor(const unsigned int& _fac_idx);
void addStateUnit(StateBlockPtr _st_ptr);
@@ -42,7 +42,7 @@ class SolverManager
void updateStateUnitStatus(StateBlockPtr _st_ptr);
- ceres::CostFunction* createCostFunction(FactorBasePtr _corrPtr);
+ ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
};
#endif
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index ce6dd438f1438642078af1759596c229ac34f74d..9150f8fa0daeb9f7999f38f35a5db02712a63c10 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory_base.h
@@ -47,7 +47,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
// factors
- void getFactorList(FactorBasePtrList & _ctr_list);
+ void getFactorList(FactorBasePtrList & _fac_list);
};
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index cf7322fa63b4fcd3d4896a281795f60509cc64a9..86165e4503302e5898a1ae0c6f2129297d2b2b73 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -115,17 +115,17 @@ void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
feature_list_.splice(feature_list_.end(), _new_ft_list);
}
-void CaptureBase::getFactorList(FactorBasePtrList& _ctr_list)
+void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
- f_ptr->getFactorList(_ctr_list);
+ f_ptr->getFactorList(_fac_list);
}
-FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _ctr_ptr)
+FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setCaptureOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setCaptureOtherPtr(shared_from_this());
+ return _fac_ptr;
}
StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 67979b7eff123b605c8b12dbe6c230ec8e6f8081..95ca1a65631b33e9322485caaf6204af84ae5833 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -36,8 +36,8 @@ CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
CeresManager::~CeresManager()
{
- while (!ctr_2_residual_idx_.empty())
- removeFactor(ctr_2_residual_idx_.begin()->first);
+ while (!fac_2_residual_idx_.empty())
+ removeFactor(fac_2_residual_idx_.begin()->first);
}
SolverManagerPtr CeresManager::create(const ProblemPtr &wolf_problem, const paramsServer &_server)
@@ -251,40 +251,40 @@ void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::addFactor(const FactorBasePtr& ctr_ptr)
+void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
{
- assert(ctr_2_costfunction_.find(ctr_ptr) == ctr_2_costfunction_.end() && "adding a factor that is already in the ctr_2_costfunction_ map");
+ assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
- auto cost_func_ptr = createCostFunction(ctr_ptr);
- ctr_2_costfunction_[ctr_ptr] = cost_func_ptr;
+ auto cost_func_ptr = createCostFunction(fac_ptr);
+ fac_2_costfunction_[fac_ptr] = cost_func_ptr;
std::vector<Scalar*> res_block_mem;
- res_block_mem.reserve(ctr_ptr->getStateBlockPtrVector().size());
- for (const StateBlockPtr& st : ctr_ptr->getStateBlockPtrVector())
+ res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
+ for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
{
res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
}
- auto loss_func_ptr = (ctr_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
+ auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
- assert(ctr_2_residual_idx_.find(ctr_ptr) == ctr_2_residual_idx_.end() && "adding a factor that is already in the ctr_2_residual_idx_ map");
+ assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
- ctr_2_residual_idx_[ctr_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
+ fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
loss_func_ptr,
res_block_mem);
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::removeFactor(const FactorBasePtr& _ctr_ptr)
+void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
{
- assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end() && "removing a factor that is not in the ctr_2_residual map");
+ assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
- ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
- ctr_2_residual_idx_.erase(_ctr_ptr);
- ctr_2_costfunction_.erase(_ctr_ptr);
+ ceres_problem_->RemoveResidualBlock(fac_2_residual_idx_[_fac_ptr]);
+ fac_2_residual_idx_.erase(_fac_ptr);
+ fac_2_costfunction_.erase(_fac_ptr);
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
@@ -335,7 +335,7 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
// get all involved factors
FactorBasePtrList involved_factors;
- for (auto pair : ctr_2_costfunction_)
+ for (auto pair : fac_2_costfunction_)
for (const StateBlockPtr& st : pair.first->getStateBlockPtrVector())
if (st == state_ptr)
{
@@ -345,8 +345,8 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
}
// Remove all involved factors (it does not remove any parameter block)
- for (auto ctr : involved_factors)
- removeFactor(ctr);
+ for (auto fac : involved_factors)
+ removeFactor(fac);
// Remove state block (it removes all involved residual blocks but they just were removed)
removeStateBlock(state_ptr);
@@ -355,8 +355,8 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
addStateBlock(state_ptr);
// Add all involved factors
- for (auto ctr : involved_factors)
- addFactor(ctr);
+ for (auto fac : involved_factors)
+ addFactor(fac);
}
bool CeresManager::hasConverged()
@@ -379,17 +379,17 @@ Scalar CeresManager::finalCost()
return Scalar(summary_.final_cost);
}
-ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _ctr_ptr)
+ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fac_ptr)
{
- assert(_ctr_ptr != nullptr);
+ assert(_fac_ptr != nullptr);
// analitic & autodiff jacobian
- if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
- return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
+ if (_fac_ptr->getJacobianMethod() == JAC_ANALYTIC || _fac_ptr->getJacobianMethod() == JAC_AUTO)
+ return std::make_shared<CostFunctionWrapper>(_fac_ptr);
// numeric jacobian
- else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
- return createNumericDiffCostFunction(_ctr_ptr);
+ else if (_fac_ptr->getJacobianMethod() == JAC_NUMERIC)
+ return createNumericDiffCostFunction(_fac_ptr);
else
throw std::invalid_argument( "Wrong Jacobian Method!" );
@@ -398,8 +398,8 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _ct
void CeresManager::check()
{
// Check numbers
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_costfunction_.size());
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size());
+ assert(ceres_problem_->NumResidualBlocks() == fac_2_costfunction_.size());
+ assert(ceres_problem_->NumResidualBlocks() == fac_2_residual_idx_.size());
assert(ceres_problem_->NumParameterBlocks() == state_blocks_.size());
// Check parameter blocks
@@ -407,20 +407,20 @@ void CeresManager::check()
assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
// Check residual blocks
- for (auto&& ctr_res_pair : ctr_2_residual_idx_)
+ for (auto&& fac_res_pair : fac_2_residual_idx_)
{
// costfunction - residual
- assert(ctr_2_costfunction_.find(ctr_res_pair.first) != ctr_2_costfunction_.end());
- assert(ctr_2_costfunction_[ctr_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second));
+ assert(fac_2_costfunction_.find(fac_res_pair.first) != fac_2_costfunction_.end());
+ assert(fac_2_costfunction_[fac_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second));
// factor - residual
- assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->getFactor());
+ assert(fac_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor());
// parameter blocks - state blocks
std::vector<Scalar*> param_blocks;
- ceres_problem_->GetParameterBlocksForResidualBlock(ctr_res_pair.second, ¶m_blocks);
+ ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
auto i = 0;
- for (const StateBlockPtr& st : ctr_res_pair.first->getStateBlockPtrVector())
+ for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
{
assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
i++;
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index c9d2f9672de856adada0de723f44ddaead24b6dd..f2e40fac0122c6eb1599cbe27a62659f439a89f0 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -24,7 +24,7 @@ QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
QRManager::~QRManager()
{
sb_2_col_.clear();
- ctr_2_row_.clear();
+ fac_2_row_.clear();
}
std::string QRManager::solve(const unsigned int& _report_level)
@@ -106,10 +106,10 @@ bool QRManager::computeDecomposition()
}
unsigned int meas_size = 0;
- for (auto ctr_pair : ctr_2_row_)
+ for (auto fac_pair : fac_2_row_)
{
- ctr_2_row_[ctr_pair.first] = meas_size;
- meas_size += ctr_pair.first->getSize();
+ fac_2_row_[fac_pair.first] = meas_size;
+ meas_size += fac_pair.first->getSize();
}
// resize and setZero A, b
@@ -120,8 +120,8 @@ bool QRManager::computeDecomposition()
if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
{
// relinearize all factors
- for (auto ctr_pair : ctr_2_row_)
- relinearizeFactor(ctr_pair.first);
+ for (auto fac_pair : fac_2_row_)
+ relinearizeFactor(fac_pair.first);
any_state_block_removed_ = false;
new_state_blocks_ = 0;
@@ -138,29 +138,29 @@ bool QRManager::computeDecomposition()
return true;
}
-void QRManager::addFactor(FactorBasePtr _ctr_ptr)
+void QRManager::addFactor(FactorBasePtr _fac_ptr)
{
- //std::cout << "add factor " << _ctr_ptr->id() << std::endl;
- assert(ctr_2_row_.find(_ctr_ptr) == ctr_2_row_.end() && "adding existing factor");
- ctr_2_row_[_ctr_ptr] = A_.rows();
- A_.conservativeResize(A_.rows() + _ctr_ptr->getSize(), A_.cols());
- b_.conservativeResize(b_.size() + _ctr_ptr->getSize());
+ //std::cout << "add factor " << _fac_ptr->id() << std::endl;
+ assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
+ fac_2_row_[_fac_ptr] = A_.rows();
+ A_.conservativeResize(A_.rows() + _fac_ptr->getSize(), A_.cols());
+ b_.conservativeResize(b_.size() + _fac_ptr->getSize());
- assert(A_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad A number of rows");
- assert(b_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad b number of rows");
+ assert(A_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad A number of rows");
+ assert(b_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad b number of rows");
- relinearizeFactor(_ctr_ptr);
+ relinearizeFactor(_fac_ptr);
pending_changes_ = true;
}
-void QRManager::removeFactor(FactorBasePtr _ctr_ptr)
+void QRManager::removeFactor(FactorBasePtr _fac_ptr)
{
- //std::cout << "remove factor " << _ctr_ptr->id() << std::endl;
- assert(ctr_2_row_.find(_ctr_ptr) != ctr_2_row_.end() && "removing unknown factor");
- eraseBlockRow(A_, ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize());
- b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()).setZero();
- ctr_2_row_.erase(_ctr_ptr);
+ //std::cout << "remove factor " << _fac_ptr->id() << std::endl;
+ assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
+ eraseBlockRow(A_, fac_2_row_[_fac_ptr], _fac_ptr->getSize());
+ b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()).setZero();
+ fac_2_row_.erase(_fac_ptr);
pending_changes_ = true;
}
@@ -202,30 +202,30 @@ void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
addStateBlock(_st_ptr);
}
-void QRManager::relinearizeFactor(FactorBasePtr _ctr_ptr)
+void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
{
// evaluate factor
- std::vector<const Scalar*> ctr_states_ptr;
- for (auto sb : _ctr_ptr->getStateBlockPtrVector())
- ctr_states_ptr.push_back(sb->get());
- Eigen::VectorXs residual(_ctr_ptr->getSize());
+ std::vector<const Scalar*> fac_states_ptr;
+ for (auto sb : _fac_ptr->getStateBlockPtrVector())
+ fac_states_ptr.push_back(sb->get());
+ Eigen::VectorXs residual(_fac_ptr->getSize());
std::vector<Eigen::MatrixXs> jacobians;
- _ctr_ptr->evaluate(ctr_states_ptr,residual,jacobians);
+ _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
// Fill jacobians
- Eigen::SparseMatrixs A_block_row(_ctr_ptr->getSize(), A_.cols());
+ Eigen::SparseMatrixs A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++)
- if (!_ctr_ptr->getStateBlockPtrVector()[i]->isFixed())
+ if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
{
- assert(sb_2_col_.find(_ctr_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
- assert(A_.cols() >= sb_2_col_[_ctr_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
+ assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
+ assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
- insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_ctr_ptr->getStateBlockPtrVector()[i]]);
+ insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
}
- assignBlockRow(A_, A_block_row, ctr_2_row_[_ctr_ptr]);
+ assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
// Fill residual
- b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()) = residual;
+ b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
}
} /* namespace wolf */
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index 8d5b187b67d52fd0a80f8d5efb98e31a7b983696..06ae2d113c1a4c4a988fa6a684be614c90ed4d9a 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -21,15 +21,15 @@ void SolverManager::update()
//std::cout << wolf_problem_->getFactorNotificationList().size() << " factor notifications" << std::endl;
// REMOVE CONSTRAINTS
- auto ctr_notification_it = wolf_problem_->getFactorNotificationList().begin();
- while ( ctr_notification_it != wolf_problem_->getFactorNotificationList().end() )
- if (ctr_notification_it->notification_ == REMOVE)
+ auto fac_notification_it = wolf_problem_->getFactorNotificationList().begin();
+ while ( fac_notification_it != wolf_problem_->getFactorNotificationList().end() )
+ if (fac_notification_it->notification_ == REMOVE)
{
- removeFactor(ctr_notification_it->factor_ptr_);
- ctr_notification_it = wolf_problem_->getFactorNotificationList().erase(ctr_notification_it);
+ removeFactor(fac_notification_it->factor_ptr_);
+ fac_notification_it = wolf_problem_->getFactorNotificationList().erase(fac_notification_it);
}
else
- ctr_notification_it++;
+ fac_notification_it++;
// REMOVE STATE BLOCKS
auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index da4e5beb411e726c9ef10a12d50527a49c27c42f..dff0efd51fe4a71d8d9dc22b6630f4b50fa62946 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -183,7 +183,7 @@ int main(int argc, char** argv)
featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- FactorFix3DPtr ctr_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
Eigen::MatrixXs featureFixBias_cov(6,6);
featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
@@ -192,7 +192,7 @@ int main(int argc, char** argv)
CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
//create a FeatureBase to factor biases
FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- FactorFixBiasPtr ctr_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
+ FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
// ___Fix/Unfix stateblocks___
KF1->getP()->fix();
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index 2cdffa833c26635359665417901a6f70b5746aa4..4415b685cecace3b3aa348d6ca4adfd748219acd 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -198,7 +198,7 @@ int main(int argc, char** argv)
featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- FactorFix3DPtr ctr_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
Eigen::MatrixXs featureFixBias_cov(6,6);
featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
@@ -207,7 +207,7 @@ int main(int argc, char** argv)
CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
//create a FeatureBase to factor biases
FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- FactorFixBiasPtr ctr_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
+ FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
// ___Fix/Unfix stateblocks___
// fix all Keyframes here
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index 7683b18202eb6861ff622e965c81694b08065aa4..f134ccc124603660a1d687fe5fa3357427677415 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -326,26 +326,26 @@ int main(int argc, char** argv)
{
if(frm_ptr->isKey())
{
- FactorBasePtrList ctr_list = frm_ptr->getConstrainedByList();
- for(FactorBasePtr ctr_ptr : ctr_list)
+ FactorBasePtrList fac_list = frm_ptr->getConstrainedByList();
+ for(FactorBasePtr fac_ptr : fac_list)
{
- if(ctr_ptr->getTypeId() == CTR_IMU)
+ if(fac_ptr->getTypeId() == FAC_IMU)
{
- //Eigen::VectorXs prev_KF_state(ctr_ptr->getFrameOther()->getState());
- //Eigen::VectorXs curr_KF_state(ctr_ptr->getFeature()->getFrame()->getState());
- p1 = ctr_ptr->getFrameOther()->getP()->getState();
- q1_vec = ctr_ptr->getFrameOther()->getO()->getState();
- v1 = ctr_ptr->getFrameOther()->getV()->getState();
- ab1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOther())->getAccBias()->getState();
- wb1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOther())->getGyroBias()->getState();
-
- p2 = ctr_ptr->getFeature()->getFrame()->getP()->getState();
- q2_vec = ctr_ptr->getFeature()->getFrame()->getO()->getState();
- v2 = ctr_ptr->getFeature()->getFrame()->getV()->getState();
- ab2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeature()->getFrame())->getAccBias()->getState();
- wb2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeature()->getFrame())->getGyroBias()->getState();
-
- std::static_pointer_cast<FactorIMU>(ctr_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
+ //Eigen::VectorXs prev_KF_state(fac_ptr->getFrameOther()->getState());
+ //Eigen::VectorXs curr_KF_state(fac_ptr->getFeature()->getFrame()->getState());
+ p1 = fac_ptr->getFrameOther()->getP()->getState();
+ q1_vec = fac_ptr->getFrameOther()->getO()->getState();
+ v1 = fac_ptr->getFrameOther()->getV()->getState();
+ ab1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getAccBias()->getState();
+ wb1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getGyroBias()->getState();
+
+ p2 = fac_ptr->getFeature()->getFrame()->getP()->getState();
+ q2_vec = fac_ptr->getFeature()->getFrame()->getO()->getState();
+ v2 = fac_ptr->getFeature()->getFrame()->getV()->getState();
+ ab2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getAccBias()->getState();
+ wb2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getGyroBias()->getState();
+
+ std::static_pointer_cast<FactorIMU>(fac_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
std::cout << "IMU residuals : " << IMU_residuals.transpose() << std::endl;
}
}
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 9d21a43c000ab3a7313980d36365e04ed922d7c2..142b9980846a38c123e01a61c4df2513f724729e 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -159,23 +159,23 @@ int main(int argc, char** argv)
std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
// Factors------------------
- FactorAHPPtr ctr_0 = FactorAHP::create(feat_0, lmk_1, nullptr);
- feat_0->addFactor(ctr_0);
- FactorAHPPtr ctr_1 = FactorAHP::create(feat_1, lmk_1, nullptr);
- feat_1->addFactor(ctr_1);
- FactorAHPPtr ctr_2 = FactorAHP::create(feat_2, lmk_1, nullptr);
- feat_2->addFactor(ctr_2);
+ FactorAHPPtr fac_0 = FactorAHP::create(feat_0, lmk_1, nullptr);
+ feat_0->addFactor(fac_0);
+ FactorAHPPtr fac_1 = FactorAHP::create(feat_1, lmk_1, nullptr);
+ feat_1->addFactor(fac_1);
+ FactorAHPPtr fac_2 = FactorAHP::create(feat_2, lmk_1, nullptr);
+ feat_2->addFactor(fac_2);
// Projections----------------------------
- Eigen::VectorXs pix_0 = ctr_0->expectation();
+ Eigen::VectorXs pix_0 = fac_0->expectation();
kp_0 = cv::KeyPoint(pix_0(0), pix_0(1), 0);
feat_0->setKeypoint(kp_0);
- Eigen::VectorXs pix_1 = ctr_1->expectation();
+ Eigen::VectorXs pix_1 = fac_1->expectation();
kp_1 = cv::KeyPoint(pix_1(0), pix_1(1), 0);
feat_1->setKeypoint(kp_1);
- Eigen::VectorXs pix_2 = ctr_2->expectation();
+ Eigen::VectorXs pix_2 = fac_2->expectation();
kp_2 = cv::KeyPoint(pix_2(0), pix_2(1), 0);
feat_2->setKeypoint(kp_2);
@@ -209,13 +209,13 @@ int main(int argc, char** argv)
std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
// New factors from kf3 and kf4
- FactorAHPPtr ctr_3 = FactorAHP::create(feat_3, lmk_2, nullptr);
- feat_3->addFactor(ctr_3);
- FactorAHPPtr ctr_4 = FactorAHP::create(feat_4, lmk_2, nullptr);
- feat_4->addFactor(ctr_4);
+ FactorAHPPtr fac_3 = FactorAHP::create(feat_3, lmk_2, nullptr);
+ feat_3->addFactor(fac_3);
+ FactorAHPPtr fac_4 = FactorAHP::create(feat_4, lmk_2, nullptr);
+ feat_4->addFactor(fac_4);
- Eigen::Vector2s pix_3 = ctr_3->expectation();
- Eigen::Vector2s pix_4 = ctr_4->expectation();
+ Eigen::Vector2s pix_3 = fac_3->expectation();
+ Eigen::Vector2s pix_4 = fac_4->expectation();
std::cout << "pix 3: " << pix_3.transpose() << std::endl;
std::cout << "pix 4: " << pix_4.transpose() << std::endl;
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index 2bfe0055a7187be7905e8f6fa02984eeed977090..1113af8b7f7e9bd40033ed60e182cc2f485eee99 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -335,7 +335,7 @@ int main(int argc, char** argv)
for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
{
- if ((*c_it)->getCategory() != CTR_FRAME) continue;
+ if ((*c_it)->getCategory() != FAC_FRAME) continue;
// ii (old)
wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
@@ -388,7 +388,7 @@ int main(int argc, char** argv)
//std::cout << "IG = " << IG << std::endl;
if (IG < 2)
- (*c_it)->setStatus(CTR_INACTIVE);
+ (*c_it)->setStatus(FAC_INACTIVE);
}
double t_ig = ((double) clock() - t1) / CLOCKS_PER_SEC;
std::cout << "manual sigma computation " << t_sigma_manual << "s" << std::endl;
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index c7b7efa863b896949ee926aaf9e5fa29e8f7421c..f3ed29007190ada48bba1758a1ac4e57843ff423 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -82,7 +82,7 @@ int main(int argc, char** argv)
Eigen::SparseMatrix<Scalar> Lambda(0,0);
// prunning
- FactorBasePtrList ordered_ctr_ptr;
+ FactorBasePtrList ordered_fac_ptr;
std::list<Scalar> ordered_ig;
// Ceres wrapper
@@ -358,7 +358,7 @@ int main(int argc, char** argv)
for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
{
- if ((*c_it)->getCategory() != CTR_FRAME) continue;
+ if ((*c_it)->getCategory() != FAC_FRAME) continue;
// Measurement covariance
Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
@@ -507,22 +507,22 @@ int main(int argc, char** argv)
if (IG < 2 && IG > 0 && !std::isnan(IG))
{
// Store as a candidate to be prunned, ordered by information gain
- auto ordered_ctr_it = ordered_ctr_ptr.begin();
- for (auto ordered_ig_it = ordered_ig.begin(); ordered_ig_it != ordered_ig.end(); ordered_ig_it++, ordered_ctr_it++ )
+ auto ordered_fac_it = ordered_fac_ptr.begin();
+ for (auto ordered_ig_it = ordered_ig.begin(); ordered_ig_it != ordered_ig.end(); ordered_ig_it++, ordered_fac_it++ )
if (IG < (*ordered_ig_it))
{
ordered_ig.insert(ordered_ig_it, IG);
- ordered_ctr_ptr.insert(ordered_ctr_it, (*c_it));
+ ordered_fac_ptr.insert(ordered_fac_it, (*c_it));
break;
}
ordered_ig.insert(ordered_ig.end(), IG);
- ordered_ctr_ptr.insert(ordered_ctr_ptr.end(), (*c_it));
+ ordered_fac_ptr.insert(ordered_fac_ptr.end(), (*c_it));
}
}
// PRUNNING
std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectory()->getFrameList().size(), false);
- for (auto c_it = ordered_ctr_ptr.begin(); c_it != ordered_ctr_ptr.end(); c_it++ )
+ for (auto c_it = ordered_fac_ptr.begin(); c_it != ordered_fac_ptr.end(); c_it++ )
{
// Check heuristic: factor do not share node with any inactive factor
unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()];
@@ -531,7 +531,7 @@ int main(int argc, char** argv)
if (!any_inactive_in_frame[index_frame] && !any_inactive_in_frame[index_frame_other])
{
std::cout << "setting inactive" << (*c_it)->id() << std::endl;
- (*c_it)->setStatus(CTR_INACTIVE);
+ (*c_it)->setStatus(FAC_INACTIVE);
std::cout << "set!" << std::endl;
any_inactive_in_frame[index_frame] = true;
any_inactive_in_frame[index_frame_other] = true;
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index ba158d9afac05727deeeb4a332e9ebfc6a496871..e3dc7b1ab767657d613dc99c9f41925cf6a290da 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -123,9 +123,9 @@ void FactorBase::setStatus(FactorStatus _status)
std::cout << "factor not linked with 'top', only status changed" << std::endl;
else if (_status != status_)
{
- if (_status == CTR_ACTIVE)
+ if (_status == FAC_ACTIVE)
getProblem()->addFactor(shared_from_this());
- else if (_status == CTR_INACTIVE)
+ else if (_status == FAC_INACTIVE)
getProblem()->removeFactor(shared_from_this());
}
status_ = _status;
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 5836bab02f10a2aa74b0c10f613d557cf4985922..39061dea53cbe3ee5e63f4ca1b542bd12afed644 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -59,7 +59,7 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
// add factor to be added in solver
if (getProblem() != nullptr)
{
- if (_co_ptr->getStatus() == CTR_ACTIVE)
+ if (_co_ptr->getStatus() == FAC_ACTIVE)
getProblem()->addFactor(_co_ptr);
}
else
@@ -72,11 +72,11 @@ FrameBasePtr FeatureBase::getFrame() const
return capture_ptr_.lock()->getFrame();
}
-FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _ctr_ptr)
+FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setFeatureOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setFeatureOtherPtr(shared_from_this());
+ return _fac_ptr;
}
FactorBasePtrList& FeatureBase::getFactorList()
@@ -84,9 +84,9 @@ FactorBasePtrList& FeatureBase::getFactorList()
return factor_list_;
}
-void FeatureBase::getFactorList(FactorBasePtrList & _ctr_list)
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
{
- _ctr_list.insert(_ctr_list.end(), factor_list_.begin(), factor_list_.end());
+ _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 3e983ef3e9a200b670439d0d739340c18fa20e73..fcf542af51341dbe6e747ae99f383e76a6304c38 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -332,17 +332,17 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
return nullptr;
}
-void FrameBase::getFactorList(FactorBasePtrList& _ctr_list)
+void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
- c_ptr->getFactorList(_ctr_list);
+ c_ptr->getFactorList(_fac_list);
}
-FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _ctr_ptr)
+FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setFrameOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setFrameOtherPtr(shared_from_this());
+ return _fac_ptr;
}
FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp,
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 32bf0b6a7634e5d216325b42f3cb5fe3714e8951..48f66967cbfc59d2fbe237ca5ae8320070c1789e 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -170,11 +170,11 @@ YAML::Node LandmarkBase::saveToYaml() const
return node;
}
-FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _ctr_ptr)
+FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setLandmarkOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setLandmarkOtherPtr(shared_from_this());
+ return _fac_ptr;
}
} // namespace wolf
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 91e790ae704729f91cf2c10797ebfa10c61b5800..b31117566ac69542436e471dd1b1639492680a86 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -177,12 +177,12 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
getProblem()->addStateBlock(state);
// remove and add all factors to the point
- for (auto ctr_ptr : getConstrainedByList())
- for (auto st_ptr : ctr_ptr->getStateBlockPtrVector())
+ for (auto fac_ptr : getConstrainedByList())
+ for (auto st_ptr : fac_ptr->getStateBlockPtrVector())
if (st_ptr == state && getProblem() != nullptr)
{
- getProblem()->removeFactor(ctr_ptr);
- getProblem()->addFactor(ctr_ptr);
+ getProblem()->removeFactor(fac_ptr);
+ getProblem()->addFactor(fac_ptr);
}
// update boolean
@@ -236,68 +236,68 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
// Change factors from remove_state to remain_state
FactorBasePtrList old_factors_list = getConstrainedByList();
std::cout << "changing factors: " << old_factors_list.size() << std::endl;
- FactorBasePtr new_ctr_ptr = nullptr;
- for (auto ctr_ptr : old_factors_list)
+ FactorBasePtr new_fac_ptr = nullptr;
+ for (auto fac_ptr : old_factors_list)
{
- FactorPoint2DPtr ctr_point_ptr;
- FactorPointToLine2DPtr ctr_point_line_ptr;
- if ( (ctr_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(ctr_ptr)))
-// if (ctr_ptr->getTypeId() == CTR_POINT_2D)
+ FactorPoint2DPtr fac_point_ptr;
+ FactorPointToLine2DPtr fac_point_line_ptr;
+ if ( (fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr)))
+// if (fac_ptr->getTypeId() == FAC_POINT_2D)
{
-// FactorPoint2DPtr ctr_point_ptr = std::static_pointer_cast<FactorPoint2D>(ctr_ptr);
+// FactorPoint2DPtr fac_point_ptr = std::static_pointer_cast<FactorPoint2D>(fac_ptr);
// If landmark point constrained -> new factor
- if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
- new_ctr_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeature()),
+ if (fac_point_ptr->getLandmarkPointId() == _remove_id)
+ new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_ptr->getProcessor(),
- ctr_point_ptr->getFeaturePointId(),
+ fac_point_ptr->getProcessor(),
+ fac_point_ptr->getFeaturePointId(),
_remain_id,
- ctr_point_ptr->getApplyLossFunction(),
- ctr_point_ptr->getStatus());
+ fac_point_ptr->getApplyLossFunction(),
+ fac_point_ptr->getStatus());
}
- else if ((ctr_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(ctr_ptr)))
-// else if (ctr_ptr->getTypeId() == CTR_POINT_TO_LINE_2D)
+ else if ((fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr)))
+// else if (fac_ptr->getTypeId() == FAC_POINT_TO_LINE_2D)
{
-// FactorPointToLine2DPtr ctr_point_line_ptr = std::static_pointer_cast<FactorPointToLine2D>(ctr_ptr);
+// FactorPointToLine2DPtr fac_point_line_ptr = std::static_pointer_cast<FactorPointToLine2D>(fac_ptr);
// If landmark point constrained -> new factor
- if (ctr_point_line_ptr->getLandmarkPointId() == _remove_id)
- new_ctr_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeature()),
+ if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
+ new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_line_ptr->getProcessor(),
- ctr_point_line_ptr->getFeaturePointId(),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
_remain_id,
- ctr_point_line_ptr->getLandmarkPointAuxId(),
- ctr_point_ptr->getApplyLossFunction(),
- ctr_point_line_ptr->getStatus());
+ fac_point_line_ptr->getLandmarkPointAuxId(),
+ fac_point_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
// If landmark point is aux point -> new factor
- else if (ctr_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
- new_ctr_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeature()),
+ else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
+ new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_line_ptr->getProcessor(),
- ctr_point_line_ptr->getFeaturePointId(),
- ctr_point_line_ptr->getLandmarkPointId(),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
+ fac_point_line_ptr->getLandmarkPointId(),
_remain_id,
- ctr_point_line_ptr->getApplyLossFunction(),
- ctr_point_line_ptr->getStatus());
+ fac_point_line_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
}
else
throw std::runtime_error ("polyline factor of unknown type");
// If new factor
- if (new_ctr_ptr != nullptr)
+ if (new_fac_ptr != nullptr)
{
- std::cout << "created new factor: " << new_ctr_ptr->id() << std::endl;
- std::cout << "deleting factor: " << ctr_ptr->id() << std::endl;
+ std::cout << "created new factor: " << new_fac_ptr->id() << std::endl;
+ std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
// add new factor
- ctr_ptr->getFeature()->addFactor(new_ctr_ptr);
+ fac_ptr->getFeature()->addFactor(new_fac_ptr);
// remove factor
- ctr_ptr->remove();
+ fac_ptr->remove();
- new_ctr_ptr = nullptr;
+ new_fac_ptr = nullptr;
}
}
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index 3db8717532a42f339ccf5e690e3b95c7017b9136..3f1f5b2445ea42bda8b863beddaf52210bbf4c4c 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -214,14 +214,14 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
{
CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
- FactorIMUPtr ctr_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
+ FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
// link ot wolf tree
- _feature_motion->addFactor(ctr_imu);
- _capture_origin->addConstrainedBy(ctr_imu);
- _capture_origin->getFrame()->addConstrainedBy(ctr_imu);
+ _feature_motion->addFactor(fac_imu);
+ _capture_origin->addConstrainedBy(fac_imu);
+ _capture_origin->getFrame()->addConstrainedBy(fac_imu);
- return ctr_imu;
+ return fac_imu;
}
void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 07b577b6d187c6116db088d90a5422825246e4f8..4e307962e94a3fc2e205cf7cd1c80f480d2871c4 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -212,14 +212,14 @@ CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin)
{
- FactorOdom2DPtr ctr_odom =
+ FactorOdom2DPtr fac_odom =
std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
shared_from_this());
- _feature->addFactor(ctr_odom);
- _capture_origin->getFrame()->addConstrainedBy(ctr_odom);
+ _feature->addFactor(fac_odom);
+ _capture_origin->getFrame()->addConstrainedBy(fac_odom);
- return ctr_odom;
+ return fac_odom;
}
FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 7a0d4be1639f6ba698df5fc222a6a42ea19c5658..9db14170b38e556224ea7a101a33e0db9e83746c 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -75,10 +75,10 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
{
// Check if the two frames currently evaluated are already
// constrained one-another.
- const FactorBasePtrList& ctr_list = key_it->getConstrainedByList();
+ const FactorBasePtrList& fac_list = key_it->getConstrainedByList();
bool are_constrained = false;
- for (const FactorBasePtr& crt : ctr_list)
+ for (const FactorBasePtr& crt : fac_list)
{
// Are the two frames constrained one-another ?
if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f20aec34d73915b74ea6e695bee164d632d4d905..4798ead84ec1c9ebdf1bea82bd2155bf9d17f454 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -135,9 +135,9 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// Modify existing factor --------
// Instead of modifying, we remove one ctr, and create a new one.
- auto ctr_to_remove = existing_feature->getFactorList().back(); // there is only one factor!
+ auto fac_to_remove = existing_feature->getFactorList().back(); // there is only one factor!
auto new_ctr = emplaceFactor(existing_feature, capture_for_keyframe_callback);
- ctr_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
break;
}
@@ -203,7 +203,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
auto key_feature_ptr = emplaceFeature(last_ptr_);
// create motion factor and link it to parent feature and other frame (which is origin's frame)
- auto ctr_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
+ auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
// create a new frame
auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index d85f115b15e2e2c15aab18d818a56d4bfac9797f..f36a7abc3a5a1fadf8276e8c6d2fc79a048d7cb6 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -154,11 +154,11 @@ CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const Sens
FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- FactorOdom2DPtr ctr_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
+ FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
shared_from_this());
- _feature->addFactor(ctr_odom);
- _capture_origin->getFrame()->addConstrainedBy(ctr_odom);
- return ctr_odom;
+ _feature->addFactor(fac_odom);
+ _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+ return fac_odom;
}
FeatureBasePtr ProcessorOdom2D::createFeature(CaptureMotionPtr _capture_motion)
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 917ee944a3babb4d1637233464118ab22e60e6dd..7415459711b62482df8a0de5d32bb56a496180d2 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -405,11 +405,11 @@ CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const Sens
FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- FactorOdom3DPtr ctr_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
+ FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
shared_from_this());
- _feature_motion->addFactor(ctr_odom);
- _capture_origin->getFrame()->addConstrainedBy(ctr_odom);
- return ctr_odom;
+ _feature_motion->addFactor(fac_odom);
+ _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+ return fac_odom;
}
FeatureBasePtr ProcessorOdom3D::createFeature(CaptureMotionPtr _capture_motion)
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index fdeacd6caca540a65ead14e3265daaaa460d3ab3..670f3aaf776d371bc1db000d84184f5643cc8e2e 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -148,18 +148,18 @@ void ProcessorTrackerFeature::establishFactors()
FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- auto ctr_ptr = createFactor(feature_in_last, feature_in_origin);
- feature_in_last ->addFactor(ctr_ptr);
- feature_in_origin->addConstrainedBy(ctr_ptr);
+ auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
+ feature_in_last ->addFactor(fac_ptr);
+ feature_in_origin->addConstrainedBy(fac_ptr);
- if (ctr_ptr != nullptr) // factor links
+ if (fac_ptr != nullptr) // factor links
{
- FrameBasePtr frm = ctr_ptr->getFrameOther();
+ FrameBasePtr frm = fac_ptr->getFrameOther();
if (frm)
- frm->addConstrainedBy(ctr_ptr);
- CaptureBasePtr cap = ctr_ptr->getCaptureOther();
+ frm->addConstrainedBy(fac_ptr);
+ CaptureBasePtr cap = fac_ptr->getCaptureOther();
if (cap)
- cap->addConstrainedBy(ctr_ptr);
+ cap->addConstrainedBy(fac_ptr);
}
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 126a504e965d4d4b32f90c9a7b07eb39e50f4c58..980c652af00ecb490250ac0baabb375ddf5ba98e 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -416,7 +416,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
FeatureBasePtr ftr_last = pair_trkid_match.second.second;
// make factor
- FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, CTR_ACTIVE);
+ FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE);
// link to wolf tree
ftr_last->addFactor(ctr);
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 9c67c53ad4d00c297ce2ca48c01187e7634636d8..3d5b75c8fcc6e9ef70ef396c31a6960a5aa3a016 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -131,18 +131,18 @@ void ProcessorTrackerLandmark::establishFactors()
for (auto last_feature : last_ptr_->getFeatureList())
{
auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
- FactorBasePtr ctr_ptr = createFactor(last_feature,
+ FactorBasePtr fac_ptr = createFactor(last_feature,
lmk);
- if (ctr_ptr != nullptr) // factor links
+ if (fac_ptr != nullptr) // factor links
{
- last_feature->addFactor(ctr_ptr);
- lmk->addConstrainedBy(ctr_ptr);
- FrameBasePtr frm = ctr_ptr->getFrameOther();
+ last_feature->addFactor(fac_ptr);
+ lmk->addConstrainedBy(fac_ptr);
+ FrameBasePtr frm = fac_ptr->getFrameOther();
if (frm)
- frm->addConstrainedBy(ctr_ptr);
- CaptureBasePtr cap = ctr_ptr->getCaptureOther();
+ frm->addConstrainedBy(fac_ptr);
+ CaptureBasePtr cap = fac_ptr->getCaptureOther();
if (cap)
- cap->addConstrainedBy(ctr_ptr);
+ cap->addConstrainedBy(fac_ptr);
}
}
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 5fe5321742ad13ccba46d777f86bea847ae1a09d..17f6e562bb242ba315369effd722391f52f7cf4c 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -14,16 +14,16 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
void SolverManager::update()
{
// REMOVE CONSTRAINTS
- auto ctr_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while ( ctr_notification_it != wolf_problem_->getFactorNotificationMap().end() )
+ auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
+ while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
{
- if (ctr_notification_it->second == REMOVE)
+ if (fac_notification_it->second == REMOVE)
{
- removeFactor(ctr_notification_it->first);
- ctr_notification_it = wolf_problem_->getFactorNotificationMap().erase(ctr_notification_it);
+ removeFactor(fac_notification_it->first);
+ fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
}
else
- ctr_notification_it++;
+ fac_notification_it++;
}
// ADD/REMOVE STATE BLOCS
@@ -67,13 +67,13 @@ void SolverManager::update()
}
// ADD CONSTRAINTS
- ctr_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while (ctr_notification_it != wolf_problem_->getFactorNotificationMap().end())
+ fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
+ while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
{
assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
- ctr_notification_it = wolf_problem_->getFactorNotificationMap().erase(ctr_notification_it);
+ fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 7d3081ad7985bf33a5896efe21150381098bceeb..2ba7af7081a64abec0149777e790c6fc355b6c49 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -48,12 +48,12 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
//std::cout << "state units removed!" << std::endl;
// ADD CONSTRAINTS
- FactorBasePtrList ctr_list;
- _problem_ptr->getTrajectory()->getFactorList(ctr_list);
- //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
- for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
- if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
- addFactor(*ctr_it);
+ FactorBasePtrList fac_list;
+ _problem_ptr->getTrajectory()->getFactorList(fac_list);
+ //std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
+ for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
+ if ((*fac_it)->getPendingStatus() == ADD_PENDING)
+ addFactor(*fac_it);
//std::cout << "factors updated!" << std::endl;
}
@@ -152,7 +152,7 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
switch (_corrPtr->getFactorType())
{
- case CTR_GPS_FIX_2D:
+ case FAC_GPS_FIX_2D:
{
FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorGPS2D,
@@ -169,7 +169,7 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D_COMPLEX_ANGLE:
+ case FAC_ODOM_2D_COMPLEX_ANGLE:
{
FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
@@ -186,7 +186,7 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D:
+ case FAC_ODOM_2D:
{
FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorOdom2D,
@@ -203,7 +203,7 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D:
+ case FAC_CORNER_2D:
{
FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorCorner2D,
@@ -220,7 +220,7 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_IMU:
+ case FAC_IMU:
{
FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorIMU,
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 2e96c6a72006f530c9e6a2598e6e8d0245281910..cf01f2d788ed6a28390a460ee526e566412c9bb6 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -38,10 +38,10 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
return _frame_ptr;
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _ctr_list)
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
{
for(auto fr_ptr : getFrameList())
- fr_ptr->getFactorList(_ctr_list);
+ fr_ptr->getFactorList(_fac_list);
}
void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index d1a593c0e112a540c30f49c2f78ce5b76427a34b..3f92fc0872e1f1869a998c0acda2fffaad25f3e3 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -60,9 +60,9 @@ class CeresManagerWrapper : public CeresManager
return ceres_problem_->NumResidualBlocks();
};
- bool isFactorRegistered(const FactorBasePtr& ctr_ptr) const
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return ctr_2_residual_idx_.find(ctr_ptr) != ctr_2_residual_idx_.end() && ctr_2_costfunction_.find(ctr_ptr) != ctr_2_costfunction_.end();
+ return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
};
bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index b7d3143faa0021c805ebed57c1015250970f0aa2..6c8990c291af5bb43dc9e042f9ca60dc84854e75 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -2459,7 +2459,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
featureFix_cov(5,5) = 0.1;
CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- FactorFix3DPtr ctr_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
//prepare problem for solving
origin_KF->getP()->fix();
@@ -2517,7 +2517,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
featureFix_cov(5,5) = 0.1;
CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- FactorFix3DPtr ctr_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
//prepare problem for solving
origin_KF->getP()->fix();
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 64d71f268636a5c26d9556567f1d77a4d37996f0..204c6ce633a36380d46eacbc65d66abe1a7747fe 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -46,7 +46,7 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS"
* Both features and factors are created in the TEST(). Hence, tests will not interfere each others.
*/
-TEST(FactorBlockAbs, ctr_block_abs_p)
+TEST(FactorBlockAbs, fac_block_abs_p)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
@@ -64,7 +64,7 @@ TEST(FactorBlockAbs, ctr_block_abs_p)
ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
}
-TEST(FactorBlockAbs, ctr_block_abs_p_tail2)
+TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
@@ -80,7 +80,7 @@ TEST(FactorBlockAbs, ctr_block_abs_p_tail2)
ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
}
-TEST(FactorBlockAbs, ctr_block_abs_v)
+TEST(FactorBlockAbs, fac_block_abs_v)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
@@ -98,7 +98,7 @@ TEST(FactorBlockAbs, ctr_block_abs_v)
ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
}
-TEST(FactorQuatAbs, ctr_block_abs_o)
+TEST(FactorQuatAbs, fac_block_abs_o)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 0372227134c81002f8ee9625bdb78245493bdde9..a053b3f82438b8ac9c358fc45b4b656dd9df65ff 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -203,12 +203,12 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
capture_ptr->addFeature(feature_ptr);
// CONSTRAINTS
- FactorOdom2DPtr ctr_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(ctr_autodiff_ptr);
- fr1_ptr->addConstrainedBy(ctr_autodiff_ptr);
- FactorOdom2DAnalyticPtr ctr_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
- feature_ptr->addFactor(ctr_analytic_ptr);
- fr1_ptr->addConstrainedBy(ctr_analytic_ptr);
+ FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(fac_autodiff_ptr);
+ fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
+ FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(fac_analytic_ptr);
+ fr1_ptr->addConstrainedBy(fac_analytic_ptr);
// COMPUTE JACOBIANS
@@ -220,13 +220,13 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
- Eigen::VectorXs residuals(ctr_autodiff_ptr->getSize());
+ Eigen::VectorXs residuals(fac_autodiff_ptr->getSize());
clock_t t = clock();
- ctr_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
+ fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
t = clock();
//TODO FactorAnalytic::evaluate
-// ctr_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+// fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
// std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
//
// for (auto i = 0; i < Jautodiff.size(); i++)
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 1de85bd61f08403640b02359243cb32eb239840a..7559bfa7582aa1449232391806e498fb294d03be 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -62,7 +62,7 @@ class FactorAutodiffDistance3D_Test : public testing::Test
F2->addCapture(C2);
f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
C2->addFeature(f2);
- c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, CTR_ACTIVE);
+ c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE);
f2->addFactor(c2);
F1->addConstrainedBy(c2);
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 1f872726f70f7fe1ad32330ba6fda8dd4116a8e2..71401381fe35c768d047b8324e0b2e2f74df35c4 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -152,7 +152,7 @@ class FactorAutodiffTrifocalTest : public testing::Test{
I3-> addFeature(f3);
// trifocal factor
- c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, CTR_ACTIVE);
+ c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE);
f3 ->addFactor (c123);
f1 ->addConstrainedBy(c123);
f2 ->addConstrainedBy(c123);
@@ -710,7 +710,7 @@ class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov));
I3->addFeature(fv3.at(i));
- cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, CTR_ACTIVE));
+ cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
fv3.at(i)->addFactor(cv123.at(i));
fv1.at(i)->addConstrainedBy(cv123.at(i));
fv2.at(i)->addConstrainedBy(cv123.at(i));
@@ -888,17 +888,17 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
F3->addCapture(Cd);
FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std));
Cd->addFeature(fd);
- FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, CTR_ACTIVE);
+ FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE);
fd->addFactor(cd);
F1->addConstrainedBy(cd);
- cd->setStatus(CTR_INACTIVE);
+ cd->setStatus(FAC_INACTIVE);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_DEBUG("DISTANCE CONSTRAINT INACTIVE: \n", report);
problem->print(1,0,1,0);
- cd->setStatus(CTR_ACTIVE);
+ cd->setStatus(FAC_ACTIVE);
report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
// Print results
diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp
index ae419726f9306956f85f41041af1c345757339f2..6c8f212be2030c5d04707eb5b5798848f36d8710 100644
--- a/test/gtest_factor_sparse.cpp
+++ b/test/gtest_factor_sparse.cpp
@@ -31,17 +31,17 @@ class FactorSparseObject : public FactorSparse<1, 2, 1>
TEST(FactorSparseAnalytic, Constructor)
{
- FactorSparseObject<JAC_ANALYTIC> ctr_analytic(CTR_AHP, false, CTR_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(ctr_analytic.getJacobianMethod(), JAC_ANALYTIC);
- ASSERT_EQ(ctr_analytic.getApplyLossFunction(), false);
- ASSERT_EQ(ctr_analytic.getStatus(), CTR_ACTIVE);
- ASSERT_EQ(ctr_analytic.getSize(), 1);
+ FactorSparseObject<JAC_ANALYTIC> fac_analytic(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ ASSERT_EQ(fac_analytic.getJacobianMethod(), JAC_ANALYTIC);
+ ASSERT_EQ(fac_analytic.getApplyLossFunction(), false);
+ ASSERT_EQ(fac_analytic.getStatus(), FAC_ACTIVE);
+ ASSERT_EQ(fac_analytic.getSize(), 1);
- FactorSparseObject<JAC_AUTO> ctr_auto(CTR_AHP, false, CTR_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(ctr_auto.getJacobianMethod(), JAC_AUTO);
+ FactorSparseObject<JAC_AUTO> fac_auto(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ ASSERT_EQ(fac_auto.getJacobianMethod(), JAC_AUTO);
- FactorSparseObject<JAC_NUMERIC> ctr_numeric(CTR_AHP, false, CTR_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(ctr_numeric.getJacobianMethod(), JAC_NUMERIC);
+ FactorSparseObject<JAC_NUMERIC> fac_numeric(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ ASSERT_EQ(fac_numeric.getJacobianMethod(), JAC_NUMERIC);
}
int main(int argc, char **argv)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 75d29551550172734abc9107ae837efcc2a5c4d6..b6b5f2b85717df13c864551845bdf6c1725c0b39 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -45,9 +45,9 @@ class SolverManagerWrapper : public SolverManager
return state_block_fixed_.at(st);
};
- bool isFactorRegistered(const FactorBasePtr& ctr_ptr) const
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return std::find(factors_.begin(), factors_.end(), ctr_ptr) != factors_.end();
+ return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
};
bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
@@ -74,13 +74,13 @@ class SolverManagerWrapper : public SolverManager
protected:
virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
- virtual void addFactor(const FactorBasePtr& ctr_ptr)
+ virtual void addFactor(const FactorBasePtr& fac_ptr)
{
- factors_.push_back(ctr_ptr);
+ factors_.push_back(fac_ptr);
};
- virtual void removeFactor(const FactorBasePtr& ctr_ptr)
+ virtual void removeFactor(const FactorBasePtr& fac_ptr)
{
- factors_.remove(ctr_ptr);
+ factors_.remove(fac_ptr);
};
virtual void addStateBlock(const StateBlockPtr& state_ptr)
{