test_fcl_octomap.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #define BOOST_TEST_MODULE "FCL_OCTOMAP"
00038 #include <boost/test/unit_test.hpp>
00039 
00040 #include "fcl/octree.h"
00041 #include "fcl/traversal/traversal_node_octree.h"
00042 #include "fcl/broadphase/broadphase.h"
00043 #include "fcl/shape/geometric_shape_to_BVH_model.h"
00044 #include "fcl/math/transform.h"
00045 #include "test_fcl_utility.h"
00046 #include "fcl_resources/config.h"
00047 #include <boost/filesystem.hpp>
00048 
00049 using namespace fcl;
00050 
00051 
00052 struct TStruct
00053 {
00054   std::vector<double> records;
00055   double overall_time;
00056 
00057   TStruct() { overall_time = 0; }
00058   
00059   void push_back(double t)
00060   {
00061     records.push_back(t);
00062     overall_time += t;
00063   }
00064 };
00065 
00067 void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
00068 
00070 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
00071 
00073 void generateBoxesFromOctomap(std::vector<CollisionObject*>& env, OcTree& tree);
00074 
00076 void generateBoxesFromOctomapMesh(std::vector<CollisionObject*>& env, OcTree& tree);
00077 
00079 octomap::OcTree* generateOcTree();
00080 
00082 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);
00083 
00085 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);
00086 
00088 void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap);
00089 
00090 
00091 template<typename BV>
00092 void octomap_collision_test_BVH(std::size_t n, bool exhaustive);
00093 
00094 
00095 template<typename BV>
00096 void octomap_distance_test_BVH(std::size_t n);
00097 
00098 BOOST_AUTO_TEST_CASE(test_octomap_cost)
00099 {
00100   octomap_cost_test(200, 100, 10, false, false);
00101   octomap_cost_test(200, 1000, 10, false, false);
00102 }
00103 
00104 BOOST_AUTO_TEST_CASE(test_octomap_cost_mesh)
00105 {
00106   octomap_cost_test(200, 100, 10, true, false);
00107   octomap_cost_test(200, 1000, 10, true, false);
00108 }
00109 
00110 BOOST_AUTO_TEST_CASE(test_octomap_collision)
00111 {
00112   octomap_collision_test(200, 100, false, 10, false, false);
00113   octomap_collision_test(200, 1000, false, 10, false, false);
00114   octomap_collision_test(200, 100, true, 1, false, false);
00115   octomap_collision_test(200, 1000, true, 1, false, false);
00116 }
00117 
00118 BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh)
00119 {
00120   octomap_collision_test(200, 100, false, 10, true, true);
00121   octomap_collision_test(200, 1000, false, 10, true, true);
00122   octomap_collision_test(200, 100, true, 1, true, true);
00123   octomap_collision_test(200, 1000, true, 1, true, true);
00124 }
00125 
00126 BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh_octomap_box)
00127 {
00128   octomap_collision_test(200, 100, false, 10, true, false);
00129   octomap_collision_test(200, 1000, false, 10, true, false);
00130   octomap_collision_test(200, 100, true, 1, true, false);
00131   octomap_collision_test(200, 1000, true, 1, true, false);
00132 }
00133 
00134 BOOST_AUTO_TEST_CASE(test_octomap_distance)
00135 {
00136   octomap_distance_test(200, 100, false, false);
00137   octomap_distance_test(200, 1000, false, false);
00138 }
00139 
00140 BOOST_AUTO_TEST_CASE(test_octomap_distance_mesh)
00141 {
00142   octomap_distance_test(200, 100, true, true);
00143   octomap_distance_test(200, 1000, true, true);
00144 }
00145 
00146 BOOST_AUTO_TEST_CASE(test_octomap_distance_mesh_octomap_box)
00147 {
00148   octomap_distance_test(200, 100, true, false);
00149   octomap_distance_test(200, 1000, true, false);
00150 }
00151 
00152 
00153 BOOST_AUTO_TEST_CASE(test_octomap_bvh_obb_collision_obb)
00154 {
00155   octomap_collision_test_BVH<OBB>(5, false);
00156   octomap_collision_test_BVH<OBB>(5, true);
00157 }
00158 
00159 BOOST_AUTO_TEST_CASE(test_octomap_bvh_rss_d_distance_rss)
00160 {
00161   octomap_distance_test_BVH<RSS>(5);
00162 }
00163 
00164 BOOST_AUTO_TEST_CASE(test_octomap_bvh_obb_d_distance_obb)
00165 {
00166   octomap_distance_test_BVH<OBBRSS>(5);
00167 }
00168 
00169 BOOST_AUTO_TEST_CASE(test_octomap_bvh_kios_d_distance_kios)
00170 {
00171   octomap_distance_test_BVH<kIOS>(5);
00172 }
00173 
00174 template<typename BV>
00175 void octomap_collision_test_BVH(std::size_t n, bool exhaustive)
00176 {
00177   std::vector<Vec3f> p1;
00178   std::vector<Triangle> t1;
00179   boost::filesystem::path path(TEST_RESOURCES_DIR);
00180   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
00181 
00182   BVHModel<BV>* m1 = new BVHModel<BV>();
00183   boost::shared_ptr<CollisionGeometry> m1_ptr(m1);
00184 
00185   m1->beginModel();
00186   m1->addSubModel(p1, t1);
00187   m1->endModel();
00188 
00189   OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
00190   boost::shared_ptr<CollisionGeometry> tree_ptr(tree);
00191 
00192   std::vector<Transform3f> transforms;
00193   FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10};
00194 
00195   generateRandomTransforms(extents, transforms, n);
00196   
00197   for(std::size_t i = 0; i < n; ++i)
00198   {
00199     Transform3f tf(transforms[i]);
00200 
00201     CollisionObject obj1(m1_ptr, tf);
00202     CollisionObject obj2(tree_ptr, tf);
00203     CollisionData cdata;
00204     if(exhaustive) cdata.request.num_max_contacts = 100000;
00205 
00206     defaultCollisionFunction(&obj1, &obj2, &cdata);
00207 
00208 
00209     std::vector<CollisionObject*> boxes;
00210     generateBoxesFromOctomap(boxes, *tree);
00211     for(std::size_t j = 0; j < boxes.size(); ++j)
00212       boxes[j]->setTransform(tf * boxes[j]->getTransform());
00213   
00214     DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
00215     manager->registerObjects(boxes);
00216     manager->setup();
00217 
00218     CollisionData cdata2;
00219     if(exhaustive) cdata2.request.num_max_contacts = 100000;
00220     manager->collide(&obj1, &cdata2, defaultCollisionFunction);
00221 
00222     for(std::size_t j = 0; j < boxes.size(); ++j)
00223       delete boxes[j];
00224     delete manager;
00225 
00226     if(exhaustive)
00227     {
00228       std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
00229       BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts());
00230     }
00231     else
00232     {
00233       std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl;
00234       BOOST_CHECK((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0));
00235     }
00236   }
00237 }
00238 
00239 
00240 template<typename BV>
00241 void octomap_distance_test_BVH(std::size_t n)
00242 {
00243   std::vector<Vec3f> p1;
00244   std::vector<Triangle> t1;
00245   boost::filesystem::path path(TEST_RESOURCES_DIR);
00246   loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
00247 
00248   BVHModel<BV>* m1 = new BVHModel<BV>();
00249   boost::shared_ptr<CollisionGeometry> m1_ptr(m1);
00250 
00251   m1->beginModel();
00252   m1->addSubModel(p1, t1);
00253   m1->endModel();
00254 
00255   OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
00256   boost::shared_ptr<CollisionGeometry> tree_ptr(tree);
00257 
00258   std::vector<Transform3f> transforms;
00259   FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10};
00260 
00261   generateRandomTransforms(extents, transforms, n);
00262   
00263   for(std::size_t i = 0; i < n; ++i)
00264   {
00265     Transform3f tf(transforms[i]);
00266 
00267     CollisionObject obj1(m1_ptr, tf);
00268     CollisionObject obj2(tree_ptr, tf);
00269     DistanceData cdata;
00270     FCL_REAL dist1 = std::numeric_limits<FCL_REAL>::max();
00271     defaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
00272 
00273 
00274     std::vector<CollisionObject*> boxes;
00275     generateBoxesFromOctomap(boxes, *tree);
00276     for(std::size_t j = 0; j < boxes.size(); ++j)
00277       boxes[j]->setTransform(tf * boxes[j]->getTransform());
00278   
00279     DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
00280     manager->registerObjects(boxes);
00281     manager->setup();
00282 
00283     DistanceData cdata2;
00284     manager->distance(&obj1, &cdata2, defaultDistanceFunction);
00285     FCL_REAL dist2 = cdata2.result.min_distance;
00286 
00287     for(std::size_t j = 0; j < boxes.size(); ++j)
00288       delete boxes[j];
00289     delete manager;
00290 
00291     std::cout << dist1 << " " << dist2 << std::endl;
00292     BOOST_CHECK(std::abs(dist1 - dist2) < 0.001);
00293   }
00294 }
00295 
00296 
00297 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)
00298 {
00299   std::vector<CollisionObject*> env;
00300   if(use_mesh)
00301     generateEnvironmentsMesh(env, env_scale, env_size);
00302   else
00303     generateEnvironments(env, env_scale, env_size);
00304 
00305   OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
00306   CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));
00307 
00308   DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
00309   manager->registerObjects(env);
00310   manager->setup();
00311   
00312   CollisionData cdata;
00313   cdata.request.enable_cost = true;
00314   cdata.request.num_max_cost_sources = num_max_cost_sources;
00315 
00316   TStruct t1;
00317   Timer timer1;
00318   timer1.start();
00319   manager->octree_as_geometry_collide = false;
00320   manager->octree_as_geometry_distance = false;
00321   manager->collide(&tree_obj, &cdata, defaultCollisionFunction);
00322   timer1.stop();
00323   t1.push_back(timer1.getElapsedTime());
00324 
00325   CollisionData cdata3;
00326   cdata3.request.enable_cost = true;
00327   cdata3.request.num_max_cost_sources = num_max_cost_sources;
00328 
00329   TStruct t3;
00330   Timer timer3;
00331   timer3.start();
00332   manager->octree_as_geometry_collide = true;
00333   manager->octree_as_geometry_distance = true;
00334   manager->collide(&tree_obj, &cdata3, defaultCollisionFunction);
00335   timer3.stop();
00336   t3.push_back(timer3.getElapsedTime());
00337 
00338   TStruct t2;
00339   Timer timer2;
00340   timer2.start();
00341   std::vector<CollisionObject*> boxes;
00342   if(use_mesh_octomap)
00343     generateBoxesFromOctomapMesh(boxes, *tree);
00344   else
00345     generateBoxesFromOctomap(boxes, *tree);
00346   timer2.stop();
00347   t2.push_back(timer2.getElapsedTime());
00348   
00349   timer2.start();
00350   DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
00351   manager2->registerObjects(boxes);
00352   manager2->setup();
00353   timer2.stop();
00354   t2.push_back(timer2.getElapsedTime());
00355 
00356 
00357   CollisionData cdata2;
00358   cdata2.request.enable_cost = true;
00359   cdata3.request.num_max_cost_sources = num_max_cost_sources;
00360 
00361   timer2.start();
00362   manager->collide(manager2, &cdata2, defaultCollisionFunction);
00363   timer2.stop();
00364   t2.push_back(timer2.getElapsedTime());
00365 
00366   std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
00367   std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl;
00368 
00369   {
00370     std::vector<CostSource> cost_sources;
00371     cdata.result.getCostSources(cost_sources);
00372     for(std::size_t i = 0; i < cost_sources.size(); ++i)
00373     {
00374       std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
00375     }
00376 
00377     std::cout << std::endl;
00378 
00379     cdata3.result.getCostSources(cost_sources);
00380     for(std::size_t i = 0; i < cost_sources.size(); ++i)
00381     {
00382       std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
00383     }
00384 
00385     std::cout << std::endl;
00386 
00387     cdata2.result.getCostSources(cost_sources);
00388     for(std::size_t i = 0; i < cost_sources.size(); ++i)
00389     {
00390       std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
00391     }
00392 
00393     std::cout << std::endl;
00394 
00395   }
00396 
00397   if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
00398   else BOOST_CHECK(cdata.result.numContacts() >= cdata2.result.numContacts());
00399 
00400   delete manager;
00401   delete manager2;
00402   for(std::size_t i = 0; i < boxes.size(); ++i)
00403     delete boxes[i];
00404 
00405   std::cout << "collision cost" << std::endl;
00406   std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
00407   std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
00408   std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
00409   std::cout << "  a) to boxes: " << t2.records[0] << std::endl;
00410   std::cout << "  b) structure init: " << t2.records[1] << std::endl;
00411   std::cout << "  c) collision: " << t2.records[2] << std::endl;
00412   std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
00413 }
00414 
00415 
00416 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)
00417 {
00418   // srand(1);
00419   std::vector<CollisionObject*> env;
00420   if(use_mesh)
00421     generateEnvironmentsMesh(env, env_scale, env_size);
00422   else
00423     generateEnvironments(env, env_scale, env_size);
00424 
00425   OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
00426   CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));
00427 
00428   DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
00429   manager->registerObjects(env);
00430   manager->setup();
00431   
00432   CollisionData cdata;
00433   if(exhaustive) cdata.request.num_max_contacts = 100000;
00434   else cdata.request.num_max_contacts = num_max_contacts;
00435 
00436   TStruct t1;
00437   Timer timer1;
00438   timer1.start();
00439   manager->octree_as_geometry_collide = false;
00440   manager->octree_as_geometry_distance = false;
00441   manager->collide(&tree_obj, &cdata, defaultCollisionFunction);
00442   timer1.stop();
00443   t1.push_back(timer1.getElapsedTime());
00444 
00445   CollisionData cdata3;
00446   if(exhaustive) cdata3.request.num_max_contacts = 100000;
00447   else cdata3.request.num_max_contacts = num_max_contacts;
00448 
00449   TStruct t3;
00450   Timer timer3;
00451   timer3.start();
00452   manager->octree_as_geometry_collide = true;
00453   manager->octree_as_geometry_distance = true;
00454   manager->collide(&tree_obj, &cdata3, defaultCollisionFunction);
00455   timer3.stop();
00456   t3.push_back(timer3.getElapsedTime());
00457 
00458   TStruct t2;
00459   Timer timer2;
00460   timer2.start();
00461   std::vector<CollisionObject*> boxes;
00462   if(use_mesh_octomap)
00463     generateBoxesFromOctomapMesh(boxes, *tree);
00464   else
00465     generateBoxesFromOctomap(boxes, *tree);
00466   timer2.stop();
00467   t2.push_back(timer2.getElapsedTime());
00468   
00469   timer2.start();
00470   DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
00471   manager2->registerObjects(boxes);
00472   manager2->setup();
00473   timer2.stop();
00474   t2.push_back(timer2.getElapsedTime());
00475 
00476 
00477   CollisionData cdata2;
00478   if(exhaustive) cdata2.request.num_max_contacts = 100000;
00479   else cdata2.request.num_max_contacts = num_max_contacts;
00480 
00481   timer2.start();
00482   manager->collide(manager2, &cdata2, defaultCollisionFunction);
00483   timer2.stop();
00484   t2.push_back(timer2.getElapsedTime());
00485 
00486   std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
00487   if(exhaustive)
00488   {
00489     if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
00490     else BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts());
00491   }
00492   else
00493   {
00494     if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
00495     else BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact
00496   }
00497 
00498   delete manager;
00499   delete manager2;
00500   for(size_t i = 0; i < boxes.size(); ++i)
00501     delete boxes[i];
00502 
00503   if(exhaustive) std::cout << "exhaustive collision" << std::endl;
00504   else std::cout << "non exhaustive collision" << std::endl;
00505   std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
00506   std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
00507   std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
00508   std::cout << "  a) to boxes: " << t2.records[0] << std::endl;
00509   std::cout << "  b) structure init: " << t2.records[1] << std::endl;
00510   std::cout << "  c) collision: " << t2.records[2] << std::endl;
00511   std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
00512 }
00513 
00514 
00515 void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap)
00516 {
00517   // srand(1);
00518   std::vector<CollisionObject*> env;
00519   if(use_mesh)
00520     generateEnvironmentsMesh(env, env_scale, env_size);
00521   else
00522     generateEnvironments(env, env_scale, env_size);
00523 
00524   OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
00525   CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));
00526 
00527   DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
00528   manager->registerObjects(env);
00529   manager->setup();
00530   
00531   DistanceData cdata;
00532   TStruct t1;
00533   Timer timer1;
00534   timer1.start();
00535   manager->octree_as_geometry_collide = false;
00536   manager->octree_as_geometry_distance = false;
00537   manager->distance(&tree_obj, &cdata, defaultDistanceFunction);
00538   timer1.stop();
00539   t1.push_back(timer1.getElapsedTime());
00540 
00541   
00542   DistanceData cdata3;
00543   TStruct t3;
00544   Timer timer3;
00545   timer3.start();
00546   manager->octree_as_geometry_collide = true;
00547   manager->octree_as_geometry_distance = true;
00548   manager->distance(&tree_obj, &cdata3, defaultDistanceFunction);
00549   timer3.stop();
00550   t3.push_back(timer3.getElapsedTime());
00551   
00552 
00553   TStruct t2;
00554   Timer timer2;
00555   timer2.start();
00556   std::vector<CollisionObject*> boxes;
00557   if(use_mesh_octomap)
00558     generateBoxesFromOctomapMesh(boxes, *tree);
00559   else
00560     generateBoxesFromOctomap(boxes, *tree);
00561   timer2.stop();
00562   t2.push_back(timer2.getElapsedTime());
00563   
00564   timer2.start();
00565   DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
00566   manager2->registerObjects(boxes);
00567   manager2->setup();
00568   timer2.stop();
00569   t2.push_back(timer2.getElapsedTime());
00570 
00571 
00572   DistanceData cdata2;
00573 
00574   timer2.start();
00575   manager->distance(manager2, &cdata2, defaultDistanceFunction);
00576   timer2.stop();
00577   t2.push_back(timer2.getElapsedTime());
00578 
00579   std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl;
00580 
00581   if(cdata.result.min_distance < 0)
00582     BOOST_CHECK(cdata2.result.min_distance <= 0);
00583   else
00584     BOOST_CHECK(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3);
00585 
00586   delete manager;
00587   delete manager2;
00588   for(size_t i = 0; i < boxes.size(); ++i)
00589     delete boxes[i];
00590 
00591 
00592   std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
00593   std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
00594   std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
00595   std::cout << "  a) to boxes: " << t2.records[0] << std::endl;
00596   std::cout << "  b) structure init: " << t2.records[1] << std::endl;
00597   std::cout << "  c) distance: " << t2.records[2] << std::endl;
00598   std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
00599 }
00600 
00601 
00602 
00603 void generateBoxesFromOctomap(std::vector<CollisionObject*>& boxes, OcTree& tree)
00604 {
00605   std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
00606 
00607   for(std::size_t i = 0; i < boxes_.size(); ++i)
00608   {
00609     FCL_REAL x = boxes_[i][0];
00610     FCL_REAL y = boxes_[i][1];
00611     FCL_REAL z = boxes_[i][2];
00612     FCL_REAL size = boxes_[i][3];
00613     FCL_REAL cost = boxes_[i][4];
00614     FCL_REAL threshold = boxes_[i][5];
00615 
00616     Box* box = new Box(size, size, size);
00617     box->cost_density = cost;
00618     box->threshold_occupied = threshold;
00619     CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), Transform3f(Vec3f(x, y, z)));
00620     boxes.push_back(obj);
00621   }
00622 
00623   std::cout << "boxes size: " << boxes.size() << std::endl;
00624 
00625 }
00626 
00627 void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
00628 {
00629   FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
00630   std::vector<Transform3f> transforms;
00631 
00632   generateRandomTransforms(extents, transforms, n);
00633   for(std::size_t i = 0; i < n; ++i)
00634   {
00635     Box* box = new Box(5, 10, 20);
00636     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), transforms[i]));
00637   }
00638 
00639   generateRandomTransforms(extents, transforms, n);
00640   for(std::size_t i = 0; i < n; ++i)
00641   {
00642     Sphere* sphere = new Sphere(30);
00643     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(sphere), transforms[i]));
00644   }
00645 
00646   generateRandomTransforms(extents, transforms, n);
00647   for(std::size_t i = 0; i < n; ++i)
00648   {
00649     Cylinder* cylinder = new Cylinder(10, 40);
00650     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(cylinder), transforms[i]));
00651   }
00652 
00653 }
00654 
00655 
00656 void generateBoxesFromOctomapMesh(std::vector<CollisionObject*>& boxes, OcTree& tree)
00657 {
00658   std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
00659 
00660   for(std::size_t i = 0; i < boxes_.size(); ++i)
00661   {
00662     FCL_REAL x = boxes_[i][0];
00663     FCL_REAL y = boxes_[i][1];
00664     FCL_REAL z = boxes_[i][2];
00665     FCL_REAL size = boxes_[i][3];
00666     FCL_REAL cost = boxes_[i][4];
00667     FCL_REAL threshold = boxes_[i][5];
00668 
00669     Box box(size, size, size);
00670     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
00671     generateBVHModel(*model, box, Transform3f());
00672     model->cost_density = cost;
00673     model->threshold_occupied = threshold;
00674     CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), Transform3f(Vec3f(x, y, z)));
00675     boxes.push_back(obj);    
00676   }
00677 
00678   std::cout << "boxes size: " << boxes.size() << std::endl;
00679 }
00680 
00681 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
00682 {
00683   FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
00684   std::vector<Transform3f> transforms;
00685 
00686   generateRandomTransforms(extents, transforms, n);
00687   Box box(5, 10, 20);
00688   for(std::size_t i = 0; i < n; ++i)
00689   {
00690     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
00691     generateBVHModel(*model, box, Transform3f());
00692     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
00693   }
00694 
00695   generateRandomTransforms(extents, transforms, n);
00696   Sphere sphere(30);
00697   for(std::size_t i = 0; i < n; ++i)
00698   {
00699     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
00700     generateBVHModel(*model, sphere, Transform3f(), 16, 16);
00701     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
00702   }
00703 
00704   generateRandomTransforms(extents, transforms, n);
00705   Cylinder cylinder(10, 40);
00706   for(std::size_t i = 0; i < n; ++i)
00707   {
00708     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
00709     generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
00710     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
00711   }
00712 }
00713 
00714 
00715 octomap::OcTree* generateOcTree()
00716 {
00717   octomap::OcTree* tree = new octomap::OcTree(0.1);
00718 
00719   // insert some measurements of occupied cells
00720   for(int x = -20; x < 20; x++) 
00721   {
00722     for(int y = -20; y < 20; y++) 
00723     {
00724       for(int z = -20; z < 20; z++) 
00725       {
00726         tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true);
00727       }
00728     }
00729   }
00730 
00731   // insert some measurements of free cells
00732   for(int x = -30; x < 30; x++) 
00733   {
00734     for(int y = -30; y < 30; y++) 
00735     {
00736       for(int z = -30; z < 30; z++) 
00737       {
00738         tree->updateNode(octomap::point3d(x*0.02 -1.0, y*0.02-1.0, z*0.02-1.0), false);
00739       }
00740     }
00741   }
00742 
00743   return tree;  
00744 }
00745 
00746 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30