testManifold.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------1------------------------------------------- */
11 
20 #include <gtsam/base/Manifold.h>
21 #include <gtsam/geometry/PinholeCamera.h>
22 #include <gtsam/geometry/Pose2.h>
23 #include <gtsam/geometry/Cal3_S2.h>
24 #include <gtsam/geometry/Cal3Bundler.h>
25 #include <gtsam/base/VectorSpace.h>
26 #include <gtsam/base/testLie.h>
27 #include <gtsam/base/Testable.h>
28 
29 #undef CHECK
30 #include <CppUnitLite/TestHarness.h>
31 
32 using namespace std;
33 using namespace gtsam;
34 
35 // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
36 typedef PinholeCamera<Cal3Bundler> Camera;
37 
38 //******************************************************************************
39 TEST(Manifold, SomeManifoldsGTSAM) {
40  //GTSAM_CONCEPT_ASSERT(IsManifold<int>); // integer is not a manifold
41  GTSAM_CONCEPT_ASSERT(IsManifold<Camera>);
42  GTSAM_CONCEPT_ASSERT(IsManifold<Cal3_S2>);
43  GTSAM_CONCEPT_ASSERT(IsManifold<Cal3Bundler>);
44  GTSAM_CONCEPT_ASSERT(IsManifold<Camera>);
45 }
46 
47 //******************************************************************************
48 TEST(Manifold, SomeLieGroupsGTSAM) {
49  GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot2>);
50  GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose2>);
51  GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot3>);
52  GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose3>);
53 }
54 
55 //******************************************************************************
56 TEST(Manifold, SomeVectorSpacesGTSAM) {
57  GTSAM_CONCEPT_ASSERT(IsVectorSpace<double>);
58  GTSAM_CONCEPT_ASSERT(IsVectorSpace<float>);
59  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>);
60  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
61 }
62 
63 //******************************************************************************
64 // dimension
65 TEST(Manifold, _dimension) {
66  //using namespace traits;
67  EXPECT_LONGS_EQUAL(2, traits<Point2>::dimension);
68  EXPECT_LONGS_EQUAL(8, traits<Matrix24>::dimension);
69  EXPECT_LONGS_EQUAL(1, traits<double>::dimension);
70 }
71 
72 //******************************************************************************
73 TEST(Manifold, Identity) {
74  EXPECT_DOUBLES_EQUAL(0.0, traits<double>::Identity(), 0.0);
75  EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits<Matrix24>::Identity())));
76  EXPECT(assert_equal(Pose3(), traits<Pose3>::Identity()));
77  EXPECT(assert_equal(Point2(0,0), traits<Point2>::Identity()));
78 }
79 
80 //******************************************************************************
81 // charts
82 TEST(Manifold, DefaultChart) {
83 
84  //DefaultChart<Point2> chart1;
85  EXPECT(traits<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
86  EXPECT(traits<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
87 
88  Vector v2(2);
89  v2 << 1, 0;
90  //DefaultChart<Vector2> chart2;
91  EXPECT(assert_equal(v2, traits<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
92  EXPECT(traits<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));
93 
94  {
95  typedef Matrix2 ManifoldPoint;
96  ManifoldPoint m;
97  //DefaultChart<ManifoldPoint> chart;
98  m << 1, 3,
99  2, 4;
100  // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
101  EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
102  EXPECT(traits<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
103  }
104 
105  {
106  typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
107  ManifoldPoint m;
108  //DefaultChart<ManifoldPoint> chart;
109  m << 1, 2;
110  EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
111  EXPECT(traits<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
112  }
113 
114  {
115  typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
116  ManifoldPoint m;
117  //DefaultChart<ManifoldPoint> chart;
118  m << 1;
119  EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
120  EXPECT(traits<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
121  }
122 
123  //DefaultChart<double> chart3;
124  Vector v1(1);
125  v1 << 1;
126  EXPECT(assert_equal(v1, traits<double>::Local(0, 1)));
127  EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
128 
129  // Dynamic does not work yet !
130  Vector z = Z_2x1, v(2);
131  v << 1, 0;
132  //DefaultChart<Vector> chart4;
133 // EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
134 // EXPECT(assert_equal(traits<Vector>::Retract(z, v), v));
135 
136  Vector v3(3);
137  v3 << 1, 1, 1;
138  Rot3 I = Rot3::Identity();
139  Rot3 R = I.retract(v3);
140  //DefaultChart<Rot3> chart5;
141  EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
142  EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
143  // Check zero vector
144  //DefaultChart<Rot3> chart6;
145  EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
146 }
147 
148 //******************************************************************************
149 int main() {
150  TestResult tr;
151  return TestRegistry::runAllTests(tr);
152 }
153 //******************************************************************************
154 
Camera
PinholeCamera< Cal3Bundler > Camera
Definition: testManifold.cpp:36
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
I
#define I
Definition: main.h:112
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
v2
Vector v2
Definition: testSerializationBase.cpp:39
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
R
Rot2 R(Rot2::fromAngle(0.1))
std
Definition: BFloat16.h:88
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
PinholeCamera.h
Base class for all pinhole cameras.
v3
Vector v3
Definition: testSerializationBase.cpp:40
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
Manifold.h
Base class and basic functions for Manifold types.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TEST
TEST(Manifold, SomeManifoldsGTSAM)
Definition: testManifold.cpp:39
gtsam
traits
Definition: chartTesting.h:28
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
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
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
main
int main()
Definition: testManifold.cpp:149
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
Pose2.h
2D Pose
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
Cal3Bundler.h
Calibration used by Bundler.
Cal3_S2.h
The most common 5DOF 3D->2D calibration.

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