/* ************************************************************************* */
int main()
{
int n = 50000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl;
// create a random pose:
double x = 4.0, y = 2.0, r = 0.3;
Vector v = (Vector(3) << x, y, r).finished();
Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
TEST(Expmap, Pose2::Expmap(v));
TEST(Retract, X.retract(v));
TEST(Logmap, Pose2::Logmap(X2));
TEST(localCoordinates, X.localCoordinates(X2));
Matrix H1, H2;
Matrix3 H1f, H2f;
TEST(Pose2BetweenFactorEvaluateErrorDefault, Pose2BetweenFactorEvaluateErrorDefault(X3, X, X2, H1, H2));
TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetween, Pose2BetweenFactorEvaluateErrorOptimizedBetween(X3, X, X2, H1, H2));
TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(X3, X, X2, H1f, H2f));
// Print timings
tictoc_print_();
return 0;
}
/* ************************************************************************* */
Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
{
Pose2 hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return measured.localCoordinates(hx);
}