[test] Use octomap binary format.

test/octree.cpp

@@ -77,10 +77,6 @@ void makeMesh (const std::vector<Vec3f>& vertices,
 hpp::fcl::OcTree makeOctree (const BVHModel <OBBRSS>& mesh,
                         const FCL_REAL& resolution)
 {
-  std::ofstream file;
-  file.open ("./env.octree");
-  Eigen::IOFormat csv (Eigen::FullPrecision, Eigen::DontAlignCols, "",
-                        ", ", "", "", "", "");
   Vec3f m (mesh.aabb_local.min_);
   Vec3f M (mesh.aabb_local.max_);
   hpp::fcl::Box box (resolution, resolution, resolution);
@@ -102,14 +98,14 @@ hpp::fcl::OcTree makeOctree (const BVHModel <OBBRSS>& mesh,
           octomap::point3d p
             ((float) center [0], (float) center [1], (float) center [2]);
           octree->updateNode (p, true);
-          file << center.format (csv) << std::endl;
           result.clear ();
         }
       }
     }
   }
+
   octree->updateInnerOccupancy();
-  file.close ();
+  octree->writeBinary("./env.octree");
   return OcTree (octree);
 }
 
@@ -130,9 +126,9 @@ BOOST_AUTO_TEST_CASE (OCTREE)
   makeMesh (pEnv, tEnv, envMesh);
   // Build octomap with environment
   envMesh.computeLocalAABB ();
-  // Load octree built from envMesh by makeOctree
+  // Load octree built from envMesh by makeOctree(envMesh, resolution)
   OcTree envOctree
-    (hpp::fcl::loadOctreeFile ((path / "env.octree").string().c_str(), resolution));
+    (hpp::fcl::loadOctreeFile ((path / "env.octree").string(), resolution));
 
   std::cout << "Finished loading octree." << std::endl;
 

test/utility.cpp

@@ -217,29 +217,15 @@ void saveOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<T
 }
 
 #ifdef HPP_FCL_HAVE_OCTOMAP
-OcTree loadOctreeFile (const char* filename, const FCL_REAL& resolution)
+OcTree loadOctreeFile (const std::string& filename, const FCL_REAL& resolution)
   {
-    std::ifstream file;
-    file.open(filename);
-    if(!file.good ())
-    {
+    octomap::OcTreePtr_t octree (new octomap::OcTree (filename));
+    if (octree->getResolution() != resolution) {
       std::ostringstream oss;
-      oss << "Could not open file " << filename;
-      throw std::runtime_error (oss.str ().c_str ());
-    }
-    octomap::OcTreePtr_t octree (new octomap::OcTree (resolution));
-    std::string line;
-    while (std::getline (file, line)) {
-      std::stringstream lineStream (line);
-      std::string cell;
-      std::vector <float> p (3);
-      std::size_t i=0;
-      while (std::getline (lineStream, cell, ',')) {
-        p [i] = (float) atof (cell.c_str ()); ++i;
-      }
-      octree->updateNode (octomap::point3d (p[0], p[1], p[2]), true);
+      oss << "Resolution of the OcTree is " << octree->getResolution() <<
+        " and not " << resolution;
+      throw std::invalid_argument(oss.str());
     }
-    octree->updateInnerOccupancy ();
     return hpp::fcl::OcTree (octree);
   }
 #endif

test/utility.h

@@ -113,7 +113,7 @@ void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<T
 void saveOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles);
 
 #ifdef HPP_FCL_HAVE_OCTOMAP
-fcl::OcTree loadOctreeFile (const char* filename, const FCL_REAL& resolution);
+fcl::OcTree loadOctreeFile (const std::string& filename, const FCL_REAL& resolution);
 #endif
 
 /// @brief Generate one random transform whose translation is constrained by extents and rotation without constraints.