geo_hyperplane.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 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
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/QR>
15 
16 template<typename HyperplaneType> void hyperplane(const HyperplaneType& _plane)
17 {
18  /* this test covers the following files:
19  Hyperplane.h
20  */
21  using std::abs;
22  const Index dim = _plane.dim();
23  enum { Options = HyperplaneType::Options };
24  typedef typename HyperplaneType::Scalar Scalar;
25  typedef typename HyperplaneType::RealScalar RealScalar;
26  typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime, 1> VectorType;
27  typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime,
28  HyperplaneType::AmbientDimAtCompileTime> MatrixType;
29 
30  VectorType p0 = VectorType::Random(dim);
31  VectorType p1 = VectorType::Random(dim);
32 
33  VectorType n0 = VectorType::Random(dim).normalized();
34  VectorType n1 = VectorType::Random(dim).normalized();
35 
36  HyperplaneType pl0(n0, p0);
37  HyperplaneType pl1(n1, p1);
38  HyperplaneType pl2 = pl1;
39 
40  Scalar s0 = internal::random<Scalar>();
41  Scalar s1 = internal::random<Scalar>();
42 
43  VERIFY_IS_APPROX( n1.dot(n1), Scalar(1) );
44 
45  VERIFY_IS_MUCH_SMALLER_THAN( pl0.absDistance(p0), Scalar(1) );
46  if(numext::abs2(s0)>RealScalar(1e-6))
47  VERIFY_IS_APPROX( pl1.signedDistance(p1 + n1 * s0), s0);
48  else
49  VERIFY_IS_MUCH_SMALLER_THAN( abs(pl1.signedDistance(p1 + n1 * s0) - s0), Scalar(1) );
50  VERIFY_IS_MUCH_SMALLER_THAN( pl1.signedDistance(pl1.projection(p0)), Scalar(1) );
51  VERIFY_IS_MUCH_SMALLER_THAN( pl1.absDistance(p1 + pl1.normal().unitOrthogonal() * s1), Scalar(1) );
52 
53  // transform
54  if (!NumTraits<Scalar>::IsComplex)
55  {
56  MatrixType rot = MatrixType::Random(dim,dim).householderQr().householderQ();
57  DiagonalMatrix<Scalar,HyperplaneType::AmbientDimAtCompileTime> scaling(VectorType::Random());
58  Translation<Scalar,HyperplaneType::AmbientDimAtCompileTime> translation(VectorType::Random());
59 
60  while(scaling.diagonal().cwiseAbs().minCoeff()<RealScalar(1e-4)) scaling.diagonal() = VectorType::Random();
61 
62  pl2 = pl1;
63  VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot).absDistance(rot * p1), Scalar(1) );
64  pl2 = pl1;
65  VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot,Isometry).absDistance(rot * p1), Scalar(1) );
66  pl2 = pl1;
67  VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling).absDistance((rot*scaling) * p1), Scalar(1) );
68  VERIFY_IS_APPROX( pl2.normal().norm(), RealScalar(1) );
69  pl2 = pl1;
70  VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling*translation)
71  .absDistance((rot*scaling*translation) * p1), Scalar(1) );
72  VERIFY_IS_APPROX( pl2.normal().norm(), RealScalar(1) );
73  pl2 = pl1;
74  VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*translation,Isometry)
75  .absDistance((rot*translation) * p1), Scalar(1) );
76  VERIFY_IS_APPROX( pl2.normal().norm(), RealScalar(1) );
77  }
78 
79  // casting
80  const int Dim = HyperplaneType::AmbientDimAtCompileTime;
81  typedef typename GetDifferentType<Scalar>::type OtherScalar;
82  Hyperplane<OtherScalar,Dim,Options> hp1f = pl1.template cast<OtherScalar>();
83  VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),pl1);
84  Hyperplane<Scalar,Dim,Options> hp1d = pl1.template cast<Scalar>();
85  VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),pl1);
86 }
87 
88 template<typename Scalar> void lines()
89 {
90  using std::abs;
91  typedef Hyperplane<Scalar, 2> HLine;
92  typedef ParametrizedLine<Scalar, 2> PLine;
93  typedef Matrix<Scalar,2,1> Vector;
94  typedef Matrix<Scalar,3,1> CoeffsType;
95 
96  for(int i = 0; i < 10; i++)
97  {
98  Vector center = Vector::Random();
99  Vector u = Vector::Random();
100  Vector v = Vector::Random();
101  Scalar a = internal::random<Scalar>();
102  while (abs(a-1) < Scalar(1e-4)) a = internal::random<Scalar>();
103  while (u.norm() < Scalar(1e-4)) u = Vector::Random();
104  while (v.norm() < Scalar(1e-4)) v = Vector::Random();
105 
106  HLine line_u = HLine::Through(center + u, center + a*u);
107  HLine line_v = HLine::Through(center + v, center + a*v);
108 
109  // the line equations should be normalized so that a^2+b^2=1
110  VERIFY_IS_APPROX(line_u.normal().norm(), Scalar(1));
111  VERIFY_IS_APPROX(line_v.normal().norm(), Scalar(1));
112 
113  Vector result = line_u.intersection(line_v);
114 
115  // the lines should intersect at the point we called "center"
116  if(abs(a-1) > Scalar(1e-2) && abs(v.normalized().dot(u.normalized()))<Scalar(0.9))
117  VERIFY_IS_APPROX(result, center);
118 
119  // check conversions between two types of lines
120  PLine pl(line_u); // gcc 3.3 will crash if we don't name this variable.
121  HLine line_u2(pl);
122  CoeffsType converted_coeffs = line_u2.coeffs();
123  if(line_u2.normal().dot(line_u.normal())<Scalar(0))
124  converted_coeffs = -line_u2.coeffs();
125  VERIFY(line_u.coeffs().isApprox(converted_coeffs));
126  }
127 }
128 
129 template<typename Scalar> void planes()
130 {
131  using std::abs;
132  typedef Hyperplane<Scalar, 3> Plane;
133  typedef Matrix<Scalar,3,1> Vector;
134 
135  for(int i = 0; i < 10; i++)
136  {
137  Vector v0 = Vector::Random();
138  Vector v1(v0), v2(v0);
139  if(internal::random<double>(0,1)>0.25)
140  v1 += Vector::Random();
141  if(internal::random<double>(0,1)>0.25)
142  v2 += v1 * std::pow(internal::random<Scalar>(0,1),internal::random<int>(1,16));
143  if(internal::random<double>(0,1)>0.25)
144  v2 += Vector::Random() * std::pow(internal::random<Scalar>(0,1),internal::random<int>(1,16));
145 
146  Plane p0 = Plane::Through(v0, v1, v2);
147 
148  VERIFY_IS_APPROX(p0.normal().norm(), Scalar(1));
149  VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v0), Scalar(1));
150  VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v1), Scalar(1));
151  VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v2), Scalar(1));
152  }
153 }
154 
155 template<typename Scalar> void hyperplane_alignment()
156 {
157  typedef Hyperplane<Scalar,3,AutoAlign> Plane3a;
158  typedef Hyperplane<Scalar,3,DontAlign> Plane3u;
159 
160  EIGEN_ALIGN_MAX Scalar array1[4];
161  EIGEN_ALIGN_MAX Scalar array2[4];
162  EIGEN_ALIGN_MAX Scalar array3[4+1];
163  Scalar* array3u = array3+1;
164 
165  Plane3a *p1 = ::new(reinterpret_cast<void*>(array1)) Plane3a;
166  Plane3u *p2 = ::new(reinterpret_cast<void*>(array2)) Plane3u;
167  Plane3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Plane3u;
168 
169  p1->coeffs().setRandom();
170  *p2 = *p1;
171  *p3 = *p1;
172 
173  VERIFY_IS_APPROX(p1->coeffs(), p2->coeffs());
174  VERIFY_IS_APPROX(p1->coeffs(), p3->coeffs());
175 }
176 
177 
178 EIGEN_DECLARE_TEST(geo_hyperplane)
179 {
180  for(int i = 0; i < g_repeat; i++) {
181  CALL_SUBTEST_1( hyperplane(Hyperplane<float,2>()) );
182  CALL_SUBTEST_2( hyperplane(Hyperplane<float,3>()) );
183  CALL_SUBTEST_2( hyperplane(Hyperplane<float,3,DontAlign>()) );
184  CALL_SUBTEST_2( hyperplane_alignment<float>() );
185  CALL_SUBTEST_3( hyperplane(Hyperplane<double,4>()) );
186  CALL_SUBTEST_4( hyperplane(Hyperplane<std::complex<double>,5>()) );
187  CALL_SUBTEST_1( lines<float>() );
188  CALL_SUBTEST_3( lines<double>() );
189  CALL_SUBTEST_2( planes<float>() );
190  CALL_SUBTEST_5( planes<double>() );
191  }
192 }
rot
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Definition: level1_real_impl.h:79
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
gtsam::internal::translation
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:84
CALL_SUBTEST_4
#define CALL_SUBTEST_4(FUNC)
Definition: split_test_helper.h:22
v2
Vector v2
Definition: testSerializationBase.cpp:39
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
v1
Vector v1
Definition: testSerializationBase.cpp:38
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
Eigen::DiagonalMatrix::diagonal
EIGEN_DEVICE_FUNC const DiagonalVectorType & diagonal() const
Definition: DiagonalMatrix.h:160
hyperplane
void hyperplane(const HyperplaneType &_plane)
Definition: geo_hyperplane.cpp:16
CALL_SUBTEST_3
#define CALL_SUBTEST_3(FUNC)
Definition: split_test_helper.h:16
Eigen::DiagonalMatrix
Represents a diagonal matrix with its storage.
Definition: DiagonalMatrix.h:140
Eigen::Isometry
Definition: Constants.h:457
MatrixType
MatrixXf MatrixType
Definition: benchmark-blocking-sizes.cpp:52
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
Eigen::NumTraits
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
EIGEN_DECLARE_TEST
EIGEN_DECLARE_TEST(geo_hyperplane)
Definition: geo_hyperplane.cpp:178
EIGEN_ALIGN_MAX
#define EIGEN_ALIGN_MAX
Definition: ConfigureVectorization.h:157
Eigen::Translation
Represents a translation transformation.
Definition: ForwardDeclarations.h:291
VERIFY_IS_APPROX
#define VERIFY_IS_APPROX(a, b)
Definition: integer_types.cpp:15
abs2
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Abs2ReturnType abs2() const
Definition: ArrayCwiseUnaryOps.h:80
CALL_SUBTEST_1
#define CALL_SUBTEST_1(FUNC)
Definition: split_test_helper.h:4
result
Values result
Definition: OdometryOptimize.cpp:8
lines
void lines()
Definition: geo_hyperplane.cpp:88
Eigen::g_repeat
static int g_repeat
Definition: main.h:169
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
planes
void planes()
Definition: geo_hyperplane.cpp:129
RealScalar
NumTraits< Scalar >::Real RealScalar
Definition: bench_gemm.cpp:47
VERIFY_IS_MUCH_SMALLER_THAN
#define VERIFY_IS_MUCH_SMALLER_THAN(a, b)
Definition: main.h:390
CALL_SUBTEST_5
#define CALL_SUBTEST_5(FUNC)
Definition: split_test_helper.h:28
VERIFY
#define VERIFY(a)
Definition: main.h:380
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
CALL_SUBTEST_2
#define CALL_SUBTEST_2(FUNC)
Definition: split_test_helper.h:10
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
abs
#define abs(x)
Definition: datatypes.h:17
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
hyperplane_alignment
void hyperplane_alignment()
Definition: geo_hyperplane.cpp:155
Eigen::ParametrizedLine
A parametrized line.
Definition: ForwardDeclarations.h:295

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:17