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 #include <boost/assign/list_of.hpp>
33 using boost::assign::list_of;
34 using boost::assign::map_list_of;
35 
36 using namespace std;
37 using namespace gtsam;
38 
39 // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
40 typedef PinholeCamera<Cal3Bundler> Camera;
41 
42 //******************************************************************************
43 TEST(Manifold, SomeManifoldsGTSAM) {
44  //BOOST_CONCEPT_ASSERT((IsManifold<int>)); // integer is not a manifold
45  BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
46  BOOST_CONCEPT_ASSERT((IsManifold<Cal3_S2>));
47  BOOST_CONCEPT_ASSERT((IsManifold<Cal3Bundler>));
48  BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
49 }
50 
51 //******************************************************************************
52 TEST(Manifold, SomeLieGroupsGTSAM) {
53  BOOST_CONCEPT_ASSERT((IsLieGroup<Rot2>));
54  BOOST_CONCEPT_ASSERT((IsLieGroup<Pose2>));
55  BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3>));
56  BOOST_CONCEPT_ASSERT((IsLieGroup<Pose3>));
57 }
58 
59 //******************************************************************************
60 TEST(Manifold, SomeVectorSpacesGTSAM) {
61  BOOST_CONCEPT_ASSERT((IsVectorSpace<double>));
62  BOOST_CONCEPT_ASSERT((IsVectorSpace<float>));
63  BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>));
64  BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>));
65 }
66 
67 //******************************************************************************
68 // dimension
69 TEST(Manifold, _dimension) {
70  //using namespace traits;
71  EXPECT_LONGS_EQUAL(2, traits<Point2>::dimension);
72  EXPECT_LONGS_EQUAL(8, traits<Matrix24>::dimension);
73  EXPECT_LONGS_EQUAL(1, traits<double>::dimension);
74 }
75 
76 //******************************************************************************
77 TEST(Manifold, Identity) {
78  EXPECT_DOUBLES_EQUAL(0.0, traits<double>::Identity(), 0.0);
79  EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits<Matrix24>::Identity())));
80  EXPECT(assert_equal(Pose3(), traits<Pose3>::Identity()));
81  EXPECT(assert_equal(Point2(0,0), traits<Point2>::Identity()));
82 }
83 
84 //******************************************************************************
85 // charts
86 TEST(Manifold, DefaultChart) {
87 
88  //DefaultChart<Point2> chart1;
89  EXPECT(traits<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
90  EXPECT(traits<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
91 
92  Vector v2(2);
93  v2 << 1, 0;
94  //DefaultChart<Vector2> chart2;
95  EXPECT(assert_equal(v2, traits<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
96  EXPECT(traits<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));
97 
98  {
99  typedef Matrix2 ManifoldPoint;
100  ManifoldPoint m;
101  //DefaultChart<ManifoldPoint> chart;
102  m << 1, 3,
103  2, 4;
104  // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
105  EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
106  EXPECT(traits<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
107  }
108 
109  {
110  typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
111  ManifoldPoint m;
112  //DefaultChart<ManifoldPoint> chart;
113  m << 1, 2;
114  EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
115  EXPECT(traits<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
116  }
117 
118  {
119  typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
120  ManifoldPoint m;
121  //DefaultChart<ManifoldPoint> chart;
122  m << 1;
123  EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
124  EXPECT(traits<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
125  }
126 
127  //DefaultChart<double> chart3;
128  Vector v1(1);
129  v1 << 1;
130  EXPECT(assert_equal(v1, traits<double>::Local(0, 1)));
131  EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
132 
133  // Dynamic does not work yet !
134  Vector z = Z_2x1, v(2);
135  v << 1, 0;
136  //DefaultChart<Vector> chart4;
137 // EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
138 // EXPECT(assert_equal(traits<Vector>::Retract(z, v), v));
139 
140  Vector v3(3);
141  v3 << 1, 1, 1;
142  Rot3 I = Rot3::identity();
143  Rot3 R = I.retract(v3);
144  //DefaultChart<Rot3> chart5;
145  EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
146  EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
147  // Check zero vector
148  //DefaultChart<Rot3> chart6;
149  EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
150 }
151 
152 //******************************************************************************
153 int main() {
154  TestResult tr;
155  return TestRegistry::runAllTests(tr);
156 }
157 //******************************************************************************
158 
Camera
PinholeCamera< Cal3Bundler > Camera
Definition: testManifold.cpp:40
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
v2
Vector v2
Definition: testSerializationBase.cpp:36
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:35
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
v
ArrayXcf v
Definition: Cwise_arg.cpp:1
gtsam::BOOST_CONCEPT_ASSERT
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
R
Rot2 R(Rot2::fromAngle(0.1))
std
Definition: Half.h:150
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
PinholeCamera.h
Base class for all pinhole cameras.
v3
Vector v3
Definition: testSerializationBase.cpp:37
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:151
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::lago::I
static const Matrix I
Definition: lago.cpp:35
TEST
TEST(Manifold, SomeManifoldsGTSAM)
Definition: testManifold.cpp:43
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
main
int main()
Definition: testManifold.cpp:153
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 Sat May 8 2021 02:48:04