test_sim2.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <unsupported/Eigen/MatrixFunctions>
4 
5 #include <sophus/sim2.hpp>
6 #include "tests.hpp"
7 
8 // Explicit instantiate all class templates so that all member methods
9 // get compiled and for code coverage analysis.
10 namespace Eigen {
11 template class Map<Sophus::Sim2<double>>;
12 template class Map<Sophus::Sim2<double> const>;
13 } // namespace Eigen
14 
15 namespace Sophus {
16 
17 template class Sim2<double, Eigen::AutoAlign>;
18 template class Sim2<float, Eigen::DontAlign>;
19 #if SOPHUS_CERES
20 template class Sim2<ceres::Jet<double, 3>>;
21 #endif
22 
23 template <class Scalar>
24 class Tests {
25  public:
26  using Sim2Type = Sim2<Scalar>;
27  using RxSO2Type = RxSO2<Scalar>;
28  using Point = typename Sim2<Scalar>::Point;
29  using Vector2Type = Vector2<Scalar>;
30  using Tangent = typename Sim2<Scalar>::Tangent;
31  Scalar const kPi = Constants<Scalar>::pi();
32 
33  Tests() {
34  sim2_vec_.push_back(
35  Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 1.)), Point(0, 0)));
36  sim2_vec_.push_back(
37  Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 1.1)), Point(10, 0)));
38  sim2_vec_.push_back(
39  Sim2Type(RxSO2Type::exp(Vector2Type(0., 0.)), Point(0, 10)));
40  sim2_vec_.push_back(
41  Sim2Type(RxSO2Type::exp(Vector2Type(0.00001, 0.)), Point(0, 0)));
42  sim2_vec_.push_back(
43  Sim2Type(RxSO2Type::exp(Vector2Type(0.00001, 0.0000001)),
44  Point(1, -1.00000001)));
45  sim2_vec_.push_back(
46  Sim2Type(RxSO2Type::exp(Vector2Type(0., 0.)), Point(0.01, 0)));
47  sim2_vec_.push_back(
48  Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0.9)), Point(4, 0)));
49  sim2_vec_.push_back(
50  Sim2Type(RxSO2Type::exp(Vector2Type(0.2, 0)), Point(0, 0)) *
51  Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0)), Point(0, 0)) *
52  Sim2Type(RxSO2Type::exp(Vector2Type(-0.2, 0)), Point(0, 0)));
53  sim2_vec_.push_back(
54  Sim2Type(RxSO2Type::exp(Vector2Type(0.3, 0)), Point(2, -7)) *
55  Sim2Type(RxSO2Type::exp(Vector2Type(kPi, 0)), Point(0, 0)) *
56  Sim2Type(RxSO2Type::exp(Vector2Type(-0.3, 0)), Point(0, 6)));
57  Tangent tmp;
58  tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0);
59  tangent_vec_.push_back(tmp);
60  tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0);
61  tangent_vec_.push_back(tmp);
62  tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(0.1);
63  tangent_vec_.push_back(tmp);
64  tmp << Scalar(-1), Scalar(1), Scalar(1), Scalar(-0.1);
65  tangent_vec_.push_back(tmp);
66  tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-0.1);
67  tangent_vec_.push_back(tmp);
68  tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(1.5);
69  tangent_vec_.push_back(tmp);
70 
71  point_vec_.push_back(Point(Scalar(1), Scalar(4)));
72  point_vec_.push_back(Point(Scalar(1), Scalar(-3)));
73  }
74 
75  void runAll() {
76  bool passed = testLieProperties();
77  passed &= testRawDataAcces();
78  passed &= testConstructors();
79  processTestResult(passed);
80  }
81 
82  private:
83  bool testLieProperties() {
84  LieGroupTests<Sim2Type> tests(sim2_vec_, tangent_vec_, point_vec_);
85  return tests.doAllTestsPass();
86  }
87 
88  bool testRawDataAcces() {
89  bool passed = true;
90  Eigen::Matrix<Scalar, 4, 1> raw;
91  raw << Scalar(0), Scalar(1), Scalar(3), Scalar(2);
92  Eigen::Map<Sim2Type const> map_of_const_sim2(raw.data());
93  SOPHUS_TEST_APPROX(passed, map_of_const_sim2.complex().eval(),
94  raw.template head<2>().eval(),
95  Constants<Scalar>::epsilon());
96  SOPHUS_TEST_APPROX(passed, map_of_const_sim2.translation().eval(),
97  raw.template tail<2>().eval(),
98  Constants<Scalar>::epsilon());
99  SOPHUS_TEST_EQUAL(passed, map_of_const_sim2.complex().data(), raw.data());
100  SOPHUS_TEST_EQUAL(passed, map_of_const_sim2.translation().data(),
101  raw.data() + 2);
102  Eigen::Map<Sim2Type const> const_shallow_copy = map_of_const_sim2;
103  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.complex().eval(),
104  map_of_const_sim2.complex().eval());
105  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
106  map_of_const_sim2.translation().eval());
107 
108  Eigen::Matrix<Scalar, 4, 1> raw2;
109  raw2 << Scalar(1), Scalar(0), Scalar(2), Scalar(1);
110  Eigen::Map<Sim2Type> map_of_sim2(raw.data());
111  Vector2<Scalar> z;
112  z = raw2.template head<2>();
113  map_of_sim2.setComplex(z);
114  map_of_sim2.translation() = raw2.template tail<2>();
115  SOPHUS_TEST_APPROX(passed, map_of_sim2.complex().eval(),
116  raw2.template head<2>().eval(),
117  Constants<Scalar>::epsilon());
118  SOPHUS_TEST_APPROX(passed, map_of_sim2.translation().eval(),
119  raw2.template tail<2>().eval(),
120  Constants<Scalar>::epsilon());
121  SOPHUS_TEST_EQUAL(passed, map_of_sim2.complex().data(), raw.data());
122  SOPHUS_TEST_EQUAL(passed, map_of_sim2.translation().data(), raw.data() + 2);
123  SOPHUS_TEST_NEQ(passed, map_of_sim2.complex().data(), z.data());
124  Eigen::Map<Sim2Type> shallow_copy = map_of_sim2;
125  SOPHUS_TEST_EQUAL(passed, shallow_copy.complex().eval(),
126  map_of_sim2.complex().eval());
127  SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
128  map_of_sim2.translation().eval());
129  Eigen::Map<Sim2Type> const const_map_of_sim3 = map_of_sim2;
130  SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.complex().eval(),
131  map_of_sim2.complex().eval());
132  SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(),
133  map_of_sim2.translation().eval());
134 
135  Sim2Type const const_sim2(z, raw2.template tail<2>().eval());
136  for (int i = 0; i < 4; ++i) {
137  SOPHUS_TEST_EQUAL(passed, const_sim2.data()[i], raw2.data()[i]);
138  }
139 
140  Sim2Type se3(z, raw2.template tail<2>().eval());
141  for (int i = 0; i < 4; ++i) {
142  SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]);
143  }
144 
145  for (int i = 0; i < 4; ++i) {
146  SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]);
147  }
148  return passed;
149  }
150 
151  bool testConstructors() {
152  bool passed = true;
153  Eigen::Matrix<Scalar, 3, 3> I = Eigen::Matrix<Scalar, 3, 3>::Identity();
154  SOPHUS_TEST_EQUAL(passed, Sim2Type().matrix(), I);
155 
156  Sim2Type sim2 = sim2_vec_.front();
157  Point translation = sim2.translation();
158  RxSO2Type rxso2 = sim2.rxso2();
159 
160  SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2, translation).matrix(),
161  sim2.matrix(), Constants<Scalar>::epsilon());
162  SOPHUS_TEST_APPROX(passed, Sim2Type(rxso2.complex(), translation).matrix(),
163  sim2.matrix(), Constants<Scalar>::epsilon());
164  SOPHUS_TEST_APPROX(passed, Sim2Type(sim2.matrix()).matrix(), sim2.matrix(),
165  Constants<Scalar>::epsilon());
166 
167  Scalar scale(1.2);
168  sim2.setScale(scale);
169  SOPHUS_TEST_APPROX(passed, scale, sim2.scale(),
170  Constants<Scalar>::epsilon(), "setScale");
171 
172  sim2.setComplex(sim2_vec_[0].rxso2().complex());
173  SOPHUS_TEST_APPROX(passed, sim2_vec_[0].rxso2().complex(),
174  sim2_vec_[0].rxso2().complex(),
175  Constants<Scalar>::epsilon(), "setComplex");
176  return passed;
177  }
178 
179  std::vector<Sim2Type, Eigen::aligned_allocator<Sim2Type>> sim2_vec_;
180  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
181  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
182 };
183 
184 int test_sim3() {
185  using std::cerr;
186  using std::endl;
187 
188  cerr << "Test Sim2" << endl << endl;
189  cerr << "Double tests: " << endl;
190  Tests<double>().runAll();
191  cerr << "Float tests: " << endl;
192  Tests<float>().runAll();
193 
194 #if SOPHUS_CERES
195  cerr << "ceres::Jet<double, 3> tests: " << endl;
196  Tests<ceres::Jet<double, 3>>().runAll();
197 #endif
198 
199  return 0;
200 }
201 } // namespace Sophus
202 
203 int main() { return Sophus::test_sim3(); }
Eigen
Definition: rxso2.hpp:16
SOPHUS_TEST_APPROX
#define SOPHUS_TEST_APPROX(passed, left, right, thr,...)
Definition: test_macros.hpp:96
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::Tests::Vector2Type
Vector2< Scalar > Vector2Type
Definition: test_sim2.cpp:29
Sophus::Tests::point_vec_
std::vector< Point, Eigen::aligned_allocator< Point > > point_vec_
Definition: test_rxso2.cpp:197
Sophus::test_sim3
int test_sim3()
Definition: test_sim2.cpp:184
Sophus::Tests
Definition: test_rxso2.cpp:22
Sophus::Vector2
Vector< Scalar, 2, Options > Vector2
Definition: types.hpp:16
Sophus::LieGroupTests
Definition: tests.hpp:21
Sophus::Tests::Tangent
typename RxSO2< Scalar >::Tangent Tangent
Definition: test_rxso2.cpp:27
Sophus::RxSO2::exp
static SOPHUS_FUNC RxSO2< Scalar > exp(Tangent const &a)
Definition: rxso2.hpp:467
Sophus::Sim2::Tangent
typename Base::Tangent Tangent
Definition: sim2.hpp:363
Sophus::Tests::Scalar
Scalar_ Scalar
Definition: test_rxso3.cpp:24
Sophus::Tests::sim2_vec_
std::vector< Sim2Type, Eigen::aligned_allocator< Sim2Type > > sim2_vec_
Definition: test_sim2.cpp:179
Sophus
Definition: average.hpp:17
Sophus::Sim2::Point
typename Base::Point Point
Definition: sim2.hpp:361
Sophus::Tests::Point
typename RxSO2< Scalar >::Point Point
Definition: test_rxso2.cpp:26
Sophus::Sim2::rxso2
SOPHUS_FUNC RxSo2Member & rxso2()
Definition: sim2.hpp:439
sim2.hpp
Sophus::processTestResult
void processTestResult(bool passed)
Definition: test_macros.hpp:38
Sophus::RxSO2::complex
SOPHUS_FUNC ComplexMember const & complex() const
Definition: rxso2.hpp:450
Sophus::Tests::testConstructors
bool testConstructors()
Definition: test_rxso2.cpp:154
Sophus::Tests::Tests
Tests()
Definition: test_sim2.cpp:33
Sophus::LieGroupTests::doAllTestsPass
enable_if_t< std::is_floating_point< S >::value, bool > doAllTestsPass()
Definition: tests.hpp:452
Sophus::Sim2::data
SOPHUS_FUNC Scalar * data()
Definition: sim2.hpp:425
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_TEST_NEQ
#define SOPHUS_TEST_NEQ(passed, left, right,...)
Definition: test_macros.hpp:81
Sophus::RxSO2
RxSO2 using storage; derived from RxSO2Base.
Definition: rxso2.hpp:11
Sophus::Constants
Definition: common.hpp:146
Sophus::Tests::testRawDataAcces
bool testRawDataAcces()
Definition: test_rxso2.cpp:118
Sophus::Sim2::translation
SOPHUS_FUNC TranslationMember & translation()
Definition: sim2.hpp:447
tests.hpp
Sophus::Sim2
Sim2 using default storage; derived from Sim2Base.
Definition: sim2.hpp:12
Sophus::Tests::runAll
void runAll()
Definition: test_sim2.cpp:75
Sophus::Tests::Sim2Type
Sim2< Scalar > Sim2Type
Definition: test_sim2.cpp:26
main
int main()
Definition: test_sim2.cpp:203

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