Esempio n. 1
0
/* ************************************************************************* */
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;
}
Esempio n. 2
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);
}