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