extended autodiff and more exhaustive gtest

test/dummy/factor_dummy_zero_1.h

+#ifndef FACTOR_DUMMY_ZERO_1_H_
+#define FACTOR_DUMMY_ZERO_1_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FactorDummyZero1);
+
+//class
+class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
+{
+    public:
+        FactorDummyZero1(const StateBlockPtr& _sb_ptr) :
+             FactorAutodiff<FactorDummyZero1, 1, 1>("FactorDummy1",
+                                                nullptr, nullptr, nullptr, nullptr,
+                                                nullptr,
+                                                false, FAC_ACTIVE,
+                                                _sb_ptr)
+        {
+            //
+        }
+
+        virtual ~FactorDummyZero1() = default;
+
+        virtual std::string getTopology() const override
+        {
+            return std::string("MOTION");
+        }
+
+        template<typename T>
+        bool operator ()(const T* const _st1,
+                         T* _residuals) const
+        {
+            _residuals[0] = _st1[0];
+            return true;
+        }
+};
+
+} // namespace wolf
+
+#endif

test/dummy/factor_dummy_zero_12.h

+#ifndef FACTOR_DUMMY_ZERO_12_H_
+#define FACTOR_DUMMY_ZERO_12_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+    
+//class
+template <unsigned int ID>
+class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
+{
+    static const unsigned int id = ID;
+    public:
+        FactorDummyZero12(const StateBlockPtr& _sb1_ptr,
+                          const StateBlockPtr& _sb2_ptr,
+                          const StateBlockPtr& _sb3_ptr,
+                          const StateBlockPtr& _sb4_ptr,
+                          const StateBlockPtr& _sb5_ptr,
+                          const StateBlockPtr& _sb6_ptr,
+                          const StateBlockPtr& _sb7_ptr,
+                          const StateBlockPtr& _sb8_ptr,
+                          const StateBlockPtr& _sb9_ptr,
+                          const StateBlockPtr& _sb10_ptr,
+                          const StateBlockPtr& _sb11_ptr,
+                          const StateBlockPtr& _sb12_ptr) :
+             FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>("FactorDummy12",
+                                                nullptr, nullptr, nullptr, nullptr,
+                                                nullptr,
+                                                false, FAC_ACTIVE,
+                                                _sb1_ptr,
+                                                _sb2_ptr,
+                                                _sb3_ptr,
+                                                _sb4_ptr,
+                                                _sb5_ptr,
+                                                _sb6_ptr,
+                                                _sb7_ptr,
+                                                _sb8_ptr,
+                                                _sb9_ptr,
+                                                _sb10_ptr,
+                                                _sb11_ptr,
+                                                _sb12_ptr)
+        {
+            assert(id > 0 && id <= 12);
+        }
+
+        virtual ~FactorDummyZero12() = default;
+
+        virtual std::string getTopology() const override
+        {
+            return std::string("MOTION");
+        }
+
+        template<typename T>
+        bool operator ()(const T* const _st1,
+                         const T* const _st2,
+                         const T* const _st3,
+                         const T* const _st4,
+                         const T* const _st5,
+                         const T* const _st6,
+                         const T* const _st7,
+                         const T* const _st8,
+                         const T* const _st9,
+                         const T* const _st10,
+                         const T* const _st11,
+                         const T* const _st12,
+                         T* _residuals) const
+        {
+            Eigen::Map<Eigen::Matrix<T,ID,1>> residuals(_residuals);
+            switch (id)
+            {
+                case 1:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st1(_st1);
+                    residuals = st1;
+                    break;
+                }
+                case 2:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st2(_st2);
+                    residuals = st2;
+                    break;
+                }
+                case 3:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st3(_st3);
+                    residuals = st3;
+                    break;
+                }
+                case 4:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st4(_st4);
+                    residuals = st4;
+                    break;
+                }
+                case 5:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st5(_st5);
+                    residuals = st5;
+                    break;
+                }
+                case 6:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st6(_st6);
+                    residuals = st6;
+                    break;
+                }
+                case 7:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st7(_st7);
+                    residuals = st7;
+                    break;
+                }
+                case 8:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st8(_st8);
+                    residuals = st8;
+                    break;
+                }
+                case 9:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st9(_st9);
+                    residuals = st9;
+                    break;
+                }
+                case 10:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st10(_st10);
+                    residuals = st10;
+                    break;
+                }
+                case 11:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st11(_st11);
+                    residuals = st11;
+                    break;
+                }
+                case 12:
+                {
+                    Eigen::Map<const Eigen::Matrix<T,ID,1>> st12(_st12);
+                    residuals = st12;
+                    break;
+                }
+                default:
+                    throw std::runtime_error("ID not found");
+            }
+            return true;
+        }
+};
+
+} // namespace wolf
+
+#endif

