diff --git a/gtsam/geometry/tests/timePose2.cpp b/gtsam/geometry/tests/timePose2.cpp new file mode 100644 index 000000000..139601664 --- /dev/null +++ b/gtsam/geometry/tests/timePose2.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timePose2.cpp + * @brief Time Pose2 geometry + * @author Richard Roberts + */ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#define TEST(TITLE,STATEMENT) \ + gttic_(TITLE); \ + for(int i = 0; i < n; i++) \ + STATEMENT; \ + gttoc_(TITLE); + +/* ************************************************************************* */ +Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) { + return between_default(r1, r2); +} + +/* ************************************************************************* */ +Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, + boost::optional H1 = boost::none, boost::optional H2 = boost::none) { + // get cosines and sines from rotation matrices + const Rot2& R1 = r1.r(), R2 = r2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = r2.t() - r1.t(); + double x = dt.x(), y = dt.y(); + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above + if (H1) { + double dt1 = -s2 * x + c2 * y; + double dt2 = -c2 * x - s2 * y; + *H1 = Matrix_(3,3, + -c, -s, dt1, + s, -c, dt2, + 0.0, 0.0,-1.0); + } + if (H2) *H2 = Matrix::Identity(3,3); + + return Pose2(R,t); +} + +/* ************************************************************************* */ +Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, + boost::optional H1, boost::optional H2) +{ + // get cosines and sines from rotation matrices + const Rot2& R1 = r1.r(), R2 = r2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = r2.t() - r1.t(); + double x = dt.x(), y = dt.y(); + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above + if (H1) { + double dt1 = -s2 * x + c2 * y; + double dt2 = -c2 * x - s2 * y; + *H1 = Matrix3(); (*H1) << + -c, -s, dt1, + s, -c, dt2, + 0.0, 0.0,-1.0; + } + if (H2) *H2 = Matrix3::Identity(); + + return Pose2(R,t); +} + +/* ************************************************************************* */ +Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2, + boost::optional H1, boost::optional H2) +{ + Pose2 hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measured.localCoordinates(hx); +} + +/* ************************************************************************* */ +Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, const Pose2& p1, const Pose2& p2, + boost::optional H1, boost::optional H2) +{ + Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + return Pose2::Logmap(Pose2betweenOptimized(measured, hx)); +} + +/* ************************************************************************* */ +Vector Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Pose2& measured, const Pose2& p1, const Pose2& p2, + boost::optional H1, boost::optional H2) +{ + // TODO: Implement + Pose2 hx = Pose2betweenOptimized(p1, p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + return Pose2::Logmap(Pose2betweenOptimized(measured, hx)); +} + +/* ************************************************************************* */ +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); + 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; +} diff --git a/gtsam/geometry/tests/timeRot2.cpp b/gtsam/geometry/tests/timeRot2.cpp new file mode 100644 index 000000000..6d828ef9c --- /dev/null +++ b/gtsam/geometry/tests/timeRot2.cpp @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeRot2.cpp + * @brief Time Rot2 geometry + * @author Richard Roberts + */ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#define TEST(TITLE,STATEMENT) \ + gttic_(TITLE); \ + for(int i = 0; i < n; i++) \ + STATEMENT; \ + gttoc_(TITLE); + +/* ************************************************************************* */ +Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) { + return between_default(r1, r2); +} + +/* ************************************************************************* */ +Rot2 Rot2betweenOptimized(const Rot2& r1, const Rot2& r2) { + // Same as compose but sign of sin for r1 is reversed + return Rot2::fromCosSin(r1.c() * r2.c() + r1.s() * r2.s(), -r1.s() * r2.c() + r1.c() * r2.s()); +} + +/* ************************************************************************* */ +Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2& measured, const Rot2& p1, const Rot2& p2, + boost::optional H1, boost::optional H2) +{ + Rot2 hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measured.localCoordinates(hx); +} + +/* ************************************************************************* */ +Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, const Rot2& p1, const Rot2& p2, + boost::optional H1, boost::optional H2) +{ + Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) + if (H1) *H1 = -eye(1); + if (H2) *H2 = eye(1); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); +} + +/* ************************************************************************* */ +Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, const Rot2& p1, const Rot2& p2, + boost::optional H1, boost::optional H2) +{ + // TODO: Implement + Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) + if (H1) *H1 = -Matrix::Identity(1,1); + if (H2) *H2 = Matrix::Identity(1,1); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); +} + +/* ************************************************************************* */ +typedef Eigen::Matrix Matrix1; +Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2& measured, const Rot2& p1, const Rot2& p2, + boost::optional H1, boost::optional H2) +{ + // TODO: Implement + Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) + if (H1) *H1 = -Matrix1::Identity(); + if (H2) *H2 = Matrix1::Identity(); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); +} + +/* ************************************************************************* */ +int main() +{ + int n = 50000000; + cout << "NOTE: Times are reported for " << n << " calls" << endl; + + // create a random direction: + double norm=sqrt(16.0+4.0); + double x=4.0/norm, y=2.0/norm; + Vector v = Vector_(2,x,y); + Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); + + TEST(Rot2_Expmap, Rot2::Expmap(v)); + TEST(Rot2_Retract, R.retract(v)); + TEST(Rot2_Logmap, Rot2::Logmap(R2)); + TEST(Rot2_between, R.between(R2)); + TEST(betweenDefault, Rot2betweenDefault(R, R2)); + TEST(betweenOptimized, Rot2betweenOptimized(R, R2)); + TEST(Rot2_localCoordinates, R.localCoordinates(R2)); + + Matrix H1, H2; + Matrix1 H1f, H2f; + TEST(Rot2BetweenFactorEvaluateErrorDefault, Rot2BetweenFactorEvaluateErrorDefault(R3, R, R2, H1, H2)); + TEST(Rot2BetweenFactorEvaluateErrorOptimizedBetween, Rot2BetweenFactorEvaluateErrorOptimizedBetween(R3, R, R2, H1, H2)); + TEST(Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye, Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(R3, R, R2, H1, H2)); + TEST(Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(R3, R, R2, H1f, H2f)); + + // Print timings + tictoc_print_(); + + return 0; +}