1 /*===================================================================
3 The Medical Imaging Interaction Toolkit (MITK)
5 Copyright (c) German Cancer Research Center,
6 Division of Medical and Biological Informatics.
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
13 See LICENSE.txt or http://www.mitk.org for details.
15 ===================================================================*/
16 #ifndef __itkShortestPathImageFilter_txx
17 #define __itkShortestPathImageFilter_txx
19 #include "itkShortestPathImageFilter.h"
21 #include "mitkMemoryUtilities.h"
29 // Constructor (initialize standard values)
30 template <class TInputImageType, class TOutputImageType>
31 ShortestPathImageFilter<TInputImageType, TOutputImageType>::ShortestPathImageFilter()
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),
43 m_endPointsClosed.clear();
45 if (m_MakeOutputImage)
47 this->SetNumberOfRequiredOutputs(1);
48 this->SetNthOutput(0, OutputImageType::New());
52 template <class TInputImageType, class TOutputImageType>
53 ShortestPathImageFilter<TInputImageType, TOutputImageType>::~ShortestPathImageFilter()
58 template <class TInputImageType, class TOutputImageType>
59 inline typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType
60 ShortestPathImageFilter<TInputImageType, TOutputImageType>::NodeToCoord(NodeNumType node)
62 const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
63 int dim = InputImageType::ImageDimension;
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]))
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]))
92 template <class TInputImageType, class TOutputImageType>
93 inline typename itk::NodeNumType ShortestPathImageFilter<TInputImageType, TOutputImageType>::CoordToNode(
96 const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
97 int dim = InputImageType::ImageDimension;
101 node = (coord[1] * size[0]) + coord[0];
105 node = (coord[2] * size[0] * size[1]) + (coord[1] * size[0]) + coord[0];
107 if ((m_Graph_NumberOfNodes > 0) && (node >= m_Graph_NumberOfNodes))
109 /*MITK_INFO << "WARNING! Coordinates outside image!";
110 MITK_INFO << "Coords = " << coord ;
111 MITK_INFO << "ImageDim = " << dim ;
112 MITK_INFO << "RequestedRegionSize = " << size ;*/
119 template <class TInputImageType, class TOutputImageType>
120 inline bool ShortestPathImageFilter<TInputImageType, TOutputImageType>::CoordIsInBounds(IndexType coord)
122 const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
123 int dim = InputImageType::ImageDimension;
127 if ((coord[0] >= 0) && ((unsigned long)coord[0] < size[0]) && (coord[1] >= 0) &&
128 ((unsigned long)coord[1] < size[1]))
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]))
144 template <class TInputImageType, class TOutputImageType>
145 inline std::vector<ShortestPathNode *> ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetNeighbors(
146 unsigned int nodeNum, bool FullNeighbors)
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;
154 int neighborDistance = 1; // if i increase that, i might not hit the endnote
156 // maybe use itkNeighborhoodIterator here, might be faster
161 NeighborCoord[0] = Coord[0];
162 NeighborCoord[1] = Coord[1] - neighborDistance;
163 if (CoordIsInBounds(NeighborCoord))
164 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
166 NeighborCoord[0] = Coord[0] + neighborDistance;
167 NeighborCoord[1] = Coord[1];
168 if (CoordIsInBounds(NeighborCoord))
169 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
171 NeighborCoord[0] = Coord[0];
172 NeighborCoord[1] = Coord[1] + neighborDistance;
173 if (CoordIsInBounds(NeighborCoord))
174 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
176 NeighborCoord[0] = Coord[0] - neighborDistance;
177 NeighborCoord[1] = Coord[1];
178 if (CoordIsInBounds(NeighborCoord))
179 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
184 NeighborCoord[0] = Coord[0] - neighborDistance;
185 NeighborCoord[1] = Coord[1] - neighborDistance;
186 if (CoordIsInBounds(NeighborCoord))
187 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
189 NeighborCoord[0] = Coord[0] + neighborDistance;
190 NeighborCoord[1] = Coord[1] - neighborDistance;
191 if (CoordIsInBounds(NeighborCoord))
192 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
194 NeighborCoord[0] = Coord[0] - neighborDistance;
195 NeighborCoord[1] = Coord[1] + neighborDistance;
196 if (CoordIsInBounds(NeighborCoord))
197 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
199 NeighborCoord[0] = Coord[0] + neighborDistance;
200 NeighborCoord[1] = Coord[1] + neighborDistance;
201 if (CoordIsInBounds(NeighborCoord))
202 nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
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)]);
376 template <class TInputImageType, class TOutputImageType>
377 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::SetStartIndex(
378 const typename TInputImageType::IndexType &StartIndex)
380 for (unsigned int i = 0; i < TInputImageType::ImageDimension; ++i)
382 m_StartIndex[i] = StartIndex[i];
384 m_Graph_StartNode = CoordToNode(m_StartIndex);
385 // MITK_INFO << "StartIndex = " << StartIndex;
386 // MITK_INFO << "StartNode = " << m_Graph_StartNode;
387 m_Initialized = false;
390 template <class TInputImageType, class TOutputImageType>
391 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::SetEndIndex(
392 const typename TInputImageType::IndexType &EndIndex)
394 for (unsigned int i = 0; i < TInputImageType::ImageDimension; ++i)
396 m_EndIndex[i] = EndIndex[i];
398 m_Graph_EndNode = CoordToNode(m_EndIndex);
399 // MITK_INFO << "EndNode = " << m_Graph_EndNode;
402 template <class TInputImageType, class TOutputImageType>
403 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::AddEndIndex(
404 const typename TInputImageType::IndexType &index)
406 // ONLY FOR MULTIPLE END POINTS SEARCH
407 IndexType newEndIndex;
408 for (unsigned int i = 0; i < TInputImageType::ImageDimension; ++i)
410 newEndIndex[i] = index[i];
412 m_endPoints.push_back(newEndIndex);
413 SetEndIndex(m_endPoints[0]);
414 multipleEndPoints = true;
417 template <class TInputImageType, class TOutputImageType>
418 inline double ShortestPathImageFilter<TInputImageType, TOutputImageType>::getEstimatedCostsToTarget(
419 const typename TInputImageType::IndexType &a)
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];
427 return m_CostFunction->GetMinCost() * v.GetNorm();
430 template <class TInputImageType, class TOutputImageType>
431 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::InitGraph()
435 // Clean up previous stuff
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];
445 // Initialize mainNodeList with that number
446 m_Nodes = new ShortestPathNode[m_Graph_NumberOfNodes];
448 // Initialize each node in nodelist
449 for (NodeNumType i = 0; i < m_Graph_NumberOfNodes; i++)
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;
458 m_Initialized = true;
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;
465 // initalize cost function
466 m_CostFunction->Initialize();
469 template <class TInputImageType, class TOutputImageType>
470 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::StartShortestPathSearch()
473 clock_t startAll = clock();
474 clock_t stopAll = clock();
477 double durationAll = 0;
478 bool timeout = false;
479 NodeNumType mainNodeListIndex = 0;
480 DistanceType curNodeDistance = 0;
481 NodeNumType numberOfNodesChecked = 0;
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>
487 std::multimap<double, ShortestPathNode *>::iterator it;
489 // At first, only startNote is discovered.
491 std::pair<double, ShortestPathNode *>(m_Nodes[m_Graph_StartNode].distAndEst, &m_Nodes[m_Graph_StartNode]));
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())
497 numberOfNodesChecked++;
498 /*if ( (numberOfNodesChecked % (m_Graph_NumberOfNodes/100)) == 0)
500 MITK_INFO << "Checked " << ( numberOfNodesChecked / (m_Graph_NumberOfNodes/100) ) << "% List: " << myMap.size()
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
510 // MITK_INFO << "INFO: size " << myMap.size();
512 for (it = myMap.begin(); it != myMap.end(); ++it)
514 MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
518 // Kicks out element with lowest score
519 myMap.erase(myMap.begin());
521 // if wanted, store vector order
522 if (m_StoreVectorOrder)
524 m_VectorOrder.push_back(mainNodeListIndex);
528 std::vector<ShortestPathNode *> neighborNodes = GetNeighbors(mainNodeListIndex, m_Graph_fullNeighbors);
529 for (NodeNumType i = 0; i < neighborNodes.size(); i++)
531 if (neighborNodes[i]->closed)
532 continue; // this nodes is already closed, go to next neighbor
534 IndexType coordCurNode = NodeToCoord(mainNodeListIndex);
535 IndexType coordNeighborNode = NodeToCoord(neighborNodes[i]->mainListIndex);
537 // calculate the new Distance to the current neighbor
538 double newDistance = curNodeDistance + (m_CostFunction->GetCost(coordCurNode, coordNeighborNode));
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))
543 // if that neighbornode is not in discoverednodeList yet, Push it there and update
544 if (neighborNodes[i]->distance == -1)
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]));
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)
557 MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
561 // or if is already in discoverednodelist, update
565 it = myMap.find(neighborNodes[i]->distAndEst);
566 if (it == myMap.end() )
568 MITK_INFO << "Nothing!";
570 for (it = myMap.begin(); it != myMap.end(); ++it)
572 if ((*it).second->mainListIndex == lookForId)
574 MITK_INFO << "But it is there!!!";
575 MITK_INFO << "Searched for: " << lookFor << " but had: " << (*it).second->distAndEst;
582 // 1st : find and delete old element
584 ret = myMap.equal_range(neighborNodes[i]->distAndEst);
586 if ((ret.first == ret.second))
588 /*+++++++++++++ no exact match +++++++++++++*/
589 // MITK_INFO << "No exact match!"; // if this happens, you are screwed
591 MITK_INFO << "Was looking for: " << lookFor << " ID: " << lookForId;
592 if (ret.first != myMap.end() )
595 MITK_INFO << "Found: " << it->first << " ID: " << it->second->mainListIndex;
597 MITK_INFO << "Found: " << it->first << " ID: " << it->second->mainListIndex;
600 MITK_INFO << "Found: " << it->first << " ID: " << it->second->mainListIndex;
603 // look if that ID is found in the map
604 for (it = myMap.begin(); it != myMap.end(); ++it)
606 if ((*it).second->mainListIndex == lookForId)
608 MITK_INFO << "But it is there!!!";
609 MITK_INFO << "Searched dist: " << lookFor << " found dist: " << (*it).second->distAndEst;
617 for (it = ret.first; it != ret.second; ++it)
619 if (it->second->mainListIndex == neighborNodes[i]->mainListIndex)
624 MITK_INFO << "INFO: size " << myMap.size();
625 MITK_INFO << "Erase: " << it->first << "|" << it->second->mainListIndex;
627 MITK_INFO << "INFO: size " << myMap.size();
628 for (it = myMap.begin(); it != myMap.end(); ++it)
630 MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
640 // MITK_INFO << "Could not find it! :(";
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]));
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)
657 MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;
662 // finished with checking all neighbors.
664 // Check Timeout, if activated
665 if (m_ActivateTimeOut)
668 durationAll = (double)(stopAll - startAll) / CLOCKS_PER_SEC;
669 if (durationAll >= 30)
671 // MITK_INFO << "TIMEOUT!! Search took over 30 seconds";
676 // Check end criteria:
677 // For multiple points
678 if (multipleEndPoints)
680 // super slow, make it faster
681 for (unsigned int i = 0; i < m_endPoints.size(); i++)
683 if (CoordToNode(m_endPoints[i]) == mainNodeListIndex)
685 m_endPointsClosed.push_back(NodeToCoord(mainNodeListIndex));
686 m_endPoints.erase(m_endPoints.begin() + i);
687 if (m_endPoints.empty())
692 if (m_Graph_EndNode == mainNodeListIndex)
695 SetEndIndex(m_endPoints[0]);
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)
703 /*if (m_StoreVectorOrder)
704 MITK_INFO << "Number of Nodes checked: " << m_VectorOrder.size() ;*/
710 template <class TInputImageType, class TOutputImageType>
711 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::MakeOutputs()
713 // MITK_INFO << "Make Output";
715 if (m_MakeOutputImage)
717 OutputImagePointer output0 = this->GetOutput(0);
718 output0->SetRegions(this->GetInput()->GetLargestPossibleRegion());
720 OutputImageIteratorType shortestPathImageIt(output0, output0->GetRequestedRegion());
721 // Create ShortestPathImage (Output 0)
722 for (shortestPathImageIt.GoToBegin(); !shortestPathImageIt.IsAtEnd(); ++shortestPathImageIt)
724 // First intialize with background color
725 shortestPathImageIt.Set(BACKGROUND);
728 if (!multipleEndPoints) // Only one path was calculated
730 for (unsigned int i = 0; i < m_VectorPath.size(); i++)
732 shortestPathImageIt.SetIndex(m_VectorPath[i]);
733 shortestPathImageIt.Set(FOREGROUND);
736 else // multiple pathes has been calculated, draw all
738 for (unsigned int i = 0; i < m_MultipleVectorPaths.size(); i++)
740 for (unsigned int j = 0; j < m_MultipleVectorPaths[i].size(); j++)
742 shortestPathImageIt.SetIndex(m_MultipleVectorPaths[i][j]);
743 shortestPathImageIt.Set(FOREGROUND);
750 template <class TInputImageType, class TOutputImageType>
751 typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::OutputImagePointer
752 ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetVectorOrderImage()
754 // Create Vector Order Image
757 OutputImagePointer image = OutputImageType::New();
758 image->SetRegions(this->GetInput()->GetLargestPossibleRegion());
760 OutputImageIteratorType vectorOrderImageIt(image, image->GetRequestedRegion());
762 // MITK_INFO << "GetVectorOrderImage";
763 for (vectorOrderImageIt.GoToBegin(); !vectorOrderImageIt.IsAtEnd(); ++vectorOrderImageIt)
765 // First intialize with background color
766 vectorOrderImageIt.Value() = BACKGROUND;
768 for (int i = 0; i < m_VectorOrder.size(); i++)
770 vectorOrderImageIt.SetIndex(NodeToCoord(m_VectorOrder[i]));
771 vectorOrderImageIt.Set(BACKGROUND + i);
776 template <class TInputImageType, class TOutputImageType>
777 typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::OutputImagePointer
778 ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetDistanceImage()
780 // Create Distance Image
783 OutputImagePointer image = OutputImageType::New();
784 image->SetRegions(this->GetInput()->GetLargestPossibleRegion());
787 OutputImageIteratorType distanceImageIt(image, image->GetRequestedRegion());
788 // Create Distance Image (Output 1)
789 NodeNumType myNodeNum;
790 for (distanceImageIt.GoToBegin(); !distanceImageIt.IsAtEnd(); ++distanceImageIt)
792 IndexType index = distanceImageIt.GetIndex();
793 myNodeNum = CoordToNode(index);
794 double newVal = m_Nodes[myNodeNum].distance;
795 distanceImageIt.Set(newVal);
799 template <class TInputImageType, class TOutputImageType>
800 std::vector<typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType>
801 ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetVectorPath()
806 template <class TInputImageType, class TOutputImageType>
807 std::vector<std::vector<typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType>>
808 ShortestPathImageFilter<TInputImageType, TOutputImageType>::GetMultipleVectorPaths()
810 return m_MultipleVectorPaths;
813 template <class TInputImageType, class TOutputImageType>
814 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::MakeShortestPathVector()
816 // MITK_INFO << "Make ShortestPath Vec";
819 if (!multipleEndPoints)
821 // fill m_VectorPath with the Shortest Path
822 m_VectorPath.clear();
824 // Go backwards from endnote to startnode
825 NodeNumType prevNode = m_Graph_EndNode;
826 while (prevNode != m_Graph_StartNode)
828 m_VectorPath.push_back(NodeToCoord(prevNode));
829 prevNode = m_Nodes[prevNode].prevNode;
831 m_VectorPath.push_back(NodeToCoord(prevNode));
833 std::reverse(m_VectorPath.begin(), m_VectorPath.end());
835 // Multiple end end points and pathes
838 for (unsigned int i = 0; i < m_endPointsClosed.size(); i++)
840 m_VectorPath.clear();
841 // Go backwards from endnote to startnode
842 NodeNumType prevNode = CoordToNode(m_endPointsClosed[i]);
843 while (prevNode != m_Graph_StartNode)
845 m_VectorPath.push_back(NodeToCoord(prevNode));
846 prevNode = m_Nodes[prevNode].prevNode;
848 m_VectorPath.push_back(NodeToCoord(prevNode));
851 std::reverse(m_VectorPath.begin(), m_VectorPath.end());
853 m_MultipleVectorPaths.push_back(m_VectorPath);
858 template <class TInputImageType, class TOutputImageType>
859 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::CleanUp()
861 m_VectorOrder.clear();
862 m_VectorPath.clear();
863 // TODO: if multiple Path, clear all multiple Paths
869 template <class TInputImageType, class TOutputImageType>
870 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::GenerateData()
875 // Calc Shortest Parth
876 StartShortestPathSearch();
878 // Fill Shortest Path
879 MakeShortestPathVector();
885 template <class TInputImageType, class TOutputImageType>
886 void ShortestPathImageFilter<TInputImageType, TOutputImageType>::PrintSelf(std::ostream &os, Indent indent) const
888 Superclass::PrintSelf(os, indent);
891 } /* end namespace itk */
893 #endif // __itkShortestPathImageFilter_txx