From 77b8d9410760f4d26053b1b5c4ac943af23fab5c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 3 Jul 2019 08:51:45 +0200
Subject: [PATCH] adapted gtest_factor_autodiff to factor_odom2D with
extrinsics
---
test/gtest_factor_autodiff.cpp | 27 +++++++++++++++------------
1 file changed, 15 insertions(+), 12 deletions(-)
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 108ae75cd..b25e66371 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/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)
--
GitLab