From 2fd0b90ef070e671b16c6375293dda02bb388a1c Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Fri, 29 Mar 2019 13:18:40 +0100
Subject: [PATCH] Renames ctr_ -> fac_ , CONSTRAINT_->FACTOR_
---
hello_wolf/factor_bearing.h | 6 +-
hello_wolf/factor_range_bearing.h | 6 +-
hello_wolf/processor_range_bearing.cpp | 2 +-
include/base/capture/capture_base.h | 4 +-
.../create_numeric_diff_cost_function.h | 12 +--
include/base/factor/factor_AHP.h | 4 +-
include/base/factor/factor_GPS_2D.h | 6 +-
.../base/factor/factor_GPS_pseudorange_2D.h | 8 +-
.../base/factor/factor_GPS_pseudorange_3D.h | 8 +-
include/base/factor/factor_IMU.h | 6 +-
include/base/factor/factor_diff_drive.h | 8 +-
include/base/factor/factor_epipolar.h | 8 +-
include/base/factor/factor_fix_bias.h | 6 +-
include/base/factor/factor_odom_2D.h | 6 +-
include/base/factor/factor_odom_2D_analytic.h | 6 +-
include/base/factor/factor_odom_3D.h | 8 +-
include/base/factor/factor_point_2D.h | 6 +-
include/base/factor/factor_point_to_line_2D.h | 6 +-
include/base/factor/factor_pose_2D.h | 6 +-
include/base/factor/factor_pose_3D.h | 6 +-
.../base/factor/factor_quaternion_absolute.h | 6 +-
.../base/factor/factor_relative_2D_analytic.h | 16 ++--
include/base/feature/feature_base.h | 4 +-
include/base/frame_base.h | 4 +-
include/base/landmark/landmark_base.h | 2 +-
include/base/solver/solver_manager.h | 4 +-
include/base/solver_suitesparse/qr_solver.h | 6 +-
include/base/trajectory_base.h | 2 +-
src/capture/capture_base.cpp | 12 +--
src/examples/test_imuDock.cpp | 4 +-
src/examples/test_imuDock_autoKFs.cpp | 4 +-
src/examples/test_imu_constrained0.cpp | 36 ++++-----
src/examples/test_simple_AHP.cpp | 30 +++----
src/examples/test_wolf_imported_graph.cpp | 4 +-
src/examples/test_wolf_prunning.cpp | 16 ++--
src/factor/factor_base.cpp | 4 +-
src/feature/feature_base.cpp | 14 ++--
src/frame_base.cpp | 12 +--
src/landmark/landmark_base.cpp | 8 +-
src/landmark/landmark_polyline_2D.cpp | 80 +++++++++----------
src/processor/processor_IMU.cpp | 10 +--
src/processor/processor_diff_drive.cpp | 8 +-
...rocessor_frame_nearest_neighbor_filter.cpp | 4 +-
src/processor/processor_motion.cpp | 6 +-
src/processor/processor_odom_2D.cpp | 8 +-
src/processor/processor_odom_3D.cpp | 8 +-
src/processor/processor_tracker_feature.cpp | 16 ++--
.../processor_tracker_feature_trifocal.cpp | 2 +-
src/processor/processor_tracker_landmark.cpp | 16 ++--
src/solver/solver_manager.cpp | 18 ++---
src/solver_suitesparse/solver_manager.cpp | 22 ++---
src/trajectory_base.cpp | 4 +-
test/gtest_ceres_manager.cpp | 4 +-
test/gtest_factor_IMU.cpp | 4 +-
test/gtest_factor_absolute.cpp | 8 +-
test/gtest_factor_autodiff.cpp | 18 ++---
test/gtest_factor_autodiff_distance_3D.cpp | 2 +-
test/gtest_factor_autodiff_trifocal.cpp | 10 +--
test/gtest_factor_sparse.cpp | 18 ++---
test/gtest_solver_manager.cpp | 12 +--
60 files changed, 297 insertions(+), 297 deletions(-)
diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h
index ce6bb49fe..5c958ba17 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 727e75a4e..1414d61a2 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 cb72112ee..25d7fd406 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 f9e4abe9e..7643f243e 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/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
index 967cf1d5e..fafa62aae 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/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
index 3c5ac2af1..a6448b064 100644
--- a/include/base/factor/factor_AHP.h
+++ b/include/base/factor/factor_AHP.h
@@ -190,9 +190,9 @@ 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
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
index b34e7c06e..a3ce0e609 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 40b8a5115..82b186fc6 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 d7a33681c..72e44d4f6 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 c1b9ba65a..fbb29d164 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_diff_drive.h b/include/base/factor/factor_diff_drive.h
index 89a54a3c1..b495859c7 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 075fa9203..42f0e8558 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 c34ff5bd0..6c1f321dc 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 3413791a5..e45570d79 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 b19757ce5..2eb329624 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 ec14e3806..ce3456ef0 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 16ed52471..2034260de 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 239fdc62b..e60c8d3c7 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 93397ae4a..306b5e15e 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 e53137aa0..b9c4da395 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 89fc55e72..1864a6c8e 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 63ead17ad..5c3aec233 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 ac42e2203..df014203d 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 90775781f..e6e4c14db 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 25bd5b630..f12da14aa 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/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 47c45714a..f64b35719 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 8e576b317..448666056 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -535,7 +535,7 @@ class SolverQR
switch (_corrPtr->getTypeId())
{
- case CTR_GPS_FIX_2D:
+ case FAC_GPS_FIX_2D:
{
FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2D, specific_ptr->residualSize,
@@ -545,7 +545,7 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr));
break;
}
- case CTR_ODOM_2D:
+ case FAC_ODOM_2D:
{
FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2D, specific_ptr->residualSize,
@@ -555,7 +555,7 @@ class SolverQR
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D:
+ case FAC_CORNER_2D:
{
FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2D, specific_ptr->residualSize,
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index ce6dd438f..9150f8fa0 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 cf7322fa6..86165e450 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/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index da4e5beb4..dff0efd51 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 2cdffa833..4415b685c 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 7683b1820..f134ccc12 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 9d21a43c0..142b99808 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 2bfe0055a..1113af8b7 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 c7b7efa86..f3ed29007 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 ba158d9af..e3dc7b1ab 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 5836bab02..39061dea5 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 3e983ef3e..fcf542af5 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 32bf0b6a7..48f66967c 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 91e790ae7..b31117566 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 3db871753..3f1f5b244 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 07b577b6d..4e307962e 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 7df561f9c..769cf871b 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -76,10 +76,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 f20aec34d..4798ead84 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 57119faf6..a474ed1f3 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 917ee944a..741545971 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 fdeacd6ca..670f3aaf7 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 126a504e9..980c652af 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 9c67c53ad..3d5b75c8f 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 5fe532174..17f6e562b 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 7d3081ad7..2ba7af708 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 2e96c6a72..cf01f2d78 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 f9a539607..3f92fc087 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 fac_2_residual_idx_.find(ctr_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(ctr_ptr) != fac_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 b7d3143fa..6c8990c29 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 64d71f268..204c6ce63 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 037222713..a053b3f82 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 1de85bd61..7559bfa75 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 1f872726f..71401381f 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 ae419726f..6c8f212be 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 75d295515..b6b5f2b85 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)
{
--
GitLab