test_se2.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <sophus/se2.hpp>
4 #include <unsupported/Eigen/MatrixFunctions>
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::SE2<double>>;
11 template class Map<Sophus::SE2<double> const>;
12 } // namespace Eigen
13 
14 namespace Sophus {
15 
16 template class SE2<double, Eigen::AutoAlign>;
17 template class SE2<double, Eigen::DontAlign>;
18 #if SOPHUS_CERES
19 template class SE2<ceres::Jet<double, 3>>;
20 #endif
21 
22 template <class Scalar>
23 class Tests {
24  public:
25  using SE2Type = SE2<Scalar>;
26  using SO2Type = SO2<Scalar>;
27  using Point = typename SE2<Scalar>::Point;
28  using Tangent = typename SE2<Scalar>::Tangent;
29  Scalar const kPi = Constants<Scalar>::pi();
30 
31  Tests() {
32  se2_vec_.push_back(
33  SE2Type(SO2Type(Scalar(0.0)), Point(Scalar(0), Scalar(0))));
34  se2_vec_.push_back(
35  SE2Type(SO2Type(Scalar(0.2)), Point(Scalar(10), Scalar(0))));
36  se2_vec_.push_back(
37  SE2Type(SO2Type(Scalar(0.)), Point(Scalar(0), Scalar(100))));
38  se2_vec_.push_back(
39  SE2Type(SO2Type(Scalar(-1.)), Point(Scalar(20), -Scalar(1))));
40  se2_vec_.push_back(
41  SE2Type(SO2Type(Scalar(0.00001)),
42  Point(Scalar(-0.00000001), Scalar(0.0000000001))));
43  se2_vec_.push_back(
44  SE2Type(SO2Type(Scalar(0.2)), Point(Scalar(0), Scalar(0))) *
45  SE2Type(SO2Type(kPi), Point(Scalar(0), Scalar(0))) *
46  SE2Type(SO2Type(Scalar(-0.2)), Point(Scalar(0), Scalar(0))));
47  se2_vec_.push_back(
48  SE2Type(SO2Type(Scalar(0.3)), Point(Scalar(2), Scalar(0))) *
49  SE2Type(SO2Type(kPi), Point(Scalar(0), Scalar(0))) *
50  SE2Type(SO2Type(Scalar(-0.3)), Point(Scalar(0), Scalar(6))));
51 
52  Tangent tmp;
53  tmp << Scalar(0), Scalar(0), Scalar(0);
54  tangent_vec_.push_back(tmp);
55  tmp << Scalar(1), Scalar(0), Scalar(0);
56  tangent_vec_.push_back(tmp);
57  tmp << Scalar(0), Scalar(1), Scalar(1);
58  tangent_vec_.push_back(tmp);
59  tmp << Scalar(-1), Scalar(1), Scalar(0);
60  tangent_vec_.push_back(tmp);
61  tmp << Scalar(20), Scalar(-1), Scalar(-1);
62  tangent_vec_.push_back(tmp);
63  tmp << Scalar(30), Scalar(5), Scalar(20);
64  tangent_vec_.push_back(tmp);
65 
66  point_vec_.push_back(Point(1, 2));
67  point_vec_.push_back(Point(1, -3));
68  }
69 
70  void runAll() {
71  bool passed = testLieProperties();
72  passed &= testRawDataAcces();
73  passed &= testMutatingAccessors();
74  passed &= testConstructors();
75  passed &= testFit();
76  processTestResult(passed);
77  }
78 
79  private:
80  bool testLieProperties() {
81  LieGroupTests<SE2Type> tests(se2_vec_, tangent_vec_, point_vec_);
82  return tests.doAllTestsPass();
83  }
84 
85  bool testRawDataAcces() {
86  bool passed = true;
87  Eigen::Matrix<Scalar, 4, 1> raw;
88  raw << Scalar(0), Scalar(1), Scalar(0), Scalar(3);
89  Eigen::Map<SE2Type const> const_se2_map(raw.data());
90  SOPHUS_TEST_APPROX(passed, const_se2_map.unit_complex().eval(),
91  raw.template head<2>().eval(),
92  Constants<Scalar>::epsilon());
93  SOPHUS_TEST_APPROX(passed, const_se2_map.translation().eval(),
94  raw.template tail<2>().eval(),
95  Constants<Scalar>::epsilon());
96  SOPHUS_TEST_EQUAL(passed, const_se2_map.unit_complex().data(), raw.data());
97  SOPHUS_TEST_EQUAL(passed, const_se2_map.translation().data(),
98  raw.data() + 2);
99  Eigen::Map<SE2Type const> const_shallow_copy = const_se2_map;
100  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.unit_complex().eval(),
101  const_se2_map.unit_complex().eval());
102  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
103  const_se2_map.translation().eval());
104 
105  Eigen::Matrix<Scalar, 4, 1> raw2;
106  raw2 << Scalar(1), Scalar(0), Scalar(3), Scalar(1);
107  Eigen::Map<SE2Type> map_of_se3(raw.data());
108  map_of_se3.setComplex(raw2.template head<2>());
109  map_of_se3.translation() = raw2.template tail<2>();
110  SOPHUS_TEST_APPROX(passed, map_of_se3.unit_complex().eval(),
111  raw2.template head<2>().eval(),
112  Constants<Scalar>::epsilon());
113  SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),
114  raw2.template tail<2>().eval(),
115  Constants<Scalar>::epsilon());
116  SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_complex().data(), raw.data());
117  SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 2);
118  SOPHUS_TEST_NEQ(passed, map_of_se3.unit_complex().data(), raw2.data());
119  Eigen::Map<SE2Type> shallow_copy = map_of_se3;
120  SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_complex().eval(),
121  map_of_se3.unit_complex().eval());
122  SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
123  map_of_se3.translation().eval());
124  Eigen::Map<SE2Type> const const_map_of_se2 = map_of_se3;
125  SOPHUS_TEST_EQUAL(passed, const_map_of_se2.unit_complex().eval(),
126  map_of_se3.unit_complex().eval());
127  SOPHUS_TEST_EQUAL(passed, const_map_of_se2.translation().eval(),
128  map_of_se3.translation().eval());
129 
130  SE2Type const const_se2(raw2.template head<2>().eval(),
131  raw2.template tail<2>().eval());
132  for (int i = 0; i < 4; ++i) {
133  SOPHUS_TEST_EQUAL(passed, const_se2.data()[i], raw2.data()[i]);
134  }
135 
136  SE2Type se2(raw2.template head<2>().eval(), raw2.template tail<2>().eval());
137  for (int i = 0; i < 4; ++i) {
138  SOPHUS_TEST_EQUAL(passed, se2.data()[i], raw2.data()[i]);
139  }
140 
141  for (int i = 0; i < 4; ++i) {
142  SOPHUS_TEST_EQUAL(passed, se2.data()[i], raw.data()[i]);
143  }
144 
145  SE2Type trans = SE2Type::transX(Scalar(0.2));
146  SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2),
147  Constants<Scalar>::epsilon());
148  trans = SE2Type::transY(Scalar(0.7));
149  SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7),
150  Constants<Scalar>::epsilon());
151 
152  return passed;
153  }
154 
155  bool testMutatingAccessors() {
156  bool passed = true;
157  SE2Type se2;
158  SO2Type R(Scalar(0.2));
159  se2.setRotationMatrix(R.matrix());
160  SOPHUS_TEST_APPROX(passed, se2.rotationMatrix(), R.matrix(),
161  Constants<Scalar>::epsilon());
162 
163  return passed;
164  }
165 
166  bool testConstructors() {
167  bool passed = true;
168  Matrix3<Scalar> I = Matrix3<Scalar>::Identity();
169  SOPHUS_TEST_EQUAL(passed, SE2Type().matrix(), I);
170 
171  SE2Type se2 = se2_vec_.front();
172  Point translation = se2.translation();
173  SO2Type so2 = se2.so2();
174 
175  SOPHUS_TEST_APPROX(passed, SE2Type(so2.log(), translation).matrix(),
176  se2.matrix(), Constants<Scalar>::epsilon());
177  SOPHUS_TEST_APPROX(passed, SE2Type(so2, translation).matrix(), se2.matrix(),
178  Constants<Scalar>::epsilon());
179  SOPHUS_TEST_APPROX(passed, SE2Type(so2.matrix(), translation).matrix(),
180  se2.matrix(), Constants<Scalar>::epsilon());
181  SOPHUS_TEST_APPROX(passed,
182  SE2Type(so2.unit_complex(), translation).matrix(),
183  se2.matrix(), Constants<Scalar>::epsilon());
184  SOPHUS_TEST_APPROX(passed, SE2Type(se2.matrix()).matrix(), se2.matrix(),
185  Constants<Scalar>::epsilon());
186 
187  return passed;
188  }
189 
190  template <class S = Scalar>
191  enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
192  bool passed = true;
193  for (int i = 0; i < 100; ++i) {
194  Matrix3<Scalar> T = Matrix3<Scalar>::Random();
195  SE2Type se2 = SE2Type::fitToSE2(T);
196  SE2Type se2_2 = SE2Type::fitToSE2(se2.matrix());
197 
198  SOPHUS_TEST_APPROX(passed, se2.matrix(), se2_2.matrix(),
199  Constants<Scalar>::epsilon());
200  }
201  return passed;
202  }
203 
204  template <class S = Scalar>
205  enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
206  return true;
207  }
208 
209  std::vector<SE2Type, Eigen::aligned_allocator<SE2Type>> se2_vec_;
210  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
211  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
212 };
213 
214 int test_se2() {
215  using std::cerr;
216  using std::endl;
217 
218  cerr << "Test SE2" << endl << endl;
219  cerr << "Double tests: " << endl;
220  Tests<double>().runAll();
221  cerr << "Float tests: " << endl;
222  Tests<float>().runAll();
223 
224 #if SOPHUS_CERES
225  cerr << "ceres::Jet<double, 3> tests: " << endl;
226  Tests<ceres::Jet<double, 3>>().runAll();
227 #endif
228 
229  return 0;
230 }
231 } // namespace Sophus
232 
233 int main() { return Sophus::test_se2(); }
Sophus::SO2
SO2 using default storage; derived from SO2Base.
Definition: so2.hpp:19
Eigen
Definition: rxso2.hpp:16
Sophus::test_se2
int test_se2()
Definition: test_se2.cpp:214
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::testMutatingAccessors
bool testMutatingAccessors()
Definition: test_se2.cpp:155
Sophus::Tests::point_vec_
std::vector< Point, Eigen::aligned_allocator< Point > > point_vec_
Definition: test_rxso2.cpp:197
se2.hpp
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::Tests::se2_vec_
std::vector< SE2Type, Eigen::aligned_allocator< SE2Type > > se2_vec_
Definition: test_se2.cpp:209
Sophus::SE2::so2
SOPHUS_FUNC SO2Member & so2()
Definition: se2.hpp:460
Sophus::LieGroupTests
Definition: tests.hpp:21
Sophus::Tests::SE2Type
SE2< Scalar > SE2Type
Definition: test_se2.cpp:25
Sophus::Tests::Tangent
typename RxSO2< Scalar >::Tangent Tangent
Definition: test_rxso2.cpp:27
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::SE2::Tangent
typename Base::Tangent Tangent
Definition: se2.hpp:375
Sophus::SE2
SE2 using default storage; derived from SE2Base.
Definition: se2.hpp:11
Sophus::processTestResult
void processTestResult(bool passed)
Definition: test_macros.hpp:38
Sophus::SE2::transY
static SOPHUS_FUNC SE2 transY(Scalar const &y)
Definition: se2.hpp:696
Sophus::Tests::testConstructors
bool testConstructors()
Definition: test_rxso2.cpp:154
Sophus::Tests::Tests
Tests()
Definition: test_se2.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::SE2::data
SOPHUS_FUNC Scalar * data()
Definition: se2.hpp:446
Sophus::Tests::SO2Type
SO2< Scalar > SO2Type
Definition: test_rxso2.cpp:24
Sophus::SE2::Point
typename Base::Point Point
Definition: se2.hpp:373
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_se2.cpp:205
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
Sophus::SE2::translation
SOPHUS_FUNC TranslationMember & translation()
Definition: se2.hpp:468
tests.hpp
Sophus::SE2::fitToSE2
static SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SE2 > fitToSE2(Matrix3< Scalar > const &T)
Definition: se2.hpp:587
main
int main()
Definition: test_se2.cpp:233
Sophus::SE2::transX
static SOPHUS_FUNC SE2 transX(Scalar const &x)
Definition: se2.hpp:690
Sophus::Tests::runAll
void runAll()
Definition: test_se2.cpp:70
Sophus::SO2::unit_complex
SOPHUS_FUNC ComplexMember const & unit_complex() const
Definition: so2.hpp:408

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