Commit 657da8fc authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[test] Use octomap binary format.

parent ab44d479
......@@ -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;
......
......@@ -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
......
......@@ -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.
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment