1 // g2o - General Graph Optimization
2 // Copyright (C) 2014 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26
27 #include "g2o/core/eigen_types.h"
28 #include "g2o/stuff/sampler.h"
29 #include "g2o/types/slam3d/se3quat.h"
30
31 namespace g2o {
32 namespace internal {
randomIsometry3()33 inline Isometry3 randomIsometry3() {
34 Eigen::Vector3d rotAxisAngle = Eigen::Vector3d::Random();
35 rotAxisAngle += Eigen::Vector3d::Random();
36 Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized());
37 Eigen::Isometry3d result = (Eigen::Isometry3d)rotation.toRotationMatrix();
38 result.translation() = Eigen::Vector3d::Random();
39 return result;
40 }
41
42 struct RandomSE3Quat {
createRandomSE3Quat43 static SE3Quat create() {
44 SE3Quat result(Quaternion::UnitRandom(), Vector3::Random());
45 result.normalizeRotation();
46 return result;
47 }
isApproxRandomSE3Quat48 static bool isApprox(const SE3Quat& a, const SE3Quat& b) { return a.toVector().isApprox(b.toVector(), 1e-5); }
49 };
50
51 struct RandomIsometry3 {
createRandomIsometry352 static Isometry3 create() { return randomIsometry3(); }
isApproxRandomIsometry353 static bool isApprox(const Isometry3& a, const Isometry3& b) { return a.isApprox(b, 1e-5); }
54 };
55
56 struct RandomDouble {
createRandomDouble57 static double create() { return g2o::Sampler::uniformRand(-1., 1.); }
isApproxRandomDouble58 static bool isApprox(const double& a, const double& b) { return std::abs(a - b) < 1e-5; }
59 };
60
61 } // namespace internal
62 } // namespace g2o
63