diff --git a/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp b/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp
index 2d8e93abb4118fa37624fd9069f8064f3cf0dc63..1f4e8d007322d9206dc46df0318740610819c77f 100644
--- a/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp
+++ b/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp
@@ -37,6 +37,8 @@
 #include "collision_space_fcl/environmentFCL.h"
 #include "fcl/geometric_shape_to_BVH_model.h"
 #include "fcl/collision.h"
+#include "fcl/primitive.h"
+#include "fcl/BVH_utility.h"
 #include <geometric_shapes/shape_operations.h>
 #include <ros/console.h>
 #include <cassert>
@@ -197,19 +199,31 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
   {
     case shapes::SPHERE:
     {
-      generateBVHModel(*g, fcl::Sphere(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding));
+      fcl::generateBVHModelPointCloud(*g, fcl::Sphere(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding));
+      fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
+      fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
+      fcl::BVHExpand(*g, uc, 1);
+      g->uc = uc;
     }
     break;
     case shapes::BOX:
     {
       const double *size = static_cast<const shapes::Box*>(shape)->size;
-      generateBVHModel(*g, fcl::Box(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0));
+      fcl::generateBVHModelPointCloud(*g, fcl::Box(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0));
+      fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
+      fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
+      fcl::BVHExpand(*g, uc, 1);
+      g->uc = uc;
     }
     break;
     case shapes::CYLINDER:
     {
-      generateBVHModel(*g, fcl::Cylinder(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
+      fcl::generateBVHModelPointCloud(*g, fcl::Cylinder(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
                        static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0));
+      fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
+      fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
+      fcl::BVHExpand(*g, uc, 1);
+      g->uc = uc;
     }
     break;
     case shapes::MESH:
@@ -256,9 +270,14 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
         }
 
         g->beginModel();
-        g->addSubModel(points, tri_indices);
+        g->addSubModel(points);
         g->endModel();
         g->computeAABB();
+
+        fcl::Uncertainty* uc = new fcl::Uncertainty[g->num_vertices];
+        fcl::estimateSamplingUncertainty(g->vertices, g->num_vertices, uc);
+        fcl::BVHExpand(*g, uc, 1);
+        g->uc = uc;
       }
     }
     break;
diff --git a/branches/point_cloud/fcl/CMakeLists.txt b/branches/point_cloud/fcl/CMakeLists.txt
index 82131b4c0ff3fe91fd8da57cee680a6f1feb87f7..d30f8cfe9077c94ffad0f200acc291362d8feaf9 100644
--- a/branches/point_cloud/fcl/CMakeLists.txt
+++ b/branches/point_cloud/fcl/CMakeLists.txt
@@ -29,8 +29,8 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #rosbuild_add_executable(example examples/example.cpp)
 #target_link_libraries(example ${PROJECT_NAME})
 
-add_definitions(-DUSE_PQP=0)
-add_definitions(-DUSE_SVMLIGHT=0)
+add_definitions(-DUSE_PQP=1)
+add_definitions(-DUSE_SVMLIGHT=1)
 
 rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp)
 
diff --git a/branches/point_cloud/fcl/include/fcl/BVH_model.h b/branches/point_cloud/fcl/include/fcl/BVH_model.h
index c376869d19658e2ff190b9256e7f138a0658c8d8..3084b1cc67da37ff09085a8f85e23e9335610ded 100644
--- a/branches/point_cloud/fcl/include/fcl/BVH_model.h
+++ b/branches/point_cloud/fcl/include/fcl/BVH_model.h
@@ -107,6 +107,8 @@ public:
 
     bv_splitter.reset(new BVSplitter<BV>(SPLIT_METHOD_MEAN));
     bv_fitter.reset(new BVFitter<BV>());
+
+    uc = NULL;
   }
 
   BVHModel(const BVHModel& other);
@@ -121,6 +123,8 @@ public:
     delete [] prev_vertices;
 
     delete [] primitive_indices;
+
+    delete [] uc;
   }
 
   /** \brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so
@@ -168,6 +172,8 @@ public:
   /** \brief Number of points */
   int num_vertices;
 
+  Uncertainty* uc;
+
   /** \brief Begin a new BVH model */
   int beginModel(int num_tris = 0, int num_vertices = 0);
 
diff --git a/branches/point_cloud/fcl/include/fcl/BVH_utility.h b/branches/point_cloud/fcl/include/fcl/BVH_utility.h
index 8acb9c5528c55c43b7b5b1d38b69064f0a7314b8..3cd45820b78cdff538b190bb500a64bb536051af 100644
--- a/branches/point_cloud/fcl/include/fcl/BVH_utility.h
+++ b/branches/point_cloud/fcl/include/fcl/BVH_utility.h
@@ -76,6 +76,7 @@ void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, BVH_REAL r)
 /** \brief Expand the BVH bounding boxes according to uncertainty, for OBB */
 void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r);
 
+
 /** \brief Expand the BVH bounding boxes according to uncertainty, for RSS */
 void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r);
 