test/gtest_factor_autodiff.cpp

@@ -13,11 +13,360 @@
 #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"
+#include "dummy/factor_dummy_zero_12.h"
 
 using namespace wolf;
 using namespace Eigen;
 
-TEST(CaptureAutodiff, ConstructorOdom2d)
+template <int First, int Last>
+struct static_for
+{
+    template <typename Lambda>
+    static inline constexpr void apply(Lambda const& f)
+    {
+        if (First < Last)
+        {
+            f(std::integral_constant<int, First>{});
+            static_for<First + 1, Last>::apply(f);
+        }
+    }
+};
+template <int N>
+struct static_for<N, N>
+{
+    template <typename Lambda>
+    static inline constexpr void apply(Lambda const& f) {}
+};
+
+
+TEST(FactorAutodiff, AutodiffDummy1)
+{
+    StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+
+    auto fac = std::make_shared<FactorDummyZero1>(sb);
+
+    // COMPUTE JACOBIANS
+    std::vector<const double*> states_ptr({sb->getStateData()});
+
+    std::vector<Eigen::MatrixXd> J;
+    Eigen::VectorXd residuals(fac->getSize());
+    fac->evaluate(states_ptr, residuals, J);
+
+    ASSERT_MATRIX_APPROX(J[0], Eigen::Matrix1d::Ones(), wolf::Constants::EPS);
+}
+
+TEST(FactorAutodiff, AutodiffDummy12)
+{
+    StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+    StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
+    StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
+    StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
+    StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
+    StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
+    StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
+    StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
+    StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
+    StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
+    StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
+    StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+
+    std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
+    std::vector<Eigen::MatrixXd> J;
+    Eigen::VectorXd residuals;
+    unsigned int i;
+    FactorBasePtr fac = nullptr;
+
+    // 1 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<1>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 2 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<2>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 3 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<3>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 4 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<4>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 5 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<5>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 6 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<6>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 7 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<7>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 8 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<8>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 9 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<9>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 10 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<10>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 11 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<11>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+
+    // 12 ------------------------------------------------------------------------
+    fac = std::make_shared<FactorDummyZero12<12>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+    i = fac->getSize();
+
+    // Jacobian
+    J.clear();
+    residuals.resize(i);
+    fac->evaluate(states_ptr, residuals, J);
+
+    std::cout << "i = " << i << std::endl;
+    for (unsigned int j = 0; j < 12; j++)
+    {
+        std::cout << "j = " << j << std::endl;
+        std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+        if (j == i-1)
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+        }
+        else
+        {
+            ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+        }
+    }
+}
+
+TEST(FactorAutodiff, EmplaceOdom2d)
 {
     FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
     FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
@@ -44,7 +393,7 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
     ASSERT_TRUE(factor_ptr->getFrameOther());
 }
 
-TEST(CaptureAutodiff, ResidualOdom2d)
+TEST(FactorAutodiff, ResidualOdom2d)
 {
     Eigen::Vector3d f1_pose, f2_pose;
     f1_pose << 2,1,M_PI;
@@ -88,7 +437,7 @@ TEST(CaptureAutodiff, ResidualOdom2d)
     ASSERT_TRUE(residuals.minCoeff() > -1e-9);
 }
 
-TEST(CaptureAutodiff, JacobianOdom2d)
+TEST(FactorAutodiff, JacobianOdom2d)
 {
     Eigen::Vector3d f1_pose, f2_pose;
     f1_pose << 2,1,M_PI;
@@ -166,7 +515,7 @@ TEST(CaptureAutodiff, JacobianOdom2d)
 //    std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
 }
 
-TEST(CaptureAutodiff, AutodiffVsAnalytic)
+TEST(FactorAutodiff, AutodiffVsAnalytic)
 {
     Eigen::Vector3d f1_pose, f2_pose;
     f1_pose << 2,1,M_PI;