Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Testing performance of SLOW_BUT_CORRECT_BETWEENFACTOR #88

Open
wants to merge 3 commits into
base: develop
Choose a base branch
from
+55 −0
Open

Testing performance of SLOW_BUT_CORRECT_BETWEENFACTOR #88

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
00af4ec
testing performance of slow but correct
yunzc Jul 17, 2019
6d27c37
Update testBetweenFactor.cpp
yunzc Jul 28, 2019
f3ccbff
Update testBetweenFactor.cpp
yunzc Jul 28, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 55 additions & 0 deletions gtsam/slam/tests/testBetweenFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,16 @@
* @date Aug 2, 2013
*/

// #define SLOW_BUT_CORRECT_BETWEENFACTOR

#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>

#include <chrono>

using namespace gtsam;
using namespace gtsam::symbol_shorthand;
using namespace gtsam::noiseModel;
Expand Down Expand Up @@ -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);
Rot3 measured = R1.between(R2)*noise ; // Some noisy measurement
BetweenFactor<Rot3> 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;
// 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;
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<double> 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;
// Comparing calculated Jacobian to numerically obtained Jacobian
Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2));
Matrix numericalH1 = numericalDerivative21<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);

Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::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
Expand Down