void octomap_distance_test_BVH(std::size_t n, double resolution) { using S = typename BV::S; std::vector<Vector3<S>> p1; std::vector<Triangle> t1; test::loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); BVHModel<BV>* m1 = new BVHModel<BV>(); std::shared_ptr<CollisionGeometry<S>> m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); OcTree<S>* tree = new OcTree<S>(std::shared_ptr<octomap::OcTree>(test::generateOcTree(resolution))); std::shared_ptr<CollisionGeometry<S>> tree_ptr(tree); Eigen::aligned_vector<Transform3<S>> transforms; S extents[] = {-10, -10, 10, 10, 10, 10}; test::generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Transform3<S> tf(transforms[i]); CollisionObject<S> obj1(m1_ptr, tf); CollisionObject<S> obj2(tree_ptr, tf); test::DistanceData<S> cdata; S dist1 = std::numeric_limits<S>::max(); test::defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); std::vector<CollisionObject<S>*> boxes; test::generateBoxesFromOctomap(boxes, *tree); for(std::size_t j = 0; j < boxes.size(); ++j) boxes[j]->setTransform(tf * boxes[j]->getTransform()); DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>(); manager->registerObjects(boxes); manager->setup(); test::DistanceData<S> cdata2; manager->distance(&obj1, &cdata2, test::defaultDistanceFunction); S dist2 = cdata2.result.min_distance; for(std::size_t j = 0; j < boxes.size(); ++j) delete boxes[j]; delete manager; std::cout << dist1 << " " << dist2 << std::endl; EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001); } }
void octomap_distance_test_BVH(std::size_t n) { std::vector<Vec3f> p1; std::vector<Triangle> t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel<BV>* m1 = new BVHModel<BV>(); boost::shared_ptr<CollisionGeometry> m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree())); boost::shared_ptr<CollisionGeometry> tree_ptr(tree); std::vector<Transform3f> transforms; FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Transform3f tf(transforms[i]); CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); DistanceData cdata; FCL_REAL dist1 = std::numeric_limits<FCL_REAL>::max(); defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); std::vector<CollisionObject*> boxes; generateBoxesFromOctomap(boxes, *tree); for(std::size_t j = 0; j < boxes.size(); ++j) boxes[j]->setTransform(tf * boxes[j]->getTransform()); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(boxes); manager->setup(); DistanceData cdata2; manager->distance(&obj1, &cdata2, defaultDistanceFunction); FCL_REAL dist2 = cdata2.result.min_distance; for(std::size_t j = 0; j < boxes.size(); ++j) delete boxes[j]; delete manager; std::cout << dist1 << " " << dist2 << std::endl; BOOST_CHECK(std::abs(dist1 - dist2) < 0.001); } }
void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution) { // srand(1); std::vector<CollisionObject<S>*> env; if(use_mesh) test::generateEnvironmentsMesh(env, env_scale, env_size); else test::generateEnvironments(env, env_scale, env_size); OcTree<S>* tree = new OcTree<S>(std::shared_ptr<const octomap::OcTree>(test::generateOcTree(resolution))); CollisionObject<S> tree_obj((std::shared_ptr<CollisionGeometry<S>>(tree))); DynamicAABBTreeCollisionManager<S>* manager = new DynamicAABBTreeCollisionManager<S>(); manager->registerObjects(env); manager->setup(); test::DistanceData<S> cdata; test::TStruct t1; test::Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->distance(&tree_obj, &cdata, test::defaultDistanceFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); test::DistanceData<S> cdata3; test::TStruct t3; test::Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; manager->distance(&tree_obj, &cdata3, test::defaultDistanceFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); test::TStruct t2; test::Timer timer2; timer2.start(); std::vector<CollisionObject<S>*> boxes; if(use_mesh_octomap) test::generateBoxesFromOctomapMesh(boxes, *tree); else test::generateBoxesFromOctomap(boxes, *tree); timer2.stop(); t2.push_back(timer2.getElapsedTime()); timer2.start(); DynamicAABBTreeCollisionManager<S>* manager2 = new DynamicAABBTreeCollisionManager<S>(); manager2->registerObjects(boxes); manager2->setup(); timer2.stop(); t2.push_back(timer2.getElapsedTime()); test::DistanceData<S> cdata2; timer2.start(); manager->distance(manager2, &cdata2, test::defaultDistanceFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl; if(cdata.result.min_distance < 0) EXPECT_TRUE(cdata2.result.min_distance <= 0); else EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3); delete manager; delete manager2; for(size_t i = 0; i < boxes.size(); ++i) delete boxes[i]; std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; std::cout << " a) to boxes: " << t2.records[0] << std::endl; std::cout << " b) structure init: " << t2.records[1] << std::endl; std::cout << " c) distance: " << t2.records[2] << std::endl; std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; }
void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap) { // srand(1); std::vector<CollisionObject*> env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree())); CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); manager->setup(); CollisionData cdata; if(exhaustive) cdata.request.num_max_contacts = 100000; else cdata.request.num_max_contacts = num_max_contacts; TStruct t1; Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->collide(&tree_obj, &cdata, defaultCollisionFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); CollisionData cdata3; if(exhaustive) cdata3.request.num_max_contacts = 100000; else cdata3.request.num_max_contacts = num_max_contacts; TStruct t3; Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); TStruct t2; Timer timer2; timer2.start(); std::vector<CollisionObject*> boxes; if(use_mesh_octomap) generateBoxesFromOctomapMesh(boxes, *tree); else generateBoxesFromOctomap(boxes, *tree); timer2.stop(); t2.push_back(timer2.getElapsedTime()); timer2.start(); DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); manager2->registerObjects(boxes); manager2->setup(); timer2.stop(); t2.push_back(timer2.getElapsedTime()); CollisionData cdata2; if(exhaustive) cdata2.request.num_max_contacts = 100000; else cdata2.request.num_max_contacts = num_max_contacts; timer2.start(); manager->collide(manager2, &cdata2, defaultCollisionFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; if(exhaustive) { if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); else BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts()); } else { if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); else BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact } delete manager; delete manager2; for(size_t i = 0; i < boxes.size(); ++i) delete boxes[i]; if(exhaustive) std::cout << "exhaustive collision" << std::endl; else std::cout << "non exhaustive collision" << std::endl; std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; std::cout << " a) to boxes: " << t2.records[0] << std::endl; std::cout << " b) structure init: " << t2.records[1] << std::endl; std::cout << " c) collision: " << t2.records[2] << std::endl; std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; }
void octomap_collision_test_BVH(std::size_t n, bool exhaustive) { std::vector<Vec3f> p1; std::vector<Triangle> t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel<BV>* m1 = new BVHModel<BV>(); boost::shared_ptr<CollisionGeometry> m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree())); boost::shared_ptr<CollisionGeometry> tree_ptr(tree); std::vector<Transform3f> transforms; FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Transform3f tf(transforms[i]); CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); CollisionData cdata; if(exhaustive) cdata.request.num_max_contacts = 100000; defaultCollisionFunction(&obj1, &obj2, &cdata); std::vector<CollisionObject*> boxes; generateBoxesFromOctomap(boxes, *tree); for(std::size_t j = 0; j < boxes.size(); ++j) boxes[j]->setTransform(tf * boxes[j]->getTransform()); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(boxes); manager->setup(); CollisionData cdata2; if(exhaustive) cdata2.request.num_max_contacts = 100000; manager->collide(&obj1, &cdata2, defaultCollisionFunction); for(std::size_t j = 0; j < boxes.size(); ++j) delete boxes[j]; delete manager; if(exhaustive) { std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts()); } else { std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl; BOOST_CHECK((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0)); } } }
void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap) { std::vector<CollisionObject*> env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); OcTree* tree = new OcTree(std::shared_ptr<const octomap::OcTree>(generateOcTree())); CollisionObject tree_obj((std::shared_ptr<CollisionGeometry>(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); manager->setup(); CollisionData cdata; cdata.request.enable_cost = true; cdata.request.num_max_cost_sources = num_max_cost_sources; TStruct t1; Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->collide(&tree_obj, &cdata, defaultCollisionFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); CollisionData cdata3; cdata3.request.enable_cost = true; cdata3.request.num_max_cost_sources = num_max_cost_sources; TStruct t3; Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); TStruct t2; Timer timer2; timer2.start(); std::vector<CollisionObject*> boxes; if(use_mesh_octomap) generateBoxesFromOctomapMesh(boxes, *tree); else generateBoxesFromOctomap(boxes, *tree); timer2.stop(); t2.push_back(timer2.getElapsedTime()); timer2.start(); DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); manager2->registerObjects(boxes); manager2->setup(); timer2.stop(); t2.push_back(timer2.getElapsedTime()); CollisionData cdata2; cdata2.request.enable_cost = true; cdata3.request.num_max_cost_sources = num_max_cost_sources; timer2.start(); manager->collide(manager2, &cdata2, defaultCollisionFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl; { std::vector<CostSource> cost_sources; cdata.result.getCostSources(cost_sources); for(std::size_t i = 0; i < cost_sources.size(); ++i) { std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl; } std::cout << std::endl; cdata3.result.getCostSources(cost_sources); for(std::size_t i = 0; i < cost_sources.size(); ++i) { std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl; } std::cout << std::endl; cdata2.result.getCostSources(cost_sources); for(std::size_t i = 0; i < cost_sources.size(); ++i) { std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl; } std::cout << std::endl; } if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); else EXPECT_TRUE(cdata.result.numContacts() >= cdata2.result.numContacts()); delete manager; delete manager2; for(std::size_t i = 0; i < boxes.size(); ++i) delete boxes[i]; std::cout << "collision cost" << std::endl; std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; std::cout << " a) to boxes: " << t2.records[0] << std::endl; std::cout << " b) structure init: " << t2.records[1] << std::endl; std::cout << " c) collision: " << t2.records[2] << std::endl; std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; }