160 lines
5.3 KiB
C++
160 lines
5.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 r1.inverse() * 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).finished();
|
|
}
|
|
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).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;
|
|
}
|