Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix failure in building point cloud models for OBB/RSS/kIOS/OBBRSS (issue #67) #72

Closed
wants to merge 3 commits into from
Closed

Fix failure in building point cloud models for OBB/RSS/kIOS/OBBRSS (issue #67) #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
49edfc6
Fix failure in building point cloud models for OBB/RSS/kIOS/OBBRSS (i…
ricardoglc Jun 9, 2015
8d0441e
Fix memory allocation error-checking
ricardoglc Jun 16, 2015
13e1b7c
Add getTree method to fcl::OcTree
ricardoglc Jun 16, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions include/fcl/octree.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ namespace fcl
class OcTree : public CollisionGeometry
{
private:
boost::shared_ptr<const octomap::OcTree> tree;
boost::shared_ptr<octomap::OcTree> tree;

FCL_REAL default_occupancy;

Expand All @@ -70,7 +70,7 @@ class OcTree : public CollisionGeometry
typedef octomap::OcTreeNode OcTreeNode;

/// @brief construct octree with a given resolution
OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<const octomap::OcTree>(new octomap::OcTree(resolution)))
OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<octomap::OcTree>(new octomap::OcTree(resolution)))
{
default_occupancy = tree->getOccupancyThres();

Expand All @@ -80,7 +80,7 @@ class OcTree : public CollisionGeometry
}

/// @brief construct octree from octomap
OcTree(const boost::shared_ptr<const octomap::OcTree>& tree_) : tree(tree_)
OcTree(const boost::shared_ptr<octomap::OcTree>& tree_) : tree(tree_)
{
default_occupancy = tree->getOccupancyThres();

Expand All @@ -97,6 +97,12 @@ class OcTree : public CollisionGeometry
aabb_radius = (aabb_local.min_ - aabb_center).length();
}

/// @brief get the underlying octree structure
boost::shared_ptr<octomap::OcTree> getTree()
{
return tree;
}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It seems this accessor is added to address #73, which looks good to me. It would also good to provide const version of getTree() for const correctness as:

boost::shared_ptr<const octomap::OcTree> getTree() const
{
  return tree;
}

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We might be able to add this later.


/// @brief get the bounding volume for the root
inline AABB getRootBV() const
{
Expand Down
44 changes: 28 additions & 16 deletions src/BVH/BVH_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include "fcl/BV/BV.h"
#include <iostream>
#include <string.h>
#include <new>

namespace fcl
{
Expand Down Expand Up @@ -124,20 +125,23 @@ int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0;
}

if(num_tris_ <= 0) num_tris_ = 8;
if(num_tris_ < 0) num_tris_ = 8;
if(num_vertices_ <= 0) num_vertices_ = 8;

num_vertices_allocated = num_vertices_;
num_tris_allocated = num_tris_;

tri_indices = new Triangle[num_tris_allocated];
vertices = new Vec3f[num_vertices_allocated];

if(!tri_indices)
if(num_tris_ > 0)
{
std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl;
return BVH_ERR_MODEL_OUT_OF_MEMORY;
tri_indices = new(std::nothrow) Triangle[num_tris_allocated];
if(!tri_indices)
{
std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl;
return BVH_ERR_MODEL_OUT_OF_MEMORY;
}
}

vertices = new(std::nothrow) Vec3f[num_vertices_allocated];
if(!vertices)
{
std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl;
Expand Down Expand Up @@ -168,7 +172,7 @@ int BVHModel<BV>::addVertex(const Vec3f& p)

if(num_vertices >= num_vertices_allocated)
{
Vec3f* temp = new Vec3f[num_vertices_allocated * 2];
Vec3f* temp = new(std::nothrow) Vec3f[num_vertices_allocated * 2];
if(!temp)
{
std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl;
Expand Down Expand Up @@ -198,7 +202,7 @@ int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)

if(num_vertices + 2 >= num_vertices_allocated)
{
Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2];
Vec3f* temp = new(std::nothrow) Vec3f[num_vertices_allocated * 2 + 2];
if(!temp)
{
std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl;
Expand All @@ -222,6 +226,10 @@ int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)

if(num_tris >= num_tris_allocated)
{
if(num_tris_allocated == 0)
{
num_tris_allocated = 1;
}
Triangle* temp = new Triangle[num_tris_allocated * 2];
if(!temp)
{
Expand Down Expand Up @@ -254,7 +262,7 @@ int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps)

if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
{
Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
Vec3f* temp = new(std::nothrow) Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
if(!temp)
{
std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
Expand Down Expand Up @@ -289,7 +297,7 @@ int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Tr

if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
{
Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
Vec3f* temp = new(std::nothrow) Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
if(!temp)
{
std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
Expand All @@ -315,7 +323,11 @@ int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Tr

if(num_tris + num_tris_to_add - 1 >= num_tris_allocated)
{
Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
if(num_tris_allocated == 0)
{
num_tris_allocated = 1;
}
Triangle* temp = new(std::nothrow) Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
if(!temp)
{
std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl;
Expand Down Expand Up @@ -355,7 +367,7 @@ int BVHModel<BV>::endModel()

if(num_tris_allocated > num_tris)
{
Triangle* new_tris = new Triangle[num_tris];
Triangle* new_tris = new(std::nothrow) Triangle[num_tris];
if(!new_tris)
{
std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl;
Expand All @@ -369,7 +381,7 @@ int BVHModel<BV>::endModel()

if(num_vertices_allocated > num_vertices)
{
Vec3f* new_vertices = new Vec3f[num_vertices];
Vec3f* new_vertices = new(std::nothrow) Vec3f[num_vertices];
if(!new_vertices)
{
std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl;
Expand All @@ -390,8 +402,8 @@ int BVHModel<BV>::endModel()
num_bvs_to_be_allocated = 2 * num_tris - 1;


bvs = new BVNode<BV> [num_bvs_to_be_allocated];
primitive_indices = new unsigned int [num_bvs_to_be_allocated];
bvs = new(std::nothrow) BVNode<BV> [num_bvs_to_be_allocated];
primitive_indices = new(std::nothrow) unsigned int [num_bvs_to_be_allocated];
if(!bvs || !primitive_indices)
{
std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
Expand Down
13 changes: 1 addition & 12 deletions test/test_fcl_bvh_models.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,19 +51,8 @@ template<typename BV>
void testBVHModelPointCloud()
{
boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);

if (model->getNodeType() != BV_AABB
&& model->getNodeType() != BV_KDOP16
&& model->getNodeType() != BV_KDOP18
&& model->getNodeType() != BV_KDOP24)
{
std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType())
<< "' does not support point cloud model. "
<< "Please see issue #67." << std::endl;
return;
}

Box box;

double a = box.side[0];
double b = box.side[1];
double c = box.side[2];
Expand Down
8 changes: 4 additions & 4 deletions test/test_fcl_octomap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,7 +215,7 @@ void octomap_collision_test_BVH(std::size_t n, bool exhaustive)
m1->addSubModel(p1, t1);
m1->endModel();

OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
boost::shared_ptr<CollisionGeometry> tree_ptr(tree);

std::vector<Transform3f> transforms;
Expand Down Expand Up @@ -331,7 +331,7 @@ void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_m
else
generateEnvironments(env, env_scale, env_size);

OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));

DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
Expand Down Expand Up @@ -451,7 +451,7 @@ void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaust
else
generateEnvironments(env, env_scale, env_size);

OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));

DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
Expand Down Expand Up @@ -550,7 +550,7 @@ void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh
else
generateEnvironments(env, env_scale, env_size);

OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));

DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
Expand Down