diff --git a/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h b/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h
index e5990f25bce44e8a03988c983a5daddba8477f65..ef4f32170574aacd4bd5bbd26cf1c1a354dc1837 100644
--- a/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/branches/point_cloud/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -227,6 +227,121 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
   model.computeAABB();
 }
 
+
+
+/** \brief Generate BVH model from box */
+template<typename BV>
+void generateBVHModelPointCloud(BVHModel<BV>& model, const Box& shape)
+{
+  double a = shape.side[0];
+  double b = shape.side[1];
+  double c = shape.side[2];
+  std::vector<Vec3f> points(8);
+  points[0] = Vec3f(0.5 * a, -0.5 * b, 0.5 * c);
+  points[1] = Vec3f(0.5 * a, 0.5 * b, 0.5 * c);
+  points[2] = Vec3f(-0.5 * a, 0.5 * b, 0.5 * c);
+  points[3] = Vec3f(-0.5 * a, -0.5 * b, 0.5 * c);
+  points[4] = Vec3f(0.5 * a, -0.5 * b, -0.5 * c);
+  points[5] = Vec3f(0.5 * a, 0.5 * b, -0.5 * c);
+  points[6] = Vec3f(-0.5 * a, 0.5 * b, -0.5 * c);
+  points[7] = Vec3f(-0.5 * a, -0.5 * b, -0.5 * c);
+
+  for(unsigned int i = 0; i < points.size(); ++i)
+  {
+    Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
+    v = MxV(shape.getRotation(), v) + shape.getTranslation();
+    points[i] = v;
+  }
+
+  model.beginModel();
+  model.addSubModel(points);
+  model.endModel();
+  model.computeAABB();
+}
+
+/** Generate BVH model from sphere */
+template<typename BV>
+void generateBVHModelPointCloud(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
+{
+  std::vector<Vec3f> points;
+
+  double r = shape.radius;
+  double phi, phid;
+  const double pi = boost::math::constants::pi<double>();
+  phid = pi * 2 / seg;
+  phi = 0;
+
+  double theta, thetad;
+  thetad = pi / (ring + 1);
+  theta = 0;
+
+  for(unsigned int i = 0; i < ring; ++i)
+  {
+    double theta_ = theta + thetad * (i + 1);
+    for(unsigned int j = 0; j < seg; ++j)
+    {
+      points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
+    }
+  }
+  points.push_back(Vec3f(0, 0, r));
+  points.push_back(Vec3f(0, 0, -r));
+
+  for(unsigned int i = 0; i < points.size(); ++i)
+  {
+    Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
+    v = MxV(shape.getRotation(), v) + shape.getTranslation();
+    points[i] = v;
+  }
+
+  model.beginModel();
+  model.addSubModel(points);
+  model.endModel();
+  model.computeAABB();
+}
+
+
+/** \brief Generate BVH model from cylinder */
+template<typename BV>
+void generateBVHModelPointCloud(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
+{
+  std::vector<Vec3f> points;
+
+  double r = shape.radius;
+  double h = shape.lz;
+  double phi, phid;
+  const double pi = boost::math::constants::pi<double>();
+  phid = pi * 2 / tot;
+  phi = 0;
+
+  double circle_edge = phid * r;
+  unsigned int h_num = ceil(h / circle_edge);
+  double hd = h / h_num;
+
+  for(unsigned int i = 0; i < tot; ++i)
+    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
+
+  for(unsigned int i = 0; i < h_num - 1; ++i)
+  {
+    for(unsigned int j = 0; j < tot; ++j)
+    {
+      points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
+    }
+  }
+
+  for(unsigned int i = 0; i < tot; ++i)
+    points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
+
+  points.push_back(Vec3f(0, 0, h / 2));
+  points.push_back(Vec3f(0, 0, -h / 2));
+
+  model.beginModel();
+  model.addSubModel(points);
+  model.endModel();
+  model.computeAABB();
+}
+
+
+
 }
 
 #endif
diff --git a/branches/point_cloud/fcl/manifest.xml b/branches/point_cloud/fcl/manifest.xml
index 370fb5d3570138e9f5fd74029aec07b2a743f33f..ac01d1469e0742eac956c069e0ceaba73221909e 100644
--- a/branches/point_cloud/fcl/manifest.xml
+++ b/branches/point_cloud/fcl/manifest.xml
@@ -10,8 +10,8 @@
   <url>http://ros.org/wiki/fcl</url>
   <depend package="libccd"/>
   <depend package="ann"/>
-  <!-- <depend package="PQP"/> -->
-  <!-- <depend package="svm_light"/> -->
+  <depend package="PQP"/>
+  <depend package="svm_light"/>
   <export>
     <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfcl"/>
   </export>
diff --git a/branches/point_cloud/fcl/src/BVH_utility.cpp b/branches/point_cloud/fcl/src/BVH_utility.cpp
index 5da59a9f961d7fe7cf0531414811754c8a2d653a..e76d0b038e613a98ce07ec7147c949618c0f0e2e 100644
--- a/branches/point_cloud/fcl/src/BVH_utility.cpp
+++ b/branches/point_cloud/fcl/src/BVH_utility.cpp
@@ -65,7 +65,6 @@ void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r = 1.0)
 
     OBB bv;
     fit(vs, bvnode.num_primitives * 6, bv);
