CustusX  18.04
An IGT application
spline3d.hpp
Go to the documentation of this file.
1 #ifndef __SPLINE_H
2 #define __SPLINE_H
3 
4 #include <stdint.h>
5 #include <vector>
6 #include <vtkPolyData.h>
7 #include <vtkSmartPointer.h>
8 #include <vtkCellArray.h>
9 #include "plane3d.hpp"
11 #include "adjlist.hpp"
12 #include "helpers.hpp"
13 #include <unordered_map>
14 #include "matrix.hpp"
15 #include "intersection.hpp"
16 #include "intersection_set.hpp"
17 #include "metaimage.hpp"
18 
19 using namespace std;
20 
25 template<typename T>
26 class Spline3D {
27 public:
28 
29  // typedef QuadraticSplineFitter<T> QSP_t;
30 
36  Spline3D(size_t npoints)
37  {
38  for(int i = 0; i < 3; i++)
39  {
40  m_points[i] = vector<T>(npoints);
41  }
42  m_initialized = false;
43  m_axis = 1;
44  m_transform = true;
45  }
46 
50 // Spline3D() { Spline3D(0); }
51 
56  ~Spline3D() { }
57 
63  inline void
64  setPoint(int idx, T const pt[3])
65  {
66  for(int i = 0; i < 3; i++)
67  {
68  m_points[i][idx] = pt[i];
69  }
70  m_initialized = false;
71  }
72 
78  inline void
79  setTransform(bool b)
80  {
81  m_transform = b;
82  }
83 
88  inline bool
89  getTransform() const
90  {
91  return m_transform;
92  }
93 
98  inline void
99  setAxis(int axis)
100  {
101  m_axis = axis;
102  }
103 
108  inline int
109  getAxis() const
110  {
111  return m_axis;
112  }
113 
117  inline size_t
118  getLength() const { return m_points[0].size(); }
119 
126  inline void
127  setPoints(vector<T> const &x,
128  vector<T> const &y,
129  vector<T> const &z)
130  {
131  m_points[0] = x;
132  m_points[1] = y;
133  m_points[2] = z;
134  }
135 
140  void
141  applyConvolution(vector<T> const &mask)
142  {
143  assert(mask.size() == 3);
144  for(int j = 0; j < 3; j++)
145  {
146  vector<T> newpoints(m_points[j].size());
147  int end = m_points[j].size()-1;
148 
149  // Reflexive behaviour
150  newpoints[0] = 2*mask[0] * m_points[j][1] + mask[1]*m_points[j][0];
151  newpoints[end] = 2*mask[0] * m_points[j][end-1] + mask[1]*m_points[j][end];
152  for(unsigned int i = 1; i < m_points[j].size()-1; i++)
153  {
154  newpoints[i] = mask[0]*m_points[j][i-1]
155  + mask[1]*m_points[j][i]
156  + mask[2]*m_points[j][i+1];
157  }
158  m_points[j] = newpoints;
159  }
160  m_initialized = false;
161  }
162 
168  void
170  {
171  // Build splines
172  for(int i = 0; i < 3; i++)
173  {
174  QuadraticSplineFitter<T> fitter(m_points[i]);
175  m_cpoints[i] = fitter.compute_control_points();
176  }
177  m_initialized = true;
178  }
179 
180 
186  inline int
187  length() const
188  {
189  return m_points[0].size();
190  }
191 
192 
193 
194 
195 
205  bool
206  intersect(T& t, Plane3D& plane, T pointOnPlane[3]) const
207  {
208  // 1. Find two neighbouring points for which the the plane distance function has opposite signs
209  double pt[3];
210  for(int i = 0; i < 3; i++)
211  {
212  pt[i] = m_points[i][0];
213  }
214  int prevsign = sgn(plane.getDistance(pt));
215  int pos = -1;
216  for(unsigned int i = 1; i < m_points[0].size(); i++)
217  {
218  for(int j = 0; j < 3; j++)
219  {
220  pt[j] = m_points[j][i];
221  }
222  int sign = sgn(plane.getDistance(pt));
223  if(prevsign != sign){
224  // We found the points:
225  // Points(i-1 and i)
226  pos = i-1;
227  break;
228  }
229  prevsign = sign;
230  }
231  // Return false if we didn't find an intersection
232  if(pos == -1){
233  return false;
234  }
235 
236  T roots[2];
237  int nroots;
238  nroots = findRoots(pos,plane, roots);
239  // Rounding errors may cause us to miss the 0-1 interval just slightly, try both
240  if(nroots == 0)
241  nroots = findRoots(++pos,plane,roots);
242  // Add the position of p0 to the roots
243  roots[0] += pos;
244  roots[1] += pos;
245 
246  switch(nroots)
247  {
248  case 0:
249  return false;
250  case 1:
251  case 2:
252  // For now, return only one intersection. May have to fix this.
253  // Subtracting 0.5 because the spline is translated by 0.5 along the t axis.
254  // This way the spline is aligned with the interpolation points (spline(0) == point 0)
255  t = roots[0] - 0.5;
256  // t = roots[0];
257  evaluateSingle(t,pointOnPlane);
258 
259  return true;
260  default:
261  assert(1 == 0);
262  return false;
263  }
264  }
265 
271  void
272  derivativeSingle(T t, T point[3]) const
273  {
274  // Align with m_points (curve(0) == m_points[0])
275  t += 1.5;
276  int pos = t;
277  t = t - pos;
278 
279  for(int i = 0; i < 3; i++)
280  {
281  point[i]
282  = t*(m_cpoints[i][pos+1]-m_cpoints[i][pos])
283  + (1-t)*(m_cpoints[i][pos]-m_cpoints[i][pos-1]);
284  }
285  }
286 
292  void
293  evaluateSingle(T t, T point[3]) const
294  {
295  if(!m_initialized )
296  {
297  reportError("ERROR: Spline3d not initialized");
298  return;
299  }
300 
301  // Align with m_points (curve(0) == m_points[0])
302  t += 1.5;
303  int pos = t;
304  t = t - pos;
305 
306  for(int i = 0; i < 3; i++)
307  {
308  point[i] = (t*t*0.5)*m_cpoints[i][pos+1]
309  + (-(t*t)+t + 0.5) * m_cpoints[i][pos]
310  + ((1-t)*(1-t)*0.5)*m_cpoints[i][pos-1];
311  }
312  }
313 
314 
315 
322  static vector<Spline3D<T> >*
323  build(vtkSmartPointer<vtkPolyData> data)
324  {
325  // Algorithm:
326  // 1. Make a data structure for each point that holds what line it belongs to (adjacency list)
327  // 2. Iterate over the lines in data, building adjacencies, as well as the rank of each point
328  // 3. Find all points with rank 1 and push it onto point stack
329  // 4. Do (multistart) depth first search
330  // For every discoevered node, add it to the current spline.
331  // When a leaf node is found and there are still points on the stack
332  // make a new spline, adding a few points from before the junction
333 
334 
335  // Step 1
336  AdjList list(data->GetNumberOfPoints());
337  vector<Spline3D<T> > *ret = new vector<Spline3D<T> >();
338 
339  // Step 2: Get the lines from the polyData structure and put them in the AdjList
340  vtkCellArray *lines = data->GetLines();
341 
342  vtkIdType n_ids;
343  vtkSmartPointer<vtkIdList> idlist = vtkSmartPointer<vtkIdList>::New();
344 
345  while(lines->GetNextCell(idlist))
346  {
347  n_ids = idlist->GetNumberOfIds();
348  assert(n_ids == 2);
349  list.adjacent(idlist->GetId(0), idlist->GetId(1));
350  }
351 
352  // Initialize node stack
353  vector<int> stack = vector<int>();
354  // Point stack - this is where the discovered points go.
355  // The array is one for each axis, and there
356  // is one vector per set of control points.
357  // I.e ptstack[2][0] is the set of z coordinates of the control points for curve 0
358  vector<vector<T> > ptstack[3];
359 
360  for(int i = 0; i < 3; i++)
361  {
362  ptstack[i] = vector<vector<T> >();
363  ptstack[i].push_back(vector<T>());
364  }
365 
366  double pt[3];
367  vector<int> children;
368 
369  // When we hit the end of a curve, we will need to know which point in which control point set
370  // comes before the new set of control points we will be about to make.
371  // The idea is then to keep a stack of "parents", and move it along with the node stack, so that this stack
372  // will have the parent information on its head for the point at the
373  // stacks head.
374  // The pair is <spline, idx>
375  vector<pair<int,int> > parents = vector<pair<int,int> >();
376 
377 
378  // Build the splines depth first
379  int curspline = 0;
380 
381  vector<int> firstNodes = list.findAllFirst();
382 
383  for(int i = firstNodes.size()-1; i >= 0; i--)
384  {
385  // The initial points have no parents
386  parents.push_back(pair<int,int>(-1, -1));
387  stack.push_back(firstNodes[i]);
388  }
389 
390  // Do depth first search
391  while(!stack.empty())
392  {
393 
394  // Pop stack
395  int curnode = stack.back();
396  parents.pop_back();
397  stack.pop_back();
398 
399  // Get the point data for this node
400  data->GetPoint(curnode,pt);
401 
402  // Assign point to current spline
403  for(int i = 0; i < 3; i++)
404  {
405  ptstack[i][curspline].push_back(pt[i]);
406  }
407 
408  list.visit(curnode);
409 
410  // Find all children, add to stack
411  children = list.findAllNext(curnode);
412  if(children.size()!=1 && !stack.empty() )
413  {
414  curspline++;
415  int parentspline;
416  int parentidx;
417  parentspline = parents.back().first;
418  parentidx = parents.back().second;
419 
420  // Leaf node, we're gonna need a new spline now
421  for(int i = 0; i < 3; i++)
422  {
423  ptstack[i].push_back(vector<T>());
424 
425 
426  // Add old points to the spline. This is needed so that the branches remain continuous
427  // ptstack[i][curspline].push_back(ptstack[i][parentspline][parentidx-1]);
428  if(parentidx != -1)
429  ptstack[i][curspline].push_back(ptstack[i][parentspline][parentidx]);
430 
431  }
432 
433  }
434  stack.insert(stack.end(), children.begin(), children.end());
435  // Push the parents of all the children we just pushed to the node stack
436  // to the parent stack
437  for(unsigned int i = 0; i < children.size(); i++)
438  {
439  parents.push_back(pair<int,int>(curspline,ptstack[0][curspline].size()-1));
440  }
441  }
442 
443  // Put all the generated data into the return spline3d structure
444  for(unsigned int i = 0; i < ptstack[0].size(); i++)
445  {
446  if(ptstack[0][i].size() > 1) {
447  Spline3D<T> newspline = Spline3D<T>(ptstack[0][i].size());
448  newspline.setPoints(ptstack[0][i], ptstack[1][i], ptstack[2][i]);
449  ret->push_back(newspline); }
450  }
451 
452  return ret;
453  }
454 
468  {
469  Plane3D plane(img->getTransform());
470  T t = 0.0;
471  T pt[3];
473  if(intersect(t,plane,pt))
474  {
475  intersection.setSpline(this);
476  intersection.setParameterPosition(t);
477  intersection.setValid(true);
478  intersection.setMetaImage(img);
479 
480  // Find cos(theta).
481  // cos(theta) = plane Y axis dot spline derivative normalized
482 
483  T spline_deriv[3];
484 
485  derivativeSingle(t,spline_deriv);
486  T y_axis[3];
487 
488  if(m_transform)
489  {
490  for(int i = 0; i < 3; i++)
491  {
492  y_axis[i] = -img->getTransform()(i,m_axis);
493  }
494  } else {
495  for(int i = 0; i < 3; i++)
496  {
497  y_axis[i] = -img->getTransform()(m_axis,i);
498  }
499  }
500  double length_y = length3d(y_axis);
501  double length_splinederiv = length3d(spline_deriv);
502 
503  T cosTheta = innerProduct(spline_deriv, y_axis)/(length_y*length_splinederiv);
504  intersection.setCosTheta(cosTheta);
505  }
506  return intersection;
507  }
508 
515  void
517  {
519 
520 
521  for(unsigned int i = 0; i < imgs.size(); i++)
522  {
523  intersection = findIntersection(&imgs[i]);
524  if(intersection.isValid())
525  {
526  m_intersections.push_back(intersection);
527 
528  }
529  }
530  }
531 
537  {
538  return m_intersections;
539  }
540 
546  {
547  return m_intersections;
548  }
549 
550 
551 
552 protected:
553 
562  int findRoots(int p, Plane3D &plane, T roots[2]) const
563  {
564  assert(m_initialized == true);
565 
566  // p0_idx is an index into the interpolation point array,
567  // we need to use control points to find the roots.
568  p++;
569  T a = 0.0;
570  T b = 0.0;
571  T c = plane.getCoefficient(3);;
572 
573  // Use the equation xS_x(t) + yS_y(t) + zS_z(t) - d = 0 to build
574  // a 2nd order equation (we are using quadratic splines, so this exists)
575  // See equation 3.29, 3.30 and 3.31 in the report.
576  // We need to build the equation on the form
577  // at^2 + bt + c = 0.
578 
579  for(int dim = 0; dim < 3; dim++)
580  {
581  T a_tmp = m_cpoints[dim][p-1]*0.5
582  - m_cpoints[dim][p]
583  + m_cpoints[dim][p+1]*0.5;
584  a_tmp *= plane.getCoefficient(dim);
585 
586  a += a_tmp;
587 
588  T b_tmp = - m_cpoints[dim][p-1]
589  + m_cpoints[dim][p];
590  b_tmp *= plane.getCoefficient(dim);
591 
592  b += b_tmp;
593 
594  T c_tmp = m_cpoints[dim][p-1]*0.5
595  + m_cpoints[dim][p]*0.5;
596  c_tmp *= plane.getCoefficient(dim);
597 
598  c += c_tmp;
599  }
600  // Now we have a, b and c, and we can solve it using the standard formula for solving 2nd order equations
601  // (-b +- sqrt(b^2-4ac))/2a
602 
603  T d = b*b - 4*a*c;
604  // Roots are complex, we are not interested in those..
605  // Actually this shouldn't happen
606  if(d >= -0.01 && d < 0.0) d = 0.0;
607 
608  if(d < 0) return 0;
609 
610  if(d == 0)
611  {
612  roots[0] = -b/(2.0*a);
613  // The root can only be a number between 0 and 1
614  if(roots[0] >= 0.0 && roots[0] <= 1.0)
615  return 1;
616  else
617  return 0;
618  }
619  else {
620  T r1, r2;
621  r1 = (-b + sqrt(d))/(2.0*a);
622  r2 = (-b - sqrt(d))/(2.0*a);
623  int nroots = 0;
624  if(r1 >= 0.0 && r1 <= 1.0)
625  {
626  roots[nroots] = r1;
627  nroots++;
628  }
629  if(r2 >= 0.0 && r2 <= 1.0)
630  {
631  roots[nroots] = r2;
632  nroots++;
633  }
634  return nroots;
635  }
636 
637 
638  }
640  std::vector<T> m_points[3];
642  std::vector<T> m_cpoints[3];
643 
648 
652  int m_axis;
653 };
654 
655 
656 #endif
int m_axis
Which axis to use from plane matrix.
Definition: spline3d.hpp:652
Scalar * end()
const IntersectionSet< T > & getConstIntersections() const
Definition: spline3d.hpp:545
void setTransform(bool b)
Definition: spline3d.hpp:79
int getAxis() const
Definition: spline3d.hpp:109
Spline3D(size_t npoints)
Definition: spline3d.hpp:36
void setSpline(const Spline3D< T > *spline)
std::vector< T > compute_control_points() const
bool m_initialized
True if m_cpoints is valid.
Definition: spline3d.hpp:647
int sgn(T val)
Definition: helpers.hpp:9
bool m_transform
Whether to transform when finding vector from plane matrix.
Definition: spline3d.hpp:650
T length3d(T const a[3])
Definition: helpers.hpp:19
void evaluateSingle(T t, T point[3]) const
Definition: spline3d.hpp:293
int findRoots(int p, Plane3D &plane, T roots[2]) const
Definition: spline3d.hpp:562
void compute()
Definition: spline3d.hpp:169
static vector< Spline3D< T > > * build(vtkSmartPointer< vtkPolyData > data)
Definition: spline3d.hpp:323
int length() const
Definition: spline3d.hpp:187
void findAllIntersections(const vector< MetaImage< inData_t > > &imgs)
Definition: spline3d.hpp:516
void setMetaImage(const MetaImage< inData_t > *img)
void derivativeSingle(T t, T point[3]) const
Definition: spline3d.hpp:272
bool isValid() const
~Spline3D()
Definition: spline3d.hpp:56
void setPoints(vector< T > const &x, vector< T > const &y, vector< T > const &z)
Definition: spline3d.hpp:127
T innerProduct(T const one[3], T const two[3])
Definition: helpers.hpp:48
void setValid(bool valid)
IntersectionSet< T > & getIntersections()
Definition: spline3d.hpp:536
int sign(double x)
Intersection< T > findIntersection(const MetaImage< inData_t > *img) const
Definition: spline3d.hpp:467
void setPoint(int idx, T const pt[3])
Definition: spline3d.hpp:64
bool getTransform() const
Definition: spline3d.hpp:89
void applyConvolution(vector< T > const &mask)
Definition: spline3d.hpp:141
bool intersect(T &t, Plane3D &plane, T pointOnPlane[3]) const
Definition: spline3d.hpp:206
void reportError(std::string errMsg)
double getCoefficient(const int i)
Definition: plane3d.hpp:89
void setAxis(int axis)
Definition: spline3d.hpp:99
IntersectionSet< T > m_intersections
The intersections (as filled by findAllIntersections)
Definition: spline3d.hpp:645
DoubleBoundingBox3D intersection(DoubleBoundingBox3D a, DoubleBoundingBox3D b)
Matrix4 getTransform()
Definition: metaimage.hpp:147
double getDistance(double const pt[3]) const
Definition: plane3d.hpp:98
void setParameterPosition(T t)
size_t getLength() const
Definition: spline3d.hpp:118
void setCosTheta(T t)