diff --git a/src/DGtal/geometry/curves/FP.ih b/src/DGtal/geometry/curves/FP.ih
index fd437e32bc..ac89b1a10e 100644
--- a/src/DGtal/geometry/curves/FP.ih
+++ b/src/DGtal/geometry/curves/FP.ih
@@ -171,19 +171,18 @@ algorithm(const TIterator& itb,
   //backward extension
   while ( currentDSS.extendBack() ) {}
 
-  //std::cerr << "First MS" << std::endl << currentDSS << std::endl; 
-
   //local convexity
   typedef details::DSSDecorator<DSSComputer> DSSDecorator; 
   DSSDecorator* adapter = 0; //adapter for the current DSS
   adapter = initConvexityConcavity<DSSDecorator>( currentDSS );
 
-  //stopping point 
-  Point stoppingPoint = adapter->lastLeaningPoint(); 
-  if (adapter->firstLeaningPoint() == stoppingPoint) {
-    myPolygon.push_back( stoppingPoint );
+  if (adapter->firstLeaningPoint() == adapter->lastLeaningPoint()) {
+    myPolygon.push_back( adapter->lastLeaningPoint() );
   }//stored in removingStep function otherwise
 
+  //first MS
+  DSSComputer firstMS = currentDSS; 
+
   //since there are more than 2 MS (closed case)
   //but no more than 2 MS sharing a leaning point
   //at the same position 
@@ -203,9 +202,9 @@ algorithm(const TIterator& itb,
 	  throw InputException();
 	}
       addingStep( adapter );
-      if ( adapter->lastLeaningPoint() == stoppingPoint )
+      if (currentDSS == firstMS)
 	{ //stop and remove the last point to avoid a repetition
-	  if (adapter->firstLeaningPoint() == stoppingPoint) {
+	  if (adapter->firstLeaningPoint() == myPolygon.front()) {
 	    ASSERT( myPolygon.size() > 0 ); 
 	    myPolygon.pop_back();
 	  }          
@@ -246,8 +245,6 @@ DGtal::FP<TIterator,TInteger,connectivity>
 ::removingStep( Adapter* adapter )
 {
 
-  //std::cerr << "Removing step " << currentDSS << std::endl; 
-
   ASSERT( adapter->isExtendableFront() == false ); 
 
   //store the last leaning point 
@@ -283,8 +280,6 @@ DGtal::FP<TIterator,TInteger,connectivity>
 	      const typename Adapter::DSS::ConstIterator& itEnd ) 
 {
 
-  //std::cerr << "Adding step " << currentDSS << std::endl; 
-
   ASSERT( adapter->isExtendableFront() == true ); 
 
   //remove the last leaning point 
@@ -315,8 +310,6 @@ DGtal::FP<TIterator,TInteger,connectivity>
 ::addingStep( Adapter* adapter ) 
 {
 
-  //std::cerr << "Adding step " << currentDSS << std::endl; 
-
   ASSERT( adapter->isExtendableFront() == true ); 
 
   //remove the last leaning point 
diff --git a/tests/geometry/curves/testFP.cpp b/tests/geometry/curves/testFP.cpp
index bcf15ae063..1ebbffac53 100644
--- a/tests/geometry/curves/testFP.cpp
+++ b/tests/geometry/curves/testFP.cpp
@@ -42,8 +42,6 @@
 ///////////////////////////////////////////////////////////////////////////////
 
 using namespace std;
-using namespace DGtal;
-using namespace LibBoard;
 
 ///////////////////////////////////////////////////////////////////////////////
 // Functions for testing class FP.
@@ -58,6 +56,8 @@ using namespace LibBoard;
 bool testFP(string filename)
 {
 
+  using namespace DGtal;
+  //using namespace LibBoard;
 
   trace.info() << endl;
   trace.info() << "Reading GridCurve from " << filename << endl;
@@ -95,29 +95,193 @@ bool testFP(string filename)
  
 }
 
+template <typename Range1, typename Range2>
+bool compare(const Range1& pts, const Range2& groundTruth)
+{
+
+  DGtal::GridCurve<> curve;
+  curve.initFromPointsRange(pts.begin(), pts.end()); 
+
+  std::cout << curve.size() << std::endl; 
+  std::copy(curve.getPointsRange().begin(), curve.getPointsRange().end(), std::ostream_iterator<DGtal::Z2i::Point>(std::cout, " ") ); 
+  std::cout << std::endl; 
+
+  typedef DGtal::GridCurve<>::PointsRange::ConstCirculator   ConstCirculator;
+  typedef DGtal::FP<ConstCirculator, DGtal::Z2i::Integer, 4>                FaithfulPolygon;
+  FaithfulPolygon theFP( curve.getPointsRange().c(), curve.getPointsRange().c() );
+
+  std::cout << theFP.polygon().size() << std::endl; 
+  std::copy(theFP.polygon().begin(), theFP.polygon().end(), std::ostream_iterator<DGtal::Z2i::Point>(std::cout, " ") ); 
+  std::cout << std::endl; 
+  
+  return ( (theFP.polygon().size() == groundTruth.size()) && 
+	   std::equal(theFP.polygon().begin(), theFP.polygon().end(), groundTruth.begin()) ); 
+}
+
+bool stoppingCriterionTest()
+{
+  using namespace DGtal;
+  using namespace Z2i;
+
+  int nbok = 0; 
+  int nb = 0; 
+
+  { //inflection part, one leaning point
+    std::vector<Point> pts, pts2;
+    pts.push_back(Point(0,0)); 
+    pts.push_back(Point(1,0)); 
+    pts.push_back(Point(1,1)); 
+    pts.push_back(Point(2,1)); 
+    pts.push_back(Point(3,1)); 
+    pts.push_back(Point(3,0)); 
+    pts.push_back(Point(4,0)); 
+    pts.push_back(Point(4,1)); 
+    pts.push_back(Point(4,2)); 
+    pts.push_back(Point(3,2)); 
+    pts.push_back(Point(2,2)); 
+    pts.push_back(Point(1,2)); 
+    pts.push_back(Point(0,2));
+    pts.push_back(Point(-1,2)); 
+    pts.push_back(Point(-2,2)); 
+    pts.push_back(Point(-2,1)); 
+    pts.push_back(Point(-2,0)); 
+    pts.push_back(Point(-1,0)); 
+    
+    pts2.push_back(Point(1,1)); 
+    pts2.push_back(Point(3,1)); 
+    pts2.push_back(Point(3,0)); 
+    pts2.push_back(Point(4,0)); 
+    pts2.push_back(Point(4,2)); 
+    pts2.push_back(Point(-2,2)); 
+    pts2.push_back(Point(-2,0)); 
+    pts2.push_back(Point(1,0)); 
+    
+    if(compare(pts, pts2))
+      nbok++; 
+    nb++; 
+    
+    trace.info() << nbok << " / " << nb << std::endl; 
+  }
+
+  { //inflection part, two distinct leaning points
+    std::vector<Point> pts, pts2;
+    pts.push_back(Point(0,0)); 
+    pts.push_back(Point(1,0)); 
+    pts.push_back(Point(1,1)); 
+    pts.push_back(Point(2,1)); 
+    pts.push_back(Point(3,1)); 
+    pts.push_back(Point(4,1)); 
+    pts.push_back(Point(4,0)); 
+    pts.push_back(Point(5,0)); 
+    pts.push_back(Point(5,1)); 
+    for (int i = 5; i >= -2; --i)
+      pts.push_back(Point(i,2)); 
+    pts.push_back(Point(-2,1)); 
+    pts.push_back(Point(-2,0)); 
+    pts.push_back(Point(-1,0)); 
+    
+    pts2.push_back(Point(1,1)); 
+    pts2.push_back(Point(4,1)); 
+    pts2.push_back(Point(4,0)); 
+    pts2.push_back(Point(5,0)); 
+    pts2.push_back(Point(5,2)); 
+    pts2.push_back(Point(-2,2)); 
+    pts2.push_back(Point(-2,0)); 
+    pts2.push_back(Point(1,0)); 
+    
+    if(compare(pts, pts2))
+      nbok++; 
+    nb++; 
+    
+    trace.info() << nbok << " / " << nb << std::endl; 
+  }
+
+  { //convex part, one leaning point
+    std::vector<Point> pts, pts2;
+    pts.push_back(Point(0,0)); 
+    pts.push_back(Point(1,0)); 
+    pts.push_back(Point(1,1)); 
+    pts.push_back(Point(2,1)); 
+    pts.push_back(Point(3,1)); 
+    pts.push_back(Point(3,2)); 
+    for (int i = 3; i >= -2; --i)
+      pts.push_back(Point(i,3)); 
+    pts.push_back(Point(-2,2)); 
+    pts.push_back(Point(-2,1)); 
+    pts.push_back(Point(-2,0)); 
+    pts.push_back(Point(-1,0)); 
+
+    //FP begins at the last leaning point of the first MS
+    pts2.push_back(Point(1,0)); 
+    pts2.push_back(Point(3,1)); 
+    pts2.push_back(Point(3,3)); 
+    pts2.push_back(Point(-2,3)); 
+    pts2.push_back(Point(-2,0)); 
+    
+    if(compare(pts, pts2))
+      nbok++; 
+    nb++; 
+    
+    trace.info() << nbok << " / " << nb << std::endl; 
+  }
+
+  { //convex part, two distinct leaning points
+    std::vector<Point> pts, pts2;
+    pts.push_back(Point(0,0)); 
+    pts.push_back(Point(1,0)); 
+    pts.push_back(Point(1,1)); 
+    pts.push_back(Point(2,1)); 
+    pts.push_back(Point(3,1)); 
+    pts.push_back(Point(3,2)); 
+    for (int i = 3; i >= -1; --i)
+      pts.push_back(Point(i,3)); 
+    pts.push_back(Point(-1,2)); 
+    pts.push_back(Point(-1,1)); 
+    pts.push_back(Point(-1,0)); 
+    
+    //FP begins at the last leaning point of the first MS
+    pts2.push_back(Point(3,1)); 
+    pts2.push_back(Point(3,3)); 
+    pts2.push_back(Point(-1,3)); 
+    pts2.push_back(Point(-1,0)); 
+    pts2.push_back(Point(1,0)); 
+    
+    if(compare(pts, pts2))
+      nbok++; 
+    nb++; 
+    
+    trace.info() << nbok << " / " << nb << std::endl; 
+  }
+
+  return (nb == nbok); 
+}
+
 ///////////////////////////////////////////////////////////////////////////////
 // Standard services - public :
 
 int main( int argc, char** argv )
 {
-  trace.beginBlock ( "Testing class FP" );
-  trace.info() << "Args:";
+  DGtal::trace.beginBlock ( "Testing class FP" );
+  DGtal::trace.info() << "Args:";
   for ( int i = 0; i < argc; ++i )
-    trace.info() << " " << argv[ i ];
-  trace.info() << endl;
+    DGtal::trace.info() << " " << argv[ i ];
+  DGtal::trace.info() << std::endl;
 
   std::string sinus2D4 = testPath + "samples/sinus2D4.dat";
   std::string square = testPath + "samples/smallSquare.dat";
   std::string dss = testPath + "samples/DSS.dat";
 
-  bool res = testFP(sinus2D4)
-            && testFP(square)
-            && testFP(dss)
-//other tests
-;
+  bool res = 
+/*testFP(sinus2D4)
+    && testFP(square)
+    && testFP(dss)
+    //other tests
+    &&*/ 
+stoppingCriterionTest()
+    ;
 
-  trace.emphase() << ( res ? "Passed." : "Error." ) << endl;
-  trace.endBlock();
+  DGtal::trace.emphase() << ( res ? "Passed." : "Error." ) << std::endl;
+  DGtal::trace.endBlock();
   return res ? 0 : 1;
 }
 //                                                                           //