covariance_ellipsoid.cpp
Go to the documentation of this file.
1 
8 /*****************************************************************************
9 ** Includes
10 *****************************************************************************/
11 
12 #include <ecl/linear_algebra.hpp>
13 #include "../../include/ecl/statistics/covariance_ellipsoid.hpp"
14 
15 /*****************************************************************************
16 ** Namespaces
17 *****************************************************************************/
18 
19 namespace ecl {
20 
21 /*****************************************************************************
22 ** Using
23 *****************************************************************************/
24 
25 using ecl::linear_algebra::Matrix2f;
26 using ecl::linear_algebra::Vector2f;
27 using ecl::linear_algebra::Matrix3f;
28 using ecl::linear_algebra::Vector3f;
29 
30 using ecl::linear_algebra::Matrix2d;
31 using ecl::linear_algebra::Vector2d;
32 using ecl::linear_algebra::Matrix3d;
33 using ecl::linear_algebra::Vector3d;
34 
35 /*****************************************************************************
36 ** Implementation [CovarianceEllipsoid2f]
37 *****************************************************************************/
38 CovarianceEllipsoid<float,2>::CovarianceEllipsoid() :
39  ellipse_lengths(Vector2f::Ones()),
40  ellipse_axes(Matrix2f::Identity())
41 {}
42 
43 CovarianceEllipsoid<float,2>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix2f &M) {
44  compute(M);
45 }
46 void CovarianceEllipsoid<float,2>::compute(const ecl::linear_algebra::Matrix2f &M) {
47 
48  // CHECK: symmetricity of M
49 
50  /*********************
51  ** Eigenvalues
52  **********************/
53  float a = M(0,0);
54  float b = M(0,1);
55  float c = M(1,0);
56  float d = M(1,1);
57 
58  float tmp = sqrtf((a+d)*(a+d)/4 - a*d + b*c);
59  ellipse_lengths << sqrtf((a+d)/2 + tmp), sqrtf((a+d)/2 - tmp);
60 
61  // CHECK: all ellipse_lengths are >= 0 (positive semi-definite check)
62  // CHECK: ordered in decreasing order of magnitude
63 
64  /*********************
65  ** Eigenvectors
66  **********************/
67  if( c != 0 ) {
68  ellipse_axes(0,0) = ellipse_lengths(0)*ellipse_lengths(0)-d;
69  ellipse_axes(1,0) = c;
70  ellipse_axes(0,1) = ellipse_lengths(1)*ellipse_lengths(1)-d;
71  ellipse_axes(1,1) = c;
72  } else if( b != 0 ) {
73  ellipse_axes(0,0) = b;
74  ellipse_axes(1,0) = ellipse_lengths(0)*ellipse_lengths(0)-a;
75  ellipse_axes(0,1) = b;
76  ellipse_axes(1,1) = ellipse_lengths(1)*ellipse_lengths(1)-a;
77  }
78  else {
79  if ( a > d ) {
80  ellipse_axes << 1, 0,
81  0, 1;
82  } else {
83  ellipse_axes << 0, 1,
84  1, 0;
85  }
86  }
87  /*********************
88  ** Normalise Evectors
89  **********************/
90  ellipse_axes.block<2,1>(0,0).normalize();
91  ellipse_axes.block<2,1>(0,1).normalize();
92 }
93 
94 double CovarianceEllipsoid<float,2>::rotation() {
95  return atan2f(ellipse_axes(1,0), ellipse_axes(0,0));
96 }
97 
98 Vector2f CovarianceEllipsoid<float,2>::intercepts() {
99 
100  Vector2f intercept_magnitudes;
101  float t;
102  float angle = rotation();
103 
104  /*********************
105  ** Parametric Form
106  **********************/
107  // here t is a parameter and theta is the rotation angle
108  //
109  // x = lambda1 cos(t)cos(theta) - lambda2 sin(t)sin(theta)
110  // y = lambda1 cos(t)sin(theta) + lambda2 sin(t)cos(theta)
111  //
112  // To find the intersection with the x axis, set y = 0, then
113  // t = arctan[ - (lambda1/lambda2) tan(theta) ]
114  // To find the intersection with the y axis, set x = 0, then
115  // t = arctan[ (lambda1/lambda2) cot(theta) ]
116 
117  /*********************
118  ** X Axis
119  **********************/
120  t = atan2f( -(ellipse_lengths(0)/ellipse_lengths(1))*tanf(angle), 1.0 );
121  intercept_magnitudes(0) = fabsf(ellipse_lengths(0)*cosf(t)*cosf(angle) - ellipse_lengths(1)*sinf(t)*sinf(angle));
122  /*********************
123  ** Y Axis
124  **********************/
125  t = atan2f( (ellipse_lengths(0)/ellipse_lengths(1)), tanf(angle) ); // Could this get dodgy with tan going to infinity?
126  intercept_magnitudes(1) = fabsf(ellipse_lengths(0)*cosf(t)*sinf(angle) + ellipse_lengths(1)*sinf(t)*sinf(angle));
127 
128  return intercept_magnitudes;
129 }
130 
131 /*****************************************************************************
132 ** Implementation [CovarianceEllipsoid2d]
133 *****************************************************************************/
134 CovarianceEllipsoid<double,2>::CovarianceEllipsoid() :
135  ellipse_lengths(Vector2d::Ones()),
136  ellipse_axes(Matrix2d::Identity())
137 {}
138 
139 CovarianceEllipsoid<double,2>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix2d &M) {
140  compute(M);
141 }
142 void CovarianceEllipsoid<double,2>::compute(const ecl::linear_algebra::Matrix2d &M) {
143 
144  // CHECK: symmetricity of M
145 
146  /*********************
147  ** Eigenvalues
148  **********************/
149  double a = M(0,0);
150  double b = M(0,1);
151  double c = M(1,0);
152  double d = M(1,1);
153 
154  double tmp = sqrt((a+d)*(a+d)/4 - a*d + b*c);
155  ellipse_lengths << sqrt((a+d)/2 + tmp), sqrt((a+d)/2 - tmp);
156 
157  // CHECK: all ellipse_lengths are >= 0 (positive semi-definite check)
158  // CHECK: ordered in decreasing order of magnitude
159 
160  /*********************
161  ** Eigenvectors
162  **********************/
163  if( c != 0 ) {
164  ellipse_axes(0,0) = ellipse_lengths(0)*ellipse_lengths(0)-d;
165  ellipse_axes(1,0) = c;
166  ellipse_axes(0,1) = ellipse_lengths(1)*ellipse_lengths(1)-d;
167  ellipse_axes(1,1) = c;
168  } else if( b != 0 ) {
169  ellipse_axes(0,0) = b;
170  ellipse_axes(1,0) = ellipse_lengths(0)*ellipse_lengths(0)-a;
171  ellipse_axes(0,1) = b;
172  ellipse_axes(1,1) = ellipse_lengths(1)*ellipse_lengths(1)-a;
173  }
174  else {
175  if ( a > d ) {
176  ellipse_axes << 1, 0,
177  0, 1;
178  } else {
179  ellipse_axes << 0, 1,
180  1, 0;
181  }
182  }
183  /*********************
184  ** Normalise Evectors
185  **********************/
186  ellipse_axes.block(0,0,2,1).normalize();
187  ellipse_axes.block(0,1,2,1).normalize();
188 }
189 
190 double CovarianceEllipsoid<double,2>::rotation() {
191  return atan2(ellipse_axes(1,0), ellipse_axes(0,0));
192 }
193 
194 Vector2d CovarianceEllipsoid<double,2>::intercepts() {
195 
196  Vector2d intercept_magnitudes;
197  double t;
198  double angle = rotation();
199 
200  /*********************
201  ** Parametric Form
202  **********************/
203  // here t is a parameter and theta is the rotation angle
204  //
205  // x = lambda1 cos(t)cos(theta) - lambda2 sin(t)sin(theta)
206  // y = lambda1 cos(t)sin(theta) + lambda2 sin(t)cos(theta)
207  //
208  // To find the intersection with the x axis, set y = 0, then
209  // t = arctan[ - (lambda1/lambda2) tan(theta) ]
210  // To find the intersection with the y axis, set x = 0, then
211  // t = arctan[ (lambda1/lambda2) cot(theta) ]
212 
213  /*********************
214  ** X Axis
215  **********************/
216  t = atan2( -(ellipse_lengths(0)/ellipse_lengths(1))*tan(angle), 1.0 );
217  intercept_magnitudes(0) = fabs(ellipse_lengths(0)*cos(t)*cos(angle) - ellipse_lengths(1)*sin(t)*sin(angle));
218  /*********************
219  ** Y Axis
220  **********************/
221  t = atan2( (ellipse_lengths(0)/ellipse_lengths(1)), tan(angle) ); // Could this get dodgy with tan going to infinity?
222  intercept_magnitudes(1) = fabs(ellipse_lengths(0)*cos(t)*sin(angle) + ellipse_lengths(1)*sin(t)*sin(angle));
223 
224  return intercept_magnitudes;
225 }
226 
227 
228 
229 /*****************************************************************************
230 ** Implementation [CovarianceEllipsoid3f]
231 *****************************************************************************/
232 CovarianceEllipsoid<float,3>::CovarianceEllipsoid() :
233  ellipse_lengths(Vector3f::Ones()),
234  ellipse_axes(Matrix3f::Identity())
235 {}
236 
237 CovarianceEllipsoid<float,3>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix3f &M, const bool sort) {
238  compute(M, sort);
239 }
240 void CovarianceEllipsoid<float,3>::compute(const ecl::linear_algebra::Matrix3f &M, const bool sort) {
241 
242  Eigen::EigenSolver<Matrix3f> esolver(M);
243 
244  ellipse_lengths[0] = sqrtf(esolver.pseudoEigenvalueMatrix()(0,0));
245  ellipse_lengths[1] = sqrtf(esolver.pseudoEigenvalueMatrix()(1,1));
246  ellipse_lengths[2] = sqrtf(esolver.pseudoEigenvalueMatrix()(2,2));
247  ellipse_axes = esolver.pseudoEigenvectors ();
248 
249  if ( sort ) {
250  // Note that sorting of eigenvalues may end up with left-hand coordinate system.
251  // So here we correctly sort it so that it does end up being righ-handed and normalised.
252  ecl::linear_algebra::Vector3f c0 = ellipse_axes.block<3,1>(0,0); c0.normalize();
253  ecl::linear_algebra::Vector3f c1 = ellipse_axes.block<3,1>(0,1); c1.normalize();
254  ecl::linear_algebra::Vector3f c2 = ellipse_axes.block<3,1>(0,2); c2.normalize();
255  ecl::linear_algebra::Vector3f cc = c0.cross(c1);
256  if (cc.dot(c2) < 0) {
257  ellipse_axes << c1, c0, c2;
258  double e = ellipse_lengths[0]; ellipse_lengths[0] = ellipse_lengths[1]; ellipse_lengths[1] = e;
259  } else {
260  ellipse_axes << c0, c1, c2;
261  }
262  }
263 }
264 
265 CovarianceEllipsoid<double,3>::CovarianceEllipsoid() :
266  ellipse_lengths(Vector3d::Ones()),
267  ellipse_axes(Matrix3d::Identity())
268 {}
269 
270 CovarianceEllipsoid<double,3>::CovarianceEllipsoid(const ecl::linear_algebra::Matrix3d &M, const bool sort) {
271  compute(M,sort);
272 }
273 void CovarianceEllipsoid<double,3>::compute(const ecl::linear_algebra::Matrix3d &M, const bool sort) {
274  Eigen::EigenSolver<Matrix3d> esolver(M);
275 
276  ellipse_lengths[0] = sqrt(esolver.pseudoEigenvalueMatrix()(0,0));
277  ellipse_lengths[1] = sqrt(esolver.pseudoEigenvalueMatrix()(1,1));
278  ellipse_lengths[2] = sqrt(esolver.pseudoEigenvalueMatrix()(2,2));
279  ellipse_axes = esolver.pseudoEigenvectors ();
280 
281  if ( sort ) {
282  // Note that sorting of eigenvalues may end up with left-hand coordinate system.
283  // So here we correctly sort it so that it does end up being righ-handed and normalised.
284  ecl::linear_algebra::Vector3d c0 = ellipse_axes.block<3,1>(0,0); c0.normalize();
285  ecl::linear_algebra::Vector3d c1 = ellipse_axes.block<3,1>(0,1); c1.normalize();
286  ecl::linear_algebra::Vector3d c2 = ellipse_axes.block<3,1>(0,2); c2.normalize();
287  ecl::linear_algebra::Vector3d cc = c0.cross(c1);
288  if (cc.dot(c2) < 0) {
289  ellipse_axes << c1, c0, c2;
290  double e = ellipse_lengths[0]; ellipse_lengths[0] = ellipse_lengths[1]; ellipse_lengths[1] = e;
291  } else {
292  ellipse_axes << c0, c1, c2;
293  }
294  }
295 }
296 
297 
298 } // namespace ecl
299 
c1
const Scalar c1
ecl::CovarianceEllipsoid
Parent template for covariance ellipsoids.
Definition: covariance_ellipsoid.hpp:54
Matrix2f
Matrix2< float > Matrix2f
linear_algebra.hpp
Vector2f
Vector2< float > Vector2f
Matrix2d
Matrix2< double > Matrix2d
Matrix3d
Matrix3< double > Matrix3d
Vector3d
Vector3< double > Vector3d
c2
const Scalar c2
Vector2d
Vector2< double > Vector2d
ecl::CovarianceEllipsoid::CovarianceEllipsoid
CovarianceEllipsoid()
Prevents usage of this template class directly.
Definition: covariance_ellipsoid.hpp:70
ecl::CovarianceEllipsoid< double, 2 >::rotation
double rotation()
Calculates the rotation of the ellipse.
Definition: covariance_ellipsoid.cpp:194
Vector3f
Vector3< float > Vector3f
Matrix3f
Matrix3< float > Matrix3f
ecl
c0
const Scalar c0

ecl_statistics
Author(s): Daniel Stonier
autogenerated on Wed Mar 2 2022 00:16:31