diff --git a/CMakeLists.txt b/CMakeLists.txt
index 43e9eb71a180e76aa1b56275001723ab6e2650a8..085812a58cbe669b65840bc21436168add28dcc5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -144,7 +144,7 @@ SET(HDRS_CAPTURE
include/vision/capture/capture_image.h
)
SET(HDRS_FACTOR
-include/vision/factor/factor_autodiff_trifocal.h
+include/vision/factor/factor_trifocal.h
include/vision/factor/factor_ahp.h
include/vision/factor/factor_pixel_hp.h
include/vision/factor/factor_epipolar.h
diff --git a/include/vision/factor/factor_autodiff_trifocal.h b/include/vision/factor/factor_trifocal.h
similarity index 92%
rename from include/vision/factor/factor_autodiff_trifocal.h
rename to include/vision/factor/factor_trifocal.h
index d243f644f3c9512c46a4768e7983082df26d1d69..4d99ae0892b7fe90d671a0f5232c615bca7bc239 100644
--- a/include/vision/factor/factor_autodiff_trifocal.h
+++ b/include/vision/factor/factor_trifocal.h
@@ -1,5 +1,5 @@
-#ifndef _FACTOR_AUTODIFF_TRIFOCAL_H_
-#define _FACTOR_AUTODIFF_TRIFOCAL_H_
+#ifndef _FACTOR_TRIFOCAL_H_
+#define _FACTOR_TRIFOCAL_H_
//Wolf includes
//#include "core/common/wolf.h"
@@ -12,17 +12,17 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorAutodiffTrifocal);
+WOLF_PTR_TYPEDEFS(FactorTrifocal);
using namespace Eigen;
-class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4>
+class FactorTrifocal : public FactorAutodiff<FactorTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4>
{
public:
/** \brief Class constructor
*/
- FactorAutodiffTrifocal(const FeatureBasePtr& _feature_1_ptr,
+ FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
const FeatureBasePtr& _feature_2_ptr,
const FeatureBasePtr& _feature_own_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -31,7 +31,7 @@ class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3,
/** \brief Class Destructor
*/
- virtual ~FactorAutodiffTrifocal();
+ virtual ~FactorTrifocal();
virtual std::string getTopology() const override
{
@@ -144,7 +144,7 @@ namespace wolf
using namespace Eigen;
// Constructor
-FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_1_ptr,
+FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
const FeatureBasePtr& _feature_2_ptr,
const FeatureBasePtr& _feature_own_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -222,17 +222,17 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_1_
}
// Destructor
-FactorAutodiffTrifocal::~FactorAutodiffTrifocal()
+FactorTrifocal::~FactorTrifocal()
{
}
-inline FeatureBasePtr FactorAutodiffTrifocal::getFeaturePrev()
+inline FeatureBasePtr FactorTrifocal::getFeaturePrev()
{
return feature_prev_ptr_.lock();
}
template<typename T>
-bool FactorAutodiffTrifocal::operator ()( const T* const _pos1,
+bool FactorTrifocal::operator ()( const T* const _pos1,
const T* const _quat1,
const T* const _pos2,
const T* const _quat2,
@@ -262,7 +262,7 @@ bool FactorAutodiffTrifocal::operator ()( const T* const _pos1,
}
template<typename D1, typename D2, class T, typename D3>
-inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
+inline void FactorTrifocal::expectation(const MatrixBase<D1>& _wtr1,
const QuaternionBase<D2>& _wqr1,
const MatrixBase<D1>& _wtr2,
const QuaternionBase<D2>& _wqr2,
@@ -333,7 +333,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,
+inline Matrix<T, 3, 1> FactorTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
const MatrixBase<D1>& _c2Ec1) const
{
// 1. COMMON COMPUTATIONS
@@ -363,7 +363,7 @@ 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,
+inline Matrix<T, 3, 1> FactorTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
const MatrixBase<D1>& _c2Ec1,
MatrixBase<D2>& _J_e_m1,
MatrixBase<D3>& _J_e_m2,
@@ -411,20 +411,20 @@ inline Matrix<T, 3, 1> FactorAutodiffTrifocal::error_jacobians(const vision_util
// Print function
template<class T, int ROWS, int COLS>
-void FactorAutodiffTrifocal::print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat) const
+void FactorTrifocal::print_matrix(const Eigen::Matrix<T, ROWS, COLS>& _mat) const
{}
template<int ROWS, int COLS>
-void FactorAutodiffTrifocal::print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat) const
+void FactorTrifocal::print_matrix(const Eigen::Matrix<double, ROWS, COLS>& _mat) const
{
std::cout << _mat << std::endl;
}
template<class T>
-void FactorAutodiffTrifocal::print_scalar(const T& _val) const
+void FactorTrifocal::print_scalar(const T& _val) const
{}
-void FactorAutodiffTrifocal::print_scalar(const double& _val) const
+void FactorTrifocal::print_scalar(const double& _val) const
{
std::cout << _val << std::endl;
}
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 34daf3b6285184675697e7ae98ac55cc572e7659..6a58f1d68e3ab291ff57d75afb8f6634a2333bc0 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -4,7 +4,7 @@
#include "vision/sensor/sensor_camera.h"
#include "vision/feature/feature_point_image.h"
-#include "vision/factor/factor_autodiff_trifocal.h"
+#include "vision/factor/factor_trifocal.h"
#include "vision/capture/capture_image.h"
// vision_utils
@@ -432,7 +432,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
assert(ftr_mid != ftr_last && "Last and middle features are the same!");
// emplace factor
- auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_first, ftr_mid, ftr_last, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
+ auto ctr = FactorBase::emplace<FactorTrifocal>(ftr_last, ftr_first, ftr_mid, ftr_last, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
}
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a1dc6f1994276d3b22ec287b479d3d0b31f3b758..193f3a0d6d953a374aafa94adb835570288fb57f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -20,9 +20,9 @@ target_link_libraries(gtest_pinhole ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp)
target_link_libraries(gtest_sensor_camera ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
-# FactorAutodiffTrifocal test
-wolf_add_gtest(gtest_factor_autodiff_trifocal gtest_factor_autodiff_trifocal.cpp)
-target_link_libraries(gtest_factor_autodiff_trifocal ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
+# FactorTrifocal test
+wolf_add_gtest(gtest_factor_trifocal gtest_factor_trifocal.cpp)
+target_link_libraries(gtest_factor_trifocal ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
# FactorFeatureEpipolar test
wolf_add_gtest(gtest_factor_epipolar gtest_factor_epipolar.cpp)
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_trifocal.cpp
similarity index 94%
rename from test/gtest_factor_autodiff_trifocal.cpp
rename to test/gtest_factor_trifocal.cpp
index 58b73ac752725b9ef5c6497bbfb3c7f7ae4d86c5..c331232d9a5b7466cea0b2fef0f42fa1f96768cc 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_trifocal.cpp
@@ -5,13 +5,13 @@
#include "core/ceres_wrapper/ceres_manager.h"
#include "vision/processor/processor_tracker_feature_trifocal.h"
#include "vision/capture/capture_image.h"
-#include "vision/factor/factor_autodiff_trifocal.h"
+#include "vision/factor/factor_trifocal.h"
#include "vision/internal/config.h"
using namespace Eigen;
using namespace wolf;
-class FactorAutodiffTrifocalTest : public testing::Test{
+class FactorTrifocalTest : public testing::Test{
public:
Vector3d pos1, pos2, pos3, pos_cam, point;
Vector3d euler1, euler2, euler3, euler_cam;
@@ -29,11 +29,11 @@ class FactorAutodiffTrifocalTest : public testing::Test{
FrameBasePtr F1, F2, F3;
CaptureImagePtr I1, I2, I3;
FeatureBasePtr f1, f2, f3;
- FactorAutodiffTrifocalPtr c123;
+ FactorTrifocalPtr c123;
double pixel_noise_std;
- virtual ~FactorAutodiffTrifocalTest()
+ virtual ~FactorTrifocalTest()
{
std::cout << "destructor\n";
}
@@ -155,15 +155,15 @@ class FactorAutodiffTrifocalTest : public testing::Test{
f3 = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", pix, pix_cov); // pixel at origin
// trifocal factor
- // c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE);
- c123 = std::static_pointer_cast<FactorAutodiffTrifocal>(FactorBase::emplace<FactorAutodiffTrifocal>(f3, f1, f2, f3, proc_trifocal, false, FAC_ACTIVE));
+ // c123 = std::make_shared<FactorTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE);
+ c123 = std::static_pointer_cast<FactorTrifocal>(FactorBase::emplace<FactorTrifocal>(f3, f1, f2, f3, proc_trifocal, false, FAC_ACTIVE));
// f3 ->addFactor (c123);
// f1 ->addConstrainedBy(c123);
// f2 ->addConstrainedBy(c123);
}
};
-TEST_F(FactorAutodiffTrifocalTest, InfoMatrix)
+TEST_F(FactorTrifocalTest, InfoMatrix)
{
/** Ground truth covariance. Rationale:
* Due to the orthogonal configuration (see line 40 and onwards), we have:
@@ -187,7 +187,7 @@ TEST_F(FactorAutodiffTrifocalTest, InfoMatrix)
}
-TEST_F(FactorAutodiffTrifocalTest, expectation)
+TEST_F(FactorTrifocalTest, expectation)
{
// Homogeneous transform C2 wrt C1
Matrix4d _c1Hc2; _c1Hc2 <<
@@ -256,7 +256,7 @@ TEST_F(FactorAutodiffTrifocalTest, expectation)
ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8);
}
-TEST_F(FactorAutodiffTrifocalTest, residual)
+TEST_F(FactorTrifocalTest, residual)
{
vision_utils::TrifocalTensor tensor;
Matrix3d c2Ec1;
@@ -269,7 +269,7 @@ TEST_F(FactorAutodiffTrifocalTest, residual)
ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-8);
}
-TEST_F(FactorAutodiffTrifocalTest, error_jacobians)
+TEST_F(FactorTrifocalTest, error_jacobians)
{
vision_utils::TrifocalTensor tensor;
Matrix3d c2Ec1;
@@ -340,7 +340,7 @@ TEST_F(FactorAutodiffTrifocalTest, error_jacobians)
}
-TEST_F(FactorAutodiffTrifocalTest, operator_parenthesis)
+TEST_F(FactorTrifocalTest, operator_parenthesis)
{
Vector3d res;
@@ -354,7 +354,7 @@ TEST_F(FactorAutodiffTrifocalTest, operator_parenthesis)
ASSERT_MATRIX_APPROX(res, Vector3d::Zero(), 1e-8);
}
-TEST_F(FactorAutodiffTrifocalTest, solve_F1)
+TEST_F(FactorTrifocalTest, solve_F1)
{
F1->setState(pose1);
F2->setState(pose2);
@@ -435,7 +435,7 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F1)
}
-TEST_F(FactorAutodiffTrifocalTest, solve_F2)
+TEST_F(FactorTrifocalTest, solve_F2)
{
F1->setState(pose1);
F2->setState(pose2);
@@ -517,7 +517,7 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F2)
}
-TEST_F(FactorAutodiffTrifocalTest, solve_F3)
+TEST_F(FactorTrifocalTest, solve_F3)
{
F1->setState(pose1);
F2->setState(pose2);
@@ -600,7 +600,7 @@ TEST_F(FactorAutodiffTrifocalTest, solve_F3)
}
-TEST_F(FactorAutodiffTrifocalTest, solve_S)
+TEST_F(FactorTrifocalTest, solve_S)
{
F1->setState(pose1);
F2->setState(pose2);
@@ -685,7 +685,7 @@ TEST_F(FactorAutodiffTrifocalTest, solve_S)
}
-class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
+class FactorTrifocalMultiPointTest : public FactorTrifocalTest
{
/*
* In this test class we add 8 more points and perform optimization on the camera frames.
@@ -699,11 +699,11 @@ class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
public:
std::vector<FeatureBasePtr> fv1, fv2, fv3;
- std::vector<FactorAutodiffTrifocalPtr> cv123;
+ std::vector<FactorTrifocalPtr> cv123;
virtual void SetUp() override
{
- FactorAutodiffTrifocalTest::SetUp();
+ FactorTrifocalTest::SetUp();
Matrix<double, 2, 9> c1p_can;
c1p_can <<
@@ -744,7 +744,7 @@ class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
fv3.push_back(f3);
// I3->addFeature(fv3.at(i));
- auto ff = std::static_pointer_cast<FactorAutodiffTrifocal>(FactorBase::emplace<FactorAutodiffTrifocal>(fv3.at(i), fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
+ auto ff = std::static_pointer_cast<FactorTrifocal>(FactorBase::emplace<FactorTrifocal>(fv3.at(i), fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
cv123.push_back(ff);
// fv3.at(i)->addFactor(cv123.at(i));
// fv1.at(i)->addConstrainedBy(cv123.at(i));
@@ -755,7 +755,7 @@ class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
};
-TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point)
+TEST_F(FactorTrifocalMultiPointTest, solve_multi_point)
{
/*
* In this test we add 8 more points and perform optimization on the camera frames.
@@ -820,7 +820,7 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point)
}
-TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
+TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_scale)
{
/*
* In this test we add 8 more points and perform optimization on the camera frames.
@@ -886,7 +886,7 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
#include "core/factor/factor_autodiff_distance_3d.h"
-TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
+TEST_F(FactorTrifocalMultiPointTest, solve_multi_point_distance)
{
/*
* In this test we add 8 more points and perform optimization on the camera frames.
@@ -975,12 +975,12 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F1";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F2";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F3";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_S";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_multi_point";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalMultiPointTest.solve_multi_point_distance";
+ // ::testing::GTEST_FLAG(filter) = "FactorTrifocalTest.solve_F1";
+ // ::testing::GTEST_FLAG(filter) = "FactorTrifocalTest.solve_F2";
+ // ::testing::GTEST_FLAG(filter) = "FactorTrifocalTest.solve_F3";
+ // ::testing::GTEST_FLAG(filter) = "FactorTrifocalTest.solve_S";
+ // ::testing::GTEST_FLAG(filter) = "FactorTrifocalTest.solve_multi_point";
+ // ::testing::GTEST_FLAG(filter) = "FactorTrifocalMultiPointTest.solve_multi_point_distance";
return RUN_ALL_TESTS();
}