diff --git a/Polyline_simplification_2/include/CGAL/Polyline_simplification_2/simplify.h b/Polyline_simplification_2/include/CGAL/Polyline_simplification_2/simplify.h
index f2bb6e6c070..9f0f46f38e5 100644
--- a/Polyline_simplification_2/include/CGAL/Polyline_simplification_2/simplify.h
+++ b/Polyline_simplification_2/include/CGAL/Polyline_simplification_2/simplify.h
@@ -267,11 +267,17 @@ operator()()
     if((*u)->is_removable()){
       boost::optional<FT> dist = cost(pct, u);
       if(! dist){
-        std::cerr << "undefined cost not handled yet" << std::endl;
+        // cost is undefined
+        if( mpq->contains(u) ){
+          mpq->erase(u);
+        }
       } else {
         (*u)->set_cost(*dist);
-        if((*mpq).contains(u)){
-        (*mpq).update(u, true);
+        if(mpq->contains(u)){
+          mpq->update(u, true);
+        }
+        else{
+          mpq->push(u);
         }
       }
     }
@@ -279,11 +285,17 @@ operator()()
     if((*w)->is_removable()){
       boost::optional<FT> dist = cost(pct, w);
       if(! dist){
-        std::cerr << "undefined cost not handled yet" << std::endl;
+        // cost is undefined
+        if( mpq->contains(w) ){
+          mpq->erase(w);
+        }
       } else {
         (*w)->set_cost(*dist);
-        if((*mpq).contains(w)){
-        (*mpq).update(w, true);
+        if(mpq->contains(w)){
+          mpq->update(w, true);
+        }
+        else{
+          mpq->push(w);
         }
 
       }
diff --git a/Polyline_simplification_2/test/Polyline_simplification_2/simplify_polygon_test.cpp b/Polyline_simplification_2/test/Polyline_simplification_2/simplify_polygon_test.cpp
index df03b5b99df..f179fa56384 100644
--- a/Polyline_simplification_2/test/Polyline_simplification_2/simplify_polygon_test.cpp
+++ b/Polyline_simplification_2/test/Polyline_simplification_2/simplify_polygon_test.cpp
@@ -18,10 +18,11 @@ typedef CGAL::Constrained_Delaunay_triangulation_2<K,TDS, Itag> CDT;
 typedef CGAL::Constrained_triangulation_plus_2<CDT>     CT;
 typedef CGAL::Polygon_2<K>                   Polygon_2;
 typedef PS::Stop_above_cost_threshold Stop;
-typedef PS::Squared_distance_cost            Cost;
-
+typedef PS::Squared_distance_cost Cost1;
+typedef PS::Scaled_squared_distance_cost Cost2;
 
 
+template <class Cost>
 void test(char* fname)
 {
   CGAL::Timer timer;
@@ -45,7 +46,8 @@ int main(int argc, char* argv[])
 {
 
   for(int i= 1;i < argc; i++){
-    test(argv[i]);
+    test<Cost1>(argv[i]);
+    test<Cost2>(argv[i]);
   }
 
   return 0;
diff --git a/STL_Extension/include/CGAL/Modifiable_priority_queue.h b/STL_Extension/include/CGAL/Modifiable_priority_queue.h
index 1671468eff4..066531c1bc4 100644
--- a/STL_Extension/include/CGAL/Modifiable_priority_queue.h
+++ b/STL_Extension/include/CGAL/Modifiable_priority_queue.h
@@ -98,6 +98,7 @@ public:
   handle update ( value_type const& v, handle h ) { mHeap.update(v); return h ; }
   
   handle erase ( value_type const& v, handle  ) { mHeap.remove(v); return null_handle() ; }
+  handle erase ( value_type const& v  ) { mHeap.remove(v); return null_handle() ; }
 
   value_type top() const { return mHeap.top() ; }