Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
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