40 #include <Eigen/Geometry> 73 mergedSphere.
center = Eigen::Vector3d(0.0, 0.0, 0.0);
78 mergedSphere = spheres[0];
79 for (
unsigned int i = 1; i < spheres.size(); ++i)
81 if (spheres[i].radius <= 0.0)
83 Eigen::Vector3d diff = spheres[i].
center - mergedSphere.
center;
84 double d = diff.norm();
85 if (d + mergedSphere.
radius <= spheres[i].radius)
87 mergedSphere.
center = spheres[i].center;
88 mergedSphere.
radius = spheres[i].radius;
90 else if (d + spheres[i].radius > mergedSphere.
radius)
92 Eigen::Vector3d delta = mergedSphere.
center - spheres[i].center;
93 mergedSphere.
radius = (delta.norm() + spheres[i].radius + mergedSphere.
radius) / 2.0;
94 mergedSphere.
center = delta.normalized() * (mergedSphere.
radius - spheres[i].radius) + spheres[i].center;
102 template <
typename T>
112 Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
113 if (fabs(q.squaredNorm() - 1.0) > 1e-3)
116 q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
118 Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) *
119 q.toRotationMatrix());
145 Eigen::Vector3d sum(0.0, 0.0, 0.0);
148 unsigned int vertex_count = 0;
149 for (
unsigned int i = 0; i <
bodies.size(); i++)
154 for (
unsigned int j = 0; j < conv->
getScaledVertices().size(); j++, vertex_count++)
160 sphere.
center = sum / (double)vertex_count;
162 double max_dist_squared = 0.0;
163 for (
unsigned int i = 0; i <
bodies.size(); i++)
171 if (dist > max_dist_squared)
173 max_dist_squared = dist;
177 sphere.
radius = sqrt(max_dist_squared);
const Eigen::Affine3d & getPose() const
Retrieve the pose of the body.
void setPose(const Eigen::Affine3d &pose)
Set the pose of the body. Default is identity.
Body * constructBodyFromMsgHelper(const T &shape_msg, const geometry_msgs::Pose &pose)
Body * constructBodyFromMsg(const shape_msgs::Mesh &shape, const geometry_msgs::Pose &pose)
Create a body from a given shape.
Definition of a cylinder.
ShapeType type
The type of the shape.
Definition of a sphere that bounds another object.
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
A basic definition of a shape. Shapes are considered centered at origin.
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
Construct the shape that corresponds to the message. Return NULL on failure.
#define CONSOLE_BRIDGE_logError(fmt,...)
const EigenSTL::vector_Vector3d & getScaledVertices() const
void computeBoundingSphere(const std::vector< const Body * > &bodies, BoundingSphere &mergedSphere)
Compute the bounding sphere for a set of bodies and store the resulting sphere in mergedSphere...
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
Type that can hold any of the desired shape message types.
This set of classes allows quickly detecting whether a given point is inside an object or not...