testPose3Upright.cpp
Go to the documentation of this file.
1 
9 
12 
14 
15 using namespace gtsam;
16 
17 static const double tol = 1e-5;
18 
19 /* ************************************************************************* */
20 TEST( testPose3Upright, basics ) {
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));
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 
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());
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());
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 /* ************************************************************************* */
124 TEST( testPose3Upright, inverse ) {
125  Pose3Upright x1(1.0, 2.0, 3.0, 0.1);
126  Pose3Upright expected(x1.pose2().inverse(), - x1.z());
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 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
static int runAllTests(TestResult &result)
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
Pose3Upright inverse_proxy(const Pose3Upright &x1)
Matrix expected
Definition: testMatrix.cpp:971
double theta() const
Definition: Pose3Upright.h:67
Vector2 Point2
Definition: Point2.h:32
double z() const
get z
Definition: Pose3.h:302
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Pose3Upright retract(const Vector &v) const
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
double x() const
Definition: Pose3Upright.h:64
Rot3 rotation() const
Some functions to compute numerical derivatives.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
double z() const
Definition: Pose3Upright.h:66
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
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.
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)
Rot2 rotation2() const
int main()
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
Point3 translation() const
Eigen::VectorXd Vector
Definition: Vector.h:38
Pose3Upright inverse(OptionalJacobian< 4, 4 > H1={}) const
inverse transformation with derivatives
Pose3Upright compose(const Pose3Upright &p2, OptionalJacobian< 4, 4 > H1={}, OptionalJacobian< 4, 4 > H2={}) const
compose this transformation onto another (first *this and then p2)
#define EXPECT(condition)
Definition: Test.h:150
Point2 translation2() const
Vector localCoordinates(const Pose3Upright &p2) const
Local 3D coordinates of Pose3Upright manifold neighborhood around current pose.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
size_t dim() const
Dimensionality of tangent space = 4 DOF.
Definition: Pose3Upright.h:84
Class compose(const Class &g) const
Definition: Lie.h:48
Pose2 pose2() const
traits
Definition: chartTesting.h:28
double y() const
Definition: Pose3Upright.h:65
Pose3Upright between_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Class between(const Class &g) const
Definition: Lie.h:52
Pose3 x1
Definition: testPose3.cpp:663
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
Pose3Upright between(const Pose3Upright &p2, OptionalJacobian< 4, 4 > H1={}, OptionalJacobian< 4, 4 > H2={}) const
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
Pose3 pose() const
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Pose3Upright compose_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
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