Use FactorOdom2d instead of FacytorOdom2dAutodiff

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"
 

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

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"

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,

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(),

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)

test/gtest_factor_odom_2d.cpp

-#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();

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());

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