Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
robot_self_filter::SelfMask< PointT > Class Template Reference

Computing a mask for a pointcloud that states which points are inside the robot. More...

#include <self_mask.h>

Classes

struct  SeeLink
 
struct  SortBodies
 

Public Types

typedef pcl::PointCloud< PointT > PointCloud
 

Public Member Functions

void assumeFrame (const std_msgs::Header &header)
 Assume subsequent calls to getMaskX() will be in the frame passed to this function. The frame in which the sensor is located is optional. More...
 
void assumeFrame (const std_msgs::Header &header, const tf::Vector3 &sensor_pos, const double min_sensor_dist)
 Assume subsequent calls to getMaskX() will be in the frame passed to this function. Also specify which possition to assume for the sensor (frame is not needed) More...
 
void assumeFrame (const std_msgs::Header &header, const std::string &sensor_frame, const double min_sensor_dist)
 Assume subsequent calls to getMaskX() will be in the frame passed to this function. The frame in which the sensor is located is optional. More...
 
void getLinkNames (std::vector< std::string > &frames) const
 Get the set of link names that have been instantiated for self filtering. More...
 
int getMaskContainment (const tf::Vector3 &pt) const
 Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No setup is performed, assumeFrame() should be called before use. More...
 
int getMaskContainment (double x, double y, double z) const
 Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No setup is performed, assumeFrame() should be called before use. More...
 
int getMaskIntersection (double x, double y, double z, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL) const
 Get the intersection mask (INSIDE, OUTSIDE or SHADOW) value for an individual point. No setup is performed, assumeFrame() should be called before use. More...
 
int getMaskIntersection (const tf::Vector3 &pt, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL) const
 Get the intersection mask (INSIDE, OUTSIDE or SHADOW) value for an individual point. No setup is performed, assumeFrame() should be called before use. More...
 
