Commit 89887e9c authored by panjia1983's avatar panjia1983
Browse files

penetration depth

parent 928b1756
......@@ -2,6 +2,7 @@ macro(add_fcl_test test_name)
add_executable(${ARGV})
target_link_libraries(${test_name}
fcl
${TINYXML_LIBRARY_DIRS}
${Boost_SYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY}
${Boost_DATE_TIME_LIBRARY}
......@@ -29,7 +30,12 @@ add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
endif()
if (FCL_HAVE_TINYXML)
add_fcl_test(test_fcl_xmldata test_fcl_xmldata.cpp test_fcl_utility.cpp libsvm/svm.cpp)
endif()
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
#include <fcl/shape/geometric_shapes.h>
#include <fcl/shape/geometric_shapes_utility.h>
#include <fcl/narrowphase/narrowphase.h>
#include <iostream>
#include <fcl/collision.h>
#include <boost/foreach.hpp>
using namespace std;
using namespace fcl;
int main(int argc, char** argv)
{
boost::shared_ptr<Box> box0(new Box(1,1,1));
boost::shared_ptr<Box> box1(new Box(1,1,1));
// GJKSolver_indep solver;
GJKSolver_libccd solver;
Vec3f contact_points;
FCL_REAL penetration_depth;
Vec3f normal;
Transform3f tf0, tf1;
tf0.setIdentity();
tf0.setTranslation(Vec3f(.9,0,0));
tf0.setQuatRotation(Quaternion3f(.6, .8, 0, 0));
tf1.setIdentity();
bool res = solver.shapeIntersect(*box0, tf0, *box1, tf1, &contact_points, &penetration_depth, &normal);
cout << "contact points: " << contact_points << endl;
cout << "pen depth: " << penetration_depth << endl;
cout << "normal: " << normal << endl;
cout << "result: " << res << endl;
static const int num_max_contacts = std::numeric_limits<int>::max();
static const bool enable_contact = true;
fcl::CollisionResult result;
fcl::CollisionRequest request(num_max_contacts,
enable_contact);
CollisionObject co0(box0, tf0);
CollisionObject co1(box1, tf1);
fcl::collide(&co0, &co1, request, result);
vector<Contact> contacts;
result.getContacts(contacts);
cout << contacts.size() << " contacts found" << endl;
BOOST_FOREACH(Contact& contact, contacts) {
cout << "position: " << contact.pos << endl;
}
}
\ No newline at end of file
#ifndef FCL_TEST_LIBSVM_CLASSIFIER_H
#define FCL_TEST_LIBSVM_CLASSIFIER_H
#include "fcl/learning/classifier.h"
#include <libsvm/svm.h>
namespace fcl
{
template<std::size_t N>
class LibSVMClassifier : public SVMClassifier<N>
{
public:
LibSVMClassifier()
{
param.svm_type = C_SVC;
param.kernel_type = RBF;
param.degree = 3;
param.gamma = 0; // 1/num_features
param.coef0 = 0;
param.nu = 0.5;
param.cache_size = 100; // can change
param.C = 1;
param.eps = 1e-3;
param.p = 0.1;
param.shrinking = 1; // use shrinking
param.probability = 0;
param.nr_weight = 0;
param.weight_label = NULL;
param.weight = NULL;
param.nr_weight = 2;
param.weight_label = (int *)realloc(param.weight_label, sizeof(int) * param.nr_weight);
param.weight = (double *)realloc(param.weight, sizeof(double) * param.nr_weight);
param.weight_label[0] = -1;
param.weight_label[1] = 1;
param.weight[0] = 1;
param.weight[1] = 1;
model = NULL;
x_space = NULL;
problem.x = NULL;
problem.y = NULL;
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; }
void setNu(FCL_REAL nu) { param.nu = nu; }
void setLinearClassifier() { param.kernel_type = LINEAR; }
void setNonLinearClassifier() { param.kernel_type = RBF; }
void setProbability(bool use_probability) { param.probability = use_probability; }
virtual void setScaler(const Scaler<N>& scaler_)
{
scaler = scaler_;
}
void setNegativeWeight(FCL_REAL c)
{
param.weight[0] = c;
}
void setPositiveWeight(FCL_REAL c)
{
param.weight[1] = c;
}
void setEPS(FCL_REAL e)
{
param.eps = e;
}
void setGamma(FCL_REAL gamma)
{
param.gamma = gamma;
}
~LibSVMClassifier()
{
svm_destroy_param(&param);
svm_free_and_destroy_model(&model);
delete [] x_space;
delete [] problem.x;
delete [] problem.y;
delete [] problem.W;
}
virtual void learn(const std::vector<Item<N> >& data)
{
if(data.size() == 0) return;
if(model) svm_free_and_destroy_model(&model);
if(param.gamma == 0) param.gamma = 1.0 / N;
problem.l = data.size();
if(problem.y) delete [] problem.y;
problem.y = new double [problem.l];
if(problem.x) delete [] problem.x;
problem.x = new svm_node* [problem.l];
if(problem.W) delete [] problem.W;
problem.W = new double [problem.l];
if(x_space) delete [] x_space;
x_space = new svm_node [(N + 1) * problem.l];
for(std::size_t i = 0; i < data.size(); ++i)
{
svm_node* cur_x_space = x_space + (N + 1) * i;
Vecnf<N> q_scaled = scaler.scale(data[i].q);
for(std::size_t j = 0; j < N; ++j)
{
cur_x_space[j].index = j + 1;
cur_x_space[j].value = q_scaled[j];
}
cur_x_space[N].index = -1;
problem.x[i] = cur_x_space;
problem.y[i] = (data[i].label ? 1 : -1);
problem.W[i] = data[i].w;
}
model = svm_train(&problem, &param);
hyperw_normsqr = svm_hyper_w_normsqr_twoclass(model);
}
virtual std::vector<PredictResult> predict(const std::vector<Vecnf<N> >& qs) const
{
std::vector<PredictResult> predict_results;
int nr_class = svm_get_nr_class(model);
double* prob_estimates = NULL;
svm_node* x = (svm_node*)malloc((N + 1) * sizeof(svm_node));
if(param.probability)
prob_estimates = (double*)malloc(nr_class * sizeof(double));
Vecnf<N> v;
for(std::size_t i = 0; i < qs.size(); ++i)
{
v = scaler.scale(qs[i]);
for(std::size_t j = 0; j < N; ++j)
{
x[j].index = j + 1;
x[j].value = v[j];
}
x[N].index = -1;
double predict_label;
if(param.probability)
{
predict_label = svm_predict_probability(model, x, prob_estimates);
predict_label = (predict_label > 0) ? 1 : 0;
predict_results.push_back(PredictResult(predict_label, *prob_estimates));
}
else
{
predict_label = svm_predict(model, x);
predict_label = (predict_label > 0) ? 1 : 0;
predict_results.push_back(PredictResult(predict_label));
}
}
if(param.probability) free(prob_estimates);
free(x);
return predict_results;
}
virtual PredictResult predict(const Vecnf<N>& q) const
{
return (predict(std::vector<Vecnf<N> >(1, q)))[0];
}
void save(const std::string& filename) const
{
if(model)
svm_save_model(filename.c_str(), model);
}
virtual std::vector<Item<N> > getSupportVectors() const
{
std::vector<Item<N> > results;
Item<N> item;
for(std::size_t i = 0; i < (std::size_t)model->l; ++i)
{
for(std::size_t j = 0; j < N; ++j)
item.q[j] = model->SV[i][j].value;
item.q = scaler.unscale(item.q);
int id = model->sv_indices[i] - 1;
item.label = (problem.y[id] > 0);
results.push_back(item);
}
return results;
}
svm_parameter param;
svm_problem problem;
svm_node* x_space;
svm_model* model;
double hyperw_normsqr;
Scaler<N> scaler;
};
}
#endif
#define BOOST_TEST_MODULE "FCL_GLOBAL_PENETRATION"
#include <boost/test/unit_test.hpp>
#include "libsvm_classifier.h"
#include "fcl/penetration_depth.h"
#include <boost/filesystem.hpp>
#include "fcl_resources/config.h"
#include "test_fcl_utility.h"
#include "fcl/collision.h"
#include "fcl/BVH/BVH_model.h"
using namespace fcl;
BOOST_AUTO_TEST_CASE(global_penetration_test)
{
LibSVMClassifier<6> classifier;
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
BVHModel<OBBRSS>* m1 = new BVHModel<OBBRSS>();
BVHModel<OBBRSS>* m2 = new BVHModel<OBBRSS>();
m1->beginModel();
m1->addSubModel(p1, t1);
m1->endModel();
m2->beginModel();
m2->addSubModel(p2, t2);
m2->endModel();
std::cout << m1->computeVolume() << std::endl;
std::cout << m2->computeVolume() << std::endl;
std::cout << m1->computeMomentofInertia() << std::endl;
std::cout << m2->computeMomentofInertia() << std::endl;
std::cout << m1->computeMomentofInertiaRelatedToCOM() << std::endl;
std::cout << m2->computeMomentofInertiaRelatedToCOM() << std::endl;
Transform3f id;
CollisionObject o1(boost::shared_ptr<CollisionGeometry>(m1), id);
CollisionObject o2(boost::shared_ptr<CollisionGeometry>(m2), id);
default_transform_distancer = DefaultTransformDistancer(o2.getCollisionGeometry());
std::size_t KNN_K = 10;
std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, PDT_GENERAL_EULER, &classifier, 10000, 0, KNN_GNAT, KNN_K);
classifier.save("model.txt");
PenetrationDepthRequest request(&classifier, default_transform_distance_func);
request.contact_vectors = contact_vectors;
PenetrationDepthResult result;
penetrationDepth(o1.getCollisionGeometry(), Transform3f(),
o2.getCollisionGeometry(), Transform3f(),
request, result);
std::cout << result.pd_value << std::endl;
}
#define BOOST_TEST_MODULE "FCL_SIMPLE"
#include <boost/test/unit_test.hpp>
#include "fcl/intersect.h"
#include "fcl/collision.h"
#include "fcl/BVH/BVH_model.h"
#include "fcl_resources/config.h"
#include <tinyxml.h>
#include <boost/filesystem.hpp>
#include <sstream>
#include "fcl/math/vec_nf.h"
#include "fcl/math/sampling.h"
#include "fcl/knn/nearest_neighbors_GNAT.h"
#include <boost/assign/list_of.hpp>
using namespace fcl;
static FCL_REAL epsilon = 1e-6;
static bool approx(FCL_REAL x, FCL_REAL y)
{
return abs(x - y) < epsilon;
}
template<std::size_t N>
double distance_Vecnf(const Vecnf<N>& a, const Vecnf<N>& b)
{
double d = 0;
for(std::size_t i = 0; i < N; ++i)
d += (a[i] - b[i]) * (a[i] - b[i]);
return d;
}
BOOST_AUTO_TEST_CASE(knn_test)
{
NearestNeighborsGNAT<Vecnf<6> > knn;
knn.setDistanceFunction(distance_Vecnf<6>);
SamplerSE3Euler sampler(Vec3f(-1, -1, -1), Vec3f(1, 1, 1));
std::vector<Vecnf<6> > data;
for(std::size_t i = 0; i < 1000; ++i)
data.push_back(sampler.sample());
knn.add(data);
int K = 10;
for(std::size_t i = 0; i < data.size(); ++i)
{
std::vector<Vecnf<6> > nbh;
knn.nearestK(data[i], K, nbh);
for(std::size_t j = 0; j < nbh.size(); ++j)
std::cout << distance_Vecnf<6>(nbh[j], data[i]) << " ";
std::cout << std::endl;
}
}
BOOST_AUTO_TEST_CASE(Vec_nf_test)
{
Vecnf<4> a;
Vecnf<4> b;
for(std::size_t i = 0; i < a.dim(); ++i)
a[i] = i;
for(std::size_t i = 0; i < b.dim(); ++i)
b[i] = 1;
std::cout << a << std::endl;
std::cout << b << std::endl;
std::cout << a + b << std::endl;
std::cout << a - b << std::endl;
std::cout << (a -= b) << std::endl;
std::cout << (a += b) << std::endl;
std::cout << a * 2 << std::endl;
std::cout << a / 2 << std::endl;
std::cout << (a *= 2) << std::endl;
std::cout << (a /= 2) << std::endl;
std::cout << a.dot(b) << std::endl;
Vecnf<8> c = combine(a, b);
std::cout << c << std::endl;
Vecnf<4> upper, lower;
for(int i = 0; i < 4; ++i)
upper[i] = 1;
Vecnf<4> aa(boost::assign::list_of<FCL_REAL>(1)(2).convert_to_container<std::vector<FCL_REAL> >());
std::cout << aa << std::endl;
SamplerR<4> sampler(lower, upper);
for(std::size_t i = 0; i < 10; ++i)
std::cout << sampler.sample() << std::endl;
SamplerSE2 sampler2(0, 1, -1, 1);
for(std::size_t i = 0; i < 10; ++i)
std::cout << sampler2.sample() << std::endl;
SamplerSE3Euler sampler3(Vec3f(0, 0, 0), Vec3f(1, 1, 1));
for(std::size_t i = 0; i < 10; ++i)
std::cout << sampler3.sample() << std::endl;
}
BOOST_AUTO_TEST_CASE(projection_test_line)
{
Vec3f v1(0, 0, 0);
Vec3f v2(2, 0, 0);
Vec3f p(1, 0, 0);
Project::ProjectResult res = Project::projectLine(v1, v2, p);
BOOST_CHECK(res.encode == 3);
BOOST_CHECK(approx(res.sqr_distance, 0));
BOOST_CHECK(approx(res.parameterization[0], 0.5));
BOOST_CHECK(approx(res.parameterization[1], 0.5));
p = Vec3f(-1, 0, 0);
res = Project::projectLine(v1, v2, p);
BOOST_CHECK(res.encode == 1);
BOOST_CHECK(approx(res.sqr_distance, 1));
BOOST_CHECK(approx(res.parameterization[0], 1));
BOOST_CHECK(approx(res.parameterization[1], 0));
p = Vec3f(3, 0, 0);
res = Project::projectLine(v1, v2, p);
BOOST_CHECK(res.encode == 2);
BOOST_CHECK(approx(res.sqr_distance, 1));
BOOST_CHECK(approx(res.parameterization[0], 0));
BOOST_CHECK(approx(res.parameterization[1], 1));
}
BOOST_AUTO_TEST_CASE(projection_test_triangle)
{
Vec3f v1(0, 0, 1);
Vec3f v2(0, 1, 0);
Vec3f v3(1, 0, 0);
Vec3f p(1, 1, 1);
Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 7);
BOOST_CHECK(approx(res.sqr_distance, 4/3.0));
BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
p = Vec3f(0, 0, 1.5);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 1);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
BOOST_CHECK(approx(res.parameterization[0], 1));
BOOST_CHECK(approx(res.parameterization[1], 0));
BOOST_CHECK(approx(res.parameterization[2], 0));
p = Vec3f(1.5, 0, 0);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 4);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
BOOST_CHECK(approx(res.parameterization[0], 0));
BOOST_CHECK(approx(res.parameterization[1], 0));
BOOST_CHECK(approx(res.parameterization[2], 1));
p = Vec3f(0, 1.5, 0);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 2);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
BOOST_CHECK(approx(res.parameterization[0], 0));
BOOST_CHECK(approx(res.parameterization[1], 1));
BOOST_CHECK(approx(res.parameterization[2], 0));
p = Vec3f(1, 1, 0);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 6);
BOOST_CHECK(approx(res.sqr_distance, 0.5));
BOOST_CHECK(approx(res.parameterization[0], 0));
BOOST_CHECK(approx(res.parameterization[1], 0.5));
BOOST_CHECK(approx(res.parameterization[2], 0.5));
p = Vec3f(1, 0, 1);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 5);
BOOST_CHECK(approx(res.sqr_distance, 0.5));
BOOST_CHECK(approx(res.parameterization[0], 0.5));
BOOST_CHECK(approx(res.parameterization[1], 0));
BOOST_CHECK(approx(res.parameterization[2], 0.5));
p = Vec3f(0, 1, 1);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 3);
BOOST_CHECK(approx(res.sqr_distance, 0.5));
BOOST_CHECK(approx(res.parameterization[0], 0.5));
BOOST_CHECK(approx(res.parameterization[1], 0.5));
BOOST_CHECK(approx(res.parameterization[2], 0));
}
BOOST_AUTO_TEST_CASE(projection_test_tetrahedron)
{
Vec3f v1(0, 0, 1);
Vec3f v2(0, 1, 0);
Vec3f v3(1, 0, 0);
Vec3f v4(1, 1, 1);
Vec3f p(0.5, 0.5, 0.5);
Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 15);
BOOST_CHECK(approx(res.sqr_distance, 0));
BOOST_CHECK(approx(res.parameterization[0], 0.25));
BOOST_CHECK(approx(res.parameterization[1], 0.25));
BOOST_CHECK(approx(res.parameterization[2], 0.25));
BOOST_CHECK(approx(res.parameterization[3], 0.25));
p = Vec3f(0, 0, 0);