Big bug stamped on wall

[FrontAlgorithms.git] / lib / fpa / Image / DijkstraWithSphereBacktracking.hxx

diff --git a/lib/fpa/Image/DijkstraWithSphereBacktracking.hxx b/lib/fpa/Image/DijkstraWithSphereBacktracking.hxx
index 346103ba9df5e722649fdef634e497c8e48908e2..7d50735014422505f4e995753a0e0b54f4fb93c6 100644 (file)
--- a/lib/fpa/Image/DijkstraWithSphereBacktracking.hxx
+++ b/lib/fpa/Image/DijkstraWithSphereBacktracking.hxx
@@ -123,7 +123,7 @@ _AfterMainLoop( )
   // are near thin branches
   typename _TCandidates::const_reverse_iterator cIt =
     this->m_Candidates.rbegin( );
-  this->m_NumberOfBranches = 0;
+  this->m_NumberOfBranches = 1;
   for( ; cIt != this->m_Candidates.rend( ); ++cIt )
   {
     // If pixel hasn't been visited, continue
@@ -152,81 +152,121 @@ _AfterMainLoop( )
     // Keep endpoint
     if( marks->GetPixel( max_vertex ) != 0 )
       continue;
-    this->m_NumberOfBranches++;
     this->m_EndPoints.push_back( max_vertex );
 
     // Construct path (at least the part that hasn't been iterated)
+    bool start = true;
+    bool change = false;
     do
     {
-      if(
-        std::find(
-          this->m_BifurcationPoints.begin( ),
-          this->m_BifurcationPoints.end( ),
-          max_vertex
-          ) == this->m_BifurcationPoints.end( )
-        )
+      if( start )
       {
-        // Mark a sphere around current point as visited
-        double dist = std::sqrt( double( input->GetPixel( max_vertex ) ) );
-        region = this->_Region( max_vertex, dist * double( 1.1 ) );
-        itk::ImageRegionIteratorWithIndex< TMarkImage >
-          mIt( marks, region );
-        for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
-          mIt.Set( this->m_NumberOfBranches );
-
-        // Next vertex in current path
-        this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
-        this->m_FinalTree[ max_vertex ] = this->_Parent( max_vertex );
+        if( this->m_FinalTree.find( max_vertex ) == this->m_FinalTree.end( ) )
+        {
+          // Mark a sphere around current point as visited
+          double dist = std::sqrt( double( input->GetPixel( max_vertex ) ) );
+          region = this->_Region( max_vertex, dist * double( 1.5 ) );
+          itk::ImageRegionIteratorWithIndex< TMarkImage >
+            mIt( marks, region );
+          for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
+            mIt.Set( this->m_NumberOfBranches );
+
+          // Next vertex in current path
+          this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
+          this->m_FinalTree[ max_vertex ] =
+            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
+          // std::cout << "New: " << this->m_NumberOfBranches << std::endl;
+        }
+        else
+        {
+          // A bifurcation point has been reached!
+          this->m_BifurcationPoints.push_back( max_vertex );
+          this->m_NumberOfBranches++;
+
+          this->m_FinalTree[ max_vertex ] =
+            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
+          // std::cout << "Bifurcation: " << this->m_NumberOfBranches << std::endl;
+
+          start = false;
+
+        } // fi
       }
       else
       {
-        // A bifurcation point has been reached!
-        this->m_BifurcationPoints.push_back( max_vertex );
-        this->m_NumberOfBranches++;
+        if(
+          std::find(
+            this->m_BifurcationPoints.begin( ),
+            this->m_BifurcationPoints.end( ),
+            max_vertex
+            ) == this->m_BifurcationPoints.end( )
+          )
+        {
+          // Mark a sphere around current point as visited
+          double dist = std::sqrt( double( input->GetPixel( max_vertex ) ) );
+          region = this->_Region( max_vertex, dist * double( 1.5 ) );
+          itk::ImageRegionIteratorWithIndex< TMarkImage >
+            mIt( marks, region );
+          for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
+            mIt.Set( this->m_NumberOfBranches );
+
+          // Next vertex in current path
+          this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
+          this->m_FinalTree[ max_vertex ] =
+            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
+          change = true;
+          // std::cout << "Change: " << this->m_NumberOfBranches << std::endl;
+        }
+        else
+        {
+          // A bifurcation point has been reached!
+          // TODO: this->m_BifurcationPoints.push_back( max_vertex );
+          this->m_NumberOfBranches++;
+          this->m_FinalTree[ max_vertex ] =
+            TTreeNode( this->_Parent( max_vertex ), this->m_NumberOfBranches );
+          // std::cout << "Change bifurcation: " << this->m_NumberOfBranches << std::endl;
+
+        } // fi
 
       } // fi
       max_vertex = this->_Parent( max_vertex );
 
     } while( max_vertex != this->_Parent( max_vertex ) );
+    if( start || change )
+      this->m_NumberOfBranches++;
 
-    /* TODO
-       bool terminate = false;
-       do
-       {
-       if( this->m_FinalTree.find( max_vertex ) == this->m_FinalTree.end( ) )
-       {
-       // Mark a sphere around current point as visited
-       double dist = std::sqrt( double( input->GetPixel( max_vertex ) ) );
-       region = this->_Region( max_vertex, dist * double( 1.25 ) );
-       itk::ImageRegionIteratorWithIndex< TMarkImage >
-       mIt( marks, region );
-       for( mIt.GoToBegin( ); !mIt.IsAtEnd( ); ++mIt )
-       mIt.Set( true );
-
-       // Next vertex in current path
-       this->InvokeEvent( TBacktrackingEvent( max_vertex, this->m_NumberOfBranches ) );
-       this->m_FinalTree[ max_vertex ] = this->_Parent( max_vertex );
-       }
-       else
-       {
-       // A bifurcation point has been reached!
-       this->m_BifurcationPoints.push_back( max_vertex );
-       terminate = true;
-
-       } // fi
-       max_vertex = this->_Parent( max_vertex );
-
-       } while( max_vertex != this->_Parent( max_vertex ) && !terminate );
-
-       if( !terminate )
-       {
-       this->m_FinalTree[ max_vertex ] = max_vertex;
-       this->InvokeEvent( TEndBacktrackingEvent( this->m_NumberOfBranches ) );
-
-       } // fi
-    */
+    this->InvokeEvent( TEndBacktrackingEvent( ) );
 
   } // rof
