Skip to content

GitLab

Commit bc75290c authored by Lucile Remigy's avatar Lucile Remigy
Browse files

delete class variance3f, 0 tests failed

parent c92151a6
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ bc75290c
......@@ -128,10 +128,8 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/types.h
include/hpp/fcl/math/transform.h
include/hpp/fcl/data_types.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h
include/hpp/fcl/BVH/BV_fitter.h
include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
......
include/hpp/fcl/BVH/BV_fitter.h deleted 100644 → 0
View file @ c92151a6
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef HPP_FCL_BV_FITTER_H
#define HPP_FCL_BV_FITTER_H
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/kIOS.h>
#include <hpp/fcl/BV/OBBRSS.h>
#include <hpp/fcl/BV/AABB.h>
#include <iostream>
namespace hpp
{
namespace fcl
{
/// @brief Compute a bounding volume that fits a set of n points.
template<typename BV>
void fit(Vec3f* ps, int n, BV& bv)
{
for(int i = 0; i < n; ++i)
{
bv += ps[i];
}
}
template<>
void fit<OBB>(Vec3f* ps, int n, OBB& bv);
template<>
void fit<RSS>(Vec3f* ps, int n, RSS& bv);
template<>
void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
template<>
void fit<OBBRSS>(Vec3f* ps, int n, OBBRSS& bv);
template<>
void fit<AABB>(Vec3f* ps, int n, AABB& bv);
/// @brief The class for the default algorithm fitting a bounding volume to a set of points
template<typename BV>
class BVFitterTpl
{
public:
/// @brief default deconstructor
virtual ~BVFitterTpl() {}
/// @brief Prepare the geometry primitive data for fitting
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
prev_vertices = NULL;
tri_indices = tri_indices_;
type = type_;
}
/// @brief Prepare the geometry primitive data for fitting, for deformable mesh
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
prev_vertices = prev_vertices_;
tri_indices = tri_indices_;
type = type_;
}
/// @brief Compute the fitting BV
virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
/// @brief Clear the geometry primitive data
void clear()
{
vertices = NULL;
prev_vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
protected:
Vec3f* vertices;
Vec3f* prev_vertices;
Triangle* tri_indices;
BVHModelType type;
};
/// @brief The class for the default algorithm fitting a bounding volume to a set of points
template<typename BV>
class BVFitter : public BVFitterTpl<BV>
{
public:
/// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
/// The primitive data was set by set function and primitive_indices is the primitive index relative to the data
BV fit(unsigned int* primitive_indices, int num_primitives)
{
BV bv;
if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle
{
for(int i = 0; i < num_primitives; ++i)
{
Triangle t = tri_indices[primitive_indices[i]];
bv += vertices[t[0]];
bv += vertices[t[1]];
bv += vertices[t[2]];
if(prev_vertices) /// can fitting both current and previous frame
{
bv += prev_vertices[t[0]];
bv += prev_vertices[t[1]];
bv += prev_vertices[t[2]];
}
}
}
else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point
{
for(int i = 0; i < num_primitives; ++i)
{
bv += vertices[primitive_indices[i]];
if(prev_vertices) /// can fitting both current and previous frame
{
bv += prev_vertices[primitive_indices[i]];
}
}
}
return bv;
}
protected:
using BVFitterTpl<BV>::vertices;
using BVFitterTpl<BV>::prev_vertices;
using BVFitterTpl<BV>::tri_indices;
using BVFitterTpl<BV>::type;
};
/// @brief Specification of BVFitter for OBB bounding volume
template<>
class BVFitter<OBB> : public BVFitterTpl<OBB>
{
public:
/// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
/// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
OBB fit(unsigned int* primitive_indices, int num_primitives);
};
/// @brief Specification of BVFitter for RSS bounding volume
template<>
class BVFitter<RSS> : public BVFitterTpl<RSS>
{
public:
/// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
/// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
RSS fit(unsigned int* primitive_indices, int num_primitives);
};
/// @brief Specification of BVFitter for kIOS bounding volume
template<>
class BVFitter<kIOS> : public BVFitterTpl<kIOS>
{
public:
/// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
/// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
kIOS fit(unsigned int* primitive_indices, int num_primitives);
};
/// @brief Specification of BVFitter for OBBRSS bounding volume
template<>
class BVFitter<OBBRSS> : public BVFitterTpl<OBBRSS>
{
public:
/// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
/// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
OBBRSS fit(unsigned int* primitive_indices, int num_primitives);
};
/// @brief Specification of BVFitter for AABB bounding volume
template<>
class BVFitter<AABB> : public BVFitterTpl<AABB>
{
public:
/// @brief Compute a bounding volume that fits a set of primitives (points or triangles).
/// The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
AABB fit(unsigned int* primitive_indices, int num_primitives);
};
}
} // namespace hpp
#endif
include/hpp/fcl/BVH/BV_splitter.h deleted 100644 → 0
View file @ c92151a6
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef HPP_FCL_BV_SPLITTER_H
#define HPP_FCL_BV_SPLITTER_H
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/kIOS.h>
#include <hpp/fcl/BV/OBBRSS.h>
#include <vector>
#include <iostream>
namespace hpp
{
namespace fcl
{
/// @brief Three types of split algorithms are provided in FCL as default
enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
/// @brief A class describing the split rule that splits each BV node
template<typename BV>
class BVSplitter
{
public:
BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method)
{
}
/// @brief Default deconstructor
virtual ~BVSplitter() {}
/// @brief Set the geometry data needed by the split rule
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
tri_indices = tri_indices_;
type = type_;
}
/// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
switch(split_method)
{
case SPLIT_METHOD_MEAN:
computeRule_mean(bv, primitive_indices, num_primitives);
break;
case SPLIT_METHOD_MEDIAN:
computeRule_median(bv, primitive_indices, num_primitives);
break;
case SPLIT_METHOD_BV_CENTER:
computeRule_bvcenter(bv, primitive_indices, num_primitives);
break;
default:
std::cerr << "Split method not supported" << std::endl;
}
}
/// @brief Apply the split rule on a given point
bool apply(const Vec3f& q) const
{
return q[split_axis] > split_value;
}
/// @brief Clear the geometry data set before
void clear()
{
vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
private:
/// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed.
/// For oriented node, we can use a vector to make a better split decision.
int split_axis;
Vec3f split_vector;
/// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold
FCL_REAL split_value;
/// @brief The mesh vertices or points handled by the splitter
Vec3f* vertices;
/// @brief The triangles handled by the splitter
Triangle* tri_indices;
/// @brief Whether the geometry is mesh or point cloud
BVHModelType type;
/// @brief The split algorithm used
SplitMethodType split_method;
/// @brief Split algorithm 1: Split the node from center
void computeRule_bvcenter(const BV& bv, unsigned int*, int)
{
Vec3f center = bv.center();
int axis = 2;
if(bv.width() >= bv.height() && bv.width() >= bv.depth())
axis = 0;
else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
axis = 1;
split_axis = axis;
split_value = center[axis];
}
/// @brief Split algorithm 2: Split the node according to the mean of the data contained
void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
int axis = 2;
if(bv.width() >= bv.height() && bv.width() >= bv.depth())
axis = 0;
else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
axis = 1;
split_axis = axis;
FCL_REAL sum = 0;
if(type == BVH_MODEL_TRIANGLES)
{
for(int i = 0; i < num_primitives; ++i)
{
const Triangle& t = tri_indices[primitive_indices[i]];
sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]);
}
sum /= 3;
}
else if(type == BVH_MODEL_POINTCLOUD)
{
for(int i = 0; i < num_primitives; ++i)
{
sum += vertices[primitive_indices[i]][split_axis];
}
}
split_value = sum / num_primitives;
}
/// @brief Split algorithm 3: Split the node according to the median of the data contained
void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
int axis = 2;
if(bv.width() >= bv.height() && bv.width() >= bv.depth())
axis = 0;
else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
axis = 1;
split_axis = axis;
std::vector<FCL_REAL> proj(num_primitives);
if(type == BVH_MODEL_TRIANGLES)
{
for(int i = 0; i < num_primitives; ++i)
{
const Triangle& t = tri_indices[primitive_indices[i]];
proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3;
}
}
else if(type == BVH_MODEL_POINTCLOUD)
{
for(int i = 0; i < num_primitives; ++i)
proj[i] = vertices[primitive_indices[i]][split_axis];
}
std::sort(proj.begin(), proj.end());
if(num_primitives % 2 == 1)
{
split_value = proj[(num_primitives - 1) / 2];
}
else
{
split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
}
}
};
template<>
bool BVSplitter<OBB>::apply(const Vec3f& q) const;
template<>
bool BVSplitter<RSS>::apply(const Vec3f& q) const;
template<>
bool BVSplitter<kIOS>::apply(const Vec3f& q) const;
template<>
bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
template<>
void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<OBBRSS>::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<OBBRSS>::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
template<>
void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
}
} // namespace hpp
#endif
include/hpp/fcl/math/types.h
View file @ bc75290c
......@@ -66,21 +66,6 @@ namespace fcl
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
/// @brief Class for variance matrix in 3d
class Variance3f
{
public:
/// @brief Variation matrix
Matrix3f Sigma;
/// @brief Variations along the eign axes
Matrix3f::Scalar sigma[3];
/// @brief Eigen axes of the variation matrix
Vec3f axis[3];
};
}
} // namespace hpp
......
src/BVH/BVH_utility.cpp
View file @ bc75290c
......@@ -45,68 +45,6 @@ namespace hpp
namespace fcl
{
void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
{
for(int i = 0; i < model.getNumBVs(); ++i)
{
BVNode<OBB>& bvnode = model.getBV(i);
Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
const Variance3f& uc = ucs[v_id];
Vec3f&v = model.vertices[bvnode.first_primitive + j];
for(int k = 0; k < 3; ++k)
{
vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
}
}
OBB bv;
fit(vs, bvnode.num_primitives * 6, bv);
delete [] vs;
bvnode.bv = bv;
}
}
void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
{
for(int i = 0; i < model.getNumBVs(); ++i)
{
BVNode<RSS>& bvnode = model.getBV(i);
Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
const Variance3f& uc = ucs[v_id];
Vec3f&v = model.vertices[bvnode.first_primitive + j];
for(int k = 0; k < 3; ++k)
{
vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
}
}
RSS bv;
fit(vs, bvnode.num_primitives * 6, bv);
delete [] vs;
bvnode.bv = bv;
}
}
template<typename BV>
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, const AABB& _aabb)
{
......
Markdown is supported
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