test_se3.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <sophus/se3.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::SE3<double>>;
10 template class Map<Sophus::SE3<double> const>;
11 } // namespace Eigen
12 
13 namespace Sophus {
14 
15 template class SE3<double, Eigen::AutoAlign>;
16 template class SE3<float, Eigen::DontAlign>;
17 #if SOPHUS_CERES
18 template class SE3<ceres::Jet<double, 3>>;
19 #endif
20 
21 template <class Scalar>
22 class Tests {
23  public:
24  using SE3Type = SE3<Scalar>;
25  using SO3Type = SO3<Scalar>;
26  using Point = typename SE3<Scalar>::Point;
27  using Tangent = typename SE3<Scalar>::Tangent;
28  Scalar const kPi = Constants<Scalar>::pi();
29 
30  Tests() {
31  se3_vec_ = getTestSE3s<Scalar>();
32 
33  Tangent tmp;
34  tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0);
35  tangent_vec_.push_back(tmp);
36  tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0);
37  tangent_vec_.push_back(tmp);
38  tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0);
39  tangent_vec_.push_back(tmp);
40  tmp << Scalar(0), Scalar(-5), Scalar(10), Scalar(0), Scalar(0), Scalar(0);
41  tangent_vec_.push_back(tmp);
42  tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(1);
43  tangent_vec_.push_back(tmp);
44  tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-1), Scalar(1), Scalar(0);
45  tangent_vec_.push_back(tmp);
46  tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(20), Scalar(-1), Scalar(0);
47  tangent_vec_.push_back(tmp);
48 
49  point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4)));
50  point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5)));
51  }
52 
53  void runAll() {
54  bool passed = testLieProperties();
55  passed &= testRawDataAcces();
56  passed &= testMutatingAccessors();
57  passed &= testConstructors();
58  passed &= testFit();
59  processTestResult(passed);
60  }
61 
62  private:
63  bool testLieProperties() {
64  LieGroupTests<SE3Type> tests(se3_vec_, tangent_vec_, point_vec_);
65  return tests.doAllTestsPass();
66  }
67 
68  bool testRawDataAcces() {
69  bool passed = true;
70  Eigen::Matrix<Scalar, 7, 1> raw;
71  raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),
72  Scalar(2);
73  Eigen::Map<SE3Type const> map_of_const_se3(raw.data());
74  SOPHUS_TEST_APPROX(
75  passed, map_of_const_se3.unit_quaternion().coeffs().eval(),
76  raw.template head<4>().eval(), Constants<Scalar>::epsilon());
77  SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(),
78  raw.template tail<3>().eval(),
79  Constants<Scalar>::epsilon());
80  SOPHUS_TEST_EQUAL(
81  passed, map_of_const_se3.unit_quaternion().coeffs().data(), raw.data());
82  SOPHUS_TEST_EQUAL(passed, map_of_const_se3.translation().data(),
83  raw.data() + 4);
84  Eigen::Map<SE3Type const> const_shallow_copy = map_of_const_se3;
85  SOPHUS_TEST_EQUAL(passed,
86  const_shallow_copy.unit_quaternion().coeffs().eval(),
87  map_of_const_se3.unit_quaternion().coeffs().eval());
88  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
89  map_of_const_se3.translation().eval());
90 
91  Eigen::Matrix<Scalar, 7, 1> raw2;
92  raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),
93  Scalar(1);
94  Eigen::Map<SE3Type> map_of_se3(raw.data());
95  Eigen::Quaternion<Scalar> quat;
96  quat.coeffs() = raw2.template head<4>();
97  map_of_se3.setQuaternion(quat);
98  map_of_se3.translation() = raw2.template tail<3>();
99  SOPHUS_TEST_APPROX(passed, map_of_se3.unit_quaternion().coeffs().eval(),
100  raw2.template head<4>().eval(),
101  Constants<Scalar>::epsilon());
102  SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),
103  raw2.template tail<3>().eval(),
104  Constants<Scalar>::epsilon());
105  SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_quaternion().coeffs().data(),
106  raw.data());
107  SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 4);
108  SOPHUS_TEST_NEQ(passed, map_of_se3.unit_quaternion().coeffs().data(),
109  quat.coeffs().data());
110  Eigen::Map<SE3Type> shallow_copy = map_of_se3;
111  SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(),
112  map_of_se3.unit_quaternion().coeffs().eval());
113  SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
114  map_of_se3.translation().eval());
115  Eigen::Map<SE3Type> const const_map_of_se3 = map_of_se3;
116  SOPHUS_TEST_EQUAL(passed,
117  const_map_of_se3.unit_quaternion().coeffs().eval(),
118  map_of_se3.unit_quaternion().coeffs().eval());
119  SOPHUS_TEST_EQUAL(passed, const_map_of_se3.translation().eval(),
120  map_of_se3.translation().eval());
121 
122  SE3Type const const_se3(quat, raw2.template tail<3>().eval());
123  for (int i = 0; i < 7; ++i) {
124  SOPHUS_TEST_EQUAL(passed, const_se3.data()[i], raw2.data()[i]);
125  }
126 
127  SE3Type se3(quat, raw2.template tail<3>().eval());
128  for (int i = 0; i < 7; ++i) {
129  SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]);
130  }
131 
132  for (int i = 0; i < 7; ++i) {
133  SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]);
134  }
135  SE3Type trans = SE3Type::transX(Scalar(0.2));
136  SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2),
137  Constants<Scalar>::epsilon());
138  trans = SE3Type::transY(Scalar(0.7));
139  SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7),
140  Constants<Scalar>::epsilon());
141  trans = SE3Type::transZ(Scalar(-0.2));
142  SOPHUS_TEST_APPROX(passed, trans.translation().z(), Scalar(-0.2),
143  Constants<Scalar>::epsilon());
144  Tangent t;
145  t << Scalar(0), Scalar(0), Scalar(0), Scalar(0.2), Scalar(0), Scalar(0);
146  SOPHUS_TEST_EQUAL(passed, SE3Type::rotX(Scalar(0.2)).matrix(),
147  SE3Type::exp(t).matrix());
148  t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.2), Scalar(0);
149  SOPHUS_TEST_EQUAL(passed, SE3Type::rotY(Scalar(-0.2)).matrix(),
150  SE3Type::exp(t).matrix());
151  t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1.1);
152  SOPHUS_TEST_EQUAL(passed, SE3Type::rotZ(Scalar(1.1)).matrix(),
153  SE3Type::exp(t).matrix());
154 
155  return passed;
156  }
157 
158  bool testMutatingAccessors() {
159  bool passed = true;
160  SE3Type se3;
161  SO3Type R(SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))));
162  se3.setRotationMatrix(R.matrix());
163  SOPHUS_TEST_APPROX(passed, se3.rotationMatrix(), R.matrix(),
164  Constants<Scalar>::epsilon());
165 
166  return passed;
167  }
168 
169  bool testConstructors() {
170  bool passed = true;
171  Eigen::Matrix<Scalar, 4, 4> I = Eigen::Matrix<Scalar, 4, 4>::Identity();
172  SOPHUS_TEST_EQUAL(passed, SE3Type().matrix(), I);
173 
174  SE3Type se3 = se3_vec_.front();
175  Point translation = se3.translation();
176  SO3Type so3 = se3.so3();
177 
178  SOPHUS_TEST_APPROX(passed, SE3Type(so3, translation).matrix(), se3.matrix(),
179  Constants<Scalar>::epsilon());
180  SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(),
181  se3.matrix(), Constants<Scalar>::epsilon());
182  SOPHUS_TEST_APPROX(passed,
183  SE3Type(so3.unit_quaternion(), translation).matrix(),
184  se3.matrix(), Constants<Scalar>::epsilon());
185  SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(),
186  Constants<Scalar>::epsilon());
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 
195  for (int i = 0; i < 100; ++i) {
196  Matrix4<Scalar> T = Matrix4<Scalar>::Random();
197  SE3Type se3 = SE3Type::fitToSE3(T);
198  SE3Type se3_2 = SE3Type::fitToSE3(se3.matrix());
199 
200  SOPHUS_TEST_APPROX(passed, se3.matrix(), se3_2.matrix(),
201  Constants<Scalar>::epsilon());
202  }
203  for (Scalar const angle :
204  {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) {
205  SOPHUS_TEST_APPROX(passed, SE3Type::rotX(angle).angleX(), angle,
206  Constants<Scalar>::epsilon());
207  SOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle,
208  Constants<Scalar>::epsilon());
209  SOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle,
210  Constants<Scalar>::epsilon());
211  }
212  return passed;
213  }
214 
215  template <class S = Scalar>
216  enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
217  return true;
218  }
219 
220  std::vector<SE3Type, Eigen::aligned_allocator<SE3Type>> se3_vec_;
221  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
222  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
223 };
224 
225 int test_se3() {
226  using std::cerr;
227  using std::endl;
228 
229  cerr << "Test SE3" << endl << endl;
230  cerr << "Double tests: " << endl;
231  Tests<double>().runAll();
232  cerr << "Float tests: " << endl;
233  Tests<float>().runAll();
234 
235 #if SOPHUS_CERES
236  cerr << "ceres::Jet<double, 3> tests: " << endl;
237  Tests<ceres::Jet<double, 3>>().runAll();
238 #endif
239 
240  return 0;
241 }
242 } // namespace Sophus
243 
244 int main() { return Sophus::test_se3(); }
Sophus::SE3::rotY
static SOPHUS_FUNC SE3 rotY(Scalar const &y)
Definition: se3.hpp:893
Sophus::SE3::so3
SOPHUS_FUNC SO3Member & so3()
Definition: se3.hpp:527
Eigen
Definition: rxso2.hpp:16
main
int main()
Definition: test_se3.cpp:244
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::SE3::data
SOPHUS_FUNC Scalar * data()
Definition: se3.hpp:513
Sophus::Tests::testMutatingAccessors
bool testMutatingAccessors()
Definition: test_se2.cpp:155
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::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:221
Sophus::SE3::Tangent
typename Base::Tangent Tangent
Definition: se3.hpp:441
Sophus::Tests
Definition: test_rxso2.cpp:22
Sophus::SE3::exp
static SOPHUS_FUNC SE3< Scalar > exp(Tangent const &a)
Definition: se3.hpp:763
Sophus::LieGroupTests
Definition: tests.hpp:21
Sophus::Tests::Tangent
typename RxSO2< Scalar >::Tangent Tangent
Definition: test_rxso2.cpp:27
Sophus::SE3::rotZ
static SOPHUS_FUNC SE3 rotZ(Scalar const &z)
Definition: se3.hpp:899
Sophus::Tests::Scalar
Scalar_ Scalar
Definition: test_rxso3.cpp:24
Sophus::Tests::SE3Type
SE3< Scalar > SE3Type
Definition: test_se3.cpp:24
Sophus
Definition: average.hpp:17
Sophus::Tests::Point
typename RxSO2< Scalar >::Point Point
Definition: test_rxso2.cpp:26
Sophus::processTestResult
void processTestResult(bool passed)
Definition: test_macros.hpp:38
Sophus::test_se3
int test_se3()
Definition: test_se3.cpp:225
Sophus::SE3::translation
SOPHUS_FUNC TranslationMember & translation()
Definition: se3.hpp:535
Sophus::Tests::testConstructors
bool testConstructors()
Definition: test_rxso2.cpp:154
Sophus::Tests::Tests
Tests()
Definition: test_se3.cpp:30
Sophus::SE3
SE3 using default storage; derived from SE3Base.
Definition: se3.hpp:11
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
se3.hpp
Sophus::SE3::fitToSE3
static SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SE3 > fitToSE3(Matrix4< Scalar > const &T)
Definition: se3.hpp:790
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::SE3::transZ
static SOPHUS_FUNC SE3 transZ(Scalar const &z)
Definition: se3.hpp:940
Sophus::Constants::pi
static SOPHUS_FUNC Scalar pi()
Definition: common.hpp:154
Sophus::SE3::transX
static SOPHUS_FUNC SE3 transX(Scalar const &x)
Definition: se3.hpp:928
Sophus::Tests::testFit
enable_if_t<!std::is_floating_point< S >::value, bool > testFit()
Definition: test_se3.cpp:216
SOPHUS_TEST_NEQ
#define SOPHUS_TEST_NEQ(passed, left, right,...)
Definition: test_macros.hpp:81
Sophus::Constants
Definition: common.hpp:146
Sophus::Tests::testRawDataAcces
bool testRawDataAcces()
Definition: test_rxso2.cpp:118
Sophus::SE3::Point
typename Base::Point Point
Definition: se3.hpp:439
tests.hpp
Sophus::Tests::se3_vec_
std::vector< SE3Type, Eigen::aligned_allocator< SE3Type > > se3_vec_
Definition: test_se3.cpp:220
Sophus::Tests::runAll
void runAll()
Definition: test_se3.cpp:53
Sophus::SE3::rotX
static SOPHUS_FUNC SE3 rotX(Scalar const &x)
Definition: se3.hpp:887
Sophus::SO3::unit_quaternion
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
Definition: so3.hpp:488
Sophus::SE3::transY
static SOPHUS_FUNC SE3 transY(Scalar const &y)
Definition: se3.hpp:934
Sophus::Matrix4
Matrix< Scalar, 4, 4 > Matrix4
Definition: types.hpp:54

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