fix memory leak in global pd

CMakeModules/CompilerSettings.cmake

 if(CMAKE_COMPILER_IS_GNUCXX)
-    add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused-parameter)
+    add_definitions(-W -Wall -g -Wextra -Wno-missing-field-initializers -Wno-unused-parameter)
 endif(CMAKE_COMPILER_IS_GNUCXX)
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
     add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas)

include/fcl/ccd/motion_base.h

@@ -183,6 +183,8 @@ protected:
   
 };
 
+typedef boost::shared_ptr<MotionBase> MotionBasePtr;
+
 
 }
 

include/fcl/learning/classifier.h

@@ -71,6 +71,8 @@ template<std::size_t N>
 class SVMClassifier
 {
 public:
+
+  ~SVMClassifier() {}
   
   virtual PredictResult predict(const Vecnf<N>& q) const = 0;
   virtual std::vector<PredictResult> predict(const std::vector<Vecnf<N> >& qs) const = 0;

src/continuous_collision.cpp

@@ -18,24 +18,24 @@ ConservativeAdvancementFunctionMatrix<GJKSolver>& getConservativeAdvancementFunc
   return table;
 }
 
-MotionBase* getMotionBase(const Transform3f& tf_beg, const Transform3f& tf_end, CCDMotionType motion_type)
+MotionBasePtr getMotionBase(const Transform3f& tf_beg, const Transform3f& tf_end, CCDMotionType motion_type)
 {
   switch(motion_type)
   {
   case CCDM_TRANS:
-    return new TranslationMotion(tf_beg, tf_end);
+    return MotionBasePtr(new TranslationMotion(tf_beg, tf_end));
     break;
   case CCDM_LINEAR:
-    return new InterpMotion(tf_beg, tf_end);
+    return MotionBasePtr(new InterpMotion(tf_beg, tf_end));
     break;
   case CCDM_SCREW:
-    return new ScrewMotion(tf_beg, tf_end);
+    return MotionBasePtr(new ScrewMotion(tf_beg, tf_end));
     break;
   case CCDM_SPLINE:
-    return new SplineMotion(tf_beg, tf_end);
+    return MotionBasePtr(new SplineMotion(tf_beg, tf_end));
     break;
   default:
-    return NULL;
+    return MotionBasePtr();
   }
 }
 
@@ -304,10 +304,10 @@ FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_b
                            const ContinuousCollisionRequest& request,
                            ContinuousCollisionResult& result)
 {
-  MotionBase* motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type);
-  MotionBase* motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type);
+  MotionBasePtr motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type);
+  MotionBasePtr motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type);
 
-  return continuousCollide(o1, motion1, o2, motion2, request, result);
+  return continuousCollide(o1, motion1.get(), o2, motion2.get(), request, result);
 }
 
 

src/penetration_depth.cpp

@@ -402,6 +402,8 @@ FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
   result.pd_value = request.distance_func(result.resolved_tf, tf2);
   result.pd_value = std::sqrt(result.pd_value);
 
+  delete knn_solver;
+
   return result.pd_value;
 }
 

test/libsvm_classifier.h

@@ -45,6 +45,7 @@ public:
     problem.W = NULL;
   }
 
+
   void setCSVM() { param.svm_type = C_SVC; }
   void setNuSVM() { param.svm_type = NU_SVC; }
   void setC(FCL_REAL C) { param.C = C; }

test/test_fcl_xmldata.cpp

@@ -13,6 +13,7 @@
 
 #include "libsvm_classifier.h"
 #include "fcl/penetration_depth.h"
+#include "fcl/collision_data.h"
 #include "fcl_resources/config.h"
 #include "test_fcl_utility.h"
 
@@ -73,7 +74,7 @@ static void loadSceneFile(const std::string& filename,
           grid = grid->NextSiblingElement("GRID");
         }
 
-        // std::cout << "#vertices " << n_vertices << std::endl;
+        std::cout << "#vertices " << n_vertices << std::endl;
 
         TiXmlElement* tri = object->FirstChildElement("TRIA");
         int n_tris = 0;
@@ -103,7 +104,7 @@ static void loadSceneFile(const std::string& filename,
           tri = tri->NextSiblingElement("TRIA");
         }
 
-        // std::cout << "#triangles " << n_tris << std::endl;
+        std::cout << "#triangles " << n_tris << std::endl;
 
         if(object_id - 1 == (int)points_array.size())
         {
@@ -127,7 +128,7 @@ static void loadSceneFile(const std::string& filename,
         i++;
       }
 
-      // std::cout << "#objects " << i << std::endl;
+      std::cout << "#objects " << i << std::endl;
     }
 
     motion = doc.FirstChildElement("MOTION");
@@ -180,7 +181,7 @@ static void loadSceneFile(const std::string& filename,
         n_frame++;
       }
 
-      // std::cout << "#frames " << n_frame << std::endl;
+      std::cout << "#frames " << n_frame << std::endl;
     }
   }
   else
@@ -265,7 +266,8 @@ static void xml2obj(const std::string& in_filename, const std::string& out_filen
   }
 }
 
-static void scenePenetrationTest(const std::string& filename)
+
+static void scenePenetrationTest(const std::string& filename, PenetrationDepthType pd_type = PDT_GENERAL_EULER)
 {
   std::vector<std::vector<Vec3f> > points_array;
   std::vector<std::vector<Triangle> > triangles_array;
@@ -293,7 +295,7 @@ static void scenePenetrationTest(const std::string& filename)
   std::size_t KNN_K = 10;
   LibSVMClassifier<6> classifier;
   
-  std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, PDT_GENERAL_EULER, &classifier, 10000, 0, KNN_GNAT, KNN_K);
+  std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, pd_type, &classifier, 100000, 0, KNN_GNAT, KNN_K);
 
   classifier.save(filename + "model.txt");
 
@@ -327,6 +329,12 @@ BOOST_AUTO_TEST_CASE(scene_test_penetration)
   RNG::setSeed(1);
   boost::filesystem::path path(TEST_RESOURCES_DIR);
 
+  /*
+  std::cout << "manyframes/Model_4" << std::endl;
+  std::string filename0 = (path / "manyframes/Model_4.xml").string();
+  scenePenetrationTest(filename0);
+  */
+
   std::cout << "scenario-1-2-3/Model_1_Scenario_1" << std::endl;
   std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
   scenePenetrationTest(filename1);
@@ -403,10 +411,19 @@ BOOST_AUTO_TEST_CASE(scene_test_penetration)
 
 }
 
-
 BOOST_AUTO_TEST_CASE(xml2obj_test)
 {
   boost::filesystem::path path(TEST_RESOURCES_DIR);
+
+  std::string filename_manyframe0 = (path / "manyframes/Model_5.xml").string();
+  xml2obj(filename_manyframe0, "Model_5");
+
+  std::string filename_manyframe1 = (path / "manyframes/Model_1.xml").string();
+  xml2obj(filename_manyframe1, "Model_1");
+  
+  std::string filename_manyframe2 = (path / "manyframes/Model_4.xml").string();
+  xml2obj(filename_manyframe2, "Model_4");
+  
   std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
   xml2obj(filename1, "Model_1_Scenario_1");