WIP: Resolve "New factorRelativePose2DWithExtrinsics"

Closes #237 (closed)

test/gtest_factor_autodiff.cpp

@@ -88,12 +88,14 @@ TEST(CaptureAutodiff, ResidualOdom2d)
 
     // EVALUATE
 
-    Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
-    Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
-    Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
-    Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+    const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+    const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+    const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+    const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+    const Eigen::VectorXs sen_pstate = sensor_ptr->getP()->getState();
+    const Eigen::VectorXs sen_ostate = sensor_ptr->getO()->getState();
 
-    std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+    std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data(), sen_pstate.data(), sen_ostate.data()});
 
     double const* const* parameters = states_ptr.data();
     Eigen::VectorXs residuals(factor_ptr->getSize());
@@ -142,8 +144,10 @@ TEST(CaptureAutodiff, JacobianOdom2d)
     const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
     const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
     const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+    const Eigen::VectorXs sen_pstate = sensor_ptr->getP()->getState();
+    const Eigen::VectorXs sen_ostate = sensor_ptr->getO()->getState();
 
-    std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+    std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data(), sen_pstate.data(), sen_ostate.data()});
 
     std::vector<Eigen::MatrixXs> Jauto;
     Eigen::VectorXs residuals(factor_ptr->getSize());
@@ -231,8 +235,10 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
     const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
     const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
     const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+    const Eigen::VectorXs sen_pstate = sensor_ptr->getP()->getState();
+    const Eigen::VectorXs sen_ostate = sensor_ptr->getO()->getState();
 
-    std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
+    std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data(), sen_pstate.data(), sen_ostate.data()});
 
     std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
     Eigen::VectorXs residuals(fac_autodiff_ptr->getSize());
@@ -241,11 +247,8 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
     std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
     t = clock();
     //TODO FactorAnalytic::evaluate
-//    fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
-//    std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
-//
-//    for (auto i = 0; i < Jautodiff.size(); i++)
-//        ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
+    fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+    std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
 }
 
 int main(int argc, char **argv)