Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
itkShortestPathImageFilter.txx
Go to the documentation of this file.
1 /*===================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
16 #ifndef __itkShortestPathImageFilter_txx
17 #define __itkShortestPathImageFilter_txx
18 
19 #include "itkShortestPathImageFilter.h"
20 
21 #include "mitkMemoryUtilities.h"
22 #include "time.h"
23 #include <algorithm>
24 #include <iostream>
25 #include <vector>
26 
27 namespace itk
28 {
29  // Constructor (initialize standard values)
30  template <class TInputImageType, class TOutputImageType>
31  ShortestPathImageFilter<TInputImageType, TOutputImageType>::ShortestPathImageFilter()
32  : m_Nodes(nullptr),
33  m_Graph_NumberOfNodes(0),
34  m_FullNeighborsMode(false),
35  m_MakeOutputImage(true),
36  m_StoreVectorOrder(false),
37  m_CalcAllDistances(false),
38  multipleEndPoints(false),
39  m_ActivateTimeOut(false),
40  m_Initialized(false)
41  {
42  m_endPoints.clear();
43  m_endPointsClosed.clear();
44 
45  if (m_MakeOutputImage)
46  {
47  this->SetNumberOfRequiredOutputs(1);
48  this->SetNthOutput(0, OutputImageType::New());
49  }
50  }
51 
52  template <class TInputImageType, class TOutputImageType>
53  ShortestPathImageFilter<TInputImageType, TOutputImageType>::~ShortestPathImageFilter()
54  {
55  delete[] m_Nodes;
56  }
57 
58  template <class TInputImageType, class TOutputImageType>
59  inline typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType
60  ShortestPathImageFilter<TInputImageType, TOutputImageType>::NodeToCoord(NodeNumType node)
61  {
62  const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
63  int dim = InputImageType::ImageDimension;
64  IndexType coord;
65  if (dim == 2)
66  {
67  coord[1] = node / size[0];
68  coord[0] = node % size[0];
69  if (((unsigned long)coord[0] >= size[0]) || ((unsigned long)coord[1] >= size[1]))
70  {
71  coord[0] = 0;
72  coord[1] = 0;
73  }
74  }
75  if (dim == 3)
76  {
77  coord[2] = node / (size[0] * size[1]);
78  coord[1] = (node % (size[0] * size[1])) / size[0];
79  coord[0] = (node % (size[0] * size[1])) % size[0];
80  if (((unsigned long)coord[0] >= size[0]) || ((unsigned long)coord[1] >= size[1]) ||
81  ((unsigned long)coord[2] >= size[2]))
82  {
83  coord[0] = 0;
84  coord[1] = 0;
85  coord[2] = 0;
86  }
87  }
88 
89  return coord;
90  }
91 
92  template <class TInputImageType, class TOutputImageType>
93  inline typename itk::NodeNumType ShortestPathImageFilter<TInputImageType, TOutputImageType>::CoordToNode(
94  IndexType coord)
95  {
96  const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
97  int dim = InputImageType::ImageDimension;
98  NodeNumType node = 0;
99  if (dim == 2)
100  {
101  node = (coord[1] * size[0]) + coord[0];
102  }
103  if (dim == 3)
104  {
105  node = (coord[2] * size[0] * size[1]) + (coord[1] * size[0]) + coord[0];
106  }
107  if ((m_Graph_NumberOfNodes > 0) && (node >= m_Graph_NumberOfNodes))
108  {
109  /*MITK_INFO << "WARNING! Coordinates outside image!";
110  MITK_INFO << "Coords = " << coord ;
111  MITK_INFO << "ImageDim = " << dim ;
112  MITK_INFO << "RequestedRegionSize = " << size ;*/
113  node = 0;
114  }
115 
116  return node;
117  }
118 
119  template <class TInputImageType, class TOutputImageType>
120  inline bool ShortestPathImageFilter<TInputImageType, TOutputImageType>::CoordIsInBounds(IndexType coord)
121  {
122  const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
123  int dim = InputImageType::ImageDimension;
124 
125  if (dim == 2)
126  {
127  if ((coord[0] >= 0) && ((unsigned long)coord[0] < size[0]) && (coord[1] >= 0) &&
128  ((unsigned long)coord[1] < size[1]))
129  {
130  return true;
131  }
132  }
133  if (dim == 3)
134  {
135  if ((coord[0] >= 0) && ((unsigned long)coord[0] < size[0]) && (coord[1] >= 0) &&
136  ((unsigned long)coord[1] < size[1]) && (coord[2] >= 0) && ((unsigned long)coord[2] < size[2]))
137  {
138  return true;
139  }
140  }
141  return false;
142  }
143 
144  template <class TInputImageType, class TOutputImageType>
145  inline std::vector<ShortestPathNode *> ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetNeighbors(
146  unsigned int nodeNum, bool FullNeighbors)
147  {
148  // returns a vector of nodepointers.. these nodes are the neighbors
149  int dim = InputImageType::ImageDimension;
150  IndexType Coord = NodeToCoord(nodeNum);
151  IndexType NeighborCoord;
152  std::vector<ShortestPathNode *> nodeList;
153 
154  int neighborDistance = 1; // if i increase that, i might not hit the endnote
155 
156  // maybe use itkNeighborhoodIterator here, might be faster
157 
158  if (dim == 2)
159  {
160  // N4
161  NeighborCoord[0] = Coord[0];
162  NeighborCoord[1] = Coord[1] - neighborDistance;
163  if (CoordIsInBounds(NeighborCoord))
164  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
165 
166  NeighborCoord[0] = Coord[0] + neighborDistance;
167  NeighborCoord[1] = Coord[1];
168  if (CoordIsInBounds(NeighborCoord))
169  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
170 
171  NeighborCoord[0] = Coord[0];
172  NeighborCoord[1] = Coord[1] + neighborDistance;
173  if (CoordIsInBounds(NeighborCoord))
174  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
175 
176  NeighborCoord[0] = Coord[0] - neighborDistance;
177  NeighborCoord[1] = Coord[1];
178  if (CoordIsInBounds(NeighborCoord))
179  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
180 
181  if (FullNeighbors)
182  {
183  // N8
184  NeighborCoord[0] = Coord[0] - neighborDistance;
185  NeighborCoord[1] = Coord[1] - neighborDistance;
186  if (CoordIsInBounds(NeighborCoord))
187  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
188 
189  NeighborCoord[0] = Coord[0] + neighborDistance;
190  NeighborCoord[1] = Coord[1] - neighborDistance;
191  if (CoordIsInBounds(NeighborCoord))
192  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
193 
194  NeighborCoord[0] = Coord[0] - neighborDistance;
195  NeighborCoord[1] = Coord[1] + neighborDistance;
196  if (CoordIsInBounds(NeighborCoord))
197  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
198 
199  NeighborCoord[0] = Coord[0] + neighborDistance;
200  NeighborCoord[1] = Coord[1] + neighborDistance;
201  if (CoordIsInBounds(NeighborCoord))
202  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
203  }
204  }
205  if (dim == 3)
206  {
207  // N6
208  NeighborCoord[0] = Coord[0];
209  NeighborCoord[1] = Coord[1] - neighborDistance;
210  NeighborCoord[2] = Coord[2];
211  if (CoordIsInBounds(NeighborCoord))
212  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
213 
214  NeighborCoord[0] = Coord[0] + neighborDistance;
215  NeighborCoord[1] = Coord[1];
216  NeighborCoord[2] = Coord[2];
217  if (CoordIsInBounds(NeighborCoord))
218  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
219 
220  NeighborCoord[0] = Coord[0];
221  NeighborCoord[1] = Coord[1] + neighborDistance;
222  NeighborCoord[2] = Coord[2];
223  if (CoordIsInBounds(NeighborCoord))
224  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
225 
226  NeighborCoord[0] = Coord[0] - neighborDistance;
227  NeighborCoord[1] = Coord[1];
228  NeighborCoord[2] = Coord[2];
229  if (CoordIsInBounds(NeighborCoord))
230  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
231 
232  NeighborCoord[0] = Coord[0];
233  NeighborCoord[1] = Coord[1];
234  NeighborCoord[2] = Coord[2] + neighborDistance;
235  if (CoordIsInBounds(NeighborCoord))
236  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
237 
238  NeighborCoord[0] = Coord[0];
239  NeighborCoord[1] = Coord[1];
240  NeighborCoord[2] = Coord[2] - neighborDistance;
241  if (CoordIsInBounds(NeighborCoord))
242  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
243 
244  if (FullNeighbors)
245  {
246  // N26
247  // Middle Slice
248  NeighborCoord[0] = Coord[0] - neighborDistance;
249  NeighborCoord[1] = Coord[1] - neighborDistance;
250  NeighborCoord[2] = Coord[2];
251  if (CoordIsInBounds(NeighborCoord))
252  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
253 
254  NeighborCoord[0] = Coord[0] + neighborDistance;
255  NeighborCoord[1] = Coord[1] - neighborDistance;
256  NeighborCoord[2] = Coord[2];
257  if (CoordIsInBounds(NeighborCoord))
258  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
259 
260  NeighborCoord[0] = Coord[0] - neighborDistance;
261  NeighborCoord[1] = Coord[1] + neighborDistance;
262  NeighborCoord[2] = Coord[2];
263  if (CoordIsInBounds(NeighborCoord))
264  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
265 
266  NeighborCoord[0] = Coord[0] + neighborDistance;
267  NeighborCoord[1] = Coord[1] + neighborDistance;
268  NeighborCoord[2] = Coord[2];
269  if (CoordIsInBounds(NeighborCoord))
270  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
271 
272  // BackSlice (Diagonal)
273  NeighborCoord[0] = Coord[0] - neighborDistance;
274  NeighborCoord[1] = Coord[1] - neighborDistance;
275  NeighborCoord[2] = Coord[2] - neighborDistance;
276  if (CoordIsInBounds(NeighborCoord))
277  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
278 
279  NeighborCoord[0] = Coord[0] + neighborDistance;
280  NeighborCoord[1] = Coord[1] - neighborDistance;
281  NeighborCoord[2] = Coord[2] - neighborDistance;
282  if (CoordIsInBounds(NeighborCoord))
283  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
284 
285  NeighborCoord[0] = Coord[0] - neighborDistance;
286  NeighborCoord[1] = Coord[1] + neighborDistance;
287  NeighborCoord[2] = Coord[2] - neighborDistance;
288  if (CoordIsInBounds(NeighborCoord))
289  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
290 
291  NeighborCoord[0] = Coord[0] + neighborDistance;
292  NeighborCoord[1] = Coord[1] + neighborDistance;
293  NeighborCoord[2] = Coord[2] - neighborDistance;
294  if (CoordIsInBounds(NeighborCoord))
295  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
296 
297  // BackSlice (Non-Diag)
298  NeighborCoord[0] = Coord[0];
299  NeighborCoord[1] = Coord[1] - neighborDistance;
300  NeighborCoord[2] = Coord[2] - neighborDistance;
301  if (CoordIsInBounds(NeighborCoord))
302  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
303 
304  NeighborCoord[0] = Coord[0] + neighborDistance;
305  NeighborCoord[1] = Coord[1];
306  NeighborCoord[2] = Coord[2] - neighborDistance;
307  if (CoordIsInBounds(NeighborCoord))
308  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
309 
310  NeighborCoord[0] = Coord[0];
311  NeighborCoord[1] = Coord[1] + neighborDistance;
312  NeighborCoord[2] = Coord[2] - neighborDistance;
313  if (CoordIsInBounds(NeighborCoord))
314  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
315 
316  NeighborCoord[0] = Coord[0] - neighborDistance;
317  NeighborCoord[1] = Coord[1];
318  NeighborCoord[2] = Coord[2] - neighborDistance;
319  if (CoordIsInBounds(NeighborCoord))
320  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
321 
322  // FrontSlice (Diagonal)
323  NeighborCoord[0] = Coord[0] - neighborDistance;
324  NeighborCoord[1] = Coord[1] - neighborDistance;
325  NeighborCoord[2] = Coord[2] + neighborDistance;
326  if (CoordIsInBounds(NeighborCoord))
327  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
328 
329  NeighborCoord[0] = Coord[0] + neighborDistance;
330  NeighborCoord[1] = Coord[1] - neighborDistance;
331  NeighborCoord[2] = Coord[2] + neighborDistance;
332  if (CoordIsInBounds(NeighborCoord))
333  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
334 
335  NeighborCoord[0] = Coord[0] - neighborDistance;
336  NeighborCoord[1] = Coord[1] + neighborDistance;
337  NeighborCoord[2] = Coord[2] + neighborDistance;
338  if (CoordIsInBounds(NeighborCoord))
339  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
340 
341  NeighborCoord[0] = Coord[0] + neighborDistance;
342  NeighborCoord[1] = Coord[1] + neighborDistance;
343  NeighborCoord[2] = Coord[2] + neighborDistance;
344  if (CoordIsInBounds(NeighborCoord))
345  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
346 
347  // FrontSlice(Non-Diag)
348  NeighborCoord[0] = Coord[0];
349  NeighborCoord[1] = Coord[1] - neighborDistance;
350  NeighborCoord[2] = Coord[2] + neighborDistance;
351  if (CoordIsInBounds(NeighborCoord))
352  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
353 
354  NeighborCoord[0] = Coord[0] + neighborDistance;
355  NeighborCoord[1] = Coord[1];
356  NeighborCoord[2] = Coord[2] + neighborDistance;
357  if (CoordIsInBounds(NeighborCoord))
358  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
359 
360  NeighborCoord[0] = Coord[0];
361  NeighborCoord[1] = Coord[1] + neighborDistance;
362  NeighborCoord[2] = Coord[2] + neighborDistance;
363  if (CoordIsInBounds(NeighborCoord))
364  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
365 
366  NeighborCoord[0] = Coord[0] - neighborDistance;
367  NeighborCoord[1] = Coord[1];
368  NeighborCoord[2] = Coord[2] + neighborDistance;
369  if (CoordIsInBounds(NeighborCoord))
370  nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
371  }
372  }
373  return nodeList;
374  }
375 
376  template <class TInputImageType, class TOutputImageType>
377  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::SetStartIndex(
378  const typename TInputImageType::IndexType &StartIndex)
379  {
380  for (unsigned int i = 0; i < TInputImageType::ImageDimension; ++i)
381  {
382  m_StartIndex[i] = StartIndex[i];
383  }
384  m_Graph_StartNode = CoordToNode(m_StartIndex);
385  // MITK_INFO << "StartIndex = " << StartIndex;
386  // MITK_INFO << "StartNode = " << m_Graph_StartNode;
387  m_Initialized = false;
388  }
389 
390  template <class TInputImageType, class TOutputImageType>
391  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::SetEndIndex(
392  const typename TInputImageType::IndexType &EndIndex)
393  {
394  for (unsigned int i = 0; i < TInputImageType::ImageDimension; ++i)
395  {
396  m_EndIndex[i] = EndIndex[i];
397  }
398  m_Graph_EndNode = CoordToNode(m_EndIndex);
399  // MITK_INFO << "EndNode = " << m_Graph_EndNode;
400  }
401 
402  template <class TInputImageType, class TOutputImageType>
403  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::AddEndIndex(
404  const typename TInputImageType::IndexType &index)
405  {
406  // ONLY FOR MULTIPLE END POINTS SEARCH
407  IndexType newEndIndex;
408  for (unsigned int i = 0; i < TInputImageType::ImageDimension; ++i)
409  {
410  newEndIndex[i] = index[i];
411  }
412  m_endPoints.push_back(newEndIndex);
413  SetEndIndex(m_endPoints[0]);
414  multipleEndPoints = true;
415  }
416 
417  template <class TInputImageType, class TOutputImageType>
418  inline double ShortestPathImageFilter<TInputImageType, TOutputImageType>::getEstimatedCostsToTarget(
419  const typename TInputImageType::IndexType &a)
420  {
421  // Returns the minimal possible costs for a path from "a" to targetnode.
422  itk::Vector<float, 3> v;
423  v[0] = m_EndIndex[0] - a[0];
424  v[1] = m_EndIndex[1] - a[1];
425  v[2] = m_EndIndex[2] - a[2];
426 
427  return m_CostFunction->GetMinCost() * v.GetNorm();
428  }
429 
430  template <class TInputImageType, class TOutputImageType>
431  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::InitGraph()
432  {
433  if (!m_Initialized)
434  {
435  // Clean up previous stuff
436  CleanUp();
437 
438  // Calc Number of nodes
439  m_ImageDimensions = TInputImageType::ImageDimension;
440  const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
441  m_Graph_NumberOfNodes = 1;
442  for (NodeNumType i = 0; i < m_ImageDimensions; ++i)
443  m_Graph_NumberOfNodes = m_Graph_NumberOfNodes * size[i];
444 
445  // Initialize mainNodeList with that number
446  m_Nodes = new ShortestPathNode[m_Graph_NumberOfNodes];
447 
448  // Initialize each node in nodelist
449  for (NodeNumType i = 0; i < m_Graph_NumberOfNodes; i++)
450  {
451  m_Nodes[i].distAndEst = -1;
452  m_Nodes[i].distance = -1;
453  m_Nodes[i].prevNode = -1;
454  m_Nodes[i].mainListIndex = i;
455  m_Nodes[i].closed = false;
456  }
457 
458  m_Initialized = true;
459  }
460 
461  // In the beginning, the Startnode needs a distance of 0
462  m_Nodes[m_Graph_StartNode].distance = 0;
463  m_Nodes[m_Graph_StartNode].distAndEst = 0;
464 
465  // initalize cost function
466  m_CostFunction->Initialize();
467  }
468 
469  template <class TInputImageType, class TOutputImageType>
470  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::StartShortestPathSearch()
471  {
472  // Setup Timer
473  clock_t startAll = clock();
474  clock_t stopAll = clock();
475 
476  // init variables
477  double durationAll = 0;
478  bool timeout = false;
479  NodeNumType mainNodeListIndex = 0;
480  DistanceType curNodeDistance = 0;
481  NodeNumType numberOfNodesChecked = 0;
482 
483  // Create Multimap (tree structure for fast searching)
484  std::multimap<double, ShortestPathNode *> myMap;
485  std::pair<std::multimap<double, ShortestPathNode *>::iterator, std::multimap<double, ShortestPathNode *>::iterator>
486  ret;
487  std::multimap<double, ShortestPathNode *>::iterator it;
488 
489  // At first, only startNote is discovered.
490  myMap.insert(
491  std::pair<double, ShortestPathNode *>(m_Nodes[m_Graph_StartNode].distAndEst, &m_Nodes[m_Graph_StartNode]));
492 
493  // While there are discovered Nodes, pick the one with lowest distance,
494  // update its neighbors and eventually delete it from the discovered Nodes list.
495  while (!myMap.empty())
496  {
497  numberOfNodesChecked++;
498  /*if ( (numberOfNodesChecked % (m_Graph_NumberOfNodes/100)) == 0)
499  {
500  MITK_INFO << "Checked " << ( numberOfNodesChecked / (m_Graph_NumberOfNodes/100) ) << "% List: " << myMap.size()
501  << "\n";
502  }*/
503 
504  // Get element with lowest score
505  mainNodeListIndex = myMap.begin()->second->mainListIndex;
506  curNodeDistance = myMap.begin()->second->distance;
507  myMap.begin()->second->closed = true; // close it
508 
509  // Debug:
510  // MITK_INFO << "INFO: size " << myMap.size();
511  /*
512  for (it = myMap.begin(); it != myMap.end(); ++it)
513  {
514  MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
515  }
516  */
517 
518  // Kicks out element with lowest score
519  myMap.erase(myMap.begin());
520 
521  // if wanted, store vector order
522  if (m_StoreVectorOrder)
523  {
524  m_VectorOrder.push_back(mainNodeListIndex);
525  }
526 
527  // Check neighbors
528  std::vector<ShortestPathNode *> neighborNodes = GetNeighbors(mainNodeListIndex, m_Graph_fullNeighbors);
529  for (NodeNumType i = 0; i < neighborNodes.size(); i++)
530  {
531  if (neighborNodes[i]->closed)
532  continue; // this nodes is already closed, go to next neighbor
533 
534  IndexType coordCurNode = NodeToCoord(mainNodeListIndex);
535  IndexType coordNeighborNode = NodeToCoord(neighborNodes[i]->mainListIndex);
536 
537  // calculate the new Distance to the current neighbor
538  double newDistance = curNodeDistance + (m_CostFunction->GetCost(coordCurNode, coordNeighborNode));
539 
540  // if it is shorter than any yet known path to this neighbor, than the current path is better. Save that!
541  if ((newDistance < neighborNodes[i]->distance) || (neighborNodes[i]->distance == -1))
542  {
543  // if that neighbornode is not in discoverednodeList yet, Push it there and update
544  if (neighborNodes[i]->distance == -1)
545  {
546  neighborNodes[i]->distance = newDistance;
547  neighborNodes[i]->distAndEst = newDistance + getEstimatedCostsToTarget(coordNeighborNode);
548  neighborNodes[i]->prevNode = mainNodeListIndex;
549  myMap.insert(std::pair<double, ShortestPathNode *>(m_Nodes[neighborNodes[i]->mainListIndex].distAndEst,
550  &m_Nodes[neighborNodes[i]->mainListIndex]));
551  /*
552  MITK_INFO << "Inserted: " << m_Nodes[neighborNodes[i]->mainListIndex].distAndEst << "|" <<
553  m_Nodes[neighborNodes[i]->mainListIndex].mainListIndex;
554  MITK_INFO << "INFO: size " << myMap.size();
555  for (it = myMap.begin(); it != myMap.end(); ++it)
556  {
557  MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
558  }
559  */
560  }
561  // or if is already in discoverednodelist, update
562  else
563  {
564  /*
565  it = myMap.find(neighborNodes[i]->distAndEst);
566  if (it == myMap.end() )
567  {
568  MITK_INFO << "Nothing!";
569  // look further
570  for (it = myMap.begin(); it != myMap.end(); ++it)
571  {
572  if ((*it).second->mainListIndex == lookForId)
573  {
574  MITK_INFO << "But it is there!!!";
575  MITK_INFO << "Searched for: " << lookFor << " but had: " << (*it).second->distAndEst;
576  }
577 
578  }
579  }
580  */
581 
582  // 1st : find and delete old element
583  bool found = false;
584  ret = myMap.equal_range(neighborNodes[i]->distAndEst);
585 
586  if ((ret.first == ret.second))
587  {
588  /*+++++++++++++ no exact match +++++++++++++*/
589  // MITK_INFO << "No exact match!"; // if this happens, you are screwed
590  /*
591  MITK_INFO << "Was looking for: " << lookFor << " ID: " << lookForId;
592  if (ret.first != myMap.end() )
593  {
594  it = ret.first;
595  MITK_INFO << "Found: " << it->first << " ID: " << it->second->mainListIndex;
596  ++it;
597  MITK_INFO << "Found: " << it->first << " ID: " << it->second->mainListIndex;
598  --it;
599  --it;
600  MITK_INFO << "Found: " << it->first << " ID: " << it->second->mainListIndex;
601  }
602 
603  // look if that ID is found in the map
604  for (it = myMap.begin(); it != myMap.end(); ++it)
605  {
606  if ((*it).second->mainListIndex == lookForId)
607  {
608  MITK_INFO << "But it is there!!!";
609  MITK_INFO << "Searched dist: " << lookFor << " found dist: " << (*it).second->distAndEst;
610  }
611 
612  }
613  */
614  }
615  else
616  {
617  for (it = ret.first; it != ret.second; ++it)
618  {
619  if (it->second->mainListIndex == neighborNodes[i]->mainListIndex)
620  {
621  found = true;
622  myMap.erase(it);
623  /*
624  MITK_INFO << "INFO: size " << myMap.size();
625  MITK_INFO << "Erase: " << it->first << "|" << it->second->mainListIndex;
626 
627  MITK_INFO << "INFO: size " << myMap.size();
628  for (it = myMap.begin(); it != myMap.end(); ++it)
629  {
630  MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
631  }
632  */
633  break;
634  }
635  }
636  }
637 
638  if (!found)
639  {
640  // MITK_INFO << "Could not find it! :(";
641  continue;
642  }
643 
644  // 2nd: update and insert new element
645  neighborNodes[i]->distance = newDistance;
646  neighborNodes[i]->distAndEst = newDistance + getEstimatedCostsToTarget(coordNeighborNode);
647  neighborNodes[i]->prevNode = mainNodeListIndex;
648  // myMap.insert( std::pair<double,ShortestPathNode*> (neighborNodes[i]->distAndEst, neighborNodes[i]));
649  myMap.insert(std::pair<double, ShortestPathNode *>(m_Nodes[neighborNodes[i]->mainListIndex].distAndEst,
650  &m_Nodes[neighborNodes[i]->mainListIndex]));
651 
652  // MITK_INFO << "Re-Inserted: " << m_Nodes[neighborNodes[i]->mainListIndex].distAndEst << "|" <<
653  // m_Nodes[neighborNodes[i]->mainListIndex].mainListIndex;
654  // MITK_INFO << "INFO: size " << myMap.size();
655  /*for (it = myMap.begin(); it != myMap.end(); ++it)
656  {
657  MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
658  }*/
659  }
660  }
661  }
662  // finished with checking all neighbors.
663 
664  // Check Timeout, if activated
665  if (m_ActivateTimeOut)
666  {
667  stopAll = clock();
668  durationAll = (double)(stopAll - startAll) / CLOCKS_PER_SEC;
669  if (durationAll >= 30)
670  {
671  // MITK_INFO << "TIMEOUT!! Search took over 30 seconds";
672  timeout = true;
673  }
674  }
675 
676  // Check end criteria:
677  // For multiple points
678  if (multipleEndPoints)
679  {
680  // super slow, make it faster
681  for (unsigned int i = 0; i < m_endPoints.size(); i++)
682  {
683  if (CoordToNode(m_endPoints[i]) == mainNodeListIndex)
684  {
685  m_endPointsClosed.push_back(NodeToCoord(mainNodeListIndex));
686  m_endPoints.erase(m_endPoints.begin() + i);
687  if (m_endPoints.empty())
688  {
689  // Finished! break
690  return;
691  }
692  if (m_Graph_EndNode == mainNodeListIndex)
693  {
694  // set new end
695  SetEndIndex(m_endPoints[0]);
696  }
697  }
698  }
699  }
700  // if single end point, then end, if this one is reached or timeout happened.
701  else if ((mainNodeListIndex == m_Graph_EndNode || timeout) && !m_CalcAllDistances)
702  {
703  /*if (m_StoreVectorOrder)
704  MITK_INFO << "Number of Nodes checked: " << m_VectorOrder.size() ;*/
705  return;
706  }
707  }
708  }
709 
710  template <class TInputImageType, class TOutputImageType>
711  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::MakeOutputs()
712  {
713  // MITK_INFO << "Make Output";
714 
715  if (m_MakeOutputImage)
716  {
717  OutputImagePointer output0 = this->GetOutput(0);
718  output0->SetRegions(this->GetInput()->GetLargestPossibleRegion());
719  output0->Allocate();
720  OutputImageIteratorType shortestPathImageIt(output0, output0->GetRequestedRegion());
721  // Create ShortestPathImage (Output 0)
722  for (shortestPathImageIt.GoToBegin(); !shortestPathImageIt.IsAtEnd(); ++shortestPathImageIt)
723  {
724  // First intialize with background color
725  shortestPathImageIt.Set(BACKGROUND);
726  }
727 
728  if (!multipleEndPoints) // Only one path was calculated
729  {
730  for (unsigned int i = 0; i < m_VectorPath.size(); i++)
731  {
732  shortestPathImageIt.SetIndex(m_VectorPath[i]);
733  shortestPathImageIt.Set(FOREGROUND);
734  }
735  }
736  else // multiple pathes has been calculated, draw all
737  {
738  for (unsigned int i = 0; i < m_MultipleVectorPaths.size(); i++)
739  {
740  for (unsigned int j = 0; j < m_MultipleVectorPaths[i].size(); j++)
741  {
742  shortestPathImageIt.SetIndex(m_MultipleVectorPaths[i][j]);
743  shortestPathImageIt.Set(FOREGROUND);
744  }
745  }
746  }
747  }
748  }
749 
750  template <class TInputImageType, class TOutputImageType>
751  typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::OutputImagePointer
752  ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetVectorOrderImage()
753  {
754  // Create Vector Order Image
755  // Return it
756 
757  OutputImagePointer image = OutputImageType::New();
758  image->SetRegions(this->GetInput()->GetLargestPossibleRegion());
759  image->Allocate();
760  OutputImageIteratorType vectorOrderImageIt(image, image->GetRequestedRegion());
761 
762  // MITK_INFO << "GetVectorOrderImage";
763  for (vectorOrderImageIt.GoToBegin(); !vectorOrderImageIt.IsAtEnd(); ++vectorOrderImageIt)
764  {
765  // First intialize with background color
766  vectorOrderImageIt.Value() = BACKGROUND;
767  }
768  for (int i = 0; i < m_VectorOrder.size(); i++)
769  {
770  vectorOrderImageIt.SetIndex(NodeToCoord(m_VectorOrder[i]));
771  vectorOrderImageIt.Set(BACKGROUND + i);
772  }
773  return image;
774  }
775 
776  template <class TInputImageType, class TOutputImageType>
777  typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::OutputImagePointer
778  ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetDistanceImage()
779  {
780  // Create Distance Image
781  // Return it
782 
783  OutputImagePointer image = OutputImageType::New();
784  image->SetRegions(this->GetInput()->GetLargestPossibleRegion());
785  image->Allocate();
786  ;
787  OutputImageIteratorType distanceImageIt(image, image->GetRequestedRegion());
788  // Create Distance Image (Output 1)
789  NodeNumType myNodeNum;
790  for (distanceImageIt.GoToBegin(); !distanceImageIt.IsAtEnd(); ++distanceImageIt)
791  {
792  IndexType index = distanceImageIt.GetIndex();
793  myNodeNum = CoordToNode(index);
794  double newVal = m_Nodes[myNodeNum].distance;
795  distanceImageIt.Set(newVal);
796  }
797  }
798 
799  template <class TInputImageType, class TOutputImageType>
800  std::vector<typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType>
801  ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetVectorPath()
802  {
803  return m_VectorPath;
804  }
805 
806  template <class TInputImageType, class TOutputImageType>
807  std::vector<std::vector<typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType>>
808  ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetMultipleVectorPaths()
809  {
810  return m_MultipleVectorPaths;
811  }
812 
813  template <class TInputImageType, class TOutputImageType>
814  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::MakeShortestPathVector()
815  {
816  // MITK_INFO << "Make ShortestPath Vec";
817 
818  // single end point
819  if (!multipleEndPoints)
820  {
821  // fill m_VectorPath with the Shortest Path
822  m_VectorPath.clear();
823 
824  // Go backwards from endnote to startnode
825  NodeNumType prevNode = m_Graph_EndNode;
826  while (prevNode != m_Graph_StartNode)
827  {
828  m_VectorPath.push_back(NodeToCoord(prevNode));
829  prevNode = m_Nodes[prevNode].prevNode;
830  }
831  m_VectorPath.push_back(NodeToCoord(prevNode));
832  // reverse it
833  std::reverse(m_VectorPath.begin(), m_VectorPath.end());
834  }
835  // Multiple end end points and pathes
836  else
837  {
838  for (unsigned int i = 0; i < m_endPointsClosed.size(); i++)
839  {
840  m_VectorPath.clear();
841  // Go backwards from endnote to startnode
842  NodeNumType prevNode = CoordToNode(m_endPointsClosed[i]);
843  while (prevNode != m_Graph_StartNode)
844  {
845  m_VectorPath.push_back(NodeToCoord(prevNode));
846  prevNode = m_Nodes[prevNode].prevNode;
847  }
848  m_VectorPath.push_back(NodeToCoord(prevNode));
849 
850  // reverse it
851  std::reverse(m_VectorPath.begin(), m_VectorPath.end());
852  // push_back
853  m_MultipleVectorPaths.push_back(m_VectorPath);
854  }
855  }
856  }
857 
858  template <class TInputImageType, class TOutputImageType>
859  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::CleanUp()
860  {
861  m_VectorOrder.clear();
862  m_VectorPath.clear();
863  // TODO: if multiple Path, clear all multiple Paths
864 
865  if (m_Nodes)
866  delete[] m_Nodes;
867  }
868 
869  template <class TInputImageType, class TOutputImageType>
870  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::GenerateData()
871  {
872  // Build Graph
873  InitGraph();
874 
875  // Calc Shortest Parth
876  StartShortestPathSearch();
877 
878  // Fill Shortest Path
879  MakeShortestPathVector();
880 
881  // Make Outputs
882  MakeOutputs();
883  }
884 
885  template <class TInputImageType, class TOutputImageType>
886  void ShortestPathImageFilter<TInputImageType, TOutputImageType>::PrintSelf(std::ostream &os, Indent indent) const
887  {
888  Superclass::PrintSelf(os, indent);
889  }
890 
891 } /* end namespace itk */
892 
893 #endif // __itkShortestPathImageFilter_txx