From 003f1c354c24fa01d7de1325f8a551c1cefac805 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Tue, 30 Aug 2011 19:34:17 +0000
Subject: [PATCH] point cloud changes
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@20 253336fb-580f-4252-a368-f3cef5a2a82b
---
.../src/environmentFCL.cpp | 27 +++-
branches/point_cloud/fcl/CMakeLists.txt | 4 +-
.../point_cloud/fcl/include/fcl/BVH_model.h | 6 +
.../point_cloud/fcl/include/fcl/BVH_utility.h | 1 +
.../fcl/geometric_shape_to_BVH_model.h | 115 ++++++++++++++++++
branches/point_cloud/fcl/manifest.xml | 4 +-
branches/point_cloud/fcl/src/BVH_utility.cpp | 17 ++-
branches/point_cloud/fcl/src/collision.cpp | 63 +++++++++-
branches/point_cloud/fcl/src/vec_3f.cpp | 2 +-
9 files changed, 227 insertions(+), 12 deletions(-)
diff --git a/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp b/branches/point_cloud/collision_space_fcl/src/environmentFCL.cpp
index 2d8e93ab..1f4e8d00 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 82131b4c..d30f8cfe 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 c376869d..3084b1cc 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 8acb9c55..3cd45820 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 e5990f25..ef4f3217 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 370fb5d3..ac01d146 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 5da59a9f..e76d0b03 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 54ee44ba..2ce4ee0b 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 c17fd4fd..cd0c1562 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;
}
--
GitLab