From 00af4ec282ac4084027c61d6bcee4e48829c6560 Mon Sep 17 00:00:00 2001 From: yunchang Date: Wed, 17 Jul 2019 16:52:44 -0400 Subject: [PATCH 1/3] testing performance of slow but correct --- gtsam/slam/tests/testBetweenFactor.cpp | 55 ++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d995bf8e7f..b9e6201e5a 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -5,12 +5,16 @@ * @date Aug 2, 2013 */ +// #define SLOW_BUT_CORRECT_BETWEENFACTOR + #include #include #include #include #include +#include + using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -44,6 +48,57 @@ TEST(BetweenFactor, Rot3) { EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } +/* ************************************************************************* * + +// Timing and error evaluation for evaluate error + +TEST(BetweenFactor, SlowButCorrectCompare) { + Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6); + Rot3 noise = Rot3::Rodrigues(0.01, 0.01, 0.01); // Uncomment to make unit test fail + Rot3 measured = R1.between(R2)*noise ; + BetweenFactor factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05)); + + Vector3 actual; + Matrix actualH1 = Eigen::MatrixXd::Zero(3,3); + Matrix actualH2 = Eigen::MatrixXd::Zero(3,3); + double duration = 0.0; + + size_t sample_size = 100; + for (size_t i = 0; i < sample_size; i++) { + Matrix H1, H2; + auto start = std::chrono::high_resolution_clock::now(); + Vector3 error = factor.evaluateError(R1, R2, H1, H2); + auto finish = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = finish - start; + duration = duration + elapsed.count(); + actual = actual + error; + actualH1 = actualH1 + H1; + actualH2 = actualH2 + H2; + } + + actual = actual / sample_size; + actualH1 = actualH1 / sample_size; + actualH2 = actualH2 / sample_size; + duration = duration / sample_size; + + Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), R1, R2, 1e-5); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), R1, R2, 1e-5); + + std::cout << "measurement error: \n" << (expected - actual).array().abs() << std::endl; + std::cout << "H1 error: \n" << (numericalH1 - actualH1).array().abs() << std::endl; + std::cout << "H2 error: \n" << (numericalH2 - actualH2).array().abs() << std::endl; + std::cout << "Average runtime for evaluaterError: " << duration << std::endl; +} + /* ************************************************************************* */ /* // Constructor scalar From 6d27c376f921f8ff28dd659f9ac2472d6c6191d3 Mon Sep 17 00:00:00 2001 From: yunzc Date: Sat, 27 Jul 2019 21:14:50 -0400 Subject: [PATCH 2/3] Update testBetweenFactor.cpp --- gtsam/slam/tests/testBetweenFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index b9e6201e5a..294bdc2ebd 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -63,7 +63,7 @@ TEST(BetweenFactor, SlowButCorrectCompare) { Matrix actualH1 = Eigen::MatrixXd::Zero(3,3); Matrix actualH2 = Eigen::MatrixXd::Zero(3,3); double duration = 0.0; - + // Running samples to get the timing of evaluateError size_t sample_size = 100; for (size_t i = 0; i < sample_size; i++) { Matrix H1, H2; From f3ccbffe1a8633a9e2765fd670d0831330caa17a Mon Sep 17 00:00:00 2001 From: yunzc Date: Sat, 27 Jul 2019 21:17:30 -0400 Subject: [PATCH 3/3] Update testBetweenFactor.cpp --- gtsam/slam/tests/testBetweenFactor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 294bdc2ebd..b90a89a712 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -55,8 +55,8 @@ TEST(BetweenFactor, Rot3) { TEST(BetweenFactor, SlowButCorrectCompare) { Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3); Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6); - Rot3 noise = Rot3::Rodrigues(0.01, 0.01, 0.01); // Uncomment to make unit test fail - Rot3 measured = R1.between(R2)*noise ; + Rot3 noise = Rot3::Rodrigues(0.01, 0.01, 0.01); + Rot3 measured = R1.between(R2)*noise ; // Some noisy measurement BetweenFactor factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05)); Vector3 actual; @@ -81,7 +81,7 @@ TEST(BetweenFactor, SlowButCorrectCompare) { actualH1 = actualH1 / sample_size; actualH2 = actualH2 / sample_size; duration = duration / sample_size; - + // Comparing calculated Jacobian to numerically obtained Jacobian Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); Matrix numericalH1 = numericalDerivative21( boost::function(boost::bind(