geo_quaternion.cpp
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
6 //
7 // This Source Code Form is subject to the terms of the Mozilla
8 // Public License v. 2.0. If a copy of the MPL was not distributed
9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10 
11 #include "main.h"
12 #include <Eigen/Geometry>
13 #include <Eigen/LU>
14 #include <Eigen/SVD>
15 
16 template<typename T> T bounded_acos(T v)
17 {
18  using std::acos;
19  using std::min;
20  using std::max;
21  return acos((max)(T(-1),(min)(v,T(1))));
22 }
23 
24 template<typename QuatType> void check_slerp(const QuatType& q0, const QuatType& q1)
25 {
26  using std::abs;
27  typedef typename QuatType::Scalar Scalar;
28  typedef AngleAxis<Scalar> AA;
29 
30  Scalar largeEps = test_precision<Scalar>();
31 
32  Scalar theta_tot = AA(q1*q0.inverse()).angle();
33  if(theta_tot>Scalar(EIGEN_PI))
34  theta_tot = Scalar(2.)*Scalar(EIGEN_PI)-theta_tot;
35  for(Scalar t=0; t<=Scalar(1.001); t+=Scalar(0.1))
36  {
37  QuatType q = q0.slerp(t,q1);
38  Scalar theta = AA(q*q0.inverse()).angle();
39  VERIFY(abs(q.norm() - 1) < largeEps);
40  if(theta_tot==0) VERIFY(theta_tot==0);
41  else VERIFY(abs(theta - t * theta_tot) < largeEps);
42  }
43 }
44 
45 template<typename Scalar, int Options> void quaternion(void)
46 {
47  /* this test covers the following files:
48  Quaternion.h
49  */
50  using std::abs;
51  typedef Matrix<Scalar,3,1> Vector3;
52  typedef Matrix<Scalar,3,3> Matrix3;
53  typedef Quaternion<Scalar,Options> Quaternionx;
54  typedef AngleAxis<Scalar> AngleAxisx;
55 
56  Scalar largeEps = test_precision<Scalar>();
57  if (internal::is_same<Scalar,float>::value)
58  largeEps = Scalar(1e-3);
59 
60  Scalar eps = internal::random<Scalar>() * Scalar(1e-2);
61 
62  Vector3 v0 = Vector3::Random(),
63  v1 = Vector3::Random(),
64  v2 = Vector3::Random(),
65  v3 = Vector3::Random();
66 
67  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)),
68  b = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
69 
70  // Quaternion: Identity(), setIdentity();
71  Quaternionx q1, q2;
72  q2.setIdentity();
73  VERIFY_IS_APPROX(Quaternionx(Quaternionx::Identity()).coeffs(), q2.coeffs());
74  q1.coeffs().setRandom();
75  VERIFY_IS_APPROX(q1.coeffs(), (q1*q2).coeffs());
76 
77  // concatenation
78  q1 *= q2;
79 
80  q1 = AngleAxisx(a, v0.normalized());
81  q2 = AngleAxisx(a, v1.normalized());
82 
83  // angular distance
84  Scalar refangle = abs(AngleAxisx(q1.inverse()*q2).angle());
85  if (refangle>Scalar(EIGEN_PI))
86  refangle = Scalar(2)*Scalar(EIGEN_PI) - refangle;
87 
88  if((q1.coeffs()-q2.coeffs()).norm() > 10*largeEps)
89  {
90  VERIFY_IS_MUCH_SMALLER_THAN(abs(q1.angularDistance(q2) - refangle), Scalar(1));
91  }
92 
93  // rotation matrix conversion
94  VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
95  VERIFY_IS_APPROX(q1 * q2 * v2,
96  q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
97 
98  VERIFY( (q2*q1).isApprox(q1*q2, largeEps)
99  || !(q2 * q1 * v2).isApprox(q1.toRotationMatrix() * q2.toRotationMatrix() * v2));
100 
101  q2 = q1.toRotationMatrix();
102  VERIFY_IS_APPROX(q1*v1,q2*v1);
103 
104  Matrix3 rot1(q1);
105  VERIFY_IS_APPROX(q1*v1,rot1*v1);
106  Quaternionx q3(rot1.transpose()*rot1);
107  VERIFY_IS_APPROX(q3*v1,v1);
108 
109 
110  // angle-axis conversion
111  AngleAxisx aa = AngleAxisx(q1);
112  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
113 
114  // Do not execute the test if the rotation angle is almost zero, or
115  // the rotation axis and v1 are almost parallel.
116  if (abs(aa.angle()) > 5*test_precision<Scalar>()
117  && (aa.axis() - v1.normalized()).norm() < Scalar(1.99)
118  && (aa.axis() + v1.normalized()).norm() < Scalar(1.99))
119  {
120  VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
121  }
122 
123  // from two vector creation
124  VERIFY_IS_APPROX( v2.normalized(),(q2.setFromTwoVectors(v1, v2)*v1).normalized());
125  VERIFY_IS_APPROX( v1.normalized(),(q2.setFromTwoVectors(v1, v1)*v1).normalized());
126  VERIFY_IS_APPROX(-v1.normalized(),(q2.setFromTwoVectors(v1,-v1)*v1).normalized());
127  if (internal::is_same<Scalar,double>::value)
128  {
129  v3 = (v1.array()+eps).matrix();
130  VERIFY_IS_APPROX( v3.normalized(),(q2.setFromTwoVectors(v1, v3)*v1).normalized());
131  VERIFY_IS_APPROX(-v3.normalized(),(q2.setFromTwoVectors(v1,-v3)*v1).normalized());
132  }
133 
134  // from two vector creation static function
135  VERIFY_IS_APPROX( v2.normalized(),(Quaternionx::FromTwoVectors(v1, v2)*v1).normalized());
136  VERIFY_IS_APPROX( v1.normalized(),(Quaternionx::FromTwoVectors(v1, v1)*v1).normalized());
137  VERIFY_IS_APPROX(-v1.normalized(),(Quaternionx::FromTwoVectors(v1,-v1)*v1).normalized());
138  if (internal::is_same<Scalar,double>::value)
139  {
140  v3 = (v1.array()+eps).matrix();
141  VERIFY_IS_APPROX( v3.normalized(),(Quaternionx::FromTwoVectors(v1, v3)*v1).normalized());
142  VERIFY_IS_APPROX(-v3.normalized(),(Quaternionx::FromTwoVectors(v1,-v3)*v1).normalized());
143  }
144 
145  // inverse and conjugate
146  VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
147  VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
148 
149  // test casting
150  Quaternion<float> q1f = q1.template cast<float>();
151  VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
152  Quaternion<double> q1d = q1.template cast<double>();
153  VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);
154 
155  // test bug 369 - improper alignment.
156  Quaternionx *q = new Quaternionx;
157  delete q;
158 
159  q1 = Quaternionx::UnitRandom();
160  q2 = Quaternionx::UnitRandom();
161  check_slerp(q1,q2);
162 
163  q1 = AngleAxisx(b, v1.normalized());
164  q2 = AngleAxisx(b+Scalar(EIGEN_PI), v1.normalized());
165  check_slerp(q1,q2);
166 
167  q1 = AngleAxisx(b, v1.normalized());
168  q2 = AngleAxisx(-b, -v1.normalized());
169  check_slerp(q1,q2);
170 
171  q1 = Quaternionx::UnitRandom();
172  q2.coeffs() = -q1.coeffs();
173  check_slerp(q1,q2);
174 }
175 
176 template<typename Scalar> void mapQuaternion(void){
177  typedef Map<Quaternion<Scalar>, Aligned> MQuaternionA;
178  typedef Map<const Quaternion<Scalar>, Aligned> MCQuaternionA;
179  typedef Map<Quaternion<Scalar> > MQuaternionUA;
180  typedef Map<const Quaternion<Scalar> > MCQuaternionUA;
181  typedef Quaternion<Scalar> Quaternionx;
182  typedef Matrix<Scalar,3,1> Vector3;
183  typedef AngleAxis<Scalar> AngleAxisx;
184 
185  Vector3 v0 = Vector3::Random(),
186  v1 = Vector3::Random();
187  Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
188 
189  EIGEN_ALIGN_MAX Scalar array1[4];
190  EIGEN_ALIGN_MAX Scalar array2[4];
191  EIGEN_ALIGN_MAX Scalar array3[4+1];
192  Scalar* array3unaligned = array3+1;
193 
194  MQuaternionA mq1(array1);
195  MCQuaternionA mcq1(array1);
196  MQuaternionA mq2(array2);
197  MQuaternionUA mq3(array3unaligned);
198  MCQuaternionUA mcq3(array3unaligned);
199 
200 // std::cerr << array1 << " " << array2 << " " << array3 << "\n";
201  mq1 = AngleAxisx(a, v0.normalized());
202  mq2 = mq1;
203  mq3 = mq1;
204 
205  Quaternionx q1 = mq1;
206  Quaternionx q2 = mq2;
207  Quaternionx q3 = mq3;
208  Quaternionx q4 = MCQuaternionUA(array3unaligned);
209 
210  VERIFY_IS_APPROX(q1.coeffs(), q2.coeffs());
211  VERIFY_IS_APPROX(q1.coeffs(), q3.coeffs());
212  VERIFY_IS_APPROX(q4.coeffs(), q3.coeffs());
213  #ifdef EIGEN_VECTORIZE
214  if(internal::packet_traits<Scalar>::Vectorizable)
215  VERIFY_RAISES_ASSERT((MQuaternionA(array3unaligned)));
216  #endif
217 
218  VERIFY_IS_APPROX(mq1 * (mq1.inverse() * v1), v1);
219  VERIFY_IS_APPROX(mq1 * (mq1.conjugate() * v1), v1);
220 
221  VERIFY_IS_APPROX(mcq1 * (mcq1.inverse() * v1), v1);
222  VERIFY_IS_APPROX(mcq1 * (mcq1.conjugate() * v1), v1);
223 
224  VERIFY_IS_APPROX(mq3 * (mq3.inverse() * v1), v1);
225  VERIFY_IS_APPROX(mq3 * (mq3.conjugate() * v1), v1);
226 
227  VERIFY_IS_APPROX(mcq3 * (mcq3.inverse() * v1), v1);
228  VERIFY_IS_APPROX(mcq3 * (mcq3.conjugate() * v1), v1);
229 
230  VERIFY_IS_APPROX(mq1*mq2, q1*q2);
231  VERIFY_IS_APPROX(mq3*mq2, q3*q2);
232  VERIFY_IS_APPROX(mcq1*mq2, q1*q2);
233  VERIFY_IS_APPROX(mcq3*mq2, q3*q2);
234 
235  // Bug 1461, compilation issue with Map<const Quat>::w(), and other reference/constness checks:
236  VERIFY_IS_APPROX(mcq3.coeffs().x() + mcq3.coeffs().y() + mcq3.coeffs().z() + mcq3.coeffs().w(), mcq3.coeffs().sum());
237  VERIFY_IS_APPROX(mcq3.x() + mcq3.y() + mcq3.z() + mcq3.w(), mcq3.coeffs().sum());
238  mq3.w() = 1;
239  const Quaternionx& cq3(q3);
240  VERIFY( &cq3.x() == &q3.x() );
241  const MQuaternionUA& cmq3(mq3);
242  VERIFY( &cmq3.x() == &mq3.x() );
243  // FIXME the following should be ok. The problem is that currently the LValueBit flag
244  // is used to determine wether we can return a coeff by reference or not, which is not enough for Map<const ...>.
245  //const MCQuaternionUA& cmcq3(mcq3);
246  //VERIFY( &cmcq3.x() == &mcq3.x() );
247 }
248 
249 template<typename Scalar> void quaternionAlignment(void){
250  typedef Quaternion<Scalar,AutoAlign> QuaternionA;
251  typedef Quaternion<Scalar,DontAlign> QuaternionUA;
252 
253  EIGEN_ALIGN_MAX Scalar array1[4];
254  EIGEN_ALIGN_MAX Scalar array2[4];
255  EIGEN_ALIGN_MAX Scalar array3[4+1];
256  Scalar* arrayunaligned = array3+1;
257 
258  QuaternionA *q1 = ::new(reinterpret_cast<void*>(array1)) QuaternionA;
259  QuaternionUA *q2 = ::new(reinterpret_cast<void*>(array2)) QuaternionUA;
260  QuaternionUA *q3 = ::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionUA;
261 
262  q1->coeffs().setRandom();
263  *q2 = *q1;
264  *q3 = *q1;
265 
266  VERIFY_IS_APPROX(q1->coeffs(), q2->coeffs());
267  VERIFY_IS_APPROX(q1->coeffs(), q3->coeffs());
268  #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
269  if(internal::packet_traits<Scalar>::Vectorizable && internal::packet_traits<Scalar>::size<=4)
270  VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(arrayunaligned)) QuaternionA));
271  #endif
272 }
273 
274 template<typename PlainObjectType> void check_const_correctness(const PlainObjectType&)
275 {
276  // there's a lot that we can't test here while still having this test compile!
277  // the only possible approach would be to run a script trying to compile stuff and checking that it fails.
278  // CMake can help with that.
279 
280  // verify that map-to-const don't have LvalueBit
281  typedef typename internal::add_const<PlainObjectType>::type ConstPlainObjectType;
282  VERIFY( !(internal::traits<Map<ConstPlainObjectType> >::Flags & LvalueBit) );
283  VERIFY( !(internal::traits<Map<ConstPlainObjectType, Aligned> >::Flags & LvalueBit) );
284  VERIFY( !(Map<ConstPlainObjectType>::Flags & LvalueBit) );
285  VERIFY( !(Map<ConstPlainObjectType, Aligned>::Flags & LvalueBit) );
286 }
287 
288 void test_geo_quaternion()
289 {
290  for(int i = 0; i < g_repeat; i++) {
291  CALL_SUBTEST_1(( quaternion<float,AutoAlign>() ));
292  CALL_SUBTEST_1( check_const_correctness(Quaternionf()) );
293  CALL_SUBTEST_2(( quaternion<double,AutoAlign>() ));
294  CALL_SUBTEST_2( check_const_correctness(Quaterniond()) );
295  CALL_SUBTEST_3(( quaternion<float,DontAlign>() ));
296  CALL_SUBTEST_4(( quaternion<double,DontAlign>() ));
297  CALL_SUBTEST_5(( quaternionAlignment<float>() ));
298  CALL_SUBTEST_6(( quaternionAlignment<double>() ));
299  CALL_SUBTEST_1( mapQuaternion<float>() );
300  CALL_SUBTEST_2( mapQuaternion<double>() );
301  }
302 }
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:33
VERIFY_RAISES_ASSERT
#define VERIFY_RAISES_ASSERT(a)
Definition: main.h:285
align_3::q2
Point2 q2
Definition: testPose2.cpp:753
max
#define max(a, b)
Definition: datatypes.h:20
v2
Vector v2
Definition: testSerializationBase.cpp:36
align_3::q1
Point2 q1
Definition: testPose2.cpp:753
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
v1
Vector v1
Definition: testSerializationBase.cpp:35
EIGEN_PI
#define EIGEN_PI
Definition: Eigen/src/Core/MathFunctions.h:15
min
#define min(a, b)
Definition: datatypes.h:19
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
v
ArrayXcf v
Definition: Cwise_arg.cpp:1
Eigen::Quaterniond
Quaternion< double > Quaterniond
Definition: 3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h:310
Eigen::LvalueBit
const unsigned int LvalueBit
Definition: Constants.h:139
theta
Rot2 theta
Definition: testMagFactor.cpp:44
quaternion
void quaternion(void)
Definition: geo_quaternion.cpp:45
a
Array33i a
Definition: Cwise_product.cpp:1
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
bounded_acos
T bounded_acos(T v)
Definition: geo_quaternion.cpp:16
VERIFY_IS_APPROX
#define VERIFY_IS_APPROX(a, b)
Definition: integer_types.cpp:15
v3
Vector v3
Definition: testSerializationBase.cpp:37
test_callbacks.value
value
Definition: test_callbacks.py:116
Eigen::g_repeat
static int g_repeat
Definition: main.h:144
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
Eigen::Aligned
Definition: Constants.h:235
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1520
align_3::q3
Point2 q3
Definition: testPose2.cpp:753
VERIFY_IS_MUCH_SMALLER_THAN
#define VERIFY_IS_MUCH_SMALLER_THAN(a, b)
Definition: main.h:335
mapQuaternion
void mapQuaternion(void)
Definition: geo_quaternion.cpp:176
acos
EIGEN_DEVICE_FUNC const AcosReturnType acos() const
Definition: ArrayCwiseUnaryOps.h:262
Eigen::Triplet
A small structure to hold a non zero as a triplet (i,j,value).
Definition: SparseUtil.h:154
VERIFY
#define VERIFY(a)
Definition: main.h:325
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:273
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
EIGEN_ALIGN_MAX
#define EIGEN_ALIGN_MAX
Definition: Macros.h:757
test_geo_quaternion
void test_geo_quaternion()
Definition: geo_quaternion.cpp:288
VERIFY_IS_NOT_APPROX
#define VERIFY_IS_NOT_APPROX(a, b)
Definition: integer_types.cpp:17
check_const_correctness
void check_const_correctness(const PlainObjectType &)
Definition: geo_quaternion.cpp:274
Eigen::Diagonal
Expression of a diagonal/subdiagonal/superdiagonal in a matrix.
Definition: Diagonal.h:63
quaternionAlignment
void quaternionAlignment(void)
Definition: geo_quaternion.cpp:249
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:102
Eigen::internal::isApprox
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
Definition: Eigen/src/Core/MathFunctions.h:1361
abs
#define abs(x)
Definition: datatypes.h:17
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
check_slerp
void check_slerp(const QuatType &q0, const QuatType &q1)
Definition: geo_quaternion.cpp:24
Eigen::AngleAxis
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:270
align_3::t
Point2 t(10, 10)
Eigen::Quaternionf
Quaternion< float > Quaternionf
Definition: 3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h:307

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:07