test_sim3.cpp
Go to the documentation of this file.
1 #include <iostream>
2 
3 #include <unsupported/Eigen/MatrixFunctions>
4 
5 #include <sophus/sim3.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::Sim3<double>>;
12 template class Map<Sophus::Sim3<double> const>;
13 } // namespace Eigen
14 
15 namespace Sophus {
16 
17 template class Sim3<double, Eigen::AutoAlign>;
18 template class Sim3<float, Eigen::DontAlign>;
19 #if SOPHUS_CERES
20 template class Sim3<ceres::Jet<double, 3>>;
21 #endif
22 
23 template <class Scalar>
24 class Tests {
25  public:
26  using Sim3Type = Sim3<Scalar>;
27  using RxSO3Type = RxSO3<Scalar>;
28  using Point = typename Sim3<Scalar>::Point;
29  using Vector4Type = Vector4<Scalar>;
30  using Tangent = typename Sim3<Scalar>::Tangent;
31  Scalar const kPi = Constants<Scalar>::pi();
32 
33  Tests() {
34  sim3_vec_.push_back(
35  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5),
36  Scalar(0.0), Scalar(1.))),
37  Point(Scalar(0), Scalar(0), Scalar(0))));
38  sim3_vec_.push_back(
39  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5),
40  Scalar(-1.0), Scalar(1.1))),
41  Point(Scalar(10), Scalar(0), Scalar(0))));
42  sim3_vec_.push_back(
43  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), Scalar(0.),
44  Scalar(0.))),
45  Point(Scalar(0), Scalar(10), Scalar(5))));
46  sim3_vec_.push_back(
47  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), Scalar(0.),
48  Scalar(1.1))),
49  Point(Scalar(0), Scalar(10), Scalar(5))));
50  sim3_vec_.push_back(
51  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.),
52  Scalar(0.00001), Scalar(0.))),
53  Point(Scalar(0), Scalar(0), Scalar(0))));
54  sim3_vec_.push_back(Sim3Type(
55  RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.), Scalar(0.00001),
56  Scalar(0.0000001))),
57  Point(Scalar(1), Scalar(-1.00000001), Scalar(2.0000000001))));
58  sim3_vec_.push_back(
59  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.), Scalar(0.),
60  Scalar(0.00001), Scalar(0))),
61  Point(Scalar(0.01), Scalar(0), Scalar(0))));
62  sim3_vec_.push_back(Sim3Type(
63  RxSO3Type::exp(Vector4Type(kPi, Scalar(0), Scalar(0), Scalar(0.9))),
64  Point(Scalar(4), Scalar(-5), Scalar(0))));
65  sim3_vec_.push_back(
66  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.2), Scalar(0.5),
67  Scalar(0.0), Scalar(0))),
68  Point(Scalar(0), Scalar(0), Scalar(0))) *
69  Sim3Type(
70  RxSO3Type::exp(Vector4Type(kPi, Scalar(0), Scalar(0), Scalar(0))),
71  Point(Scalar(0), Scalar(0), Scalar(0))) *
72  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(-0.2), Scalar(-0.5),
73  Scalar(-0.0), Scalar(0))),
74  Point(Scalar(0), Scalar(0), Scalar(0))));
75  sim3_vec_.push_back(
76  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(0.3), Scalar(0.5),
77  Scalar(0.1), Scalar(0))),
78  Point(Scalar(2), Scalar(0), Scalar(-7))) *
79  Sim3Type(
80  RxSO3Type::exp(Vector4Type(kPi, Scalar(0), Scalar(0), Scalar(0))),
81  Point(Scalar(0), Scalar(0), Scalar(0))) *
82  Sim3Type(RxSO3Type::exp(Vector4Type(Scalar(-0.3), Scalar(-0.5),
83  Scalar(-0.1), Scalar(0))),
84  Point(Scalar(0), Scalar(6), Scalar(0))));
85  Tangent tmp;
86  tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
87  Scalar(0);
88  tangent_vec_.push_back(tmp);
89  tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
90  Scalar(0);
91  tangent_vec_.push_back(tmp);
92  tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0),
93  Scalar(0.1);
94  tangent_vec_.push_back(tmp);
95  tmp << Scalar(0), Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0),
96  Scalar(0.1);
97  tangent_vec_.push_back(tmp);
98  tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
99  Scalar(-0.1);
100  tangent_vec_.push_back(tmp);
101  tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-1), Scalar(1), Scalar(0),
102  Scalar(-0.1);
103  tangent_vec_.push_back(tmp);
104  tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(20), Scalar(-1), Scalar(0),
105  Scalar(1.5);
106  tangent_vec_.push_back(tmp);
107 
108  point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4)));
109  point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5)));
110  }
111 
112  void runAll() {
113  bool passed = testLieProperties();
114  passed &= testRawDataAcces();
115  passed &= testConstructors();
116  processTestResult(passed);
117  }
118 
119  private:
120  bool testLieProperties() {
121  LieGroupTests<Sim3Type> tests(sim3_vec_, tangent_vec_, point_vec_);
122  return tests.doAllTestsPass();
123  }
124 
125  bool testRawDataAcces() {
126  bool passed = true;
127  Eigen::Matrix<Scalar, 7, 1> raw;
128  raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),
129  Scalar(2);
130  Eigen::Map<Sim3Type const> map_of_const_sim3(raw.data());
131  SOPHUS_TEST_APPROX(passed, map_of_const_sim3.quaternion().coeffs().eval(),
132  raw.template head<4>().eval(),
133  Constants<Scalar>::epsilon());
134  SOPHUS_TEST_APPROX(passed, map_of_const_sim3.translation().eval(),
135  raw.template tail<3>().eval(),
136  Constants<Scalar>::epsilon());
137  SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.quaternion().coeffs().data(),
138  raw.data());
139  SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.translation().data(),
140  raw.data() + 4);
141  Eigen::Map<Sim3Type const> const_shallow_copy = map_of_const_sim3;
142  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
143  map_of_const_sim3.quaternion().coeffs().eval());
144  SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
145  map_of_const_sim3.translation().eval());
146 
147  Eigen::Matrix<Scalar, 7, 1> raw2;
148  raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),
149  Scalar(1);
150  Eigen::Map<Sim3Type> map_of_sim3(raw.data());
151  Eigen::Quaternion<Scalar> quat;
152  quat.coeffs() = raw2.template head<4>();
153  map_of_sim3.setQuaternion(quat);
154  map_of_sim3.translation() = raw2.template tail<3>();
155  SOPHUS_TEST_APPROX(passed, map_of_sim3.quaternion().coeffs().eval(),
156  raw2.template head<4>().eval(),
157  Constants<Scalar>::epsilon());
158  SOPHUS_TEST_APPROX(passed, map_of_sim3.translation().eval(),
159  raw2.template tail<3>().eval(),
160  Constants<Scalar>::epsilon());
161  SOPHUS_TEST_EQUAL(passed, map_of_sim3.quaternion().coeffs().data(),
162  raw.data());
163  SOPHUS_TEST_EQUAL(passed, map_of_sim3.translation().data(), raw.data() + 4);
164  SOPHUS_TEST_NEQ(passed, map_of_sim3.quaternion().coeffs().data(),
165  quat.coeffs().data());
166  Eigen::Map<Sim3Type> shallow_copy = map_of_sim3;
167  SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
168  map_of_sim3.quaternion().coeffs().eval());
169  SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
170  map_of_sim3.translation().eval());
171  Eigen::Map<Sim3Type> const const_map_of_sim3 = map_of_sim3;
172  SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.quaternion().coeffs().eval(),
173  map_of_sim3.quaternion().coeffs().eval());
174  SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(),
175  map_of_sim3.translation().eval());
176 
177  Sim3Type const const_sim3(quat, raw2.template tail<3>().eval());
178  for (int i = 0; i < 7; ++i) {
179  SOPHUS_TEST_EQUAL(passed, const_sim3.data()[i], raw2.data()[i]);
180  }
181 
182  Sim3Type se3(quat, raw2.template tail<3>().eval());
183  for (int i = 0; i < 7; ++i) {
184  SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]);
185  }
186 
187  for (int i = 0; i < 7; ++i) {
188  SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]);
189  }
190  return passed;
191  }
192 
193  bool testConstructors() {
194  bool passed = true;
195  Eigen::Matrix<Scalar, 4, 4> I = Eigen::Matrix<Scalar, 4, 4>::Identity();
196  SOPHUS_TEST_EQUAL(passed, Sim3Type().matrix(), I);
197 
198  Sim3Type sim3 = sim3_vec_.front();
199  Point translation = sim3.translation();
200  RxSO3Type rxso3 = sim3.rxso3();
201 
202  SOPHUS_TEST_APPROX(passed, Sim3Type(rxso3, translation).matrix(),
203  sim3.matrix(), Constants<Scalar>::epsilon());
204  SOPHUS_TEST_APPROX(passed,
205  Sim3Type(rxso3.quaternion(), translation).matrix(),
206  sim3.matrix(), Constants<Scalar>::epsilon());
207  SOPHUS_TEST_APPROX(passed, Sim3Type(sim3.matrix()).matrix(), sim3.matrix(),
208  Constants<Scalar>::epsilon());
209 
210  Scalar scale(1.2);
211  sim3.setScale(scale);
212  SOPHUS_TEST_APPROX(passed, scale, sim3.scale(),
213  Constants<Scalar>::epsilon(), "setScale");
214 
215  sim3.setQuaternion(sim3_vec_[0].rxso3().quaternion());
216  SOPHUS_TEST_APPROX(passed, sim3_vec_[0].rxso3().quaternion().coeffs(),
217  sim3_vec_[0].rxso3().quaternion().coeffs(),
218  Constants<Scalar>::epsilon(), "setQuaternion");
219  return passed;
220  }
221 
222  std::vector<Sim3Type, Eigen::aligned_allocator<Sim3Type>> sim3_vec_;
223  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
224  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
225 };
226 
227 int test_sim3() {
228  using std::cerr;
229  using std::endl;
230 
231  cerr << "Test Sim3" << endl << endl;
232  cerr << "Double tests: " << endl;
233  Tests<double>().runAll();
234  cerr << "Float tests: " << endl;
235  Tests<float>().runAll();
236 
237 #if SOPHUS_CERES
238  cerr << "ceres::Jet<double, 3> tests: " << endl;
239  Tests<ceres::Jet<double, 3>>().runAll();
240 #endif
241 
242  return 0;
243 }
244 } // namespace Sophus
245 
246 int main() { return Sophus::test_sim3(); }
Sophus::Tests::Vector4Type
Vector4< Scalar > Vector4Type
Definition: test_sim3.cpp:29
Sophus::RxSO3::quaternion
SOPHUS_FUNC QuaternionMember const & quaternion() const
Definition: rxso3.hpp:492
Eigen
Definition: rxso2.hpp:16
Sophus::Tests::Sim3Type
Sim3< Scalar > Sim3Type
Definition: test_sim3.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
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
main
int main()
Definition: test_sim3.cpp:246
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::sim3_vec_
std::vector< Sim3Type, Eigen::aligned_allocator< Sim3Type > > sim3_vec_
Definition: test_sim3.cpp:222
Sophus::Tests
Definition: test_rxso2.cpp:22
Sophus::Sim3::Point
typename Base::Point Point
Definition: sim3.hpp:361
Sophus::LieGroupTests
Definition: tests.hpp:21
Sophus::Tests::Tangent
typename RxSO2< Scalar >::Tangent Tangent
Definition: test_rxso2.cpp:27
Sophus::Sim3::data
SOPHUS_FUNC Scalar * data()
Definition: sim3.hpp:427
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::processTestResult
void processTestResult(bool passed)
Definition: test_macros.hpp:38
Sophus::Sim3::Tangent
typename Base::Tangent Tangent
Definition: sim3.hpp:363
Sophus::Tests::testConstructors
bool testConstructors()
Definition: test_rxso2.cpp:154
Sophus::Tests::Tests
Tests()
Definition: test_sim3.cpp:33
Sophus::LieGroupTests::doAllTestsPass
enable_if_t< std::is_floating_point< S >::value, bool > doAllTestsPass()
Definition: tests.hpp:452
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::Constants
Definition: common.hpp:146
Sophus::Tests::testRawDataAcces
bool testRawDataAcces()
Definition: test_rxso2.cpp:118
Sophus::Vector4
Vector< Scalar, 4 > Vector4
Definition: types.hpp:26
Sophus::Sim3
Sim3 using default storage; derived from Sim3Base.
Definition: sim3.hpp:12
tests.hpp
Sophus::Sim3::translation
SOPHUS_FUNC TranslationMember & translation()
Definition: sim3.hpp:449
Sophus::Tests::runAll
void runAll()
Definition: test_sim3.cpp:112
Sophus::RxSO3
RxSO3 using storage; derived from RxSO3Base.
Definition: rxso3.hpp:11
sim3.hpp
Sophus::Sim3::rxso3
SOPHUS_FUNC RxSo3Member & rxso3()
Definition: sim3.hpp:441

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