testSO4.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  * -------------------------------------------------------------------------- */
11 
18 #include <gtsam/base/Manifold.h>
19 #include <gtsam/base/Testable.h>
20 #include <gtsam/base/lieProxies.h>
22 #include <gtsam/geometry/SO4.h>
23 #include <gtsam/geometry/SO3.h>
24 
26 
27 #include <iostream>
28 #include <random>
29 
30 using namespace std;
31 using namespace gtsam;
32 
33 //******************************************************************************
34 TEST(SO4, Identity) {
35  const SO4 R;
36  EXPECT_LONGS_EQUAL(4, R.rows());
37  EXPECT_LONGS_EQUAL(6, SO4::dimension);
38  EXPECT_LONGS_EQUAL(6, SO4::Dim());
39  EXPECT_LONGS_EQUAL(6, R.dim());
41 }
42 
43 //******************************************************************************
44 TEST(SO4, Concept) {
46  BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
48 }
49 
50 //******************************************************************************
52 Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
54 Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
56 Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
58 
59 //******************************************************************************
60 TEST(SO4, Random) {
61  std::mt19937 rng(42);
62  auto Q = SO4::Random(rng);
64 }
65 //******************************************************************************
67  // If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
68  auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
69  EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
70 
71  // Same here
72  auto R2 = SO3::Expmap(v2.tail<3>()).matrix();
73  EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2)));
74 
75  // Check commutative subgroups
76  for (size_t i = 0; i < 6; i++) {
77  Vector6 xi = Vector6::Zero();
78  xi[i] = 2;
79  SO4 Q1 = SO4::Expmap(xi);
80  xi[i] = 3;
81  SO4 Q2 = SO4::Expmap(xi);
82  EXPECT(assert_equal(Q1 * Q2, Q2 * Q1));
83  }
84 }
85 
86 //******************************************************************************
87 TEST(SO4, Hat) {
88  // Check that Hat specialization is equal to dynamic version
89  EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
90  EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
91  EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
92 }
93 
94 //******************************************************************************
95 TEST(SO4, Vee) {
96  // Check that Hat specialization is equal to dynamic version
97  auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
98  EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
99  EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
100  EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3)));
101 }
102 
103 //******************************************************************************
104 TEST(SO4, Retract) {
105  // Check that Cayley yields the same as Expmap for small values
106  EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
107  EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
108 
109  // Check that Cayley is identical to dynamic version
110  EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100)));
111  EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100)));
112 
113  auto v = Vector6::Zero();
114  SO4 actual = traits<SO4>::Retract(id, v);
115  EXPECT(assert_equal(id, actual));
116 }
117 
118 //******************************************************************************
119 TEST(SO4, Local) {
120  // Check that Cayley is identical to dynamic version
121  EXPECT(
122  assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
123  EXPECT(
124  assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2))));
125 
126  auto v0 = Vector6::Zero();
127  Vector6 actual = traits<SO4>::Local(id, id);
128  EXPECT(assert_equal((Vector)v0, actual));
129 }
130 
131 //******************************************************************************
132 TEST(SO4, Invariants) {
133  EXPECT(check_group_invariants(id, id));
134  EXPECT(check_group_invariants(id, Q1));
135  EXPECT(check_group_invariants(Q2, id));
136  EXPECT(check_group_invariants(Q2, Q1));
137  EXPECT(check_group_invariants(Q1, Q2));
138 
139  EXPECT(check_manifold_invariants(id, id));
140  EXPECT(check_manifold_invariants(id, Q1));
141  EXPECT(check_manifold_invariants(Q2, id));
142  EXPECT(check_manifold_invariants(Q2, Q1));
143  EXPECT(check_manifold_invariants(Q1, Q2));
144 }
145 
146 //******************************************************************************
148  SO4 expected = Q1 * Q2;
149  Matrix actualH1, actualH2;
150  SO4 actual = Q1.compose(Q2, actualH1, actualH2);
151  EXPECT(assert_equal(expected, actual));
152 
153  Matrix numericalH1 =
154  numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
155  EXPECT(assert_equal(numericalH1, actualH1));
156 
157  Matrix numericalH2 =
158  numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
159  EXPECT(assert_equal(numericalH2, actualH2));
160 }
161 
162 //******************************************************************************
163 TEST(SO4, vec) {
164  using Vector16 = SO4::VectorN2;
165  const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
166  Matrix actualH;
167  const Vector16 actual = Q2.vec(actualH);
168  EXPECT(assert_equal(expected, actual));
169  boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
170  return Q.vec();
171  };
172  const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
173  EXPECT(assert_equal(numericalH, actualH));
174 }
175 
176 //******************************************************************************
178  const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>();
179  Matrix actualH;
180  const Matrix3 actual = topLeft(Q3, actualH);
181  EXPECT(assert_equal(expected, actual));
182  boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
183  return topLeft(Q3);
184  };
185  const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
186  EXPECT(assert_equal(numericalH, actualH));
187 }
188 
189 //******************************************************************************
191  const Matrix43 expected = Q3.matrix().leftCols<3>();
192  Matrix actualH;
193  const Matrix43 actual = stiefel(Q3, actualH);
194  EXPECT(assert_equal(expected, actual));
195  boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
196  return stiefel(Q3);
197  };
198  const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
199  EXPECT(assert_equal(numericalH, actualH));
200 }
201 
202 //******************************************************************************
203 int main() {
204  TestResult tr;
205  return TestRegistry::runAllTests(tr);
206 }
207 //******************************************************************************
Quaternion Q
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Definition: SOn-inl.h:88
Concept check for values that can be used in unit tests.
Provides convenient mappings of common member functions for testing.
static int runAllTests(TestResult &result)
Matrix expected
Definition: testMatrix.cpp:974
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
ArrayXcf v
Definition: Cwise_arg.cpp:1
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const
static std::mt19937 rng
size_t dim() const
Definition: SOn.h:211
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
Rot2 R(Rot2::fromAngle(0.1))
4*4 matrix representation of SO(4)
Pose2_ Expmap(const Vector3_ &xi)
Definition: Half.h:150
Some functions to compute numerical derivatives.
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:152
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
TEST(SO4, Identity)
Definition: testSO4.cpp:34
3*3 matrix representation of SO(3)
Vector6 v3
Definition: testSO4.cpp:56
Definition: SOn.h:50
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
SO4 Q1
Definition: testSO4.cpp:53
EIGEN_DEVICE_FUNC const VectorBlock< const Coefficients, 3 > vec() const
SO4 id
Definition: testSO4.cpp:51
EIGEN_DEVICE_FUNC RotationMatrixType matrix() const
Definition: RotationBase.h:50
Class compose(const Class &g) const
Definition: Lie.h:47
Eigen::VectorXd Vector
Definition: Vector.h:38
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
size_t rows() const
Definition: SOn.h:154
SO4 Q2
Definition: testSO4.cpp:55
Base class and basic functions for Manifold types.
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H)
Definition: SO4.cpp:220
#define EXPECT(condition)
Definition: Test.h:151
Vector6 v2
Definition: testSO4.cpp:54
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
SO4 Q3
Definition: testSO4.cpp:57
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
Definition: SO4.cpp:206
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
Vector xi
Definition: testPose2.cpp:150
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
int main()
Definition: testSO4.cpp:203
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
The quaternion class used to represent 3D orientations and rotations.
static const double v0
SO< Eigen::Dynamic > SOn
Definition: SOn.h:335
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
The matrix class, also used for vectors and row-vectors.
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)))
Vector6 v1
Definition: testSO4.cpp:52


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:53