testSOn.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/geometry/SO3.h>
19 #include <gtsam/geometry/SO4.h>
20 #include <gtsam/geometry/SOn.h>
21 
22 #include <gtsam/base/Lie.h>
23 #include <gtsam/base/Manifold.h>
24 #include <gtsam/base/Matrix.h>
25 #include <gtsam/base/Testable.h>
26 #include <gtsam/base/lieProxies.h>
27 #include <gtsam/base/numericalDerivative.h>
28 #include <gtsam/base/testLie.h>
29 #include <gtsam/nonlinear/Values.h>
30 
31 #include <CppUnitLite/TestHarness.h>
32 
33 #include <iostream>
34 #include <random>
35 #include <stdexcept>
36 #include <type_traits>
37 
38 using namespace std;
39 using namespace gtsam;
40 
41 //******************************************************************************
42 // Test dynamic with n=0
43 TEST(SOn, SOn0) {
44  const auto R = SOn(0);
45  EXPECT_LONGS_EQUAL(0, R.rows());
46  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
47  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
48  EXPECT_LONGS_EQUAL(0, R.dim());
49  EXPECT_LONGS_EQUAL(0, traits<SOn>::GetDimension(R));
50 }
51 
52 //******************************************************************************
53 // Test dynamic with n=5
54 TEST(SOn, SOn5) {
55  const auto R = SOn(5);
56  EXPECT_LONGS_EQUAL(5, R.rows());
57  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
58  EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
59  EXPECT_LONGS_EQUAL(10, R.dim());
60  EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
61 }
62 
63 //******************************************************************************
64 // Test fixed with n=2
65 TEST(SOn, SO0) {
66  const auto R = SO<2>();
67  EXPECT_LONGS_EQUAL(2, R.rows());
68  EXPECT_LONGS_EQUAL(1, SO<2>::dimension);
69  EXPECT_LONGS_EQUAL(1, SO<2>::Dim());
70  EXPECT_LONGS_EQUAL(1, R.dim());
71  EXPECT_LONGS_EQUAL(1, traits<SO<2>>::GetDimension(R));
72 }
73 
74 //******************************************************************************
75 // Test fixed with n=5
76 TEST(SOn, SO5) {
77  const auto R = SO<5>();
78  EXPECT_LONGS_EQUAL(5, R.rows());
79  EXPECT_LONGS_EQUAL(10, SO<5>::dimension);
80  EXPECT_LONGS_EQUAL(10, SO<5>::Dim());
81  EXPECT_LONGS_EQUAL(10, R.dim());
82  EXPECT_LONGS_EQUAL(10, traits<SO<5>>::GetDimension(R));
83 }
84 
85 //******************************************************************************
86 TEST(SOn, Concept) {
87  BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
88  BOOST_CONCEPT_ASSERT((IsManifold<SOn>));
89  BOOST_CONCEPT_ASSERT((IsLieGroup<SOn>));
90 }
91 
92 //******************************************************************************
93 TEST(SOn, CopyConstructor) {
94  const auto R = SOn(5);
95  const auto B(R);
96  EXPECT_LONGS_EQUAL(5, B.rows());
97  EXPECT_LONGS_EQUAL(10, B.dim());
98 }
99 
100 //******************************************************************************
101 TEST(SOn, Values) {
102  const auto R = SOn(5);
103  Values values;
104  const Key key(0);
105  values.insert(key, R);
106  const auto B = values.at<SOn>(key);
107  EXPECT_LONGS_EQUAL(5, B.rows());
108  EXPECT_LONGS_EQUAL(10, B.dim());
109 }
110 
111 //******************************************************************************
112 TEST(SOn, Random) {
113  static std::mt19937 rng(42);
114  EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows());
115  EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows());
116  EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows());
117 }
118 
119 //******************************************************************************
120 TEST(SOn, HatVee) {
121  Vector10 v;
122  v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
123 
124  Matrix expected2(2, 2);
125  expected2 << 0, -1, 1, 0;
126  const auto actual2 = SOn::Hat(v.head<1>());
127  EXPECT(assert_equal(expected2, actual2));
128  EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
129 
130  Matrix expected3(3, 3);
131  expected3 << 0, -3, 2, //
132  3, 0, -1, //
133  -2, 1, 0;
134  const auto actual3 = SOn::Hat(v.head<3>());
135  EXPECT(assert_equal(expected3, actual3));
136  EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
137  EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
138 
139  Matrix expected4(4, 4);
140  expected4 << 0, -6, 5, 3, //
141  6, 0, -4, -2, //
142  -5, 4, 0, 1, //
143  -3, 2, -1, 0;
144  const auto actual4 = SOn::Hat(v.head<6>());
145  EXPECT(assert_equal(expected4, actual4));
146  EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
147 
148  Matrix expected5(5, 5);
149  expected5 << 0, -10, 9, 7, -4, //
150  10, 0, -8, -6, 3, //
151  -9, 8, 0, 5, -2, //
152  -7, 6, -5, 0, 1, //
153  4, -3, 2, -1, 0;
154  const auto actual5 = SOn::Hat(v);
155  EXPECT(assert_equal(expected5, actual5));
156  EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
157 }
158 
159 //******************************************************************************
160 TEST(SOn, RetractLocal) {
161  Vector6 v1 = (Vector(6) << 0, 0, 0, 1, 0, 0).finished() / 10000;
162  Vector6 v2 = (Vector(6) << 0, 0, 0, 1, 2, 3).finished() / 10000;
163  Vector6 v3 = (Vector(6) << 3, 2, 1, 1, 2, 3).finished() / 10000;
164 
165  // Check that Cayley yields the same as Expmap for small values
166  SOn id(4);
167  EXPECT(assert_equal(id.retract(v1), SOn(SO4::Expmap(v1))));
168  EXPECT(assert_equal(id.retract(v2), SOn(SO4::Expmap(v2))));
169  EXPECT(assert_equal(id.retract(v3), SOn(SO4::Expmap(v3))));
170 
171  // Same for SO3:
172  SOn I3(3);
173  EXPECT(
174  assert_equal(I3.retract(v1.tail<3>()), SOn(SO3::Expmap(v1.tail<3>()))));
175  EXPECT(
176  assert_equal(I3.retract(v2.tail<3>()), SOn(SO3::Expmap(v2.tail<3>()))));
177 
178  // If we do expmap in SO(3) subgroup, topleft should be equal to R1.
179  Matrix R1 = SO3().retract(v1.tail<3>()).matrix();
180  SOn Q1 = SOn::Retract(v1);
181  CHECK(assert_equal(R1, Q1.matrix().block(0, 0, 3, 3), 1e-7));
182  CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7));
183 }
184 
185 //******************************************************************************
186 
187 Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
188 
190 TEST(SOn, RetractJacobian) {
191  Matrix actualH = RetractJacobian(3);
192  boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
193  return SOn::ChartAtOrigin::Retract(v).matrix();
194  };
195  Vector3 v;
196  v.setZero();
197  const Matrix expectedH = numericalDerivative11<Matrix, Vector, 3>(h, v, 1e-5);
198  CHECK(assert_equal(expectedH, actualH));
199 }
200 
201 //******************************************************************************
202 TEST(SOn, vec) {
203  Vector10 v;
204  v << 0, 0, 0, 0, 1, 2, 3, 4, 5, 6;
205  SOn Q = SOn::ChartAtOrigin::Retract(v);
206  Matrix actualH;
207  const Vector actual = Q.vec(actualH);
208  boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
209  const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
210  CHECK(assert_equal(H, actualH));
211 }
212 
213 //******************************************************************************
214 TEST(SOn, VectorizedGenerators) {
215  // Default fixed
216  auto actual2 = SO<2>::VectorizedGenerators();
217  CHECK(actual2.rows()==4 && actual2.cols()==1)
218 
219  // Specialized
220  auto actual3 = SO<3>::VectorizedGenerators();
221  CHECK(actual3.rows()==9 && actual3.cols()==3)
222  auto actual4 = SO<4>::VectorizedGenerators();
223  CHECK(actual4.rows()==16 && actual4.cols()==6)
224 
225  // Dynamic
226  auto actual5 = SOn::VectorizedGenerators(5);
227  CHECK(actual5.rows()==25 && actual5.cols()==10)
228 }
229 
230 //******************************************************************************
231 int main() {
232  TestResult tr;
233  return TestRegistry::runAllTests(tr);
234 }
235 //******************************************************************************
CHECK
#define CHECK(condition)
Definition: Test.h:109
gtsam::lago::I3
static const Matrix I3
Definition: lago.cpp:36
Q
Quaternion Q
Definition: testQuaternion.cpp:27
v2
Vector v2
Definition: testSerializationBase.cpp:36
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Definition: SOn-inl.h:88
Testable.h
Concept check for values that can be used in unit tests.
lieProxies.h
Provides convenient mappings of common member functions for testing.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
v1
Vector v1
Definition: testSerializationBase.cpp:35
RetractJacobian
Matrix RetractJacobian(size_t n)
Definition: testSOn.cpp:187
Values.h
A non-templated config holding any types of Manifold-group elements.
id
Q id(Eigen::AngleAxisd(0, Q_z_axis))
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
v
ArrayXcf v
Definition: Cwise_arg.cpp:1
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::BOOST_CONCEPT_ASSERT
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
SO4.h
4*4 matrix representation of SO(4)
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
std
Definition: Half.h:150
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:152
H
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 set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
SO3.h
3*3 matrix representation of SO(3)
B
Matrix< SCALARB, Dynamic, Dynamic > B
Definition: bench_gemm.cpp:36
gtsam::SO
Definition: SOn.h:50
Q1
SO4 Q1
Definition: testSO4.cpp:53
gtsam::SO3
SO< 3 > SO3
Definition: SO3.h:34
v3
Vector v3
Definition: testSerializationBase.cpp:37
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:342
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.)
TEST
TEST(SOn, SOn0)
Definition: testSOn.cpp:43
R1
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
Lie.h
Base class and basic functions for Lie types.
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
h
const double h
Definition: testSimpleHelicopter.cpp:16
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:404
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
main
int main()
Definition: testSOn.cpp:231
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:273
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
gtsam::SOn
SO< Eigen::Dynamic > SOn
Definition: SOn.h:335
Eigen::Dynamic
const int Dynamic
Definition: Constants.h:21
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
SOn.h
N*N matrix representation of SO(N). N can be Eigen::Dynamic.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61

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