diff --git a/CMakeLists.txt b/CMakeLists.txt index fde2538defbb476e0dae54ad109b8e5f131906f9..7728a55e104b8ac03aabcf15fbe9595db70c047b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -220,8 +220,8 @@ SET(HDRS_FACTOR include/core/factor/factor_block_absolute.h include/core/factor/factor_block_difference.h include/core/factor/factor_diff_drive.h - include/core/factor/factor_odom_2d.h include/core/factor/factor_distance_3d.h + include/core/factor/factor_odom_2d_autodiff.h include/core/factor/factor_odom_2d_closeloop.h include/core/factor/factor_odom_2d_analytic.h include/core/factor/factor_odom_3d.h diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h index 203c38ef635cf02d6ded7aa0a0491169ef67a04d..6fb0fb5ac86ed3216ed9fd698d7d1aa5cc10cb48 100644 --- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h +++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h @@ -8,11 +8,12 @@ #ifndef SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_ #define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_ +#include <core/factor/factor_odom_2d_autodiff.h> + #include "ceres/cost_function.h" #include "ceres/numeric_diff_cost_function.h" // Factors -#include "core/factor/factor_odom_2d.h" #include "core/factor/factor_base.h" namespace wolf { diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d_autodiff.h similarity index 86% rename from include/core/factor/factor_odom_2d.h rename to include/core/factor/factor_odom_2d_autodiff.h index 298cc35840163d92a353a450ed118360e2acf3d5..a63ca8cdd630affd0e2fda685e9344d82fcb1f79 100644 --- a/include/core/factor/factor_odom_2d.h +++ b/include/core/factor/factor_odom_2d_autodiff.h @@ -1,5 +1,5 @@ -#ifndef FACTOR_ODOM_2d_THETA_H_ -#define FACTOR_ODOM_2d_THETA_H_ +#ifndef FACTOR_ODOM_2d_AUTODIFF_H_ +#define FACTOR_ODOM_2d_AUTODIFF_H_ //Wolf includes #include "core/factor/factor_autodiff.h" @@ -10,18 +10,18 @@ namespace wolf { -WOLF_PTR_TYPEDEFS(FactorOdom2d); +WOLF_PTR_TYPEDEFS(FactorOdom2dAutodiff); //class -class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1> +class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1> { public: - FactorOdom2d(const FeatureBasePtr& _ftr_ptr, + FactorOdom2dAutodiff(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : - FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d", + FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>("FactorOdom2d", _ftr_ptr, _frame_other_ptr, nullptr, nullptr, nullptr, _processor_ptr, @@ -34,7 +34,7 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1> // } - ~FactorOdom2d() override = default; + ~FactorOdom2dAutodiff() override = default; std::string getTopology() const override { @@ -55,7 +55,7 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1> }; template<typename T> -inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, const T* const _p2, +inline bool FactorOdom2dAutodiff::operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const { @@ -102,4 +102,4 @@ inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, co } // namespace wolf -#endif +#endif // FACTOR_ODOM_2d_AUTODIFF_H_ diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h index cb65fa06b01d72cb32d1209752b44d6826f0d76b..78522f6b5666dc59f69c2d74f5dc33c10da06b00 100644 --- a/include/core/feature/feature_odom_2d.h +++ b/include/core/feature/feature_odom_2d.h @@ -2,8 +2,8 @@ #define FEATURE_ODOM_2d_H_ //Wolf includes +#include <core/factor/factor_odom_2d_autodiff.h> #include "core/feature/feature_base.h" -#include "core/factor/factor_odom_2d.h" #include "core/factor/factor_odom_2d_analytic.h" //std includes diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index 2ad5b846b59e3c40d98351a0f8b30f75b3f71635..051fce0052749e63cceb700bf9d40a0f6f0ba21f 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -8,9 +8,9 @@ #ifndef SRC_PROCESSOR_ODOM_2d_H_ #define SRC_PROCESSOR_ODOM_2d_H_ +#include <core/factor/factor_odom_2d_autodiff.h> #include "core/processor/processor_motion.h" #include "core/capture/capture_odom_2d.h" -#include "core/factor/factor_odom_2d.h" #include "core/math/rotations.h" #include "core/utils/params_server.h" #include "core/math/SE2.h" diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h index b586278c7e7f5a7cb949b78d2e90f58323031109..d208800d1958194b880bea05c0babec5ca3f1493 100644 --- a/include/core/solver_suitesparse/qr_solver.h +++ b/include/core/solver_suitesparse/qr_solver.h @@ -9,13 +9,13 @@ #define TRUNK_SRC_SOLVER_QR_SOLVER_H_ //std includes +#include <core/factor/factor_odom_2d_autodiff.h> #include <iostream> #include <ctime> //Wolf includes #include "core/state_block/state_block.h" #include "../factor_sparse.h" -#include "core/factor/factor_odom_2d.h" #include "core/factor/factor_corner_2d.h" #include "core/factor/factor_container.h" #include "core/solver_suitesparse/sparse_utils.h" @@ -547,8 +547,8 @@ class SolverQR } case FAC_ODOM_2d: { - FactorOdom2d* specific_ptr = (FactorOdom2d*)(_fac_ptr); - return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2d, specific_ptr->residualSize, + FactorOdom2dAutodiff* specific_ptr = (FactorOdom2dAutodiff*)(_fac_ptr); + return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2dAutodiff, specific_ptr->residualSize, specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size, specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size, specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size, diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 2da7c330b59ffce0c7eceb11515e4f992a9eb3d9..88278d0908cbc163e1f23bb39aeccaf6b1527617 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -143,7 +143,7 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own, FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) { - auto fac_odom = FactorBase::emplace<FactorOdom2d>(_feature, + auto fac_odom = FactorBase::emplace<FactorOdom2dAutodiff>(_feature, _feature, _capture_origin->getFrame(), shared_from_this(), diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 21ed3167006049866bcbc481ec5bfa81c760a23c..863664bee6f91ea3a00f70f4e71de61346e47e89 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -111,7 +111,7 @@ TEST(Emplace, Factor) ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem()); ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem()); ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture()); - auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false); + auto cnt = FactorBase::emplace<FactorOdom2dAutodiff>(ftr, ftr, frm, nullptr, false); ASSERT_NE(nullptr, ftr->getFactorList().front().get()); } @@ -145,9 +145,9 @@ TEST(Emplace, ReturnDerived) auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr); auto cov = Eigen::MatrixXd::Identity(2,2); auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov); - auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false); + auto cnt = FactorBase::emplace<FactorOdom2dAutodiff>(ftr, ftr, frm, nullptr, false); - FactorOdom2dPtr fac = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false); + FactorOdom2dAutodiffPtr fac = FactorBase::emplace<FactorOdom2dAutodiff>(ftr, ftr, frm, nullptr, false); } int main(int argc, char **argv) diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index f4e3108d6bdf2e129358b851e82772dc5cffae3a..d852ea30c3e600bc564cd5720b4e0446971f2570 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -5,12 +5,12 @@ * Author: jvallve */ +#include <core/factor/factor_odom_2d_autodiff.h> #include "core/utils/utils_gtest.h" #include "core/sensor/sensor_odom_2d.h" #include "core/capture/capture_void.h" #include "core/feature/feature_odom_2d.h" -#include "core/factor/factor_odom_2d.h" #include "core/factor/factor_odom_2d_analytic.h" #include "core/factor/factor_autodiff.h" #include "dummy/factor_dummy_zero_1.h" @@ -369,7 +369,7 @@ TEST(FactorAutodiff, EmplaceOdom2d) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()); // FACTOR - auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); ASSERT_TRUE(factor_ptr->getFeature()); ASSERT_TRUE(factor_ptr->getFeature()->getCapture()); @@ -405,7 +405,7 @@ TEST(FactorAutodiff, ResidualOdom2d) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity()); // FACTOR - auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); // EVALUATE @@ -450,7 +450,7 @@ TEST(FactorAutodiff, JacobianOdom2d) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity()); // FACTOR - auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); // COMPUTE JACOBIANS @@ -530,7 +530,7 @@ TEST(FactorAutodiff, AutodiffVsAnalytic) auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity()); // FACTOR - auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); + auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false); // COMPUTE JACOBIANS diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp index 740933a67158634dfd7c6194c8b6a46bd926c063..7adb358a7bb2ef8b9d3d697e133414611010b502 100644 --- a/test/gtest_factor_odom_2d.cpp +++ b/test/gtest_factor_odom_2d.cpp @@ -1,7 +1,7 @@ +#include <core/factor/factor_odom_2d_autodiff.h> #include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.h" -#include "core/factor/factor_odom_2d.h" #include "core/capture/capture_odom_2d.h" #include "core/math/rotations.h" @@ -25,12 +25,12 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero()); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov); -TEST(FactorOdom2d, check_tree) +TEST(FactorOdom2dAutodiff, check_tree) { ASSERT_TRUE(problem_ptr->check(0)); } -TEST(FactorOdom2d, fix_0_solve) +TEST(FactorOdom2dAutodiff, fix_0_solve) { for (int i = 0; i < 1e3; i++) { @@ -54,7 +54,7 @@ TEST(FactorOdom2d, fix_0_solve) // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); - FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); + FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false); // Fix frm0, perturb frm1 frm0->fix(); @@ -71,7 +71,7 @@ TEST(FactorOdom2d, fix_0_solve) } } -TEST(FactorOdom2d, fix_1_solve) +TEST(FactorOdom2dAutodiff, fix_1_solve) { for (int i = 0; i < 1e3; i++) { @@ -95,7 +95,7 @@ TEST(FactorOdom2d, fix_1_solve) // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); - FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); + FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false); // Fix frm1, perturb frm0 frm1->fix(); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index ac405e6fa1e9f4bcdc7976d8f0adc2014cb79c5f..2af1639c752b76c83100ac524f0eb857adce05e9 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -5,13 +5,13 @@ * Author: jsola */ +#include <core/factor/factor_odom_2d_autodiff.h> #include "core/utils/utils_gtest.h" #include "core/frame/frame_base.h" #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" -#include "core/factor/factor_odom_2d.h" #include "core/capture/capture_motion.h" #include <iostream> @@ -75,7 +75,7 @@ TEST(FrameBase, LinksToTree) //auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>()); WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01); - auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false); + auto c = FactorBase::emplace<FactorOdom2dAutodiff>(f, f, F2, p, false); //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic. ASSERT_FALSE(F2->getConstrainedByList().empty()); diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index ab545f2eca3122493a20ac31024faab520f46c64..cbdcf2435fe2b3e478bcbc37fd7f3713cecba253 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -5,12 +5,13 @@ * \author: jsola */ +#include <core/factor/factor_odom_2d_autodiff.h> #include "core/utils/utils_gtest.h" // Classes under test #include "core/sensor/sensor_odom_2d.h" #include "core/processor/processor_odom_2d.h" -#include "core/factor/factor_odom_2d.h" +#include "core/factor/factor_odom_2d_analytic.h" #include "core/capture/capture_odom_2d.h" // Wolf includes @@ -132,14 +133,14 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) FrameBasePtr F1 = Pr->emplaceFrame(t, Vector3d::Zero()); auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t); auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov); - auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false); + auto c1 = FactorBase::emplace<FactorOdom2dAnalytic>(f1, f1, F0, nullptr, false); // KF2 and motion from KF1 t += dt; FrameBasePtr F2 = Pr->emplaceFrame(t, Vector3d::Zero()); auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t); auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov); - auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false); + auto c2 = FactorBase::emplace<FactorOdom2dAnalytic>(f2, f2, F1, nullptr, false); ASSERT_TRUE(Pr->check(0));