void maskContainment (const PointCloud &data_in, 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 maskIntersection (const PointCloud &data_in, const std::string &sensor_frame, const double min_sensor_dist, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL)
 Compute the intersection mask for a given pointcloud. If a mask element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW, the point is on a ray behind the robot and should not have been seen. If the mask element is INSIDE, the point is inside the robot. The sensor frame is specified to obtain the origin of the sensor. A callback can be registered for the first intersection point on each body. More...
 
void maskIntersection (const PointCloud &data_in, const tf::Vector3 &sensor_pos, const double min_sensor_dist, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &intersectionCallback=NULL)
 Compute the intersection mask for a given pointcloud. If a mask element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW, the point is on a ray behind the robot and should not have been seen. If the mask element is INSIDE, the point is inside the robot. The origin of the sensor is specified as well. More...
 
 SelfMask (tf::TransformListener &tf, const std::vector< LinkInfo > &links)
 Construct the filter. More...
 
 ~SelfMask (void)
 Destructor to clean up. More...
 

Protected Member Functions

void computeBoundingSpheres (void)
 Compute bounding spheres for the checked robot links. More...
 
bool configure (const std::vector< LinkInfo > &links)
 Configure the filter. More...
 
void freeMemory (void)
 Free memory. More...
 
void maskAuxContainment (const PointCloud &data_in, std::vector< int > &mask)
 Perform the actual mask computation. More...
 
void maskAuxIntersection (const PointCloud &data_in, std::vector< int > &mask, const boost::function< void(const tf::Vector3 &)> &callback)
 Perform the actual mask computation. More...
 

Protected Attributes

std::vector< SeeLinkbodies_
 
std::vector< bodies::BoundingSpherebspheres_
 
std::vector< double > bspheresRadius2_
 
double min_sensor_dist_
 
ros::NodeHandle nh_
 
tf::Vector3 sensor_pos_
 
tf::TransformListenertf_
 

Detailed Description

template<typename PointT>
class robot_self_filter::SelfMask< PointT >

Computing a mask for a pointcloud that states which points are inside the robot.

Definition at line 145 of file self_mask.h.

Member Typedef Documentation

template<typename PointT>
typedef pcl::PointCloud<PointT> robot_self_filter::SelfMask< PointT >::PointCloud

Definition at line 172 of file self_mask.h.

Constructor & Destructor Documentation

template<typename PointT>
robot_self_filter::SelfMask< PointT >::SelfMask ( tf::TransformListener tf,
const std::vector< LinkInfo > &  links 
)
inline

Construct the filter.

Definition at line 175 of file self_mask.h.

template<typename PointT>
robot_self_filter::SelfMask< PointT >::~SelfMask ( void  )
inline

Destructor to clean up.

Definition at line 182 of file self_mask.h.

Member Function Documentation

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::assumeFrame ( const std_msgs::Header header)
inline

Assume subsequent calls to getMaskX() will be in the frame passed to this function. The frame in which the sensor is located is optional.

Definition at line 256 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::assumeFrame ( const std_msgs::Header header,
const tf::Vector3 sensor_pos,
const double  min_sensor_dist 
)
inline

Assume subsequent calls to getMaskX() will be in the frame passed to this function. Also specify which possition to assume for the sensor (frame is not needed)

Definition at line 293 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::assumeFrame ( const std_msgs::Header header,
const std::string &  sensor_frame,
const double  min_sensor_dist 
)
inline

Assume subsequent calls to getMaskX() will be in the frame passed to this function. The frame in which the sensor is located is optional.

Definition at line 302 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::computeBoundingSpheres ( void  )
inlineprotected

Compute bounding spheres for the checked robot links.

Definition at line 528 of file self_mask.h.

template<typename PointT>
bool robot_self_filter::SelfMask< PointT >::configure ( const std::vector< LinkInfo > &  links)
inlineprotected

Configure the filter.

Definition at line 430 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::freeMemory ( void  )
inlineprotected

Free memory.

Definition at line 415 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::getLinkNames ( std::vector< std::string > &  frames) const
inline

Get the set of link names that have been instantiated for self filtering.

Definition at line 406 of file self_mask.h.

template<typename PointT>
int robot_self_filter::SelfMask< PointT >::getMaskContainment ( const tf::Vector3 pt) const
inline

Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No setup is performed, assumeFrame() should be called before use.

Definition at line 332 of file self_mask.h.

template<typename PointT>
int robot_self_filter::SelfMask< PointT >::getMaskContainment ( double  x,
double  y,
double  z 
) const
inline

Get the containment mask (INSIDE or OUTSIDE) value for an individual point. No setup is performed, assumeFrame() should be called before use.

Definition at line 344 of file self_mask.h.

template<typename PointT>
int robot_self_filter::SelfMask< PointT >::getMaskIntersection ( double  x,
double  y,
double  z,
const boost::function< void(const tf::Vector3 &)> &  intersectionCallback = NULL 
) const
inline

Get the intersection mask (INSIDE, OUTSIDE or SHADOW) value for an individual point. No setup is performed, assumeFrame() should be called before use.

Definition at line 352 of file self_mask.h.

template<typename PointT>
int robot_self_filter::SelfMask< PointT >::getMaskIntersection ( const tf::Vector3 pt,
const boost::function< void(const tf::Vector3 &)> &  intersectionCallback = NULL 
) const
inline

Get the intersection mask (INSIDE, OUTSIDE or SHADOW) value for an individual point. No setup is performed, assumeFrame() should be called before use.

Definition at line 360 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::maskAuxContainment ( const PointCloud data_in,
std::vector< int > &  mask 
)
inlineprotected

Perform the actual mask computation.

Definition at line 540 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::maskAuxIntersection ( const PointCloud data_in,
std::vector< int > &  mask,
const boost::function< void(const tf::Vector3 &)> &  callback 
)
inlineprotected

Perform the actual mask computation.

Definition at line 566 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::maskContainment ( const PointCloud data_in,
std::vector< int > &  mask 
)
inline

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 190 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::maskIntersection ( const PointCloud data_in,
const std::string &  sensor_frame,
const double  min_sensor_dist,
std::vector< int > &  mask,
const boost::function< void(const tf::Vector3 &)> &  intersectionCallback = NULL 
)
inline

Compute the intersection mask for a given pointcloud. If a mask element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW, the point is on a ray behind the robot and should not have been seen. If the mask element is INSIDE, the point is inside the robot. The sensor frame is specified to obtain the origin of the sensor. A callback can be registered for the first intersection point on each body.

Definition at line 213 of file self_mask.h.

template<typename PointT>
void robot_self_filter::SelfMask< PointT >::maskIntersection ( const PointCloud data_in,
const tf::Vector3 sensor_pos,
const double  min_sensor_dist,
std::vector< int > &  mask,
const boost::function< void(const tf::Vector3 &)> &  intersectionCallback = NULL 
)
inline

Compute the intersection mask for a given pointcloud. If a mask element can have one of the values INSIDE, OUTSIDE or SHADOW. If the value is SHADOW, the point is on a ray behind the robot and should not have been seen. If the mask element is INSIDE, the point is inside the robot. The origin of the sensor is specified as well.

Definition at line 239 of file self_mask.h.

Member Data Documentation

template<typename PointT>
std::vector<SeeLink> robot_self_filter::SelfMask< PointT >::bodies_
protected

Definition at line 643 of file self_mask.h.

template<typename PointT>
std::vector<bodies::BoundingSphere> robot_self_filter::SelfMask< PointT >::bspheres_
protected

Definition at line 645 of file self_mask.h.

template<typename PointT>
std::vector<double> robot_self_filter::SelfMask< PointT >::bspheresRadius2_
protected

Definition at line 644 of file self_mask.h.

template<typename PointT>
double robot_self_filter::SelfMask< PointT >::min_sensor_dist_
protected

Definition at line 641 of file self_mask.h.

template<typename PointT>
ros::NodeHandle robot_self_filter::SelfMask< PointT >::nh_
protected

Definition at line 638 of file self_mask.h.

template<typename PointT>
tf::Vector3 robot_self_filter::SelfMask< PointT >::sensor_pos_
protected

Definition at line 640 of file self_mask.h.

template<typename PointT>
tf::TransformListener& robot_self_filter::SelfMask< PointT >::tf_
protected

Definition at line 637 of file self_mask.h.


The documentation for this class was generated from the following file:


robot_self_filter
Author(s): Eitan Marder-Eppstein
autogenerated on Thu Jun 6 2019 19:59:05