MNE-CPP  0.1.9
A Framework for Electrophysiology
sphere.cpp
Go to the documentation of this file.
1 //=============================================================================================================
36 //=============================================================================================================
37 // INCLUDES
38 //=============================================================================================================
39 
40 #include "sphere.h"
41 #include "simplex_algorithm.h"
42 
43 #include <QDebug>
44 
45 //=============================================================================================================
46 // EIGEN INCLUDES
47 //=============================================================================================================
48 
49 #include <Eigen/Dense>
50 
51 #include <iostream>
52 
53 //=============================================================================================================
54 // USED NAMESPACES
55 //=============================================================================================================
56 
57 using namespace UTILSLIB;
58 using namespace Eigen;
59 
60 //=============================================================================================================
61 // DEFINE MEMBER METHODS
62 //=============================================================================================================
63 
64 Sphere::Sphere(const Vector3f& center, float radius)
65 : m_center(center)
66 , m_r(radius)
67 {
68 }
69 
70 //=============================================================================================================
71 
72 Sphere Sphere::fit_sphere(const MatrixX3f& points)
73 {
74  const VectorXf& x = points.col(0);
75  const VectorXf& y = points.col(1);
76  const VectorXf& z = points.col(2);
77 
78  VectorXf point_means = points.colwise().mean();
79 
80  VectorXf x_mean_free = x.array() - point_means(0);
81  VectorXf y_mean_free = y.array() - point_means(1);
82  VectorXf z_mean_free = z.array() - point_means(2);
83 
84  Matrix3f A;
85  A << (x.cwiseProduct(x_mean_free)).mean(), 2*(x.cwiseProduct(y_mean_free)).mean(), 2*(x.cwiseProduct(z_mean_free)).mean(),
86  0, (y.cwiseProduct(y_mean_free)).mean(), 2*(y.cwiseProduct(z_mean_free)).mean(),
87  0, 0, (z.cwiseProduct(z_mean_free)).mean();
88 
89  Matrix3f A_T = A.transpose();
90  A += A_T;
91 
92  Vector3f b;
93  VectorXf sq_sum = x.array().pow(2)+y.array().pow(2)+z.array().pow(2);
94  b << (sq_sum.cwiseProduct(x_mean_free)).mean(),
95  (sq_sum.cwiseProduct(y_mean_free)).mean(),
96  (sq_sum.cwiseProduct(z_mean_free)).mean();
97 
98  Vector3f center = A.ldlt().solve(b);
99 
100  MatrixX3f tmp(points.rows(),3);
101  tmp.col(0) = x.array() - center(0);
102  tmp.col(1) = y.array() - center(1);
103  tmp.col(2) = z.array() - center(2);
104 
105  float r = sqrt(tmp.array().pow(2).rowwise().sum().mean());
106 
107  return Sphere(center, r);
108 }
109 
110 //=============================================================================================================
111 
112 Sphere Sphere::fit_sphere_simplex(const MatrixX3f& points, double simplex_size)
113 {
114  VectorXf center;
115  float R;
116  if(fit_sphere_to_points( points, simplex_size, center, R)) {
117  return Sphere(center, R);
118  }
119 
120  return Sphere(Vector3f(), 0.0f);
121 }
122 
123 //=============================================================================================================
124 
125 bool Sphere::fit_sphere_to_points(float **rr, int np, float simplex_size, float *r0, float *R)
126 {
127  MatrixXf rr_eigen(np,3);
128  VectorXf r0_eigen(3);
129 
130  MatrixX3f dig_rr_eigen;
131  for (int k = 0; k < np; k++) {
132  rr_eigen(k, 0) = rr[k][0];
133  rr_eigen(k, 1) = rr[k][1];
134  rr_eigen(k, 2) = rr[k][2];
135  }
136 
137  if(rr_eigen.rows() < 0) {
138  std::cout << "Sphere::fit_sphere_to_points - No points were passed." << std::endl;
139  return false;
140  }
141 
142  r0_eigen(0) = r0[0];
143  r0_eigen(1) = r0[1];
144  r0_eigen(2) = r0[2];
145 
146  bool state = UTILSLIB::Sphere::fit_sphere_to_points(rr_eigen,simplex_size,r0_eigen,*R);
147 
148  r0[0] = r0_eigen(0);
149  r0[1] = r0_eigen(1);
150  r0[2] = r0_eigen(2);
151 
152  return state;
153 }
154 
155 //=============================================================================================================
156 
157 bool Sphere::fit_sphere_to_points(const MatrixXf &rr, float simplex_size, VectorXf &r0, float &R)
158 {
159 // int np = rr.rows();
160 
161  /*
162  * Find the optimal sphere origin
163  */
164  fitUserRecNew user;
165  float ftol = 1e-5f;
166  int max_eval = 500;
167  int report_interval = -1;
168  int neval;
169  MatrixXf init_simplex;
170  VectorXf init_vals(4);
171 
172  VectorXf cm(3);
173  float R0 = 0.1f;
174 
175  user.rr = rr;
176 
177  calculate_cm_ave_dist(rr, cm, R0);// [done]
178 
179  init_simplex = make_initial_simplex( cm, simplex_size );
180 
181  user.report = false;
182 
183  for (int k = 0; k < 4; k++) {
184  init_vals[k] = fit_eval( static_cast<VectorXf>(init_simplex.row(k)), &user );
185  }
186 
187  user.report = false;
188 
189  //Start the minimization
190  if(!SimplexAlgorithm::simplex_minimize<float>( init_simplex, /* The initial simplex */
191  init_vals, /* Function values at the vertices */
192  ftol, /* Relative convergence tolerance */
193  fit_eval, /* The function to be evaluated */
194  &user, /* Data to be passed to the above function in each evaluation */
195  max_eval, /* Maximum number of function evaluations */
196  neval, /* Number of function evaluations */
197  report_interval,/* How often to report (-1 = no_reporting) */
198  report_func)) /* The function to be called when reporting */
199  {
200  return false;
201  }
202 
203  r0 = init_simplex.row(0);
204  R = opt_rad(r0, &user);
205 
206  return true;
207 }
208 
209 //=============================================================================================================
210 
211 bool Sphere::report_func(int loop, const VectorXf &fitpar, double fval)
212 {
213  /*
214  * Report periodically
215  */
216  const VectorXf& r0 = fitpar;
217 
218  std::cout << "loop: " << loop << "; r0: " << 1000*r0[0] << ", r1: " << 1000*r0[1] << ", r2: " << 1000*r0[2] << "; fval: " << fval << std::endl;
219 
220  return true;
221 }
222 
223 //=============================================================================================================
224 
225 void Sphere::calculate_cm_ave_dist(const MatrixXf &rr, VectorXf &cm, float &avep)
226 {
227  cm = rr.colwise().mean();
228  MatrixXf diff = rr.rowwise() - cm.transpose();
229  avep = diff.rowwise().norm().mean();
230 }
231 
232 //=============================================================================================================
233 
234 MatrixXf Sphere::make_initial_simplex(const VectorXf &pars, float size)
235 {
236  /*
237  * Make the initial tetrahedron
238  */
239  int npar = pars.size();
240 
241  MatrixXf simplex = MatrixXf::Zero(npar+1,npar);
242 
243  simplex.rowwise() += pars.transpose();
244 
245  for (int k = 1; k < npar+1; k++) {
246  simplex(k,k-1) += size;
247  }
248 
249  return simplex;
250 }
251 
252 //=============================================================================================================
253 
254 float Sphere::fit_eval(const VectorXf &fitpar, const void *user_data)
255 {
256  /*
257  * Calculate the cost function value
258  * Optimize for the radius inside here
259  */
260  const fitUserNew& user = (fitUserNew)user_data;
261  const VectorXf& r0 = fitpar;
262 
263  float F;
264 
265  MatrixXf diff = user->rr.rowwise() - r0.transpose();
266  VectorXf one = diff.rowwise().norm();
267 
268  float sum = one.sum();
269  float sum2 = one.dot(one);
270 
271  F = sum2 - sum*sum/user->rr.rows();
272 
273  if(user->report)
274  std::cout << "r0: " << 1000*r0[0] << ", r1: " << 1000*r0[1] << ", r2: " << 1000*r0[2] << "; R: " << 1000*sum/user->rr.rows() << "; fval: "<<F<<std::endl;
275 
276  return F;
277 }
278 
279 //=============================================================================================================
280 
281 float Sphere::opt_rad(const VectorXf &r0,const fitUserNew user)
282 {
283  MatrixXf diff = user->rr.rowwise() - r0.transpose();
284  return diff.rowwise().norm().mean();
285 }
static Sphere fit_sphere(const Eigen::MatrixX3f &points)
Definition: sphere.cpp:72
Sphere(const Eigen::Vector3f &center, float radius)
Definition: sphere.cpp:64
Eigen::Vector3f & center()
Definition: sphere.h:112
SimplexAlgorithm Template Implementation.
static bool fit_sphere_to_points(const Eigen::MatrixXf &rr, float simplex_size, Eigen::VectorXf &r0, float &R)
Describes a 3D sphere object.
Definition: sphere.h:72
Sphere class declaration.
static Sphere fit_sphere_simplex(const Eigen::MatrixX3f &points, double simplex_size=2e-2)
Definition: sphere.cpp:112