-
     delete [] vs;
 
     bvnode.bv = bv;
@@ -154,6 +153,7 @@ void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty*
     double r = dists[knn_k - 1];
     double sigma = scale * r;
 
+
     double weight_sum = 0;
     for(int j = 1; j < knn_k; ++j)
     {
@@ -182,6 +182,21 @@ void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty*
 
     ucs[i].preprocess();
     ucs[i].sqrt();
+
+    /*
+    sigma = dists[0] * 2 * r;
+    ucs[i].axis[0] = Vec3f(1, 0, 0);
+    ucs[i].axis[1] = Vec3f(0, 1, 0);
+    ucs[i].axis[2] = Vec3f(0, 0, 1);
+    ucs[i].sigma[0] = sigma;
+    ucs[i].sigma[1] = sigma;
+    ucs[i].sigma[2] = sigma;
+
+    ucs[i].Sigma[0] = Vec3f(sigma, 0, 0);
+    ucs[i].Sigma[1] = Vec3f(0, sigma, 0);
+    ucs[i].Sigma[2] = Vec3f(0, 0, sigma);
+    */
+
   }
 
   delete [] nnIdx;
diff --git a/branches/point_cloud/fcl/src/collision.cpp b/branches/point_cloud/fcl/src/collision.cpp
index 54ee44ba7410e45e69ceabb48246d9a04e0ddf19..2ce4ee0bc3f86e3884f8e07c3383c831b01c3d12 100644
--- a/branches/point_cloud/fcl/src/collision.cpp
+++ b/branches/point_cloud/fcl/src/collision.cpp
@@ -34,7 +34,7 @@
 
 /** \author Jia Pan */
 
-
+/*
 #include "fcl/collision.h"
 #include "fcl/collision_func_matrix.h"
 
@@ -43,7 +43,6 @@
 namespace fcl
 {
 
-
 static CollisionFunctionMatrix CollisionFunctionLookTable;
 
 int collide(const CollisionObject* o1, const CollisionObject* o2,
@@ -87,3 +86,63 @@ int collide(const CollisionObject* o1, const CollisionObject* o2,
 }
 
 }
+
+*/
+
+#include "fcl/collision.h"
+#include "fcl/simple_setup.h"
+#include "fcl/geometric_shapes.h"
+#include "fcl/BVH_model.h"
+#include "fcl/collision_node.h"
+#include "fcl/geometric_shapes_intersect.h"
+
+namespace fcl
+{
+
+int collide(const CollisionObject* o1, const CollisionObject* o2,
+             int num_max_contacts, bool exhaustive, bool enable_contact,
+             std::vector<Contact>& contacts)
+{
+  const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
+  const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
+
+  PointCloudCollisionTraversalNodeOBB node;
+
+  node.model1 = obj1;
+  node.model2 = obj2;
+  node.vertices1 = obj1->vertices;
+  node.vertices2 = obj2->vertices;
+
+
+
+  Uncertainty* uc1 = new Uncertainty[obj1->num_vertices];
+  Uncertainty* uc2 = new Uncertainty[obj2->num_vertices];
+  memcpy(uc1, obj1->uc, sizeof(Uncertainty) * obj1->num_vertices);
+  memcpy(uc2, obj2->uc, sizeof(Uncertainty) * obj2->num_vertices);
+
+  node.uc1.reset(uc1);
+  node.uc2.reset(uc2);
+
+  node.num_max_contacts = num_max_contacts;
+  node.exhaustive = exhaustive;
+  node.enable_contact = enable_contact;
+  node.collision_prob_threshold = 0.6;
+  node.leaf_size_threshold = 20;
+
+  relativeTransform(obj1->getRotation(), obj1->getTranslation(), obj2->getRotation(), obj2->getTranslation(), node.R, node.T);
+
+  collide(&node);
+  int num_contacts = node.pairs.size();
+  if(num_contacts > 0)
+  {
+    if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+    contacts.resize(num_contacts);
+    for(int i = 0; i < num_contacts; ++i)
+    {
+      contacts[i] = Contact(obj1, obj2, node.pairs[i].id1_start, node.pairs[i].id2_start);
+    }
+  }
+  return num_contacts;
+}
+
+}
diff --git a/branches/point_cloud/fcl/src/vec_3f.cpp b/branches/point_cloud/fcl/src/vec_3f.cpp
index c17fd4fd5d02e3d85ae38ff45020b6b76e4bb57a..cd0c15627ec4f75ebae88b67fb0d3aff226b775d 100644
--- a/branches/point_cloud/fcl/src/vec_3f.cpp
+++ b/branches/point_cloud/fcl/src/vec_3f.cpp
@@ -173,7 +173,7 @@ void Meigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3])
     }
   }
 
-  std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+  //std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
 
   return;
 }