testPose3Upright.cpp
Go to the documentation of this file.
1 
8 #include <CppUnitLite/TestHarness.h>
9 
10 #include <gtsam/base/TestableAssertions.h>
11 #include <gtsam/base/numericalDerivative.h>
12 
13 #include <gtsam_unstable/geometry/Pose3Upright.h>
14 
15 using namespace gtsam;
16 
17 static const double tol = 1e-5;
18 
19 /* ************************************************************************* */
20 TEST( testPose3Upright, basics ) {
21  Pose3Upright origin;
22  EXPECT_DOUBLES_EQUAL(0.0, origin.x(), tol);
23  EXPECT_DOUBLES_EQUAL(0.0, origin.y(), tol);
24  EXPECT_DOUBLES_EQUAL(0.0, origin.z(), tol);
25  EXPECT_DOUBLES_EQUAL(0.0, origin.theta(), tol);
26 
27  Pose3Upright actual1(Rot2::fromAngle(0.1), Point3(1.0, 2.0, 3.0));
28  EXPECT_DOUBLES_EQUAL(1.0, actual1.x(), tol);
29  EXPECT_DOUBLES_EQUAL(2.0, actual1.y(), tol);
30  EXPECT_DOUBLES_EQUAL(3.0, actual1.z(), tol);
31  EXPECT_DOUBLES_EQUAL(0.1, actual1.theta(), tol);
32 
33  Pose3Upright actual2(1.0, 2.0, 3.0, 0.1);
34  EXPECT_DOUBLES_EQUAL(1.0, actual2.x(), tol);
35  EXPECT_DOUBLES_EQUAL(2.0, actual2.y(), tol);
36  EXPECT_DOUBLES_EQUAL(3.0, actual2.z(), tol);
37  EXPECT_DOUBLES_EQUAL(0.1, actual2.theta(), tol);
38 }
39 
40 /* ************************************************************************* */
41 TEST( testPose3Upright, equals ) {
42  Pose3Upright origin, actual1(1.0, 2.0, 3.0, 0.1),
43  actual2(1.0, 2.0, 3.0, 0.1), actual3(4.0,-7.0, 3.0, 0.3);
44  EXPECT(assert_equal(origin, origin, tol));
45  EXPECT(assert_equal(actual1, actual1, tol));
46  EXPECT(assert_equal(actual1, actual2, tol));
47  EXPECT(assert_equal(actual2, actual1, tol));
48 
49  EXPECT(assert_inequal(actual1, actual3, tol));
50  EXPECT(assert_inequal(actual3, actual1, tol));
51  EXPECT(assert_inequal(actual1, origin, tol));
52  EXPECT(assert_inequal(origin, actual1, tol));
53 }
54 
55 /* ************************************************************************* */
56 TEST( testPose3Upright, conversions ) {
57  Pose3Upright pose(1.0, 2.0, 3.0, 0.1);
58  EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol));
59  EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol));
60  EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol));
61  EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol));
62  EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol));
63  EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
64 }
65 
66 /* ************************************************************************* */
67 TEST( testPose3Upright, manifold ) {
68  Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0);
69  EXPECT_LONGS_EQUAL(4, origin.dim());
70 
71  EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol));
72  EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol));
73  EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol));
74 
75  Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12;
76  EXPECT(assert_equal(x2, x1.retract(delta12), tol));
77  EXPECT(assert_equal(x1, x2.retract(delta21), tol));
78 
79  EXPECT(assert_equal(delta12, x1.localCoordinates(x2), tol));
80  EXPECT(assert_equal(delta21, x2.localCoordinates(x1), tol));
81 }
82 
83 /* ************************************************************************* */
84 TEST( testPose3Upright, lie ) {
85  Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1);
86  EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol));
87  EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol));
88 
89  EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol));
90 }
91 
92 /* ************************************************************************* */
93 Pose3Upright between_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.between(x2); }
94 TEST( testPose3Upright, between ) {
95  Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3);
96  Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z());
97  EXPECT(assert_equal(expected, x1.between(x2), tol));
98 
99  Matrix actualH1, actualH2, numericH1, numericH2;
100  x1.between(x2, actualH1, actualH2);
101  numericH1 = numericalDerivative21(between_proxy, x1, x2, 1e-5);
102  numericH2 = numericalDerivative22(between_proxy, x1, x2, 1e-5);
103  EXPECT(assert_equal(numericH1, actualH1, tol));
104  EXPECT(assert_equal(numericH2, actualH2, tol));
105 }
106 
107 /* ************************************************************************* */
108 Pose3Upright compose_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.compose(x2); }
109 TEST( testPose3Upright, compose ) {
110  Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3);
111  Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z());
112  EXPECT(assert_equal(x2, x1.compose(expected), tol));
113 
114  Matrix actualH1, actualH2, numericH1, numericH2;
115  x1.compose(expected, actualH1, actualH2);
116  numericH1 = numericalDerivative21(compose_proxy, x1, expected, 1e-5);
117  numericH2 = numericalDerivative22(compose_proxy, x1, expected, 1e-5);
118  EXPECT(assert_equal(numericH1, actualH1, tol));
119  EXPECT(assert_equal(numericH2, actualH2, tol));
120 }
121 
122 /* ************************************************************************* */
123 Pose3Upright inverse_proxy(const Pose3Upright& x1) { return x1.inverse(); }
124 TEST( testPose3Upright, inverse ) {
125  Pose3Upright x1(1.0, 2.0, 3.0, 0.1);
126  Pose3Upright expected(x1.pose2().inverse(), - x1.z());
127  EXPECT(assert_equal(expected, x1.inverse(), tol));
128 
129  Matrix actualH1, numericH1;
130  x1.inverse(actualH1);
131  numericH1 = numericalDerivative11(inverse_proxy, x1, 1e-5);
132  EXPECT(assert_equal(numericH1, actualH1, tol));
133 }
134 
135 /* ************************************************************************* */
136 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
137 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
main
int main()
Definition: testPose3Upright.cpp:136
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
gtsam::Pose3Upright::z
double z() const
Definition: Pose3Upright.h:66
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
compose_proxy
Pose3Upright compose_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
Definition: testPose3Upright.cpp:108
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
inverse_proxy
Pose3Upright inverse_proxy(const Pose3Upright &x1)
Definition: testPose3Upright.cpp:123
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Pose3Upright::theta
double theta() const
Definition: Pose3Upright.h:67
gtsam::Pose3Upright::Expmap
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
Definition: Pose3Upright.cpp:152
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
numericalDerivative.h
Some functions to compute numerical derivatives.
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
gtsam::Pose3Upright::Logmap
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3Upright.cpp:159
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Pose3Upright.h
Variation of a Pose3 in which the rotation is constained to purely yaw This state is essentially a Po...
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
between_proxy
Pose3Upright between_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
Definition: testPose3Upright.cpp:93
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
gtsam::Pose3Upright::y
double y() const
Definition: Pose3Upright.h:65
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Definition: gnuplot_common_settings.hh:45
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Pose3Upright::x
double x() const
Definition: Pose3Upright.h:64
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
gtsam::Pose3Upright
Definition: Pose3Upright.h:25
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::Pose3::z
double z() const
get z
Definition: Pose3.h:302
gtsam::Pose2
Definition: Pose2.h:39

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:54