test_so3.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <sophus/interpolate.hpp>
4 #include <sophus/so3.hpp>
5 #include "tests.hpp"
6 
7 // Explicit instantiate all class templates so that all member methods
8 // get compiled and for code coverage analysis.
9 namespace Eigen {
10 template class Map<Sophus::SO3<double>>;
11 template class Map<Sophus::SO3<double> const>;
12 } // namespace Eigen
13 
14 namespace Sophus {
15 
16 template class SO3<double, Eigen::AutoAlign>;
17 template class SO3<float, Eigen::DontAlign>;
18 #if SOPHUS_CERES
19 template class SO3<ceres::Jet<double, 3>>;
20 #endif
21 
22 template <class Scalar>
23 class Tests {
24  public:
25  using SO3Type = SO3<Scalar>;
26  using Point = typename SO3<Scalar>::Point;
27  using Tangent = typename SO3<Scalar>::Tangent;
28  Scalar const kPi = Constants<Scalar>::pi();
29 
30  Tests() {
31  so3_vec_.push_back(SO3Type(Eigen::Quaternion<Scalar>(
32  Scalar(0.1e-11), Scalar(0.), Scalar(1.), Scalar(0.))));
33  so3_vec_.push_back(SO3Type(Eigen::Quaternion<Scalar>(
34  Scalar(-1), Scalar(0.00001), Scalar(0.0), Scalar(0.0))));
35  so3_vec_.push_back(
36  SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))));
37  so3_vec_.push_back(
38  SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0))));
39  so3_vec_.push_back(SO3Type::exp(Point(Scalar(0.), Scalar(0.), Scalar(0.))));
40  so3_vec_.push_back(
41  SO3Type::exp(Point(Scalar(0.), Scalar(0.), Scalar(0.00001))));
42  so3_vec_.push_back(SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))));
43  so3_vec_.push_back(
44  SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))) *
45  SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))) *
46  SO3Type::exp(Point(Scalar(-0.2), Scalar(-0.5), Scalar(-0.0))));
47  so3_vec_.push_back(
48  SO3Type::exp(Point(Scalar(0.3), Scalar(0.5), Scalar(0.1))) *
49  SO3Type::exp(Point(kPi, Scalar(0), Scalar(0))) *
50  SO3Type::exp(Point(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1))));
51  tangent_vec_.push_back(Tangent(Scalar(0), Scalar(0), Scalar(0)));
52  tangent_vec_.push_back(Tangent(Scalar(1), Scalar(0), Scalar(0)));
53  tangent_vec_.push_back(Tangent(Scalar(0), Scalar(1), Scalar(0)));
54  tangent_vec_.push_back(
55  Tangent(Scalar(kPi / 2.), Scalar(kPi / 2.), Scalar(0)));
56  tangent_vec_.push_back(Tangent(Scalar(-1), Scalar(1), Scalar(0)));
57  tangent_vec_.push_back(Tangent(Scalar(20), Scalar(-1), Scalar(0)));
58  tangent_vec_.push_back(Tangent(Scalar(30), Scalar(5), Scalar(-1)));
59 
60  point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4)));
61  point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5)));
62  }
63 
64  void runAll() {
65  bool passed = testLieProperties();
66  passed &= testUnity();
67  passed &= testRawDataAcces();
68  passed &= testConstructors();
69  passed &= testFit();
70  processTestResult(passed);
71  }
72 
73  private:
74  bool testLieProperties() {
75  LieGroupTests<SO3Type> tests(so3_vec_, tangent_vec_, point_vec_);
76  return tests.doAllTestsPass();
77  }
78 
79  bool testUnity() {
80  bool passed = true;
81  // Test that the complex number magnitude stays close to one.
82  SO3Type current_q;
83  for (std::size_t i = 0; i < 1000; ++i) {
84  for (SO3Type const& q : so3_vec_) {
85  current_q *= q;
86  }
87  }
88  SOPHUS_TEST_APPROX(passed, current_q.unit_quaternion().norm(), Scalar(1),
89  Constants<Scalar>::epsilon(), "Magnitude drift");
90  return passed;
91  }
92 
93  bool testRawDataAcces() {
94  bool passed = true;
95  Eigen::Matrix<Scalar, 4, 1> raw = {Scalar(0), Scalar(1), Scalar(0),
96  Scalar(0)};
97  Eigen::Map<SO3Type const> map_of_const_so3(raw.data());
98  SOPHUS_TEST_APPROX(passed,
99  map_of_const_so3.unit_quaternion().coeffs().eval(), raw,
100  Constants<Scalar>::epsilon());
101  SOPHUS_TEST_EQUAL(
102  passed, map_of_const_so3.unit_quaternion().coeffs().data(), raw.data());
103  Eigen::Map<SO3Type const> const_shallow_copy = map_of_const_so3;
104  SOPHUS_TEST_EQUAL(passed,
105  const_shallow_copy.unit_quaternion().coeffs().eval(),
106  map_of_const_so3.unit_quaternion().coeffs().eval());
107 
108  Eigen::Matrix<Scalar, 4, 1> raw2 = {Scalar(1), Scalar(0), Scalar(0),
109  Scalar(0)};
110  Eigen::Map<SO3Type> map_of_so3(raw.data());
111  Eigen::Quaternion<Scalar> quat;
112  quat.coeffs() = raw2;
113  map_of_so3.setQuaternion(quat);
114  SOPHUS_TEST_APPROX(passed, map_of_so3.unit_quaternion().coeffs().eval(),
115  raw2, Constants<Scalar>::epsilon());
116  SOPHUS_TEST_EQUAL(passed, map_of_so3.unit_quaternion().coeffs().data(),
117  raw.data());
118  SOPHUS_TEST_NEQ(passed, map_of_so3.unit_quaternion().coeffs().data(),
119  quat.coeffs().data());
120  Eigen::Map<SO3Type> shallow_copy = map_of_so3;
121  SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(),
122  map_of_so3.unit_quaternion().coeffs().eval());
123 
124  SO3Type const const_so3(quat);
125  for (int i = 0; i < 4; ++i) {
126  SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
127  }
128 
129  SO3Type so3(quat);
130  for (int i = 0; i < 4; ++i) {
131  so3.data()[i] = raw[i];
132  }
133 
134  for (int i = 0; i < 4; ++i) {
135  SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
136  }
137 
138  SOPHUS_TEST_EQUAL(
139  passed, SO3Type::rotX(Scalar(0.2)).matrix(),
140  SO3Type::exp(Point(Scalar(0.2), Scalar(0), Scalar(0))).matrix());
141  SOPHUS_TEST_EQUAL(
142  passed, SO3Type::rotY(Scalar(-0.2)).matrix(),
143  SO3Type::exp(Point(Scalar(0), Scalar(-0.2), Scalar(0))).matrix());
144  SOPHUS_TEST_EQUAL(
145  passed, SO3Type::rotZ(Scalar(1.1)).matrix(),
146  SO3Type::exp(Point(Scalar(0), Scalar(0), Scalar(1.1))).matrix());
147 
148  return passed;
149  }
150 
151  bool testConstructors() {
152  bool passed = true;
153  Matrix3<Scalar> R = so3_vec_.front().matrix();
154  SO3Type so3(R);
155  SOPHUS_TEST_APPROX(passed, R, so3.matrix(), Constants<Scalar>::epsilon());
156 
157  return passed;
158  }
159 
160  template <class S = Scalar>
161  enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
162  bool passed = true;
163 
164  for (int i = 0; i < 100; ++i) {
165  Matrix3<Scalar> R = Matrix3<Scalar>::Random();
166  SO3Type so3 = SO3Type::fitToSO3(R);
167  SO3Type so3_2 = SO3Type::fitToSO3(so3.matrix());
168 
169  SOPHUS_TEST_APPROX(passed, so3.matrix(), so3_2.matrix(),
170  Constants<Scalar>::epsilon());
171  }
172 
173  for (Scalar const angle :
174  {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) {
175  SOPHUS_TEST_APPROX(passed, SO3Type::rotX(angle).angleX(), angle,
176  Constants<Scalar>::epsilon());
177  SOPHUS_TEST_APPROX(passed, SO3Type::rotY(angle).angleY(), angle,
178  Constants<Scalar>::epsilon());
179  SOPHUS_TEST_APPROX(passed, SO3Type::rotZ(angle).angleZ(), angle,
180  Constants<Scalar>::epsilon());
181  }
182  return passed;
183  }
184 
185  template <class S = Scalar>
186  enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
187  return true;
188  }
189 
190  std::vector<SO3Type, Eigen::aligned_allocator<SO3Type>> so3_vec_;
191  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
192  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
193 };
194 
195 int test_so3() {
196  using std::cerr;
197  using std::endl;
198 
199  cerr << "Test SO3" << endl << endl;
200  cerr << "Double tests: " << endl;
201  Tests<double>().runAll();
202  cerr << "Float tests: " << endl;
203  Tests<float>().runAll();
204 
205 #if SOPHUS_CERES
206  cerr << "ceres::Jet<double, 3> tests: " << endl;
207  Tests<ceres::Jet<double, 3>>().runAll();
208 #endif
209 
210  return 0;
211 }
212 } // namespace Sophus
213 
214 int main() { return Sophus::test_so3(); }
Eigen
Definition: rxso2.hpp:16
Sophus::SO3::rotX
static SOPHUS_FUNC SO3 rotX(Scalar const &x)
Definition: so3.hpp:699
SOPHUS_TEST_APPROX
#define SOPHUS_TEST_APPROX(passed, left, right, thr,...)
Definition: test_macros.hpp:96
Sophus::SO3
SO3 using default storage; derived from SO3Base.
Definition: so3.hpp:19
Sophus::Tests::testLieProperties
bool testLieProperties()
Definition: test_rxso2.cpp:98
Sophus::Tests::tangent_vec_
std::vector< Tangent, Eigen::aligned_allocator< Tangent > > tangent_vec_
Definition: test_rxso2.cpp:196
Sophus::SO3::exp
static SOPHUS_FUNC SO3< Scalar > exp(Tangent const &omega)
Definition: so3.hpp:571
Sophus::Tests::point_vec_
std::vector< Point, Eigen::aligned_allocator< Point > > point_vec_
Definition: test_rxso2.cpp:197
main
int main()
Definition: test_so3.cpp:214
Sophus::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:221
Sophus::Tests
Definition: test_rxso2.cpp:22
Sophus::SO3::Tangent
typename Base::Tangent Tangent
Definition: so3.hpp:436
Sophus::LieGroupTests
Definition: tests.hpp:21
so3.hpp
Sophus::Tests::Tangent
typename RxSO2< Scalar >::Tangent Tangent
Definition: test_rxso2.cpp:27
Sophus::Tests::Scalar
Scalar_ Scalar
Definition: test_rxso3.cpp:24
interpolate.hpp
Sophus
Definition: average.hpp:17
Sophus::Tests::Point
typename RxSO2< Scalar >::Point Point
Definition: test_rxso2.cpp:26
Sophus::Tests::so3_vec_
std::vector< SO3Type, Eigen::aligned_allocator< SO3Type > > so3_vec_
Definition: test_so3.cpp:190
Sophus::test_so3
int test_so3()
Definition: test_so3.cpp:195
Sophus::processTestResult
void processTestResult(bool passed)
Definition: test_macros.hpp:38
Sophus::Tests::testConstructors
bool testConstructors()
Definition: test_rxso2.cpp:154
Sophus::Tests::Tests
Tests()
Definition: test_so3.cpp:30
Sophus::LieGroupTests::doAllTestsPass
enable_if_t< std::is_floating_point< S >::value, bool > doAllTestsPass()
Definition: tests.hpp:452
Sophus::SO3::rotY
static SOPHUS_FUNC SO3 rotY(Scalar const &y)
Definition: so3.hpp:705
Sophus::Tests::testFit
enable_if_t< std::is_floating_point< S >::value, bool > testFit()
Definition: test_rxso2.cpp:65
Sophus::Tests::testUnity
bool testUnity()
Definition: test_so2.cpp:66
SOPHUS_TEST_EQUAL
#define SOPHUS_TEST_EQUAL(passed, left, right,...)
Definition: test_macros.hpp:66
Sophus::Tests::kPi
const Scalar kPi
Definition: test_rxso2.cpp:28
Sophus::SO3::fitToSO3
static SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SO3 > fitToSO3(Transformation const &R)
Definition: so3.hpp:622
Sophus::Constants::pi
static SOPHUS_FUNC Scalar pi()
Definition: common.hpp:154
Sophus::Tests::testFit
enable_if_t<!std::is_floating_point< S >::value, bool > testFit()
Definition: test_so3.cpp:186
SOPHUS_TEST_NEQ
#define SOPHUS_TEST_NEQ(passed, left, right,...)
Definition: test_macros.hpp:81
Sophus::SO3::rotZ
static SOPHUS_FUNC SO3 rotZ(Scalar const &z)
Definition: so3.hpp:711
Sophus::Constants
Definition: common.hpp:146
Sophus::Matrix3
Matrix< Scalar, 3, 3 > Matrix3
Definition: types.hpp:49
Sophus::Tests::testRawDataAcces
bool testRawDataAcces()
Definition: test_rxso2.cpp:118
tests.hpp
Sophus::SO3::Point
typename Base::Point Point
Definition: so3.hpp:434
Sophus::Tests::SO3Type
SO3< Scalar > SO3Type
Definition: test_rxso3.cpp:25
Sophus::Tests::runAll
void runAll()
Definition: test_so3.cpp:64
Sophus::SO3::unit_quaternion
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
Definition: so3.hpp:488

sophus
Author(s): Hauke Strasdat
autogenerated on Wed Mar 2 2022 01:01:48