From f60905d42e7f55c3be8fb499c5bf064ce61662da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Fri, 15 Jan 2021 21:25:05 +0100 Subject: [PATCH] Use FactorOdom2d instead of FacytorOdom2dAutodiff --- .../create_numeric_diff_cost_function.h | 2 -- include/core/feature/feature_odom_2d.h | 1 - include/core/processor/processor_odom_2d.h | 1 - include/core/solver_suitesparse/qr_solver.h | 6 +++--- src/processor/processor_odom_2d.cpp | 3 ++- test/gtest_emplace.cpp | 6 +++--- test/gtest_factor_odom_2d.cpp | 12 ++++++------ test/gtest_frame_base.cpp | 4 ++-- test/gtest_odom_2d.cpp | 1 - 9 files changed, 16 insertions(+), 20 deletions(-) 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 6fb0fb5ac..2504d0b10 100644 --- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h +++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h @@ -8,8 +8,6 @@ #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" diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h index 3c9c87a2e..071976faf 100644 --- a/include/core/feature/feature_odom_2d.h +++ b/include/core/feature/feature_odom_2d.h @@ -3,7 +3,6 @@ //Wolf includes #include <core/factor/factor_odom_2d.h> -#include <core/factor/factor_odom_2d_autodiff.h> #include "core/feature/feature_base.h" //std includes diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index 051fce005..254eded5c 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -8,7 +8,6 @@ #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/math/rotations.h" diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h index d208800d1..b89d49c8c 100644 --- a/include/core/solver_suitesparse/qr_solver.h +++ b/include/core/solver_suitesparse/qr_solver.h @@ -9,7 +9,7 @@ #define TRUNK_SRC_SOLVER_QR_SOLVER_H_ //std includes -#include <core/factor/factor_odom_2d_autodiff.h> +#include <core/factor/factor_odom_2d.h> #include <iostream> #include <ctime> @@ -547,8 +547,8 @@ class SolverQR } case FAC_ODOM_2d: { - FactorOdom2dAutodiff* specific_ptr = (FactorOdom2dAutodiff*)(_fac_ptr); - return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2dAutodiff, specific_ptr->residualSize, + FactorOdom2d* specific_ptr = (FactorOdom2d*)(_fac_ptr); + return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2d, specific_ptr->residualSize, specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size, specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size, 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 88278d090..d4f40c9eb 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -2,6 +2,7 @@ #include "core/sensor/sensor_odom_2d.h" #include "core/math/covariance.h" #include "core/state_block/state_composite.h" +#include "core/factor/factor_odom_2d.h" namespace wolf { @@ -143,7 +144,7 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own, FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) { - auto fac_odom = FactorBase::emplace<FactorOdom2dAutodiff>(_feature, + auto fac_odom = FactorBase::emplace<FactorOdom2d>(_feature, _feature, _capture_origin->getFrame(), shared_from_this(), diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 863664bee..21ed31670 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<FactorOdom2dAutodiff>(ftr, ftr, frm, nullptr, false); + auto cnt = FactorBase::emplace<FactorOdom2d>(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<FactorOdom2dAutodiff>(ftr, ftr, frm, nullptr, false); + auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false); - FactorOdom2dAutodiffPtr fac = FactorBase::emplace<FactorOdom2dAutodiff>(ftr, ftr, frm, nullptr, false); + FactorOdom2dPtr fac = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false); } int main(int argc, char **argv) diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp index 7adb358a7..5638239af 100644 --- a/test/gtest_factor_odom_2d.cpp +++ b/test/gtest_factor_odom_2d.cpp @@ -1,4 +1,4 @@ -#include <core/factor/factor_odom_2d_autodiff.h> +#include <core/factor/factor_odom_2d.h> #include "../include/core/ceres_wrapper/solver_ceres.h" #include "core/utils/utils_gtest.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(FactorOdom2dAutodiff, check_tree) +TEST(FactorOdom2d, check_tree) { ASSERT_TRUE(problem_ptr->check(0)); } -TEST(FactorOdom2dAutodiff, fix_0_solve) +TEST(FactorOdom2d, fix_0_solve) { for (int i = 0; i < 1e3; i++) { @@ -54,7 +54,7 @@ TEST(FactorOdom2dAutodiff, fix_0_solve) // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); - FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false); + FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); // Fix frm0, perturb frm1 frm0->fix(); @@ -71,7 +71,7 @@ TEST(FactorOdom2dAutodiff, fix_0_solve) } } -TEST(FactorOdom2dAutodiff, fix_1_solve) +TEST(FactorOdom2d, fix_1_solve) { for (int i = 0; i < 1e3; i++) { @@ -95,7 +95,7 @@ TEST(FactorOdom2dAutodiff, fix_1_solve) // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); - FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false); + FactorBase::emplace<FactorOdom2d>(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 2af1639c7..fc170fe35 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -5,7 +5,7 @@ * Author: jsola */ -#include <core/factor/factor_odom_2d_autodiff.h> +#include <core/factor/factor_odom_2d.h> #include "core/utils/utils_gtest.h" @@ -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<FactorOdom2dAutodiff>(f, f, F2, p, false); + auto c = FactorBase::emplace<FactorOdom2d>(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 3fb6a8bf2..7f9afd039 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -6,7 +6,6 @@ */ #include <core/factor/factor_odom_2d.h> -#include <core/factor/factor_odom_2d_autodiff.h> #include "core/utils/utils_gtest.h" // Classes under test -- GitLab