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 /* ************************************************************************* */
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Pose3Upright::Expmap
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
Definition: Pose3Upright.cpp:152
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
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:12
inverse_proxy
Pose3Upright inverse_proxy(const Pose3Upright &x1)
Definition: testPose3Upright.cpp:123
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Pose3Upright::theta
double theta() const
Definition: Pose3Upright.h:67
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Pose3::z
double z() const
get z
Definition: Pose3.h:302
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Pose3Upright::retract
Pose3Upright retract(const Vector &v) const
Definition: Pose3Upright.cpp:137
gtsam::Pose3Upright::Logmap
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3Upright.cpp:159
gtsam::Pose3Upright::x
double x() const
Definition: Pose3Upright.h:64
gtsam::Pose3Upright::rotation
Rot3 rotation() const
Definition: Pose3Upright.cpp:68
numericalDerivative.h
Some functions to compute numerical derivatives.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::Pose3Upright::z
double z() const
Definition: Pose3Upright.h:66
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
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::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::rotation2
Rot2 rotation2() const
Definition: Pose3Upright.cpp:63
main
int main()
Definition: testPose3Upright.cpp:136
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
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
gtsam::Pose3Upright::translation
Point3 translation() const
Definition: Pose3Upright.cpp:53
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Pose3Upright::inverse
Pose3Upright inverse(OptionalJacobian< 4, 4 > H1={}) const
inverse transformation with derivatives
Definition: Pose3Upright.cpp:83
gtsam::Pose3Upright::compose
Pose3Upright compose(const Pose3Upright &p2, OptionalJacobian< 4, 4 > H1={}, OptionalJacobian< 4, 4 > H2={}) const
compose this transformation onto another (first *this and then p2)
Definition: Pose3Upright.cpp:99
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Pose3Upright::translation2
Point2 translation2() const
Definition: Pose3Upright.cpp:58
gtsam::Pose3Upright::localCoordinates
Vector localCoordinates(const Pose3Upright &p2) const
Local 3D coordinates of Pose3Upright manifold neighborhood around current pose.
Definition: Pose3Upright.cpp:144
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Pose3Upright::dim
size_t dim() const
Dimensionality of tangent space = 4 DOF.
Definition: Pose3Upright.h:84
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::Pose3Upright::pose2
Pose2 pose2() const
Definition: Pose3Upright.cpp:73
gtsam
traits
Definition: chartTesting.h:28
gtsam::Pose3Upright::y
double y() const
Definition: Pose3Upright.h:65
between_proxy
Pose3Upright between_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
Definition: testPose3Upright.cpp:93
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
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::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
gtsam::Pose3Upright::between
Pose3Upright between(const Pose3Upright &p2, OptionalJacobian< 4, 4 > H1={}, OptionalJacobian< 4, 4 > H2={}) const
Definition: Pose3Upright.cpp:118
gtsam::tol
const G double tol
Definition: Group.h:86
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
gtsam::Pose3Upright::pose
Pose3 pose() const
Definition: Pose3Upright.cpp:78
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
compose_proxy
Pose3Upright compose_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
Definition: testPose3Upright.cpp:108
Pose3Upright.h
Variation of a Pose3 in which the rotation is constained to purely yaw This state is essentially a Po...

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:56