+
+  std::map< TMark, unsigned long > histo;
+  for(
+    typename TTree::iterator treeIt = this->m_FinalTree.begin( );
+    treeIt != this->m_FinalTree.end( );
+    ++treeIt
+    )
+    histo[ treeIt->second.second ]++;
+
+  std::map< TMark, TMark > changes;
+  TMark last_change = 1;
+  for( TMark i = 1; i <= this->m_NumberOfBranches; ++i )
+  {
+    if( histo[ i ] != 0 )
+      changes[ i ] = last_change++;
+
+  } // rof
+  this->m_NumberOfBranches = changes.size( );
+
+  for(
+    typename TTree::iterator treeIt = this->m_FinalTree.begin( );
+    treeIt != this->m_FinalTree.end( );
+    ++treeIt
+    )
+  {
+    TMark old = treeIt->second.second;
+    treeIt->second.second = changes[ old ];
+
+  } // fi
+
 }
 
 // -------------------------------------------------------------------------
@@ -237,8 +277,13 @@ _UpdateNeigh( _TNode& nn, const _TNode& n )
   C nc = this->_Cost( nn.Vertex, n.Vertex );
   if( TCost( 0 ) < nc )
   {
+    typename I::PointType pnn, pn;
+    this->GetInput( )->TransformIndexToPhysicalPoint( nn.Vertex, pnn );
+    this->GetInput( )->TransformIndexToPhysicalPoint( n.Vertex, pn );
+
+
     nc += TCost( 1 );
-    nn.Cost = n.Cost + ( TCost( 1 ) / std::pow( nc, 4 ) );
+    nn.Cost = n.Cost + ( TCost( pnn.EuclideanDistanceTo( pn ) ) / std::pow( nc, 4 ) );
     nn.Result = nn.Cost;
     return( true );
   }