Computing a mask for a pointcloud that states which points are inside the robot. More...
#include <shape_mask.h>
Classes | |
struct | SeeShape |
struct | SortBodies |
Public Types | |
enum | { INSIDE = 0, OUTSIDE = 1, CLIP = 2 } |
The possible values of a mask computed for a point. More... | |
typedef boost::function< bool(ShapeHandle, Eigen::Isometry3d &)> | TransformCallback |
Public Member Functions | |
ShapeHandle | addShape (const shapes::ShapeConstPtr &shape, double scale=1.0, double padding=0.0) |
int | getMaskContainment (const Eigen::Vector3d &pt) const |
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point is in the frame corresponding to the TransformCallback. More... | |
int | getMaskContainment (double x, double y, double z) const |
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point is in the frame corresponding to the TransformCallback. More... | |
void | maskContainment (const sensor_msgs::PointCloud2 &data_in, const Eigen::Vector3d &sensor_pos, const double min_sensor_dist, const double max_sensor_dist, std::vector< int > &mask) |
Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE, the point is inside the robot. The point is outside if the mask element is OUTSIDE. More... | |
void | removeShape (ShapeHandle handle) |
void | setTransformCallback (const TransformCallback &transform_callback) |
ShapeMask (const TransformCallback &transform_callback=TransformCallback()) | |
Construct the filter. More... | |
virtual | ~ShapeMask () |
Destructor to clean up. More... | |
Protected Attributes | |
std::set< SeeShape, SortBodies > | bodies_ |
std::vector< bodies::BoundingSphere > | bspheres_ |
boost::mutex | shapes_lock_ |
Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration. More... | |
TransformCallback | transform_callback_ |
Private Member Functions | |
void | freeMemory () |
Free memory. More... | |
Private Attributes | |
ShapeHandle | min_handle_ |
ShapeHandle | next_handle_ |
std::map< ShapeHandle, std::set< SeeShape, SortBodies >::iterator > | used_handles_ |
Computing a mask for a pointcloud that states which points are inside the robot.
Definition at line 85 of file shape_mask.h.
typedef boost::function<bool(ShapeHandle, Eigen::Isometry3d&)> point_containment_filter::ShapeMask::TransformCallback |
Definition at line 96 of file shape_mask.h.
anonymous enum |
The possible values of a mask computed for a point.
Enumerator | |
---|---|
INSIDE | |
OUTSIDE | |
CLIP |
Definition at line 89 of file shape_mask.h.
point_containment_filter::ShapeMask::ShapeMask | ( | const TransformCallback & | transform_callback = TransformCallback() | ) |
Construct the filter.
Definition at line 44 of file shape_mask.cpp.
|
virtual |
Destructor to clean up.
Definition at line 49 of file shape_mask.cpp.
point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addShape | ( | const shapes::ShapeConstPtr & | shape, |
double | scale = 1.0 , |
||
double | padding = 0.0 |
||
) |
Definition at line 67 of file shape_mask.cpp.
|
private |
Free memory.
Definition at line 54 of file shape_mask.cpp.
int point_containment_filter::ShapeMask::getMaskContainment | ( | const Eigen::Vector3d & | pt | ) | const |
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point is in the frame corresponding to the TransformCallback.
Definition at line 179 of file shape_mask.cpp.
int point_containment_filter::ShapeMask::getMaskContainment | ( | double | x, |
double | y, | ||
double | z | ||
) | const |
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point is in the frame corresponding to the TransformCallback.
Definition at line 190 of file shape_mask.cpp.
void point_containment_filter::ShapeMask::maskContainment | ( | const sensor_msgs::PointCloud2 & | data_in, |
const Eigen::Vector3d & | sensor_pos, | ||
const double | min_sensor_dist, | ||
const double | max_sensor_dist, | ||
std::vector< int > & | mask | ||
) |
Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE, the point is inside the robot. The point is outside if the mask element is OUTSIDE.
Definition at line 117 of file shape_mask.cpp.
void point_containment_filter::ShapeMask::removeShape | ( | ShapeHandle | handle | ) |
Definition at line 102 of file shape_mask.cpp.
void point_containment_filter::ShapeMask::setTransformCallback | ( | const TransformCallback & | transform_callback | ) |
Definition at line 61 of file shape_mask.cpp.
|
protected |
Definition at line 153 of file shape_mask.h.
|
protected |
Definition at line 154 of file shape_mask.h.
|
private |
Definition at line 161 of file shape_mask.h.
|
private |
Definition at line 160 of file shape_mask.h.
|
mutableprotected |
Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration.
Definition at line 152 of file shape_mask.h.
|
protected |
Definition at line 149 of file shape_mask.h.
|
private |
Definition at line 162 of file shape_mask.h.