test_rxso3.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <sophus/rxso3.hpp>
4 #include "tests.hpp"
5 
6 // Explicit instantiate all class templates so that all member methods
7 // get compiled and for code coverage analysis.
8 namespace Eigen {
9 template class Map<Sophus::RxSO3<double>>;
10 template class Map<Sophus::RxSO3<double> const>;
11 } // namespace Eigen
12 
13 namespace Sophus {
14 
15 template class RxSO3<double, Eigen::AutoAlign>;
16 template class RxSO3<float, Eigen::DontAlign>;
17 #if SOPHUS_CERES
18 template class RxSO3<ceres::Jet<double, 3>>;
19 #endif
20 
21 template <class Scalar_>
22 class Tests {
23  public:
24  using Scalar = Scalar_;
25  using SO3Type = SO3<Scalar>;
26  using RxSO3Type = RxSO3<Scalar>;
27  using Point = typename RxSO3<Scalar>::Point;
28  using Tangent = typename RxSO3<Scalar>::Tangent;
29  Scalar const kPi = Constants<Scalar>::pi();
30 
31  Tests() {
32  rxso3_vec_.push_back(RxSO3Type::exp(
33  Tangent(Scalar(0.2), Scalar(0.5), Scalar(0.0), Scalar(1.))));
34  rxso3_vec_.push_back(RxSO3Type::exp(
35  Tangent(Scalar(0.2), Scalar(0.5), Scalar(-1.0), Scalar(1.1))));
36  rxso3_vec_.push_back(RxSO3Type::exp(
37  Tangent(Scalar(0.), Scalar(0.), Scalar(0.), Scalar(1.1))));
38  rxso3_vec_.push_back(RxSO3Type::exp(
39  Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0.))));
40  rxso3_vec_.push_back(RxSO3Type::exp(
41  Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0.00001))));
42  rxso3_vec_.push_back(RxSO3Type::exp(
43  Tangent(Scalar(0.), Scalar(0.), Scalar(0.00001), Scalar(0))));
44  rxso3_vec_.push_back(
45  RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0.9))));
46  rxso3_vec_.push_back(
47  RxSO3Type::exp(
48  Tangent(Scalar(0.2), Scalar(-0.5), Scalar(0), Scalar(0))) *
49  RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0))) *
50  RxSO3Type::exp(
51  Tangent(-Scalar(0.2), Scalar(-0.5), Scalar(0), Scalar(0))));
52  rxso3_vec_.push_back(
53  RxSO3Type::exp(
54  Tangent(Scalar(0.3), Scalar(0.5), Scalar(0.1), Scalar(0))) *
55  RxSO3Type::exp(Tangent(kPi, Scalar(0), Scalar(0), Scalar(0))) *
56  RxSO3Type::exp(
57  Tangent(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1), Scalar(0))));
58 
59  Tangent tmp;
60  tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0);
61  tangent_vec_.push_back(tmp);
62  tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0);
63  tangent_vec_.push_back(tmp);
64  tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0.1);
65  tangent_vec_.push_back(tmp);
66  tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(0.1);
67  tangent_vec_.push_back(tmp);
68  tmp << Scalar(0), Scalar(0), Scalar(1), Scalar(-0.1);
69  tangent_vec_.push_back(tmp);
70  tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(-0.1);
71  tangent_vec_.push_back(tmp);
72  tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(2);
73  tangent_vec_.push_back(tmp);
74 
75  point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4)));
76  point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5)));
77  }
78 
79  void runAll() {
80  bool passed = testLieProperties();
81  passed &= testSaturation();
82  passed &= testRawDataAcces();
83  passed &= testConstructors();
84  passed &= testFit();
85  processTestResult(passed);
86  }
87 
88  private:
89  bool testLieProperties() {
90  LieGroupTests<RxSO3Type> tests(rxso3_vec_, tangent_vec_, point_vec_);
91  return tests.doAllTestsPass();
92  }
93 
94  bool testSaturation() {
95  bool passed = true;
96  RxSO3Type small1(Constants<Scalar>::epsilon(), SO3Type());
97  RxSO3Type small2(Constants<Scalar>::epsilon(),
98  SO3Type::exp(Vector3<Scalar>(Constants<Scalar>::pi(),
99  Scalar(0), Scalar(0))));
100  RxSO3Type saturated_product = small1 * small2;
101  SOPHUS_TEST_APPROX(passed, saturated_product.scale(),
102  Constants<Scalar>::epsilon(),
103  Constants<Scalar>::epsilon());
104  SOPHUS_TEST_APPROX(passed, saturated_product.so3().matrix(),
105  (small1.so3() * small2.so3()).matrix(),
106  Constants<Scalar>::epsilon());
107  return passed;
108  }
109 
110  bool testRawDataAcces() {
111  bool passed = true;
112  Eigen::Matrix<Scalar, 4, 1> raw = {Scalar(0), Scalar(1), Scalar(0),
113  Scalar(0)};
114  Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
115  SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
116  raw, Constants<Scalar>::epsilon());
117  SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
118  raw.data());
119  Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
120  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
121  map_of_const_rxso3.quaternion().coeffs().eval());
122 
123  Eigen::Matrix<Scalar, 4, 1> raw2 = {Scalar(1), Scalar(0), Scalar(0),
124  Scalar(0)};
125  Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
126  Eigen::Quaternion<Scalar> quat;
127  quat.coeffs() = raw2;
128  map_of_rxso3.setQuaternion(quat);
129  SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
130  Constants<Scalar>::epsilon());
131  SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
132  raw.data());
133  SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
134  quat.coeffs().data());
135  Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
136  SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
137  map_of_rxso3.quaternion().coeffs().eval());
138 
139  RxSO3Type const const_so3(quat);
140  for (int i = 0; i < 4; ++i) {
141  SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
142  }
143 
144  RxSO3Type so3(quat);
145  for (int i = 0; i < 4; ++i) {
146  so3.data()[i] = raw[i];
147  }
148 
149  for (int i = 0; i < 4; ++i) {
150  SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
151  }
152 
153  return passed;
154  }
155 
156  bool testConstructors() {
157  bool passed = true;
158  RxSO3Type rxso3;
159  Scalar scale(1.2);
160  rxso3.setScale(scale);
161  SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(),
162  Constants<Scalar>::epsilon(), "setScale");
163  auto so3 = rxso3_vec_[0].so3();
164  rxso3.setSO3(so3);
165  SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(),
166  Constants<Scalar>::epsilon(), "setScale");
167  SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3).matrix(), rxso3.matrix(),
168  Constants<Scalar>::epsilon(), "RxSO3(scale, SO3)");
169  SOPHUS_TEST_APPROX(passed, RxSO3Type(scale, so3.matrix()).matrix(),
170  rxso3.matrix(), Constants<Scalar>::epsilon(),
171  "RxSO3(scale, SO3)");
172  Matrix3<Scalar> R =
173  SO3<Scalar>::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0)))
174  .matrix();
175  Matrix3<Scalar> sR = R * Scalar(1.3);
176  SOPHUS_TEST_APPROX(passed, RxSO3Type(sR).matrix(), sR,
177  Constants<Scalar>::epsilon(), "RxSO3(sR)");
178  rxso3.setScaledRotationMatrix(sR);
179  SOPHUS_TEST_APPROX(passed, sR, rxso3.matrix(), Constants<Scalar>::epsilon(),
180  "setScaleRotationMatrix");
181  rxso3.setScale(scale);
182  rxso3.setRotationMatrix(R);
183  SOPHUS_TEST_APPROX(passed, R, rxso3.rotationMatrix(),
184  Constants<Scalar>::epsilon(), "setRotationMatrix");
185  SOPHUS_TEST_APPROX(passed, scale, rxso3.scale(),
186  Constants<Scalar>::epsilon(), "setScale");
187 
188  return passed;
189  }
190 
191  template <class S = Scalar>
192  enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
193  bool passed = true;
194  for (int i = 0; i < 10; ++i) {
195  Matrix3<Scalar> M = Matrix3<Scalar>::Random();
196  for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
197  Matrix3<Scalar> R = makeRotationMatrix(M);
198  Matrix3<Scalar> sR = scale * R;
199  SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
200  "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
201  Matrix3<Scalar> sR_cols_swapped;
202  sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
203  SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
204  "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
205  }
206  }
207  return passed;
208  }
209 
210  template <class S = Scalar>
211  enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
212  return true;
213  }
214 
215  std::vector<RxSO3Type, Eigen::aligned_allocator<RxSO3Type>> rxso3_vec_;
216  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
217  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
218 };
219 
220 int test_rxso3() {
221  using std::cerr;
222  using std::endl;
223 
224  cerr << "Test RxSO3" << endl << endl;
225  cerr << "Double tests: " << endl;
226  Tests<double>().runAll();
227  cerr << "Float tests: " << endl;
228  Tests<float>().runAll();
229 
230 #if SOPHUS_CERES
231  cerr << "ceres::Jet<double, 3> tests: " << endl;
232  Tests<ceres::Jet<double, 3>>().runAll();
233 #endif
234 
235  return 0;
236 }
237 
238 } // namespace Sophus
239 
240 int main() { return Sophus::test_rxso3(); }
Eigen
Definition: rxso2.hpp:16
Sophus::Tests::RxSO3Type
RxSO3< Scalar > RxSO3Type
Definition: test_rxso3.cpp:26
Sophus::RxSO3::exp
static SOPHUS_FUNC RxSO3< Scalar > exp(Tangent const &a)
Definition: rxso3.hpp:509
SOPHUS_TEST_APPROX
#define SOPHUS_TEST_APPROX(passed, left, right, thr,...)
Definition: test_macros.hpp:96
main
int main()
Definition: test_rxso3.cpp:240
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
Sophus::Vector3
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:21
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
rxso3.hpp
Sophus::Tests::testSaturation
bool testSaturation()
Definition: test_rxso2.cpp:103
Sophus::LieGroupTests
Definition: tests.hpp:21
SOPHUS_TEST
#define SOPHUS_TEST(passed, condition,...)
Definition: test_macros.hpp:53
Sophus::Tests::Tangent
typename RxSO2< Scalar >::Tangent Tangent
Definition: test_rxso2.cpp:27
Sophus::test_rxso3
int test_rxso3()
Definition: test_rxso3.cpp:220
Sophus::Tests::Scalar
Scalar_ Scalar
Definition: test_rxso3.cpp:24
Sophus
Definition: average.hpp:17
Sophus::Tests::Point
typename RxSO2< Scalar >::Point Point
Definition: test_rxso2.cpp:26
Sophus::Tests::rxso3_vec_
std::vector< RxSO3Type, Eigen::aligned_allocator< RxSO3Type > > rxso3_vec_
Definition: test_rxso3.cpp:215
Sophus::processTestResult
void processTestResult(bool passed)
Definition: test_macros.hpp:38
Sophus::RxSO3::Tangent
typename Base::Tangent Tangent
Definition: rxso3.hpp:418
Sophus::Tests::testConstructors
bool testConstructors()
Definition: test_rxso2.cpp:154
Sophus::Tests::Tests
Tests()
Definition: test_rxso3.cpp:31
Sophus::LieGroupTests::doAllTestsPass
enable_if_t< std::is_floating_point< S >::value, bool > doAllTestsPass()
Definition: tests.hpp:452
Sophus::Tests::testFit
enable_if_t< std::is_floating_point< S >::value, bool > testFit()
Definition: test_rxso2.cpp:65
Sophus::RxSO3::Point
typename Base::Point Point
Definition: rxso3.hpp:416
Sophus::makeRotationMatrix
SOPHUS_FUNC enable_if_t< std::is_floating_point< typename D::Scalar >::value, Matrix< typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime > > makeRotationMatrix(Eigen::MatrixBase< D > const &R)
Definition: rotation_matrix.hpp:62
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::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_rxso3.cpp:211
SOPHUS_TEST_NEQ
#define SOPHUS_TEST_NEQ(passed, left, right,...)
Definition: test_macros.hpp:81
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::Tests::SO3Type
SO3< Scalar > SO3Type
Definition: test_rxso3.cpp:25
Sophus::Tests::runAll
void runAll()
Definition: test_rxso3.cpp:79
Sophus::RxSO3
RxSO3 using storage; derived from RxSO3Base.
Definition: rxso3.hpp:11
Sophus::isScaledOrthogonalAndPositive
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase< D > const &sR)
Definition: rotation_matrix.hpp:33

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