Added timing scripts for trying alternative implementations of Rot2 and Pose2 functions
Richard Roberts
parent
2c53a58c9f
commit
5548d78071
|
|
@ -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 <iostream>
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix3&> H1, boost::optional<Matrix3&> 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<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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2BetweenFactorEvaluateErrorOptimizedBetween(const Pose2& measured, const Pose2& p1, const Pose2& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3&> H1, boost::optional<Matrix3&> 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;
|
||||
}
|
||||
|
|
@ -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 <iostream>
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
|
||||
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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<double,1,1> Matrix1;
|
||||
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2& measured, const Rot2& p1, const Rot2& p2,
|
||||
boost::optional<Matrix1&> H1, boost::optional<Matrix1&> 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;
|
||||
}
|
||||
Loading…
Reference in New Issue