Apply ament_uncrustify --reformat (#297)
Signed-off-by: Sean Kelly <sean@seankelly.dev>
This commit is contained in:
Sean Kelly
GitHub
@@ -45,44 +45,43 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
|
||||
* criterion. The code makes use of the estimated surface normals at each point in the input dataset.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class BoundaryEstimation: public FeatureFromNormals
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
|
||||
* criterion. The code makes use of the estimated surface normals at each point in the input dataset.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class BoundaryEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_BOUNDARY_H_
|
||||
|
||||
|
||||
|
||||
@@ -54,198 +54,209 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b Feature represents the base feature class. Some generic 3D operations that
|
||||
* are applicable to all features are defined here as static methods.
|
||||
* \author Radu Bogdan Rusu
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b Feature represents the base feature class. Some generic 3D operations that
|
||||
* are applicable to all features are defined here as static methods.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class Feature : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
typedef pcl::KdTree<pcl::PointXYZ> KdTree;
|
||||
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
|
||||
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
|
||||
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
typedef pcl::IndicesConstPtr IndicesConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
Feature()
|
||||
: /*input_(), indices_(), surface_(), */ tree_(), k_(0), search_radius_(0),
|
||||
use_surface_(false), spatial_locator_type_(-1)
|
||||
{}
|
||||
|
||||
protected:
|
||||
/** \brief The input point cloud dataset. */
|
||||
//PointCloudInConstPtr input_;
|
||||
|
||||
/** \brief A pointer to the vector of point indices to use. */
|
||||
//IndicesConstPtr indices_;
|
||||
|
||||
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
|
||||
//PointCloudInConstPtr surface_;
|
||||
|
||||
/** \brief A pointer to the spatial search object. */
|
||||
KdTreePtr tree_;
|
||||
|
||||
/** \brief The number of K nearest neighbors to use for each point. */
|
||||
int k_;
|
||||
|
||||
/** \brief The nearest neighbors search radius for each point. */
|
||||
double search_radius_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The surface PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
|
||||
bool use_surface_;
|
||||
|
||||
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
|
||||
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
|
||||
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
|
||||
* 2: Organized spatial dataset index
|
||||
*/
|
||||
class Feature : public PCLNodelet
|
||||
int spatial_locator_type_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<FeatureConfig>> srv_;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
virtual bool childInit(ros::NodeHandle & nh) = 0;
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
|
||||
|
||||
/** \brief Compute the feature and publish it. Internal method. */
|
||||
virtual void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices) = 0;
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(FeatureConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_pi_;
|
||||
message_filters::PassThrough<PointCloudIn> nf_pc_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback(const PointCloudInConstPtr & input)
|
||||
{
|
||||
public:
|
||||
typedef pcl::KdTree<pcl::PointXYZ> KdTree;
|
||||
typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
|
||||
PointIndices indices;
|
||||
indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
|
||||
PointCloudIn cloud;
|
||||
cloud.header.stamp = input->header.stamp;
|
||||
nf_pc_.add(ros_ptr(cloud.makeShared()));
|
||||
nf_pi_.add(boost::make_shared<PointIndices>(indices));
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
|
||||
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
|
||||
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
|
||||
private:
|
||||
/** \brief Synchronized input, surface, and point indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
|
||||
PointCloudIn, PointIndices>>> sync_input_surface_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
|
||||
PointCloudIn, PointIndices>>> sync_input_surface_indices_e_;
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
typedef pcl::IndicesConstPtr IndicesConstPtr;
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
Feature () : /*input_(), indices_(), surface_(), */tree_(), k_(0), search_radius_(0),
|
||||
use_surface_(false), spatial_locator_type_(-1)
|
||||
{};
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
virtual void subscribe();
|
||||
virtual void unsubscribe();
|
||||
|
||||
protected:
|
||||
/** \brief The input point cloud dataset. */
|
||||
//PointCloudInConstPtr input_;
|
||||
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_surface the pointer to the surface point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_surface_indices_callback(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & cloud_surface,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief A pointer to the vector of point indices to use. */
|
||||
//IndicesConstPtr indices_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
|
||||
//PointCloudInConstPtr surface_;
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
class FeatureFromNormals : public Feature
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
|
||||
/** \brief A pointer to the spatial search object. */
|
||||
KdTreePtr tree_;
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
|
||||
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
|
||||
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
|
||||
|
||||
/** \brief The number of K nearest neighbors to use for each point. */
|
||||
int k_;
|
||||
FeatureFromNormals()
|
||||
: normals_() {}
|
||||
|
||||
/** \brief The nearest neighbors search radius for each point. */
|
||||
double search_radius_;
|
||||
protected:
|
||||
/** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
|
||||
PointCloudNConstPtr normals_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The surface PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
virtual bool childInit(ros::NodeHandle & nh) = 0;
|
||||
|
||||
/** \brief Set to true if the nodelet needs to listen for incoming point clouds representing the search surface. */
|
||||
bool use_surface_;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
|
||||
|
||||
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
|
||||
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
|
||||
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
|
||||
* 2: Organized spatial dataset index
|
||||
*/
|
||||
int spatial_locator_type_;
|
||||
/** \brief Compute the feature and publish it. */
|
||||
virtual void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices) = 0;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<FeatureConfig> > srv_;
|
||||
private:
|
||||
/** \brief The normals PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
virtual bool childInit (ros::NodeHandle &nh) = 0;
|
||||
/** \brief Synchronized input, normals, surface, and point indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
|
||||
PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
|
||||
PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_e_;
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
|
||||
/** \brief Internal method. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr &,
|
||||
const PointCloudInConstPtr &,
|
||||
const IndicesPtr &) {} // This should never be called
|
||||
|
||||
/** \brief Compute the feature and publish it. Internal method. */
|
||||
virtual void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices) = 0;
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (FeatureConfig &config, uint32_t level);
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
virtual void subscribe();
|
||||
virtual void unsubscribe();
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_pi_;
|
||||
message_filters::PassThrough<PointCloudIn> nf_pc_;
|
||||
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_normals the pointer to the input point cloud normals
|
||||
* \param cloud_surface the pointer to the surface point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_normals_surface_indices_callback(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & cloud_normals,
|
||||
const PointCloudInConstPtr & cloud_surface,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudInConstPtr &input)
|
||||
{
|
||||
PointIndices indices;
|
||||
indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
|
||||
PointCloudIn cloud;
|
||||
cloud.header.stamp = input->header.stamp;
|
||||
nf_pc_.add (ros_ptr(cloud.makeShared ()));
|
||||
nf_pi_.add (boost::make_shared<PointIndices> (indices));
|
||||
}
|
||||
|
||||
private:
|
||||
/** \brief Synchronized input, surface, and point indices.*/
|
||||
boost::shared_ptr <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_a_;
|
||||
boost::shared_ptr <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > > sync_input_surface_indices_e_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
virtual void subscribe ();
|
||||
virtual void unsubscribe ();
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_surface the pointer to the surface point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &cloud_surface,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
class FeatureFromNormals : public Feature
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
|
||||
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
|
||||
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
|
||||
|
||||
FeatureFromNormals () : normals_() {};
|
||||
|
||||
protected:
|
||||
/** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
|
||||
PointCloudNConstPtr normals_;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
virtual bool childInit (ros::NodeHandle &nh) = 0;
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
virtual void emptyPublish (const PointCloudInConstPtr &cloud) = 0;
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
virtual void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices) = 0;
|
||||
|
||||
private:
|
||||
/** \brief The normals PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
|
||||
|
||||
/** \brief Synchronized input, normals, surface, and point indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > sync_input_normals_surface_indices_e_;
|
||||
|
||||
/** \brief Internal method. */
|
||||
void computePublish (const PointCloudInConstPtr &,
|
||||
const PointCloudInConstPtr &,
|
||||
const IndicesPtr &) {} // This should never be called
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
virtual void subscribe ();
|
||||
virtual void unsubscribe ();
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_normals the pointer to the input point cloud normals
|
||||
* \param cloud_surface the pointer to the surface point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_normals_surface_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &cloud_normals,
|
||||
const PointCloudInConstPtr &cloud_surface,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FEATURE_H_
|
||||
|
||||
@@ -43,58 +43,57 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
|
||||
* dataset containing points and normals.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, M. Beetz.
|
||||
* Fast Point Feature Histograms (FPFH) for 3D Registration.
|
||||
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
|
||||
* Kobe, Japan, May 12-17 2009.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
|
||||
* Fast Geometric Point Labeling using Conditional Random Fields.
|
||||
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* St. Louis, MO, USA, October 11-15 2009.
|
||||
* </li>
|
||||
* </ul>
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class FPFHEstimation : public FeatureFromNormals
|
||||
/** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
|
||||
* dataset containing points and normals.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, M. Beetz.
|
||||
* Fast Point Feature Histograms (FPFH) for 3D Registration.
|
||||
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
|
||||
* Kobe, Japan, May 12-17 2009.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
|
||||
* Fast Geometric Point Labeling using Conditional Random Fields.
|
||||
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* St. Louis, MO, USA, October 11-15 2009.
|
||||
* </li>
|
||||
* </ul>
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class FPFHEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_FPFH_H_
|
||||
|
||||
|
||||
|
||||
@@ -43,54 +43,54 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
|
||||
* dataset containing points and normals, in parallel, using the OpenMP standard.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, M. Beetz.
|
||||
* Fast Point Feature Histograms (FPFH) for 3D Registration.
|
||||
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
|
||||
* Kobe, Japan, May 12-17 2009.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
|
||||
* Fast Geometric Point Labeling using Conditional Random Fields.
|
||||
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* St. Louis, MO, USA, October 11-15 2009.
|
||||
* </li>
|
||||
* </ul>
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class FPFHEstimationOMP : public FeatureFromNormals
|
||||
/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
|
||||
* dataset containing points and normals, in parallel, using the OpenMP standard.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, M. Beetz.
|
||||
* Fast Point Feature Histograms (FPFH) for 3D Registration.
|
||||
* In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
|
||||
* Kobe, Japan, May 12-17 2009.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
|
||||
* Fast Geometric Point Labeling using Conditional Random Fields.
|
||||
* In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* St. Louis, MO, USA, October 11-15 2009.
|
||||
* </li>
|
||||
* </ul>
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class FPFHEstimationOMP : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FPFH_OMP_H_
|
||||
|
||||
|
||||
@@ -43,41 +43,40 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class MomentInvariantsEstimation: public Feature
|
||||
/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class MomentInvariantsEstimation : public Feature
|
||||
{
|
||||
private:
|
||||
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_MOMENT_INVARIANTS_H_
|
||||
|
||||
|
||||
|
||||
@@ -43,44 +43,44 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimation: public Feature
|
||||
/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimation : public Feature
|
||||
{
|
||||
private:
|
||||
/** \brief PCL implementation object. */
|
||||
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
/** \brief PCL implementation object. */
|
||||
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type.
|
||||
* \param cloud the input point cloud to copy the header from.
|
||||
*/
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type.
|
||||
* \param cloud the input point cloud to copy the header from.
|
||||
*/
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_NORMAL_3D_H_
|
||||
|
||||
|
||||
@@ -43,37 +43,38 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures, in parallel, using the OpenMP standard.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimationOMP: public Feature
|
||||
/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures, in parallel, using the OpenMP standard.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimationOMP : public Feature
|
||||
{
|
||||
private:
|
||||
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_NORMAL_3D_OMP_H_
|
||||
|
||||
@@ -46,41 +46,40 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures, in parallel, using Intel's Threading Building Blocks library.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimationTBB: public Feature
|
||||
/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
|
||||
* curvatures, in parallel, using Intel's Threading Building Blocks library.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class NormalEstimationTBB : public Feature
|
||||
{
|
||||
private:
|
||||
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloud>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloud> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
//#endif // HAVE_TBB
|
||||
|
||||
#endif //#ifndef PCL_ROS_NORMAL_3D_TBB_H_
|
||||
|
||||
|
||||
|
||||
@@ -43,58 +43,57 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
|
||||
* containing points and normals.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
|
||||
* Aligning Point Cloud Views using Persistent Feature Histograms.
|
||||
* In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* Nice, France, September 22-26 2008.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
|
||||
* Learning Informative Point Classes for the Acquisition of Object Model Maps.
|
||||
* In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
|
||||
* Hanoi, Vietnam, December 17-20 2008.
|
||||
* </li>
|
||||
* </ul>
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PFHEstimation : public FeatureFromNormals
|
||||
/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
|
||||
* containing points and normals.
|
||||
*
|
||||
* @note If you use this code in any academic work, please cite:
|
||||
*
|
||||
* <ul>
|
||||
* <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
|
||||
* Aligning Point Cloud Views using Persistent Feature Histograms.
|
||||
* In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
|
||||
* Nice, France, September 22-26 2008.
|
||||
* </li>
|
||||
* <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
|
||||
* Learning Informative Point Classes for the Acquisition of Object Model Maps.
|
||||
* In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
|
||||
* Hanoi, Vietnam, December 17-20 2008.
|
||||
* </li>
|
||||
* </ul>
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PFHEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_PFH_H_
|
||||
|
||||
|
||||
|
||||
@@ -44,41 +44,42 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
|
||||
* principal surface curvatures for a given point cloud dataset containing points and normals.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu, Jared Glover
|
||||
*/
|
||||
class PrincipalCurvaturesEstimation : public FeatureFromNormals
|
||||
/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
|
||||
* principal surface curvatures for a given point cloud dataset containing points and normals.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
|
||||
* \author Radu Bogdan Rusu, Jared Glover
|
||||
*/
|
||||
class PrincipalCurvaturesEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_PRINCIPAL_CURVATURES_H_
|
||||
|
||||
@@ -42,39 +42,38 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b SHOTEstimation estimates SHOT descriptor.
|
||||
*
|
||||
*/
|
||||
class SHOTEstimation : public FeatureFromNormals
|
||||
/** \brief @b SHOTEstimation estimates SHOT descriptor.
|
||||
*
|
||||
*/
|
||||
class SHOTEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_SHOT_H_
|
||||
|
||||
|
||||
|
||||
@@ -42,37 +42,37 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
|
||||
*/
|
||||
class SHOTEstimationOMP : public FeatureFromNormals
|
||||
/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
|
||||
*/
|
||||
class SHOTEstimationOMP : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_SHOT_OMP_H_
|
||||
|
||||
|
||||
@@ -43,41 +43,42 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
|
||||
* dataset containing points and normals.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class VFHEstimation : public FeatureFromNormals
|
||||
/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
|
||||
* dataset containing points and normals.
|
||||
*
|
||||
* @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
|
||||
* \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class VFHEstimation : public FeatureFromNormals
|
||||
{
|
||||
private:
|
||||
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
|
||||
|
||||
typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit(ros::NodeHandle & nh)
|
||||
{
|
||||
private:
|
||||
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish(const PointCloudInConstPtr & cloud);
|
||||
|
||||
/** \brief Child initialization routine. Internal method. */
|
||||
inline bool
|
||||
childInit (ros::NodeHandle &nh)
|
||||
{
|
||||
// Create the output publisher
|
||||
pub_output_ = advertise<PointCloudOut> (nh, "output", max_queue_size_);
|
||||
return (true);
|
||||
}
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointCloudNConstPtr & normals,
|
||||
const PointCloudInConstPtr & surface,
|
||||
const IndicesPtr & indices);
|
||||
|
||||
/** \brief Publish an empty point cloud of the feature output type. */
|
||||
void emptyPublish (const PointCloudInConstPtr &cloud);
|
||||
|
||||
/** \brief Compute the feature and publish it. */
|
||||
void computePublish (const PointCloudInConstPtr &cloud,
|
||||
const PointCloudNConstPtr &normals,
|
||||
const PointCloudInConstPtr &surface,
|
||||
const IndicesPtr &indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FEATURES_VFH_H_
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
*
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
@@ -32,7 +32,7 @@
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: cropbox.cpp
|
||||
* $Id: cropbox.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -48,57 +48,59 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
|
||||
*
|
||||
* \author Radu Bogdan Rusu
|
||||
* \author Justin Rosen
|
||||
* \author Marti Morta Garriga
|
||||
/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
|
||||
*
|
||||
* \author Radu Bogdan Rusu
|
||||
* \author Justin Rosen
|
||||
* \author Marti Morta Garriga
|
||||
*/
|
||||
class CropBox : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::CropBoxConfig>> srv_; // TODO
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class CropBox : public Filter
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::CropBoxConfig> > srv_; // TODO
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
inline void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL (*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool
|
||||
child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool
|
||||
child_init (ros::NodeHandle &nh, bool &has_service);
|
||||
|
||||
/** \brief Dynamic reconfigure service callback.
|
||||
* \param config the dynamic reconfigure CropBoxConfig object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback (pcl_ros::CropBoxConfig &config, uint32_t level);
|
||||
/** \brief Dynamic reconfigure service callback.
|
||||
* \param config the dynamic reconfigure CropBoxConfig object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback(pcl_ros::CropBoxConfig & config, uint32_t level);
|
||||
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::CropBox<pcl::PCLPointCloud2> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::CropBox<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FILTERS_CROPBOX_H_
|
||||
|
||||
@@ -46,53 +46,54 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class ExtractIndices : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>> srv_;
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class ExtractIndices : public Filter
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig> > srv_;
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
inline void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
virtual bool
|
||||
child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
virtual bool
|
||||
child_init (ros::NodeHandle &nh, bool &has_service);
|
||||
/** \brief Dynamic reconfigure service callback. */
|
||||
void
|
||||
config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Dynamic reconfigure service callback. */
|
||||
void
|
||||
config_callback (pcl_ros::ExtractIndicesConfig &config, uint32_t level);
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FILTERS_EXTRACTINDICES_H_
|
||||
|
||||
|
||||
@@ -48,110 +48,115 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
|
||||
* are defined here as static methods.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b Filter represents the base filter class. Some generic 3D operations that are applicable to all filters
|
||||
* are defined here as static methods.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class Filter : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
typedef pcl::IndicesConstPtr IndicesConstPtr;
|
||||
|
||||
Filter() {}
|
||||
|
||||
protected:
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
message_filters::Subscriber<PointCloud2> sub_input_filter_;
|
||||
|
||||
/** \brief The desired user filter field name. */
|
||||
std::string filter_field_name_;
|
||||
|
||||
/** \brief The minimum allowed filter value a point will be considered from. */
|
||||
double filter_limit_min_;
|
||||
|
||||
/** \brief The maximum allowed filter value a point will be considered from. */
|
||||
double filter_limit_max_;
|
||||
|
||||
/** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
|
||||
bool filter_limit_negative_;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
class Filter : public PCLNodelet
|
||||
virtual bool
|
||||
child_init(ros::NodeHandle & nh, bool & has_service)
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
has_service = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
typedef pcl::IndicesConstPtr IndicesConstPtr;
|
||||
/** \brief Virtual abstract filter method. To be implemented by every child.
|
||||
* \param input the input point cloud dataset.
|
||||
* \param indices a pointer to the vector of point indices to use.
|
||||
* \param output the resultant filtered PointCloud2
|
||||
*/
|
||||
virtual void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output) = 0;
|
||||
|
||||
Filter () {}
|
||||
/** \brief Lazy transport subscribe routine. */
|
||||
virtual void
|
||||
subscribe();
|
||||
|
||||
protected:
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
/** \brief Lazy transport unsubscribe routine. */
|
||||
virtual void
|
||||
unsubscribe();
|
||||
|
||||
message_filters::Subscriber<PointCloud2> sub_input_filter_;
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void
|
||||
onInit();
|
||||
|
||||
/** \brief The desired user filter field name. */
|
||||
std::string filter_field_name_;
|
||||
/** \brief Call the child filter () method, optionally transform the result, and publish it.
|
||||
* \param input the input point cloud dataset.
|
||||
* \param indices a pointer to the vector of point indices to use.
|
||||
*/
|
||||
void
|
||||
computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices);
|
||||
|
||||
/** \brief The minimum allowed filter value a point will be considered from. */
|
||||
double filter_limit_min_;
|
||||
private:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
|
||||
|
||||
/** \brief The maximum allowed filter value a point will be considered from. */
|
||||
double filter_limit_max_;
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
PointIndices>>> sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
PointIndices>>> sync_input_indices_a_;
|
||||
|
||||
/** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
|
||||
bool filter_limit_negative_;
|
||||
/** \brief Dynamic reconfigure service callback. */
|
||||
virtual void
|
||||
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
/** \brief PointCloud2 + Indices data callback. */
|
||||
void
|
||||
input_indices_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
virtual bool
|
||||
child_init (ros::NodeHandle &nh, bool &has_service)
|
||||
{
|
||||
has_service = false;
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Virtual abstract filter method. To be implemented by every child.
|
||||
* \param input the input point cloud dataset.
|
||||
* \param indices a pointer to the vector of point indices to use.
|
||||
* \param output the resultant filtered PointCloud2
|
||||
*/
|
||||
virtual void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output) = 0;
|
||||
|
||||
/** \brief Lazy transport subscribe routine. */
|
||||
virtual void
|
||||
subscribe();
|
||||
|
||||
/** \brief Lazy transport unsubscribe routine. */
|
||||
virtual void
|
||||
unsubscribe();
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void
|
||||
onInit ();
|
||||
|
||||
/** \brief Call the child filter () method, optionally transform the result, and publish it.
|
||||
* \param input the input point cloud dataset.
|
||||
* \param indices a pointer to the vector of point indices to use.
|
||||
*/
|
||||
void
|
||||
computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
/** \brief Dynamic reconfigure service callback. */
|
||||
virtual void
|
||||
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
|
||||
|
||||
/** \brief PointCloud2 + Indices data callback. */
|
||||
void
|
||||
input_indices_callback (const PointCloud2::ConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices);
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FILTER_H_
|
||||
|
||||
@@ -44,55 +44,57 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
|
||||
* constraints.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
|
||||
* constraints.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PassThrough : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class PassThrough : public Filter
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > srv_;
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
inline void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL (*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool
|
||||
child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool
|
||||
child_init (ros::NodeHandle &nh, bool &has_service);
|
||||
|
||||
/** \brief Dynamic reconfigure service callback.
|
||||
* \param config the dynamic reconfigure FilterConfig object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback (pcl_ros::FilterConfig &config, uint32_t level);
|
||||
/** \brief Dynamic reconfigure service callback.
|
||||
* \param config the dynamic reconfigure FilterConfig object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
|
||||
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::PassThrough<pcl::PCLPointCloud2> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::PassThrough<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FILTERS_PASSTHROUGH_H_
|
||||
|
||||
@@ -46,69 +46,75 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
|
||||
* separate PointCloud.
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
|
||||
* separate PointCloud.
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class ProjectInliers : public Filter
|
||||
{
|
||||
public:
|
||||
ProjectInliers()
|
||||
: model_() {}
|
||||
|
||||
protected:
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class ProjectInliers : public Filter
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
public:
|
||||
ProjectInliers () : model_ () {}
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients);
|
||||
pcl_conversions::toPCL(*(model_), *(pcl_model));
|
||||
impl_.setModelCoefficients(pcl_model);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
protected:
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
inline void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL (*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients);
|
||||
pcl_conversions::toPCL(*(model_), *(pcl_model));
|
||||
impl_.setModelCoefficients (pcl_model);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
private:
|
||||
/** \brief A pointer to the vector of model coefficients. */
|
||||
ModelCoefficientsConstPtr model_;
|
||||
|
||||
private:
|
||||
/** \brief A pointer to the vector of model coefficients. */
|
||||
ModelCoefficientsConstPtr model_;
|
||||
/** \brief The message filter subscriber for model coefficients. */
|
||||
message_filters::Subscriber<ModelCoefficients> sub_model_;
|
||||
|
||||
/** \brief The message filter subscriber for model coefficients. */
|
||||
message_filters::Subscriber<ModelCoefficients> sub_model_;
|
||||
/** \brief Synchronized input, indices, and model coefficients.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
PointIndices, ModelCoefficients>>> sync_input_indices_model_a_;
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
/** \brief Synchronized input, indices, and model coefficients.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void
|
||||
onInit();
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void
|
||||
onInit ();
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
|
||||
/** \brief NodeletLazy connection routine. */
|
||||
void subscribe ();
|
||||
void unsubscribe ();
|
||||
/** \brief PointCloud2 + Indices + Model data callback. */
|
||||
void
|
||||
input_indices_model_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices,
|
||||
const ModelCoefficientsConstPtr & model);
|
||||
|
||||
/** \brief PointCloud2 + Indices + Model data callback. */
|
||||
void
|
||||
input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices,
|
||||
const ModelCoefficientsConstPtr &model);
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_FILTERS_PROJECT_INLIERS_H_
|
||||
|
||||
@@ -47,54 +47,55 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
|
||||
* search radius is smaller than a given K.
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
|
||||
* search radius is smaller than a given K.
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class RadiusOutlierRemoval : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig>> srv_;
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class RadiusOutlierRemoval : public Filter
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::RadiusOutlierRemovalConfig> > srv_;
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
inline void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL (*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
virtual inline bool child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
virtual inline bool child_init (ros::NodeHandle &nh, bool &has_service);
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(pcl_ros::RadiusOutlierRemovalConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (pcl_ros::RadiusOutlierRemovalConfig &config, uint32_t level);
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_FILTERS_RADIUSOUTLIERREMOVAL_H_
|
||||
|
||||
@@ -47,60 +47,62 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
|
||||
* information check:
|
||||
* <ul>
|
||||
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
|
||||
* Towards 3D Point Cloud Based Object Maps for Household Environments
|
||||
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
|
||||
* </ul>
|
||||
*
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
|
||||
* information check:
|
||||
* <ul>
|
||||
* <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
|
||||
* Towards 3D Point Cloud Based Object Maps for Household Environments
|
||||
* Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
|
||||
* </ul>
|
||||
*
|
||||
* \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class StatisticalOutlierRemoval : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>> srv_;
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class StatisticalOutlierRemoval : public Filter
|
||||
inline void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output)
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig> > srv_;
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud(pcl_input);
|
||||
impl_.setIndices(indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter(pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
inline void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output)
|
||||
{
|
||||
boost::mutex::scoped_lock lock (mutex_);
|
||||
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
|
||||
pcl_conversions::toPCL(*(input), *(pcl_input));
|
||||
impl_.setInputCloud (pcl_input);
|
||||
impl_.setIndices (indices);
|
||||
pcl::PCLPointCloud2 pcl_output;
|
||||
impl_.filter (pcl_output);
|
||||
pcl_conversions::moveFromPCL(pcl_output, output);
|
||||
}
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool child_init (ros::NodeHandle &nh, bool &has_service);
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level);
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_FILTERS_STATISTICALOUTLIERREMOVAL_H_
|
||||
|
||||
@@ -47,43 +47,45 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class VoxelGrid : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>> srv_;
|
||||
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
class VoxelGrid : public Filter
|
||||
{
|
||||
protected:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_;
|
||||
virtual void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output);
|
||||
|
||||
/** \brief The PCL filter implementation used. */
|
||||
pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool
|
||||
child_init(ros::NodeHandle & nh, bool & has_service);
|
||||
|
||||
/** \brief Call the actual filter.
|
||||
* \param input the input point cloud dataset
|
||||
* \param indices the input set of indices to use from \a input
|
||||
* \param output the resultant filtered dataset
|
||||
*/
|
||||
virtual void
|
||||
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
|
||||
PointCloud2 &output);
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Child initialization routine.
|
||||
* \param nh ROS node handle
|
||||
* \param has_service set to true if the child has a Dynamic Reconfigure service
|
||||
*/
|
||||
bool
|
||||
child_init (ros::NodeHandle &nh, bool &has_service);
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void
|
||||
config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level);
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_FILTERS_VOXEL_H_
|
||||
|
||||
@@ -46,178 +46,176 @@ using pcl_conversions::toPCL;
|
||||
namespace pcl_ros
|
||||
{
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template <typename PointT> void
|
||||
transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
|
||||
template<typename PointT>
|
||||
void
|
||||
transformPointCloudWithNormals(
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform)
|
||||
{
|
||||
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
|
||||
// of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
|
||||
// this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
|
||||
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
|
||||
// the conversion of the point cloud anyway. Idem for the origin.
|
||||
tf::Quaternion q = transform.getRotation ();
|
||||
Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w)
|
||||
tf::Vector3 v = transform.getOrigin ();
|
||||
Eigen::Vector3f origin (v.x (), v.y (), v.z ());
|
||||
tf::Quaternion q = transform.getRotation();
|
||||
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
|
||||
tf::Vector3 v = transform.getOrigin();
|
||||
Eigen::Vector3f origin(v.x(), v.y(), v.z());
|
||||
// Eigen::Translation3f translation(v);
|
||||
// Assemble an Eigen Transform
|
||||
//Eigen::Transform3f t;
|
||||
//t = translation * rotation;
|
||||
transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation);
|
||||
transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template <typename PointT> void
|
||||
transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
|
||||
template<typename PointT>
|
||||
void
|
||||
transformPointCloud(
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform)
|
||||
{
|
||||
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
|
||||
// of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
|
||||
// this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
|
||||
// Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
|
||||
// the conversion of the point cloud anyway. Idem for the origin.
|
||||
tf::Quaternion q = transform.getRotation ();
|
||||
Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ()); // internally stored as (x,y,z,w)
|
||||
tf::Vector3 v = transform.getOrigin ();
|
||||
Eigen::Vector3f origin (v.x (), v.y (), v.z ());
|
||||
tf::Quaternion q = transform.getRotation();
|
||||
Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w)
|
||||
tf::Vector3 v = transform.getOrigin();
|
||||
Eigen::Vector3f origin(v.x(), v.y(), v.z());
|
||||
// Eigen::Translation3f translation(v);
|
||||
// Assemble an Eigen Transform
|
||||
//Eigen::Transform3f t;
|
||||
//t = translation * rotation;
|
||||
transformPointCloud (cloud_in, cloud_out, origin, rotation);
|
||||
transformPointCloud(cloud_in, cloud_out, origin, rotation);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template <typename PointT> bool
|
||||
transformPointCloudWithNormals (const std::string &target_frame,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener)
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener)
|
||||
{
|
||||
if (cloud_in.header.frame_id == target_frame)
|
||||
{
|
||||
if (cloud_in.header.frame_id == target_frame) {
|
||||
cloud_out = cloud_in;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
|
||||
}
|
||||
catch (tf::LookupException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
}
|
||||
catch (tf::ExtrapolationException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
try {
|
||||
tf_listener.lookupTransform(
|
||||
target_frame, cloud_in.header.frame_id, fromPCL(
|
||||
cloud_in.header).stamp, transform);
|
||||
} catch (tf::LookupException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
} catch (tf::ExtrapolationException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
transformPointCloudWithNormals (cloud_in, cloud_out, transform);
|
||||
transformPointCloudWithNormals(cloud_in, cloud_out, transform);
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template <typename PointT> bool
|
||||
transformPointCloud (const std::string &target_frame,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener)
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener)
|
||||
{
|
||||
if (cloud_in.header.frame_id == target_frame)
|
||||
{
|
||||
if (cloud_in.header.frame_id == target_frame) {
|
||||
cloud_out = cloud_in;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
|
||||
try {
|
||||
tf_listener.lookupTransform(
|
||||
target_frame, cloud_in.header.frame_id, fromPCL(
|
||||
cloud_in.header).stamp, transform);
|
||||
} catch (tf::LookupException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
} catch (tf::ExtrapolationException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf::LookupException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
}
|
||||
catch (tf::ExtrapolationException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
}
|
||||
transformPointCloud (cloud_in, cloud_out, transform);
|
||||
transformPointCloud(cloud_in, cloud_out, transform);
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template <typename PointT> bool
|
||||
transformPointCloud (const std::string &target_frame,
|
||||
const ros::Time & target_time,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
const std::string &fixed_frame,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener)
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const ros::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener)
|
||||
{
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
|
||||
}
|
||||
catch (tf::LookupException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
}
|
||||
catch (tf::ExtrapolationException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
try {
|
||||
tf_listener.lookupTransform(
|
||||
target_frame, target_time, cloud_in.header.frame_id,
|
||||
fromPCL(cloud_in.header).stamp, fixed_frame, transform);
|
||||
} catch (tf::LookupException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
} catch (tf::ExtrapolationException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
transformPointCloud (cloud_in, cloud_out, transform);
|
||||
transformPointCloud(cloud_in, cloud_out, transform);
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
std_msgs::Header header;
|
||||
header.stamp = target_time;
|
||||
cloud_out.header = toPCL(header);
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
template <typename PointT> bool
|
||||
transformPointCloudWithNormals (const std::string &target_frame,
|
||||
const ros::Time & target_time,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
const std::string &fixed_frame,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener)
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const ros::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener)
|
||||
{
|
||||
tf::StampedTransform transform;
|
||||
try
|
||||
{
|
||||
tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
|
||||
}
|
||||
catch (tf::LookupException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
}
|
||||
catch (tf::ExtrapolationException &e)
|
||||
{
|
||||
ROS_ERROR ("%s", e.what ());
|
||||
return (false);
|
||||
try {
|
||||
tf_listener.lookupTransform(
|
||||
target_frame, target_time, cloud_in.header.frame_id,
|
||||
fromPCL(cloud_in.header).stamp, fixed_frame, transform);
|
||||
} catch (tf::LookupException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
} catch (tf::ExtrapolationException & e) {
|
||||
ROS_ERROR("%s", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
transformPointCloudWithNormals (cloud_in, cloud_out, transform);
|
||||
transformPointCloudWithNormals(cloud_in, cloud_out, transform);
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
std_msgs::Header header;
|
||||
header.stamp = target_time;
|
||||
cloud_out.header = toPCL(header);
|
||||
return (true);
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace pcl_ros
|
||||
|
||||
@@ -45,85 +45,86 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief BAG PointCloud file format reader.
|
||||
* \author Radu Bogdan Rusu
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief BAG PointCloud file format reader.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class BAGReader : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
BAGReader()
|
||||
: publish_rate_(0), output_() /*, cloud_received_ (false)*/ {}
|
||||
|
||||
/** \brief Set the publishing rate in seconds.
|
||||
* \param publish_rate the publishing rate in seconds
|
||||
*/
|
||||
class BAGReader: public nodelet::Nodelet
|
||||
inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
|
||||
|
||||
/** \brief Get the publishing rate in seconds. */
|
||||
inline double getPublishRate() {return publish_rate_;}
|
||||
|
||||
/** \brief Get the next point cloud dataset in the BAG file.
|
||||
* \return the next point cloud dataset as read from the file
|
||||
*/
|
||||
inline PointCloudConstPtr
|
||||
getNextCloud()
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
if (it_ != view_.end()) {
|
||||
output_ = it_->instantiate<sensor_msgs::PointCloud2>();
|
||||
++it_;
|
||||
}
|
||||
return output_;
|
||||
}
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
BAGReader () : publish_rate_ (0), output_ ()/*, cloud_received_ (false)*/ {};
|
||||
/** \brief Open a BAG file for reading and select a specified topic
|
||||
* \param file_name the BAG file to open
|
||||
* \param topic_name the topic that we want to read data from
|
||||
*/
|
||||
bool open(const std::string & file_name, const std::string & topic_name);
|
||||
|
||||
/** \brief Set the publishing rate in seconds.
|
||||
* \param publish_rate the publishing rate in seconds
|
||||
*/
|
||||
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
|
||||
/** \brief Close an open BAG file. */
|
||||
inline void
|
||||
close()
|
||||
{
|
||||
bag_.close();
|
||||
}
|
||||
|
||||
/** \brief Get the publishing rate in seconds. */
|
||||
inline double getPublishRate () { return (publish_rate_); }
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief Get the next point cloud dataset in the BAG file.
|
||||
* \return the next point cloud dataset as read from the file
|
||||
*/
|
||||
inline PointCloudConstPtr
|
||||
getNextCloud ()
|
||||
{
|
||||
if (it_ != view_.end ())
|
||||
{
|
||||
output_ = it_->instantiate<sensor_msgs::PointCloud2> ();
|
||||
++it_;
|
||||
}
|
||||
return (output_);
|
||||
}
|
||||
private:
|
||||
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
|
||||
double publish_rate_;
|
||||
|
||||
/** \brief Open a BAG file for reading and select a specified topic
|
||||
* \param file_name the BAG file to open
|
||||
* \param topic_name the topic that we want to read data from
|
||||
*/
|
||||
bool open (const std::string &file_name, const std::string &topic_name);
|
||||
/** \brief The BAG object. */
|
||||
rosbag::Bag bag_;
|
||||
|
||||
/** \brief Close an open BAG file. */
|
||||
inline void
|
||||
close ()
|
||||
{
|
||||
bag_.close ();
|
||||
}
|
||||
/** \brief The BAG view object. */
|
||||
rosbag::View view_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
/** \brief The BAG view iterator object. */
|
||||
rosbag::View::iterator it_;
|
||||
|
||||
private:
|
||||
/** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
|
||||
double publish_rate_;
|
||||
/** \brief The name of the topic that contains the PointCloud data. */
|
||||
std::string topic_name_;
|
||||
|
||||
/** \brief The BAG object. */
|
||||
rosbag::Bag bag_;
|
||||
/** \brief The name of the BAG file that contains the PointCloud data. */
|
||||
std::string file_name_;
|
||||
|
||||
/** \brief The BAG view object. */
|
||||
rosbag::View view_;
|
||||
/** \brief The output point cloud dataset containing the points loaded from the file. */
|
||||
PointCloudPtr output_;
|
||||
|
||||
/** \brief The BAG view iterator object. */
|
||||
rosbag::View::iterator it_;
|
||||
/** \brief Signals that a new PointCloud2 message has been read from the file. */
|
||||
//bool cloud_received_;
|
||||
|
||||
/** \brief The name of the topic that contains the PointCloud data. */
|
||||
std::string topic_name_;
|
||||
|
||||
/** \brief The name of the BAG file that contains the PointCloud data. */
|
||||
std::string file_name_;
|
||||
|
||||
/** \brief The output point cloud dataset containing the points loaded from the file. */
|
||||
PointCloudPtr output_;
|
||||
|
||||
/** \brief Signals that a new PointCloud2 message has been read from the file. */
|
||||
//bool cloud_received_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -49,88 +49,94 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
|
||||
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
|
||||
* checks their timestamps, and concatenates their fields together into a single
|
||||
* PointCloud output message.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
|
||||
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
|
||||
* checks their timestamps, and concatenates their fields together into a single
|
||||
* PointCloud output message.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PointCloudConcatenateDataSynchronizer : public nodelet_topic_tools::NodeletLazy
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef PointCloud2::Ptr PointCloud2Ptr;
|
||||
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PointCloudConcatenateDataSynchronizer()
|
||||
: maximum_queue_size_(3) {}
|
||||
|
||||
/** \brief Empty constructor.
|
||||
* \param queue_size the maximum queue size
|
||||
*/
|
||||
class PointCloudConcatenateDataSynchronizer: public nodelet_topic_tools::NodeletLazy
|
||||
PointCloudConcatenateDataSynchronizer(int queue_size)
|
||||
: maximum_queue_size_(queue_size), approximate_sync_(false) {}
|
||||
|
||||
/** \brief Empty destructor. */
|
||||
virtual ~PointCloudConcatenateDataSynchronizer() {}
|
||||
|
||||
void onInit();
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
|
||||
private:
|
||||
/** \brief The output PointCloud publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
|
||||
/** \brief The maximum number of messages that we can store in the queue. */
|
||||
int maximum_queue_size_;
|
||||
|
||||
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
|
||||
bool approximate_sync_;
|
||||
|
||||
/** \brief A vector of message filters. */
|
||||
std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2>>> filters_;
|
||||
|
||||
/** \brief Output TF frame the concatenated points should be transformed to. */
|
||||
std::string output_frame_;
|
||||
|
||||
/** \brief Input point cloud topics. */
|
||||
XmlRpc::XmlRpcValue input_topics_;
|
||||
|
||||
/** \brief TF listener object. */
|
||||
tf::TransformListener tf_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointCloud2> nf_;
|
||||
|
||||
/** \brief Synchronizer.
|
||||
* \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
|
||||
*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2>>> ts_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2>>> ts_e_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback(const PointCloud2ConstPtr & input)
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef PointCloud2::Ptr PointCloud2Ptr;
|
||||
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
|
||||
PointCloud2 cloud;
|
||||
cloud.header.stamp = input->header.stamp;
|
||||
nf_.add(boost::make_shared<PointCloud2>(cloud));
|
||||
}
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {};
|
||||
/** \brief Input callback for 8 synchronized topics. */
|
||||
void input(
|
||||
const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2,
|
||||
const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4,
|
||||
const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6,
|
||||
const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8);
|
||||
|
||||
/** \brief Empty constructor.
|
||||
* \param queue_size the maximum queue size
|
||||
*/
|
||||
PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {};
|
||||
|
||||
/** \brief Empty destructor. */
|
||||
virtual ~PointCloudConcatenateDataSynchronizer () {};
|
||||
|
||||
void onInit ();
|
||||
void subscribe ();
|
||||
void unsubscribe ();
|
||||
|
||||
private:
|
||||
/** \brief The output PointCloud publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
|
||||
/** \brief The maximum number of messages that we can store in the queue. */
|
||||
int maximum_queue_size_;
|
||||
|
||||
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
|
||||
bool approximate_sync_;
|
||||
|
||||
/** \brief A vector of message filters. */
|
||||
std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2> > > filters_;
|
||||
|
||||
/** \brief Output TF frame the concatenated points should be transformed to. */
|
||||
std::string output_frame_;
|
||||
|
||||
/** \brief Input point cloud topics. */
|
||||
XmlRpc::XmlRpcValue input_topics_;
|
||||
|
||||
/** \brief TF listener object. */
|
||||
tf::TransformListener tf_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointCloud2> nf_;
|
||||
|
||||
/** \brief Synchronizer.
|
||||
* \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
|
||||
*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_e_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloud2ConstPtr &input)
|
||||
{
|
||||
PointCloud2 cloud;
|
||||
cloud.header.stamp = input->header.stamp;
|
||||
nf_.add (boost::make_shared<PointCloud2> (cloud));
|
||||
}
|
||||
|
||||
/** \brief Input callback for 8 synchronized topics. */
|
||||
void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
|
||||
const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
|
||||
const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
|
||||
const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8);
|
||||
|
||||
void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out);
|
||||
};
|
||||
void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out);
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_IO_CONCATENATE_H_
|
||||
|
||||
@@ -49,53 +49,55 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
|
||||
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
|
||||
* a single PointCloud output message.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
|
||||
* input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
|
||||
* a single PointCloud output message.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PointCloudConcatenateFieldsSynchronizer : public nodelet_topic_tools::NodeletLazy
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PointCloudConcatenateFieldsSynchronizer()
|
||||
: maximum_queue_size_(3), maximum_seconds_(0) {}
|
||||
|
||||
/** \brief Empty constructor.
|
||||
* \param queue_size the maximum queue size
|
||||
*/
|
||||
class PointCloudConcatenateFieldsSynchronizer: public nodelet_topic_tools::NodeletLazy
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
PointCloudConcatenateFieldsSynchronizer(int queue_size)
|
||||
: maximum_queue_size_(queue_size), maximum_seconds_(0) {}
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PointCloudConcatenateFieldsSynchronizer () : maximum_queue_size_ (3), maximum_seconds_ (0) {};
|
||||
/** \brief Empty destructor. */
|
||||
virtual ~PointCloudConcatenateFieldsSynchronizer() {}
|
||||
|
||||
/** \brief Empty constructor.
|
||||
* \param queue_size the maximum queue size
|
||||
*/
|
||||
PointCloudConcatenateFieldsSynchronizer (int queue_size) : maximum_queue_size_ (queue_size), maximum_seconds_ (0) {};
|
||||
void onInit();
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
void input_callback(const PointCloudConstPtr & cloud);
|
||||
|
||||
/** \brief Empty destructor. */
|
||||
virtual ~PointCloudConcatenateFieldsSynchronizer () {};
|
||||
private:
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
void onInit ();
|
||||
void subscribe ();
|
||||
void unsubscribe ();
|
||||
void input_callback (const PointCloudConstPtr &cloud);
|
||||
/** \brief The output PointCloud publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
|
||||
private:
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
/** \brief The number of input messages that we expect on the input topic. */
|
||||
int input_messages_;
|
||||
|
||||
/** \brief The output PointCloud publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
/** \brief The maximum number of messages that we can store in the queue. */
|
||||
int maximum_queue_size_;
|
||||
|
||||
/** \brief The number of input messages that we expect on the input topic. */
|
||||
int input_messages_;
|
||||
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
|
||||
double maximum_seconds_;
|
||||
|
||||
/** \brief The maximum number of messages that we can store in the queue. */
|
||||
int maximum_queue_size_;
|
||||
|
||||
/** \brief The maximum number of seconds to wait until we drop the synchronization. */
|
||||
double maximum_seconds_;
|
||||
|
||||
/** \brief A queue for messages. */
|
||||
std::map<ros::Time, std::vector<PointCloudConstPtr> > queue_;
|
||||
};
|
||||
/** \brief A queue for messages. */
|
||||
std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_IO_CONCATENATE_H_
|
||||
|
||||
@@ -43,90 +43,94 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief Point Cloud Data (PCD) file format reader.
|
||||
* \author Radu Bogdan Rusu
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief Point Cloud Data (PCD) file format reader.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PCDReader : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef PointCloud2::Ptr PointCloud2Ptr;
|
||||
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PCDReader()
|
||||
: publish_rate_(0), tf_frame_("/base_link") {}
|
||||
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief Set the publishing rate in seconds.
|
||||
* \param publish_rate the publishing rate in seconds
|
||||
*/
|
||||
class PCDReader : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef PointCloud2::Ptr PointCloud2Ptr;
|
||||
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
|
||||
inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PCDReader () : publish_rate_ (0), tf_frame_ ("/base_link") {};
|
||||
/** \brief Get the publishing rate in seconds. */
|
||||
inline double getPublishRate() {return publish_rate_;}
|
||||
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief Set the publishing rate in seconds.
|
||||
* \param publish_rate the publishing rate in seconds
|
||||
*/
|
||||
inline void setPublishRate (double publish_rate) { publish_rate_ = publish_rate; }
|
||||
|
||||
/** \brief Get the publishing rate in seconds. */
|
||||
inline double getPublishRate () { return (publish_rate_); }
|
||||
|
||||
/** \brief Set the TF frame the PointCloud will be published in.
|
||||
* \param tf_frame the TF frame the PointCloud will be published in
|
||||
*/
|
||||
inline void setTFframe (std::string tf_frame) { tf_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the PointCloud is published in. */
|
||||
inline std::string getTFframe () { return (tf_frame_); }
|
||||
|
||||
protected:
|
||||
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
|
||||
double publish_rate_;
|
||||
|
||||
/** \brief The TF frame the data should be published in ("/base_link" by default). */
|
||||
std::string tf_frame_;
|
||||
|
||||
/** \brief The name of the file that contains the PointCloud data. */
|
||||
std::string file_name_;
|
||||
|
||||
/** \brief The output point cloud dataset containing the points loaded from the file. */
|
||||
PointCloud2Ptr output_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::PCDReader impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief Point Cloud Data (PCD) file format writer.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief Set the TF frame the PointCloud will be published in.
|
||||
* \param tf_frame the TF frame the PointCloud will be published in
|
||||
*/
|
||||
class PCDWriter : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
PCDWriter () : file_name_ (""), binary_mode_ (true) {}
|
||||
inline void setTFframe(std::string tf_frame) {tf_frame_ = tf_frame;}
|
||||
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef PointCloud2::Ptr PointCloud2Ptr;
|
||||
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
|
||||
/** \brief Get the TF frame the PointCloud is published in. */
|
||||
inline std::string getTFframe() {return tf_frame_;}
|
||||
|
||||
virtual void onInit ();
|
||||
void input_callback (const PointCloud2ConstPtr &cloud);
|
||||
protected:
|
||||
/** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
|
||||
double publish_rate_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
/** \brief The TF frame the data should be published in ("/base_link" by default). */
|
||||
std::string tf_frame_;
|
||||
|
||||
protected:
|
||||
/** \brief The name of the file that contains the PointCloud data. */
|
||||
std::string file_name_;
|
||||
/** \brief The name of the file that contains the PointCloud data. */
|
||||
std::string file_name_;
|
||||
|
||||
/** \brief Set to true if the output files should be saved in binary mode (true). */
|
||||
bool binary_mode_;
|
||||
/** \brief The output point cloud dataset containing the points loaded from the file. */
|
||||
PointCloud2Ptr output_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::PCDWriter impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::PCDReader impl_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief Point Cloud Data (PCD) file format writer.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class PCDWriter : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
PCDWriter()
|
||||
: file_name_(""), binary_mode_(true) {}
|
||||
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef PointCloud2::Ptr PointCloud2Ptr;
|
||||
typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
|
||||
|
||||
virtual void onInit();
|
||||
void input_callback(const PointCloud2ConstPtr & cloud);
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
protected:
|
||||
/** \brief The name of the file that contains the PointCloud data. */
|
||||
std::string file_name_;
|
||||
|
||||
/** \brief Set to true if the output files should be saved in binary mode (true). */
|
||||
bool binary_mode_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::PCDWriter impl_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_IO_PCD_IO_H_
|
||||
|
||||
@@ -66,168 +66,177 @@ using pcl_conversions::fromPCL;
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
|
||||
class PCLNodelet : public nodelet_topic_tools::NodeletLazy
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class. */
|
||||
class PCLNodelet : public nodelet_topic_tools::NodeletLazy
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
|
||||
typedef pcl_msgs::PointIndices PointIndices;
|
||||
typedef PointIndices::Ptr PointIndicesPtr;
|
||||
typedef PointIndices::ConstPtr PointIndicesConstPtr;
|
||||
|
||||
typedef pcl_msgs::ModelCoefficients ModelCoefficients;
|
||||
typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
|
||||
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
typedef pcl::IndicesConstPtr IndicesConstPtr;
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PCLNodelet()
|
||||
: use_indices_(false), latched_indices_(false),
|
||||
max_queue_size_(3), approximate_sync_(false) {}
|
||||
|
||||
protected:
|
||||
/** \brief Set to true if point indices are used.
|
||||
*
|
||||
* When receiving a point cloud, if use_indices_ is false, the entire
|
||||
* point cloud is processed for the given operation. If use_indices_ is
|
||||
* true, then the ~indices topic is read to get the vector of point
|
||||
* indices specifying the subset of the point cloud that will be used for
|
||||
* the operation. In the case where use_indices_ is true, the ~input and
|
||||
* ~indices topics must be synchronised in time, either exact or within a
|
||||
* specified jitter. See also @ref latched_indices_ and approximate_sync.
|
||||
**/
|
||||
bool use_indices_;
|
||||
/** \brief Set to true if the indices topic is latched.
|
||||
*
|
||||
* If use_indices_ is true, the ~input and ~indices topics generally must
|
||||
* be synchronised in time. By setting this flag to true, the most recent
|
||||
* value from ~indices can be used instead of requiring a synchronised
|
||||
* message.
|
||||
**/
|
||||
bool latched_indices_;
|
||||
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_input_filter_;
|
||||
|
||||
/** \brief The message filter subscriber for PointIndices. */
|
||||
message_filters::Subscriber<PointIndices> sub_indices_filter_;
|
||||
|
||||
/** \brief The output PointCloud publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
|
||||
/** \brief The maximum queue size (default: 3). */
|
||||
int max_queue_size_;
|
||||
|
||||
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
|
||||
bool approximate_sync_;
|
||||
|
||||
/** \brief TF listener object. */
|
||||
tf::TransformListener tf_listener_;
|
||||
|
||||
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
|
||||
* \param cloud the point cloud to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "input")
|
||||
*/
|
||||
inline bool
|
||||
isValid(const PointCloud2::ConstPtr & cloud, const std::string & topic_name = "input")
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
|
||||
NODELET_WARN(
|
||||
"[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!",
|
||||
getName().c_str(),
|
||||
cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
topic_name).c_str());
|
||||
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl_msgs::PointIndices PointIndices;
|
||||
typedef PointIndices::Ptr PointIndicesPtr;
|
||||
typedef PointIndices::ConstPtr PointIndicesConstPtr;
|
||||
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
|
||||
* \param cloud the point cloud to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "input")
|
||||
*/
|
||||
inline bool
|
||||
isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input")
|
||||
{
|
||||
if (cloud->width * cloud->height != cloud->points.size()) {
|
||||
NODELET_WARN(
|
||||
"[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!",
|
||||
getName().c_str(), cloud->points.size(), cloud->width, cloud->height,
|
||||
fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
|
||||
pnh_->resolveName(topic_name).c_str());
|
||||
|
||||
typedef pcl_msgs::ModelCoefficients ModelCoefficients;
|
||||
typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
|
||||
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
typedef pcl::IndicesConstPtr IndicesConstPtr;
|
||||
/** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
|
||||
* \param indices the point indices message to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "indices")
|
||||
*/
|
||||
inline bool
|
||||
isValid(const PointIndicesConstPtr & indices, const std::string & topic_name = "indices")
|
||||
{
|
||||
/*if (indices->indices.empty ())
|
||||
{
|
||||
NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
|
||||
return (true);
|
||||
}*/
|
||||
return true;
|
||||
}
|
||||
|
||||
/** \brief Empty constructor. */
|
||||
PCLNodelet () : use_indices_ (false), latched_indices_ (false),
|
||||
max_queue_size_ (3), approximate_sync_ (false) {};
|
||||
/** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
|
||||
* \param model the model coefficients to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "model")
|
||||
*/
|
||||
inline bool
|
||||
isValid(const ModelCoefficientsConstPtr & model, const std::string & topic_name = "model")
|
||||
{
|
||||
/*if (model->values.empty ())
|
||||
{
|
||||
NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
|
||||
return (false);
|
||||
}*/
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
/** \brief Set to true if point indices are used.
|
||||
*
|
||||
* When receiving a point cloud, if use_indices_ is false, the entire
|
||||
* point cloud is processed for the given operation. If use_indices_ is
|
||||
* true, then the ~indices topic is read to get the vector of point
|
||||
* indices specifying the subset of the point cloud that will be used for
|
||||
* the operation. In the case where use_indices_ is true, the ~input and
|
||||
* ~indices topics must be synchronised in time, either exact or within a
|
||||
* specified jitter. See also @ref latched_indices_ and approximate_sync.
|
||||
**/
|
||||
bool use_indices_;
|
||||
/** \brief Set to true if the indices topic is latched.
|
||||
*
|
||||
* If use_indices_ is true, the ~input and ~indices topics generally must
|
||||
* be synchronised in time. By setting this flag to true, the most recent
|
||||
* value from ~indices can be used instead of requiring a synchronised
|
||||
* message.
|
||||
**/
|
||||
bool latched_indices_;
|
||||
/** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
|
||||
virtual void subscribe() {}
|
||||
virtual void unsubscribe() {}
|
||||
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_input_filter_;
|
||||
/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
|
||||
virtual void
|
||||
onInit()
|
||||
{
|
||||
nodelet_topic_tools::NodeletLazy::onInit();
|
||||
|
||||
/** \brief The message filter subscriber for PointIndices. */
|
||||
message_filters::Subscriber<PointIndices> sub_indices_filter_;
|
||||
// Parameters that we care about only at startup
|
||||
pnh_->getParam("max_queue_size", max_queue_size_);
|
||||
|
||||
/** \brief The output PointCloud publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam("use_indices", use_indices_);
|
||||
pnh_->getParam("latched_indices", latched_indices_);
|
||||
pnh_->getParam("approximate_sync", approximate_sync_);
|
||||
|
||||
/** \brief The maximum queue size (default: 3). */
|
||||
int max_queue_size_;
|
||||
NODELET_DEBUG(
|
||||
"[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
|
||||
" - approximate_sync : %s\n"
|
||||
" - use_indices : %s\n"
|
||||
" - latched_indices : %s\n"
|
||||
" - max_queue_size : %d",
|
||||
getName().c_str(),
|
||||
(approximate_sync_) ? "true" : "false",
|
||||
(use_indices_) ? "true" : "false",
|
||||
(latched_indices_) ? "true" : "false",
|
||||
max_queue_size_);
|
||||
}
|
||||
|
||||
/** \brief True if we use an approximate time synchronizer versus an exact one (false by default). */
|
||||
bool approximate_sync_;
|
||||
|
||||
/** \brief TF listener object. */
|
||||
tf::TransformListener tf_listener_;
|
||||
|
||||
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
|
||||
* \param cloud the point cloud to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "input")
|
||||
*/
|
||||
inline bool
|
||||
isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input")
|
||||
{
|
||||
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
|
||||
{
|
||||
NODELET_WARN ("[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->data.size (), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
|
||||
|
||||
return (false);
|
||||
}
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
|
||||
* \param cloud the point cloud to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "input")
|
||||
*/
|
||||
inline bool
|
||||
isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input")
|
||||
{
|
||||
if (cloud->width * cloud->height != cloud->points.size ())
|
||||
{
|
||||
NODELET_WARN ("[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->points.size (), cloud->width, cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
|
||||
|
||||
return (false);
|
||||
}
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
|
||||
* \param indices the point indices message to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "indices")
|
||||
*/
|
||||
inline bool
|
||||
isValid (const PointIndicesConstPtr &indices, const std::string &topic_name = "indices")
|
||||
{
|
||||
/*if (indices->indices.empty ())
|
||||
{
|
||||
NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
|
||||
return (true);
|
||||
}*/
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
|
||||
* \param model the model coefficients to test
|
||||
* \param topic_name an optional topic name (only used for printing, defaults to "model")
|
||||
*/
|
||||
inline bool
|
||||
isValid (const ModelCoefficientsConstPtr &model, const std::string &topic_name = "model")
|
||||
{
|
||||
/*if (model->values.empty ())
|
||||
{
|
||||
NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
|
||||
return (false);
|
||||
}*/
|
||||
return (true);
|
||||
}
|
||||
|
||||
/** \brief Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility. */
|
||||
virtual void subscribe () {}
|
||||
virtual void unsubscribe () {}
|
||||
|
||||
/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
|
||||
virtual void
|
||||
onInit ()
|
||||
{
|
||||
nodelet_topic_tools::NodeletLazy::onInit();
|
||||
|
||||
// Parameters that we care about only at startup
|
||||
pnh_->getParam ("max_queue_size", max_queue_size_);
|
||||
|
||||
// ---[ Optional parameters
|
||||
pnh_->getParam ("use_indices", use_indices_);
|
||||
pnh_->getParam ("latched_indices", latched_indices_);
|
||||
pnh_->getParam ("approximate_sync", approximate_sync_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
|
||||
" - approximate_sync : %s\n"
|
||||
" - use_indices : %s\n"
|
||||
" - latched_indices : %s\n"
|
||||
" - max_queue_size : %d",
|
||||
getName ().c_str (),
|
||||
(approximate_sync_) ? "true" : "false",
|
||||
(use_indices_) ? "true" : "false",
|
||||
(latched_indices_) ? "true" : "false",
|
||||
max_queue_size_);
|
||||
}
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_NODELET_H_
|
||||
|
||||
@@ -11,286 +11,295 @@
|
||||
#include <boost/mpl/size.hpp>
|
||||
#include <boost/ref.hpp>
|
||||
|
||||
namespace pcl
|
||||
namespace pcl
|
||||
{
|
||||
namespace detail
|
||||
namespace detail
|
||||
{
|
||||
template<typename Stream, typename PointT>
|
||||
struct FieldStreamer
|
||||
{
|
||||
FieldStreamer(Stream & stream)
|
||||
: stream_(stream) {}
|
||||
|
||||
template<typename U>
|
||||
void operator()()
|
||||
{
|
||||
template<typename Stream, typename PointT>
|
||||
struct FieldStreamer
|
||||
{
|
||||
FieldStreamer(Stream& stream) : stream_(stream) {}
|
||||
const char * name = traits::name<PointT, U>::value;
|
||||
std::uint32_t name_length = strlen(name);
|
||||
stream_.next(name_length);
|
||||
if (name_length > 0) {
|
||||
memcpy(stream_.advance(name_length), name, name_length);
|
||||
}
|
||||
|
||||
template<typename U> void operator() ()
|
||||
{
|
||||
const char* name = traits::name<PointT, U>::value;
|
||||
std::uint32_t name_length = strlen(name);
|
||||
stream_.next(name_length);
|
||||
if (name_length > 0)
|
||||
memcpy(stream_.advance(name_length), name, name_length);
|
||||
std::uint32_t offset = traits::offset<PointT, U>::value;
|
||||
stream_.next(offset);
|
||||
|
||||
std::uint32_t offset = traits::offset<PointT, U>::value;
|
||||
stream_.next(offset);
|
||||
std::uint8_t datatype = traits::datatype<PointT, U>::value;
|
||||
stream_.next(datatype);
|
||||
|
||||
std::uint8_t datatype = traits::datatype<PointT, U>::value;
|
||||
stream_.next(datatype);
|
||||
std::uint32_t count = traits::datatype<PointT, U>::size;
|
||||
stream_.next(count);
|
||||
}
|
||||
|
||||
std::uint32_t count = traits::datatype<PointT, U>::size;
|
||||
stream_.next(count);
|
||||
}
|
||||
Stream & stream_;
|
||||
};
|
||||
|
||||
Stream& stream_;
|
||||
};
|
||||
template<typename PointT>
|
||||
struct FieldsLength
|
||||
{
|
||||
FieldsLength()
|
||||
: length(0) {}
|
||||
|
||||
template<typename PointT>
|
||||
struct FieldsLength
|
||||
{
|
||||
FieldsLength() : length(0) {}
|
||||
template<typename U>
|
||||
void operator()()
|
||||
{
|
||||
std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
|
||||
length += name_length + 13;
|
||||
}
|
||||
|
||||
template<typename U> void operator() ()
|
||||
{
|
||||
std::uint32_t name_length = strlen(traits::name<PointT, U>::value);
|
||||
length += name_length + 13;
|
||||
}
|
||||
|
||||
std::uint32_t length;
|
||||
};
|
||||
} // namespace pcl::detail
|
||||
std::uint32_t length;
|
||||
};
|
||||
} // namespace pcl::detail
|
||||
} // namespace pcl
|
||||
|
||||
namespace ros
|
||||
namespace ros
|
||||
{
|
||||
// In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
|
||||
// on the subscriber side. This allows us to generate the mapping between message
|
||||
// data and object fields only once and reuse it.
|
||||
// In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
|
||||
// on the subscriber side. This allows us to generate the mapping between message
|
||||
// data and object fields only once and reuse it.
|
||||
#if ROS_VERSION_MINIMUM(1, 3, 1)
|
||||
template<typename T>
|
||||
struct DefaultMessageCreator<pcl::PointCloud<T> >
|
||||
{
|
||||
boost::shared_ptr<pcl::MsgFieldMap> mapping_;
|
||||
template<typename T>
|
||||
struct DefaultMessageCreator<pcl::PointCloud<T>>
|
||||
{
|
||||
boost::shared_ptr<pcl::MsgFieldMap> mapping_;
|
||||
|
||||
DefaultMessageCreator()
|
||||
: mapping_( boost::make_shared<pcl::MsgFieldMap>() )
|
||||
{
|
||||
}
|
||||
|
||||
boost::shared_ptr<pcl::PointCloud<T> > operator() ()
|
||||
{
|
||||
boost::shared_ptr<pcl::PointCloud<T> > msg (new pcl::PointCloud<T> ());
|
||||
pcl::detail::getMapping(*msg) = mapping_;
|
||||
return msg;
|
||||
}
|
||||
};
|
||||
DefaultMessageCreator()
|
||||
: mapping_(boost::make_shared<pcl::MsgFieldMap>() )
|
||||
{
|
||||
}
|
||||
|
||||
boost::shared_ptr<pcl::PointCloud<T>> operator()()
|
||||
{
|
||||
boost::shared_ptr<pcl::PointCloud<T>> msg(new pcl::PointCloud<T>());
|
||||
pcl::detail::getMapping(*msg) = mapping_;
|
||||
return msg;
|
||||
}
|
||||
};
|
||||
#endif
|
||||
|
||||
namespace message_traits
|
||||
namespace message_traits
|
||||
{
|
||||
template<typename T>
|
||||
struct MD5Sum<pcl::PointCloud<T>>
|
||||
{
|
||||
static const char * value() {return MD5Sum<sensor_msgs::PointCloud2>::value();}
|
||||
static const char * value(const pcl::PointCloud<T> &) {return value();}
|
||||
|
||||
static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
|
||||
static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
|
||||
|
||||
// If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
|
||||
ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
|
||||
ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct DataType<pcl::PointCloud<T>>
|
||||
{
|
||||
static const char * value() {return DataType<sensor_msgs::PointCloud2>::value();}
|
||||
static const char * value(const pcl::PointCloud<T> &) {return value();}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct Definition<pcl::PointCloud<T>>
|
||||
{
|
||||
static const char * value() {return Definition<sensor_msgs::PointCloud2>::value();}
|
||||
static const char * value(const pcl::PointCloud<T> &) {return value();}
|
||||
};
|
||||
|
||||
// pcl point clouds message don't have a ROS compatible header
|
||||
// the specialized meta functions below (TimeStamp and FrameId)
|
||||
// can be used to get the header data.
|
||||
template<typename T>
|
||||
struct HasHeader<pcl::PointCloud<T>>: FalseType {};
|
||||
|
||||
template<typename T>
|
||||
struct TimeStamp<pcl::PointCloud<T>>
|
||||
{
|
||||
// This specialization could be dangerous, but it's the best I can do.
|
||||
// If this TimeStamp struct is destroyed before they are done with the
|
||||
// pointer returned by the first functions may go out of scope, but there
|
||||
// isn't a lot I can do about that. This is a good reason to refuse to
|
||||
// returning pointers like this...
|
||||
static ros::Time * pointer(typename pcl::PointCloud<T> & m)
|
||||
{
|
||||
template<typename T> struct MD5Sum<pcl::PointCloud<T> >
|
||||
{
|
||||
static const char* value() { return MD5Sum<sensor_msgs::PointCloud2>::value(); }
|
||||
static const char* value(const pcl::PointCloud<T>&) { return value(); }
|
||||
|
||||
static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
|
||||
static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
|
||||
|
||||
// If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
|
||||
ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
|
||||
ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
|
||||
};
|
||||
|
||||
template<typename T> struct DataType<pcl::PointCloud<T> >
|
||||
{
|
||||
static const char* value() { return DataType<sensor_msgs::PointCloud2>::value(); }
|
||||
static const char* value(const pcl::PointCloud<T>&) { return value(); }
|
||||
};
|
||||
|
||||
template<typename T> struct Definition<pcl::PointCloud<T> >
|
||||
{
|
||||
static const char* value() { return Definition<sensor_msgs::PointCloud2>::value(); }
|
||||
static const char* value(const pcl::PointCloud<T>&) { return value(); }
|
||||
};
|
||||
|
||||
// pcl point clouds message don't have a ROS compatible header
|
||||
// the specialized meta functions below (TimeStamp and FrameId)
|
||||
// can be used to get the header data.
|
||||
template<typename T> struct HasHeader<pcl::PointCloud<T> > : FalseType {};
|
||||
|
||||
template<typename T>
|
||||
struct TimeStamp<pcl::PointCloud<T> >
|
||||
{
|
||||
// This specialization could be dangerous, but it's the best I can do.
|
||||
// If this TimeStamp struct is destroyed before they are done with the
|
||||
// pointer returned by the first functions may go out of scope, but there
|
||||
// isn't a lot I can do about that. This is a good reason to refuse to
|
||||
// returning pointers like this...
|
||||
static ros::Time* pointer(typename pcl::PointCloud<T> &m) {
|
||||
header_.reset(new std_msgs::Header());
|
||||
pcl_conversions::fromPCL(m.header, *(header_));
|
||||
return &(header_->stamp);
|
||||
}
|
||||
static ros::Time const* pointer(const typename pcl::PointCloud<T>& m) {
|
||||
header_const_.reset(new std_msgs::Header());
|
||||
pcl_conversions::fromPCL(m.header, *(header_const_));
|
||||
return &(header_const_->stamp);
|
||||
}
|
||||
static ros::Time value(const typename pcl::PointCloud<T>& m) {
|
||||
return pcl_conversions::fromPCL(m.header).stamp;
|
||||
}
|
||||
private:
|
||||
static boost::shared_ptr<std_msgs::Header> header_;
|
||||
static boost::shared_ptr<std_msgs::Header> header_const_;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct FrameId<pcl::PointCloud<T> >
|
||||
{
|
||||
static std::string* pointer(pcl::PointCloud<T>& m) { return &m.header.frame_id; }
|
||||
static std::string const* pointer(const pcl::PointCloud<T>& m) { return &m.header.frame_id; }
|
||||
static std::string value(const pcl::PointCloud<T>& m) { return m.header.frame_id; }
|
||||
};
|
||||
|
||||
} // namespace ros::message_traits
|
||||
|
||||
namespace serialization
|
||||
header_.reset(new std_msgs::Header());
|
||||
pcl_conversions::fromPCL(m.header, *(header_));
|
||||
return &(header_->stamp);
|
||||
}
|
||||
static ros::Time const * pointer(const typename pcl::PointCloud<T> & m)
|
||||
{
|
||||
template<typename T>
|
||||
struct Serializer<pcl::PointCloud<T> >
|
||||
header_const_.reset(new std_msgs::Header());
|
||||
pcl_conversions::fromPCL(m.header, *(header_const_));
|
||||
return &(header_const_->stamp);
|
||||
}
|
||||
static ros::Time value(const typename pcl::PointCloud<T> & m)
|
||||
{
|
||||
return pcl_conversions::fromPCL(m.header).stamp;
|
||||
}
|
||||
|
||||
private:
|
||||
static boost::shared_ptr<std_msgs::Header> header_;
|
||||
static boost::shared_ptr<std_msgs::Header> header_const_;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct FrameId<pcl::PointCloud<T>>
|
||||
{
|
||||
static std::string * pointer(pcl::PointCloud<T> & m) {return &m.header.frame_id;}
|
||||
static std::string const * pointer(const pcl::PointCloud<T> & m) {return &m.header.frame_id;}
|
||||
static std::string value(const pcl::PointCloud<T> & m) {return m.header.frame_id;}
|
||||
};
|
||||
|
||||
} // namespace ros::message_traits
|
||||
|
||||
namespace serialization
|
||||
{
|
||||
template<typename T>
|
||||
struct Serializer<pcl::PointCloud<T>>
|
||||
{
|
||||
template<typename Stream>
|
||||
inline static void write(Stream & stream, const pcl::PointCloud<T> & m)
|
||||
{
|
||||
stream.next(m.header);
|
||||
|
||||
// Ease the user's burden on specifying width/height for unorganized datasets
|
||||
uint32_t height = m.height, width = m.width;
|
||||
if (height == 0 && width == 0) {
|
||||
width = m.points.size();
|
||||
height = 1;
|
||||
}
|
||||
stream.next(height);
|
||||
stream.next(width);
|
||||
|
||||
// Stream out point field metadata
|
||||
typedef typename pcl::traits::fieldList<T>::type FieldList;
|
||||
uint32_t fields_size = boost::mpl::size<FieldList>::value;
|
||||
stream.next(fields_size);
|
||||
pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
|
||||
|
||||
// Assume little-endian...
|
||||
uint8_t is_bigendian = false;
|
||||
stream.next(is_bigendian);
|
||||
|
||||
// Write out point data as binary blob
|
||||
uint32_t point_step = sizeof(T);
|
||||
stream.next(point_step);
|
||||
uint32_t row_step = point_step * width;
|
||||
stream.next(row_step);
|
||||
uint32_t data_size = row_step * height;
|
||||
stream.next(data_size);
|
||||
memcpy(stream.advance(data_size), &m.points[0], data_size);
|
||||
|
||||
uint8_t is_dense = m.is_dense;
|
||||
stream.next(is_dense);
|
||||
}
|
||||
|
||||
template<typename Stream>
|
||||
inline static void read(Stream & stream, pcl::PointCloud<T> & m)
|
||||
{
|
||||
std_msgs::Header header;
|
||||
stream.next(header);
|
||||
pcl_conversions::toPCL(header, m.header);
|
||||
stream.next(m.height);
|
||||
stream.next(m.width);
|
||||
|
||||
/// @todo Check that fields haven't changed!
|
||||
std::vector<sensor_msgs::PointField> fields;
|
||||
stream.next(fields);
|
||||
|
||||
// Construct field mapping if deserializing for the first time
|
||||
boost::shared_ptr<pcl::MsgFieldMap> & mapping_ptr = pcl::detail::getMapping(m);
|
||||
if (!mapping_ptr) {
|
||||
// This normally should get allocated by DefaultMessageCreator, but just in case
|
||||
mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
|
||||
}
|
||||
pcl::MsgFieldMap & mapping = *mapping_ptr;
|
||||
if (mapping.empty()) {
|
||||
pcl::createMapping<T>(fields, mapping);
|
||||
}
|
||||
|
||||
uint8_t is_bigendian;
|
||||
stream.next(is_bigendian); // ignoring...
|
||||
uint32_t point_step, row_step;
|
||||
stream.next(point_step);
|
||||
stream.next(row_step);
|
||||
|
||||
// Copy point data
|
||||
uint32_t data_size;
|
||||
stream.next(data_size);
|
||||
assert(data_size == m.height * m.width * point_step);
|
||||
m.points.resize(m.height * m.width);
|
||||
uint8_t * m_data = reinterpret_cast<uint8_t *>(&m.points[0]);
|
||||
// If the data layouts match, can copy a whole row in one memcpy
|
||||
if (mapping.size() == 1 &&
|
||||
mapping[0].serialized_offset == 0 &&
|
||||
mapping[0].struct_offset == 0 &&
|
||||
point_step == sizeof(T))
|
||||
{
|
||||
template<typename Stream>
|
||||
inline static void write(Stream& stream, const pcl::PointCloud<T>& m)
|
||||
{
|
||||
stream.next(m.header);
|
||||
|
||||
// Ease the user's burden on specifying width/height for unorganized datasets
|
||||
uint32_t height = m.height, width = m.width;
|
||||
if (height == 0 && width == 0) {
|
||||
width = m.points.size();
|
||||
height = 1;
|
||||
uint32_t m_row_step = sizeof(T) * m.width;
|
||||
// And if the row steps match, can copy whole point cloud in one memcpy
|
||||
if (m_row_step == row_step) {
|
||||
memcpy(m_data, stream.advance(data_size), data_size);
|
||||
} else {
|
||||
for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) {
|
||||
memcpy(m_data, stream.advance(row_step), m_row_step);
|
||||
}
|
||||
stream.next(height);
|
||||
stream.next(width);
|
||||
|
||||
// Stream out point field metadata
|
||||
typedef typename pcl::traits::fieldList<T>::type FieldList;
|
||||
uint32_t fields_size = boost::mpl::size<FieldList>::value;
|
||||
stream.next(fields_size);
|
||||
pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
|
||||
|
||||
// Assume little-endian...
|
||||
uint8_t is_bigendian = false;
|
||||
stream.next(is_bigendian);
|
||||
|
||||
// Write out point data as binary blob
|
||||
uint32_t point_step = sizeof(T);
|
||||
stream.next(point_step);
|
||||
uint32_t row_step = point_step * width;
|
||||
stream.next(row_step);
|
||||
uint32_t data_size = row_step * height;
|
||||
stream.next(data_size);
|
||||
memcpy(stream.advance(data_size), &m.points[0], data_size);
|
||||
|
||||
uint8_t is_dense = m.is_dense;
|
||||
stream.next(is_dense);
|
||||
}
|
||||
|
||||
template<typename Stream>
|
||||
inline static void read(Stream& stream, pcl::PointCloud<T>& m)
|
||||
{
|
||||
std_msgs::Header header;
|
||||
stream.next(header);
|
||||
pcl_conversions::toPCL(header, m.header);
|
||||
stream.next(m.height);
|
||||
stream.next(m.width);
|
||||
|
||||
/// @todo Check that fields haven't changed!
|
||||
std::vector<sensor_msgs::PointField> fields;
|
||||
stream.next(fields);
|
||||
|
||||
// Construct field mapping if deserializing for the first time
|
||||
boost::shared_ptr<pcl::MsgFieldMap>& mapping_ptr = pcl::detail::getMapping(m);
|
||||
if (!mapping_ptr)
|
||||
{
|
||||
// This normally should get allocated by DefaultMessageCreator, but just in case
|
||||
mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
|
||||
}
|
||||
pcl::MsgFieldMap& mapping = *mapping_ptr;
|
||||
if (mapping.empty())
|
||||
pcl::createMapping<T> (fields, mapping);
|
||||
|
||||
uint8_t is_bigendian;
|
||||
stream.next(is_bigendian); // ignoring...
|
||||
uint32_t point_step, row_step;
|
||||
stream.next(point_step);
|
||||
stream.next(row_step);
|
||||
|
||||
// Copy point data
|
||||
uint32_t data_size;
|
||||
stream.next(data_size);
|
||||
assert(data_size == m.height * m.width * point_step);
|
||||
m.points.resize(m.height * m.width);
|
||||
uint8_t* m_data = reinterpret_cast<uint8_t*>(&m.points[0]);
|
||||
// If the data layouts match, can copy a whole row in one memcpy
|
||||
if (mapping.size() == 1 &&
|
||||
mapping[0].serialized_offset == 0 &&
|
||||
mapping[0].struct_offset == 0 &&
|
||||
point_step == sizeof(T))
|
||||
{
|
||||
uint32_t m_row_step = sizeof(T) * m.width;
|
||||
// And if the row steps match, can copy whole point cloud in one memcpy
|
||||
if (m_row_step == row_step)
|
||||
{
|
||||
memcpy (m_data, stream.advance(data_size), data_size);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step)
|
||||
memcpy (m_data, stream.advance(row_step), m_row_step);
|
||||
} else {
|
||||
// If not, do a lot of memcpys to copy over the fields
|
||||
for (uint32_t row = 0; row < m.height; ++row) {
|
||||
const uint8_t * stream_data = stream.advance(row_step);
|
||||
for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
|
||||
BOOST_FOREACH(const pcl::detail::FieldMapping & fm, mapping) {
|
||||
memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
|
||||
}
|
||||
m_data += sizeof(T);
|
||||
}
|
||||
else
|
||||
{
|
||||
// If not, do a lot of memcpys to copy over the fields
|
||||
for (uint32_t row = 0; row < m.height; ++row) {
|
||||
const uint8_t* stream_data = stream.advance(row_step);
|
||||
for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
|
||||
BOOST_FOREACH(const pcl::detail::FieldMapping& fm, mapping) {
|
||||
memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
|
||||
}
|
||||
m_data += sizeof(T);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t is_dense;
|
||||
stream.next(is_dense);
|
||||
m.is_dense = is_dense;
|
||||
}
|
||||
}
|
||||
|
||||
inline static uint32_t serializedLength(const pcl::PointCloud<T>& m)
|
||||
{
|
||||
uint32_t length = 0;
|
||||
uint8_t is_dense;
|
||||
stream.next(is_dense);
|
||||
m.is_dense = is_dense;
|
||||
}
|
||||
|
||||
length += serializationLength(m.header);
|
||||
length += 8; // height/width
|
||||
inline static uint32_t serializedLength(const pcl::PointCloud<T> & m)
|
||||
{
|
||||
uint32_t length = 0;
|
||||
|
||||
pcl::detail::FieldsLength<T> fl;
|
||||
typedef typename pcl::traits::fieldList<T>::type FieldList;
|
||||
pcl::for_each_type<FieldList>(boost::ref(fl));
|
||||
length += 4; // size of 'fields'
|
||||
length += fl.length;
|
||||
length += serializationLength(m.header);
|
||||
length += 8; // height/width
|
||||
|
||||
length += 1; // is_bigendian
|
||||
length += 4; // point_step
|
||||
length += 4; // row_step
|
||||
length += 4; // size of 'data'
|
||||
length += m.points.size() * sizeof(T); // data
|
||||
length += 1; // is_dense
|
||||
pcl::detail::FieldsLength<T> fl;
|
||||
typedef typename pcl::traits::fieldList<T>::type FieldList;
|
||||
pcl::for_each_type<FieldList>(boost::ref(fl));
|
||||
length += 4; // size of 'fields'
|
||||
length += fl.length;
|
||||
|
||||
return length;
|
||||
}
|
||||
};
|
||||
} // namespace ros::serialization
|
||||
length += 1; // is_bigendian
|
||||
length += 4; // point_step
|
||||
length += 4; // row_step
|
||||
length += 4; // size of 'data'
|
||||
length += m.points.size() * sizeof(T); // data
|
||||
length += 1; // is_dense
|
||||
|
||||
/// @todo Printer specialization in message_operations
|
||||
return length;
|
||||
}
|
||||
};
|
||||
} // namespace ros::serialization
|
||||
|
||||
/// @todo Printer specialization in message_operations
|
||||
|
||||
} // namespace ros
|
||||
|
||||
@@ -315,103 +324,101 @@ namespace ros
|
||||
|
||||
namespace pcl
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
|
||||
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
|
||||
template <class T>
|
||||
constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
|
||||
pcl::shared_ptr<T>>::value;
|
||||
template<class T>
|
||||
constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
|
||||
pcl::shared_ptr<T>>::value;
|
||||
#else
|
||||
template <class T>
|
||||
constexpr static bool pcl_uses_boost = true;
|
||||
template<class T>
|
||||
constexpr static bool pcl_uses_boost = true;
|
||||
#endif
|
||||
|
||||
template<class SharedPointer> struct Holder
|
||||
{
|
||||
SharedPointer p;
|
||||
template<class SharedPointer>
|
||||
struct Holder
|
||||
{
|
||||
SharedPointer p;
|
||||
|
||||
Holder(const SharedPointer &p) : p(p) {}
|
||||
Holder(const Holder &other) : p(other.p) {}
|
||||
Holder(Holder &&other) : p(std::move(other.p)) {}
|
||||
Holder(const SharedPointer & p)
|
||||
: p(p) {}
|
||||
Holder(const Holder & other)
|
||||
: p(other.p) {}
|
||||
Holder(Holder && other)
|
||||
: p(std::move(other.p)) {}
|
||||
|
||||
void operator () (...) { p.reset(); }
|
||||
};
|
||||
void operator()(...) {p.reset();}
|
||||
};
|
||||
|
||||
template<class T>
|
||||
inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> &p)
|
||||
{
|
||||
typedef Holder<std::shared_ptr<T>> H;
|
||||
if(H *h = boost::get_deleter<H>(p))
|
||||
{
|
||||
return h->p;
|
||||
}
|
||||
else
|
||||
{
|
||||
return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
|
||||
}
|
||||
}
|
||||
template<class T>
|
||||
inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> & p)
|
||||
{
|
||||
typedef Holder<std::shared_ptr<T>> H;
|
||||
if (H * h = boost::get_deleter<H>(p)) {
|
||||
return h->p;
|
||||
} else {
|
||||
return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
|
||||
}
|
||||
}
|
||||
|
||||
template<class T>
|
||||
inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> &p)
|
||||
{
|
||||
typedef Holder<boost::shared_ptr<T>> H;
|
||||
if(H * h = std::get_deleter<H>(p))
|
||||
{
|
||||
return h->p;
|
||||
}
|
||||
else
|
||||
{
|
||||
return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
|
||||
}
|
||||
}
|
||||
template<class T>
|
||||
inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
|
||||
{
|
||||
typedef Holder<boost::shared_ptr<T>> H;
|
||||
if (H * h = std::get_deleter<H>(p)) {
|
||||
return h->p;
|
||||
} else {
|
||||
return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
} // namespace pcl::detail
|
||||
} // namespace pcl::detail
|
||||
|
||||
// add functions to convert to smart pointer used by ROS
|
||||
template <class T>
|
||||
inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> &p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
template<class T>
|
||||
inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> & p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
|
||||
#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
|
||||
template <class T>
|
||||
inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> &p)
|
||||
{
|
||||
return detail::to_boost_ptr(p);
|
||||
}
|
||||
template<class T>
|
||||
inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> & p)
|
||||
{
|
||||
return detail::to_boost_ptr(p);
|
||||
}
|
||||
|
||||
// add functions to convert to smart pointer used by PCL, based on PCL's own pointer
|
||||
template <class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
|
||||
inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> &p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
|
||||
inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
|
||||
template <class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
|
||||
inline std::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p)
|
||||
{
|
||||
return detail::to_std_ptr(p);
|
||||
}
|
||||
template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
|
||||
inline std::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
|
||||
{
|
||||
return detail::to_std_ptr(p);
|
||||
}
|
||||
|
||||
template <class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
|
||||
inline boost::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> &p)
|
||||
{
|
||||
return detail::to_boost_ptr(p);
|
||||
}
|
||||
template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
|
||||
inline boost::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
|
||||
{
|
||||
return detail::to_boost_ptr(p);
|
||||
}
|
||||
|
||||
template <class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
|
||||
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
|
||||
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
#else
|
||||
template <class T>
|
||||
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> &p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
template<class T>
|
||||
inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
|
||||
{
|
||||
return p;
|
||||
}
|
||||
#endif
|
||||
} // namespace pcl
|
||||
|
||||
|
||||
@@ -38,7 +38,7 @@
|
||||
|
||||
\author Patrick Mihelich
|
||||
|
||||
@b Publisher represents a ROS publisher for the templated PointCloud implementation.
|
||||
@b Publisher represents a ROS publisher for the templated PointCloud implementation.
|
||||
|
||||
**/
|
||||
|
||||
@@ -51,99 +51,99 @@
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
namespace pcl_ros
|
||||
namespace pcl_ros
|
||||
{
|
||||
class BasePublisher
|
||||
class BasePublisher
|
||||
{
|
||||
public:
|
||||
void
|
||||
advertise(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
|
||||
{
|
||||
public:
|
||||
void
|
||||
advertise (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
|
||||
{
|
||||
pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
|
||||
}
|
||||
pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
|
||||
}
|
||||
|
||||
std::string
|
||||
getTopic ()
|
||||
{
|
||||
return (pub_.getTopic ());
|
||||
}
|
||||
|
||||
uint32_t
|
||||
getNumSubscribers () const
|
||||
{
|
||||
return (pub_.getNumSubscribers ());
|
||||
}
|
||||
|
||||
void
|
||||
shutdown ()
|
||||
{
|
||||
pub_.shutdown ();
|
||||
}
|
||||
|
||||
operator void*() const
|
||||
{
|
||||
return (pub_) ? (void*)1 : (void*)0;
|
||||
}
|
||||
|
||||
protected:
|
||||
ros::Publisher pub_;
|
||||
};
|
||||
|
||||
template <typename PointT>
|
||||
class Publisher : public BasePublisher
|
||||
std::string
|
||||
getTopic()
|
||||
{
|
||||
public:
|
||||
Publisher () {}
|
||||
return pub_.getTopic();
|
||||
}
|
||||
|
||||
Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
|
||||
{
|
||||
advertise (nh, topic, queue_size);
|
||||
}
|
||||
|
||||
~Publisher () {}
|
||||
|
||||
inline void
|
||||
publish (const boost::shared_ptr<const pcl::PointCloud<PointT> > &point_cloud) const
|
||||
{
|
||||
publish (*point_cloud);
|
||||
}
|
||||
|
||||
inline void
|
||||
publish (const pcl::PointCloud<PointT>& point_cloud) const
|
||||
{
|
||||
// Fill point cloud binary data
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
pcl::toROSMsg (point_cloud, msg);
|
||||
pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (msg));
|
||||
}
|
||||
};
|
||||
|
||||
template <>
|
||||
class Publisher<sensor_msgs::PointCloud2> : public BasePublisher
|
||||
uint32_t
|
||||
getNumSubscribers() const
|
||||
{
|
||||
public:
|
||||
Publisher () {}
|
||||
return pub_.getNumSubscribers();
|
||||
}
|
||||
|
||||
Publisher (ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size)
|
||||
{
|
||||
advertise (nh, topic, queue_size);
|
||||
}
|
||||
void
|
||||
shutdown()
|
||||
{
|
||||
pub_.shutdown();
|
||||
}
|
||||
|
||||
~Publisher () {}
|
||||
operator void *() const
|
||||
{
|
||||
return (pub_) ? (void *)1 : (void *)0;
|
||||
}
|
||||
|
||||
void
|
||||
publish (const sensor_msgs::PointCloud2Ptr& point_cloud) const
|
||||
{
|
||||
pub_.publish (point_cloud);
|
||||
//pub_.publish (*point_cloud);
|
||||
}
|
||||
protected:
|
||||
ros::Publisher pub_;
|
||||
};
|
||||
|
||||
void
|
||||
publish (const sensor_msgs::PointCloud2& point_cloud) const
|
||||
{
|
||||
pub_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (point_cloud));
|
||||
//pub_.publish (point_cloud);
|
||||
}
|
||||
};
|
||||
template<typename PointT>
|
||||
class Publisher : public BasePublisher
|
||||
{
|
||||
public:
|
||||
Publisher() {}
|
||||
|
||||
Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
|
||||
{
|
||||
advertise(nh, topic, queue_size);
|
||||
}
|
||||
|
||||
~Publisher() {}
|
||||
|
||||
inline void
|
||||
publish(const boost::shared_ptr<const pcl::PointCloud<PointT>> & point_cloud) const
|
||||
{
|
||||
publish(*point_cloud);
|
||||
}
|
||||
|
||||
inline void
|
||||
publish(const pcl::PointCloud<PointT> & point_cloud) const
|
||||
{
|
||||
// Fill point cloud binary data
|
||||
sensor_msgs::PointCloud2 msg;
|
||||
pcl::toROSMsg(point_cloud, msg);
|
||||
pub_.publish(boost::make_shared<const sensor_msgs::PointCloud2>(msg));
|
||||
}
|
||||
};
|
||||
|
||||
template<>
|
||||
class Publisher<sensor_msgs::PointCloud2>: public BasePublisher
|
||||
{
|
||||
public:
|
||||
Publisher() {}
|
||||
|
||||
Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
|
||||
{
|
||||
advertise(nh, topic, queue_size);
|
||||
}
|
||||
|
||||
~Publisher() {}
|
||||
|
||||
void
|
||||
publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const
|
||||
{
|
||||
pub_.publish(point_cloud);
|
||||
//pub_.publish (*point_cloud);
|
||||
}
|
||||
|
||||
void
|
||||
publish(const sensor_msgs::PointCloud2 & point_cloud) const
|
||||
{
|
||||
pub_.publish(boost::make_shared<const sensor_msgs::PointCloud2>(point_cloud));
|
||||
//pub_.publish (point_cloud);
|
||||
}
|
||||
};
|
||||
}
|
||||
#endif //#ifndef PCL_ROS_PUBLISHER_H_
|
||||
|
||||
@@ -47,63 +47,68 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
|
||||
* \author Radu Bogdan Rusu
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class EuclideanClusterExtraction : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
/** \brief Empty constructor. */
|
||||
EuclideanClusterExtraction()
|
||||
: publish_indices_(false), max_clusters_(std::numeric_limits<int>::max()) {}
|
||||
|
||||
protected:
|
||||
// ROS nodelet attributes
|
||||
/** \brief Publish indices or convert to PointCloud clusters. Default: false */
|
||||
bool publish_indices_;
|
||||
|
||||
/** \brief Maximum number of clusters to publish. */
|
||||
int max_clusters_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>> srv_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
class EuclideanClusterExtraction : public PCLNodelet
|
||||
{
|
||||
public:
|
||||
/** \brief Empty constructor. */
|
||||
EuclideanClusterExtraction () : publish_indices_ (false), max_clusters_ (std::numeric_limits<int>::max ()) {};
|
||||
|
||||
protected:
|
||||
// ROS nodelet attributes
|
||||
/** \brief Publish indices or convert to PointCloud clusters. Default: false */
|
||||
bool publish_indices_;
|
||||
void config_callback(EuclideanClusterExtractionConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Maximum number of clusters to publish. */
|
||||
int max_clusters_;
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > srv_;
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit ();
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
void subscribe ();
|
||||
void unsubscribe ();
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
|
||||
PointIndices>>> sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointIndices>>> sync_input_indices_a_;
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (EuclideanClusterExtractionConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_EXTRACT_CLUSTERS_H_
|
||||
|
||||
@@ -52,79 +52,83 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
|
||||
* a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
|
||||
* inside it.
|
||||
*
|
||||
* An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
|
||||
* a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
|
||||
* inside it.
|
||||
*
|
||||
* An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class ExtractPolygonalPrismData : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
|
||||
protected:
|
||||
/** \brief The output PointIndices publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_hull_filter_;
|
||||
|
||||
/** \brief Synchronized input, planar hull, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud,
|
||||
PointIndices>>> sync_input_hull_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointCloud, PointIndices>>> sync_input_hull_indices_a_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>> srv_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
class ExtractPolygonalPrismData : public PCLNodelet
|
||||
inline void
|
||||
input_callback(const PointCloudConstPtr & input)
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
PointIndices cloud;
|
||||
cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
|
||||
nf_.add(boost::make_shared<PointIndices>(cloud));
|
||||
}
|
||||
|
||||
protected:
|
||||
/** \brief The output PointIndices publisher. */
|
||||
ros::Publisher pub_output_;
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit();
|
||||
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_hull_filter_;
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
|
||||
/** \brief Synchronized input, planar hull, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > sync_input_hull_indices_a_;
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(ExtractPolygonalPrismDataConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > srv_;
|
||||
/** \brief Input point cloud callback. Used when \a use_indices is set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param hull the pointer to the planar hull point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_hull_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointCloudConstPtr & hull,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_;
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudConstPtr &input)
|
||||
{
|
||||
PointIndices cloud;
|
||||
cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
|
||||
nf_.add (boost::make_shared<PointIndices> (cloud));
|
||||
}
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit ();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
void subscribe ();
|
||||
void unsubscribe ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices is set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param hull the pointer to the planar hull point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_hull_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &hull,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_
|
||||
|
||||
@@ -51,235 +51,243 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in
|
||||
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
|
||||
* \author Radu Bogdan Rusu
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in
|
||||
* the sense that it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class SACSegmentation : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
|
||||
public:
|
||||
/** \brief Constructor. */
|
||||
SACSegmentation()
|
||||
: min_inliers_(0) {}
|
||||
|
||||
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
|
||||
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
|
||||
*/
|
||||
class SACSegmentation : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
|
||||
|
||||
public:
|
||||
/** \brief Constructor. */
|
||||
SACSegmentation () : min_inliers_(0) {}
|
||||
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
|
||||
inline std::string getInputTFframe() {return tf_input_frame_;}
|
||||
|
||||
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
|
||||
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
|
||||
*/
|
||||
inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
|
||||
inline std::string getInputTFframe () { return (tf_input_frame_); }
|
||||
|
||||
/** \brief Set the output TF frame the data should be transformed into after processing.
|
||||
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
|
||||
*/
|
||||
inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
|
||||
inline std::string getOutputTFframe () { return (tf_output_frame_); }
|
||||
|
||||
protected:
|
||||
// The minimum number of inliers a model must have in order to be considered valid.
|
||||
int min_inliers_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The output PointIndices publisher. */
|
||||
ros::Publisher pub_indices_;
|
||||
|
||||
/** \brief The output ModelCoefficients publisher. */
|
||||
ros::Publisher pub_model_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig> > srv_;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe ();
|
||||
virtual void unsubscribe ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (SACSegmentationConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices is set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices);
|
||||
|
||||
/** \brief Pointer to a set of indices stored internally.
|
||||
* (used when \a latched_indices_ is set).
|
||||
*/
|
||||
PointIndices indices_;
|
||||
|
||||
/** \brief Indices callback. Used when \a latched_indices_ is set.
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
inline void
|
||||
indices_callback (const PointIndicesConstPtr &indices)
|
||||
{
|
||||
indices_ = *indices;
|
||||
}
|
||||
|
||||
/** \brief Input callback. Used when \a latched_indices_ is set.
|
||||
* \param input the pointer to the input point cloud
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudConstPtr &input)
|
||||
{
|
||||
indices_.header = fromPCL(input->header);
|
||||
PointIndicesConstPtr indices;
|
||||
indices.reset (new PointIndices (indices_));
|
||||
nf_pi_.add (indices);
|
||||
}
|
||||
|
||||
private:
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SACSegmentation<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
|
||||
* models that require the use of surface normals for estimation.
|
||||
/** \brief Set the output TF frame the data should be transformed into after processing.
|
||||
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
|
||||
*/
|
||||
class SACSegmentationFromNormals: public SACSegmentation
|
||||
inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
|
||||
|
||||
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
|
||||
inline std::string getOutputTFframe() {return tf_output_frame_;}
|
||||
|
||||
protected:
|
||||
// The minimum number of inliers a model must have in order to be considered valid.
|
||||
int min_inliers_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The output PointIndices publisher. */
|
||||
ros::Publisher pub_indices_;
|
||||
|
||||
/** \brief The output ModelCoefficients publisher. */
|
||||
ros::Publisher pub_model_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe();
|
||||
virtual void unsubscribe();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(SACSegmentationConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback. Used when \a use_indices is set.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief Pointer to a set of indices stored internally.
|
||||
* (used when \a latched_indices_ is set).
|
||||
*/
|
||||
PointIndices indices_;
|
||||
|
||||
/** \brief Indices callback. Used when \a latched_indices_ is set.
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
inline void
|
||||
indices_callback(const PointIndicesConstPtr & indices)
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
indices_ = *indices;
|
||||
}
|
||||
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
|
||||
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
|
||||
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
|
||||
/** \brief Input callback. Used when \a latched_indices_ is set.
|
||||
* \param input the pointer to the input point cloud
|
||||
*/
|
||||
inline void
|
||||
input_callback(const PointCloudConstPtr & input)
|
||||
{
|
||||
indices_.header = fromPCL(input->header);
|
||||
PointIndicesConstPtr indices;
|
||||
indices.reset(new PointIndices(indices_));
|
||||
nf_pi_.add(indices);
|
||||
}
|
||||
|
||||
public:
|
||||
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
|
||||
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
|
||||
*/
|
||||
inline void setInputTFframe (std::string tf_frame) { tf_input_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
|
||||
inline std::string getInputTFframe () { return (tf_input_frame_); }
|
||||
private:
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief Set the output TF frame the data should be transformed into after processing.
|
||||
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
|
||||
*/
|
||||
inline void setOutputTFframe (std::string tf_frame) { tf_output_frame_ = tf_frame; }
|
||||
|
||||
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
|
||||
inline std::string getOutputTFframe () { return (tf_output_frame_); }
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SACSegmentation<pcl::PointXYZ> impl_;
|
||||
|
||||
protected:
|
||||
// ROS nodelet attributes
|
||||
/** \brief The normals PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
|
||||
PointIndices>>> sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointIndices>>> sync_input_indices_a_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_axis_;
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig> > srv_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback (const PointCloudConstPtr &cloud)
|
||||
{
|
||||
PointIndices indices;
|
||||
indices.header.stamp = fromPCL(cloud->header).stamp;
|
||||
nf_.add (boost::make_shared<PointIndices> (indices));
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
|
||||
* models that require the use of surface normals for estimation.
|
||||
*/
|
||||
class SACSegmentationFromNormals : public SACSegmentation
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_;
|
||||
typedef pcl::PointCloud<pcl::Normal> PointCloudN;
|
||||
typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
|
||||
typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
public:
|
||||
/** \brief Set the input TF frame the data should be transformed into before processing, if input.header.frame_id is different.
|
||||
* \param tf_frame the TF frame the input PointCloud should be transformed into before processing
|
||||
*/
|
||||
inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
/** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
|
||||
inline std::string getInputTFframe() {return tf_input_frame_;}
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe ();
|
||||
virtual void unsubscribe ();
|
||||
/** \brief Set the output TF frame the data should be transformed into after processing.
|
||||
* \param tf_frame the TF frame the PointCloud should be transformed into after processing
|
||||
*/
|
||||
inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
|
||||
|
||||
/** \brief Model callback
|
||||
* \param model the sample consensus model found
|
||||
*/
|
||||
void axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model);
|
||||
/** \brief Get the TF frame the PointCloud should be transformed into after processing. */
|
||||
inline std::string getOutputTFframe() {return tf_output_frame_;}
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (SACSegmentationFromNormalsConfig &config, uint32_t level);
|
||||
protected:
|
||||
// ROS nodelet attributes
|
||||
/** \brief The normals PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudN> sub_normals_filter_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_normals the pointer to the input point cloud normals
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_normals_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudNConstPtr &cloud_normals,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
private:
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_axis_;
|
||||
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>> srv_;
|
||||
|
||||
/** \brief Synchronized input, normals, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices> > > sync_input_normals_indices_e_;
|
||||
/** \brief Input point cloud callback.
|
||||
* Because we want to use the same synchronizer object, we push back
|
||||
* empty elements with the same timestamp.
|
||||
*/
|
||||
inline void
|
||||
input_callback(const PointCloudConstPtr & cloud)
|
||||
{
|
||||
PointIndices indices;
|
||||
indices.header.stamp = fromPCL(cloud->header).stamp;
|
||||
nf_.add(boost::make_shared<PointIndices>(indices));
|
||||
}
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
/** \brief Null passthrough filter, used for pushing empty elements in the
|
||||
* synchronizer */
|
||||
message_filters::PassThrough<PointIndices> nf_;
|
||||
|
||||
/** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_input_frame_;
|
||||
/** \brief The original data input TF frame. */
|
||||
std::string tf_input_orig_frame_;
|
||||
/** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe();
|
||||
virtual void unsubscribe();
|
||||
|
||||
/** \brief Model callback
|
||||
* \param model the sample consensus model found
|
||||
*/
|
||||
void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model);
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_normals the pointer to the input point cloud normals
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_normals_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointCloudNConstPtr & cloud_normals,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
private:
|
||||
/** \brief Internal mutex. */
|
||||
boost::mutex mutex_;
|
||||
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
|
||||
|
||||
/** \brief Synchronized input, normals, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointCloudN, PointIndices>>> sync_input_normals_indices_a_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN,
|
||||
PointIndices>>> sync_input_normals_indices_e_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_SAC_SEGMENTATION_H_
|
||||
|
||||
@@ -48,63 +48,66 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
|
||||
* difference between them for a maximum given distance threshold.
|
||||
* \author Radu Bogdan Rusu
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
|
||||
* difference between them for a maximum given distance threshold.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class SegmentDifferences : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
|
||||
public:
|
||||
/** \brief Empty constructor. */
|
||||
SegmentDifferences() {}
|
||||
|
||||
protected:
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_target_filter_;
|
||||
|
||||
/** \brief Synchronized input, and planar hull.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
|
||||
PointCloud>>> sync_input_target_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointCloud>>> sync_input_target_a_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig>> srv_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
void subscribe();
|
||||
void unsubscribe();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
class SegmentDifferences : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
void config_callback(SegmentDifferencesConfig & config, uint32_t level);
|
||||
|
||||
public:
|
||||
/** \brief Empty constructor. */
|
||||
SegmentDifferences () {};
|
||||
|
||||
protected:
|
||||
/** \brief The message filter subscriber for PointCloud2. */
|
||||
message_filters::Subscriber<PointCloud> sub_target_filter_;
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_target the pointcloud that we want to segment \a cloud from
|
||||
*/
|
||||
void input_target_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointCloudConstPtr & cloud_target);
|
||||
|
||||
/** \brief Synchronized input, and planar hull.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > sync_input_target_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > sync_input_target_a_;
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SegmentDifferences<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig> > srv_;
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
void onInit ();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
void subscribe ();
|
||||
void unsubscribe ();
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (SegmentDifferencesConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param cloud_target the pointcloud that we want to segment \a cloud from
|
||||
*/
|
||||
void input_target_callback (const PointCloudConstPtr &cloud,
|
||||
const PointCloudConstPtr &cloud_target);
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::SegmentDifferences<pcl::PointXYZ> impl_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_SEGMENT_DIFFERENCES_H_
|
||||
|
||||
@@ -48,49 +48,52 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
|
||||
* \author Radu Bogdan Rusu
|
||||
/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class ConvexHull2D : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
|
||||
private:
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe();
|
||||
virtual void unsubscribe();
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
class ConvexHull2D : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
|
||||
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
|
||||
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
|
||||
void input_indices_callback(
|
||||
const PointCloudConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
private:
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::ConvexHull<pcl::PointXYZ> impl_;
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe ();
|
||||
virtual void unsubscribe ();
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices);
|
||||
/** \brief Publisher for PolygonStamped. */
|
||||
ros::Publisher pub_plane_;
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::ConvexHull<pcl::PointXYZ> impl_;
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
|
||||
PointIndices>>> sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
|
||||
PointIndices>>> sync_input_indices_a_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief Publisher for PolygonStamped. */
|
||||
ros::Publisher pub_plane_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_CONVEX_HULL_2D_H_
|
||||
|
||||
@@ -46,105 +46,107 @@
|
||||
|
||||
// Dynamic reconfigure
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include "pcl_ros/MLSConfig.h"
|
||||
#include "pcl_ros/MLSConfig.h"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
namespace sync_policies = message_filters::sync_policies;
|
||||
|
||||
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
|
||||
* The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters.
|
||||
* Normals are estimated at each point as well and published on a separate topic.
|
||||
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton
|
||||
/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
|
||||
* The type of the output is the same as the input, it only smooths the XYZ coordinates according to the parameters.
|
||||
* Normals are estimated at each point as well and published on a separate topic.
|
||||
* \author Radu Bogdan Rusu, Zoltan-Csaba Marton
|
||||
*/
|
||||
class MovingLeastSquares : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointXYZ PointIn;
|
||||
typedef pcl::PointNormal NormalOut;
|
||||
|
||||
typedef pcl::PointCloud<PointIn> PointCloudIn;
|
||||
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
|
||||
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
|
||||
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
|
||||
|
||||
typedef pcl::KdTree<PointIn> KdTree;
|
||||
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
|
||||
|
||||
protected:
|
||||
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
|
||||
PointCloudInConstPtr surface_;
|
||||
|
||||
/** \brief A pointer to the spatial search object. */
|
||||
KdTreePtr tree_;
|
||||
|
||||
/** \brief The nearest neighbors search radius for each point. */
|
||||
double search_radius_;
|
||||
|
||||
/** \brief The number of K nearest neighbors to use for each point. */
|
||||
//int k_;
|
||||
|
||||
/** \brief Whether to use a polynomial fit. */
|
||||
bool use_polynomial_fit_;
|
||||
|
||||
/** \brief The order of the polynomial to be fit. */
|
||||
int polynomial_order_;
|
||||
|
||||
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
|
||||
double gaussian_parameter_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The surface PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
|
||||
|
||||
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
|
||||
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
|
||||
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
|
||||
* 2: Organized spatial dataset index
|
||||
*/
|
||||
class MovingLeastSquares : public PCLNodelet
|
||||
{
|
||||
typedef pcl::PointXYZ PointIn;
|
||||
typedef pcl::PointNormal NormalOut;
|
||||
int spatial_locator_type_;
|
||||
|
||||
typedef pcl::PointCloud<PointIn> PointCloudIn;
|
||||
typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
|
||||
typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
|
||||
typedef pcl::PointCloud<NormalOut> NormalCloudOut;
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<MLSConfig>> srv_;
|
||||
|
||||
typedef pcl::KdTree<PointIn> KdTree;
|
||||
typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback(MLSConfig & config, uint32_t level);
|
||||
|
||||
protected:
|
||||
/** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
|
||||
PointCloudInConstPtr surface_;
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit();
|
||||
|
||||
/** \brief A pointer to the spatial search object. */
|
||||
KdTreePtr tree_;
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe();
|
||||
virtual void unsubscribe();
|
||||
|
||||
/** \brief The nearest neighbors search radius for each point. */
|
||||
double search_radius_;
|
||||
private:
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback(
|
||||
const PointCloudInConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices);
|
||||
|
||||
/** \brief The number of K nearest neighbors to use for each point. */
|
||||
//int k_;
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
|
||||
|
||||
/** \brief Whether to use a polynomial fit. */
|
||||
bool use_polynomial_fit_;
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief The order of the polynomial to be fit. */
|
||||
int polynomial_order_;
|
||||
/** \brief The output PointCloud (containing the normals) publisher. */
|
||||
ros::Publisher pub_normals_;
|
||||
|
||||
/** \brief How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit). */
|
||||
double gaussian_parameter_;
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
|
||||
PointIndices>>> sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
|
||||
PointIndices>>> sync_input_indices_a_;
|
||||
|
||||
// ROS nodelet attributes
|
||||
/** \brief The surface PointCloud subscriber filter. */
|
||||
message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
|
||||
|
||||
/** \brief Parameter for the spatial locator tree. By convention, the values represent:
|
||||
* 0: ANN (Approximate Nearest Neigbor library) kd-tree
|
||||
* 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
|
||||
* 2: Organized spatial dataset index
|
||||
*/
|
||||
int spatial_locator_type_;
|
||||
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr <dynamic_reconfigure::Server<MLSConfig> > srv_;
|
||||
|
||||
/** \brief Dynamic reconfigure callback
|
||||
* \param config the config object
|
||||
* \param level the dynamic reconfigure level
|
||||
*/
|
||||
void config_callback (MLSConfig &config, uint32_t level);
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void onInit ();
|
||||
|
||||
/** \brief LazyNodelet connection routine. */
|
||||
virtual void subscribe ();
|
||||
virtual void unsubscribe ();
|
||||
|
||||
private:
|
||||
/** \brief Input point cloud callback.
|
||||
* \param cloud the pointer to the input point cloud
|
||||
* \param indices the pointer to the input point cloud indices
|
||||
*/
|
||||
void input_indices_callback (const PointCloudInConstPtr &cloud,
|
||||
const PointIndicesConstPtr &indices);
|
||||
|
||||
|
||||
private:
|
||||
/** \brief The PCL implementation used. */
|
||||
pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
|
||||
/** \brief The output PointCloud (containing the normals) publisher. */
|
||||
ros::Publisher pub_normals_;
|
||||
|
||||
/** \brief Synchronized input, and indices.*/
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointIndices> > > sync_input_indices_e_;
|
||||
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointIndices> > > sync_input_indices_a_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
}
|
||||
|
||||
#endif //#ifndef PCL_ROS_MOVING_LEAST_SQUARES_H_
|
||||
|
||||
@@ -44,124 +44,138 @@
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param transform a rigid transformation from tf
|
||||
* \note calls the Eigen version
|
||||
*/
|
||||
template <typename PointT> void
|
||||
transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::Transform &transform);
|
||||
/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param transform a rigid transformation from tf
|
||||
* \note calls the Eigen version
|
||||
*/
|
||||
template<typename PointT>
|
||||
void
|
||||
transformPointCloudWithNormals(
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::Transform & transform);
|
||||
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template <typename PointT> bool
|
||||
transformPointCloudWithNormals (const std::string &target_frame,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener);
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener);
|
||||
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param target_time the target timestamp
|
||||
* \param cloud_in the input point cloud
|
||||
* \param fixed_frame fixed TF frame
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template <typename PointT> bool
|
||||
transformPointCloudWithNormals (const std::string &target_frame,
|
||||
const ros::Time & target_time,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
const std::string &fixed_frame,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener);
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param target_time the target timestamp
|
||||
* \param cloud_in the input point cloud
|
||||
* \param fixed_frame fixed TF frame
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const ros::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener);
|
||||
|
||||
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param transform a rigid transformation from tf
|
||||
* \note calls the Eigen version
|
||||
*/
|
||||
template <typename PointT> void
|
||||
transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::Transform &transform);
|
||||
/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param transform a rigid transformation from tf
|
||||
* \note calls the Eigen version
|
||||
*/
|
||||
template<typename PointT>
|
||||
void
|
||||
transformPointCloud(
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::Transform & transform);
|
||||
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template <typename PointT> bool
|
||||
transformPointCloud (const std::string &target_frame,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener);
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param cloud_in the input point cloud
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener);
|
||||
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param target_time the target timestamp
|
||||
* \param cloud_in the input point cloud
|
||||
* \param fixed_frame fixed TF frame
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template <typename PointT> bool
|
||||
transformPointCloud (const std::string &target_frame, const ros::Time & target_time,
|
||||
const pcl::PointCloud <PointT> &cloud_in,
|
||||
const std::string &fixed_frame,
|
||||
pcl::PointCloud <PointT> &cloud_out,
|
||||
const tf::TransformListener &tf_listener);
|
||||
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
|
||||
* \param target_frame the target TF frame the point cloud should be transformed to
|
||||
* \param target_time the target timestamp
|
||||
* \param cloud_in the input point cloud
|
||||
* \param fixed_frame fixed TF frame
|
||||
* \param cloud_out the input point cloud
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
template<typename PointT>
|
||||
bool
|
||||
transformPointCloud(
|
||||
const std::string & target_frame, const ros::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
pcl::PointCloud<PointT> & cloud_out,
|
||||
const tf::TransformListener & tf_listener);
|
||||
|
||||
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
|
||||
* \param target_frame the target TF frame
|
||||
* \param in the input PointCloud2 dataset
|
||||
* \param out the resultant transformed PointCloud2 dataset
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
bool
|
||||
transformPointCloud (const std::string &target_frame,
|
||||
const sensor_msgs::PointCloud2 &in,
|
||||
sensor_msgs::PointCloud2 &out,
|
||||
const tf::TransformListener &tf_listener);
|
||||
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
|
||||
* \param target_frame the target TF frame
|
||||
* \param in the input PointCloud2 dataset
|
||||
* \param out the resultant transformed PointCloud2 dataset
|
||||
* \param tf_listener a TF listener object
|
||||
*/
|
||||
bool
|
||||
transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const sensor_msgs::PointCloud2 & in,
|
||||
sensor_msgs::PointCloud2 & out,
|
||||
const tf::TransformListener & tf_listener);
|
||||
|
||||
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
|
||||
* \param target_frame the target TF frame
|
||||
* \param net_transform the TF transformer object
|
||||
* \param in the input PointCloud2 dataset
|
||||
* \param out the resultant transformed PointCloud2 dataset
|
||||
*/
|
||||
void
|
||||
transformPointCloud (const std::string &target_frame,
|
||||
const tf::Transform &net_transform,
|
||||
const sensor_msgs::PointCloud2 &in,
|
||||
sensor_msgs::PointCloud2 &out);
|
||||
/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
|
||||
* \param target_frame the target TF frame
|
||||
* \param net_transform the TF transformer object
|
||||
* \param in the input PointCloud2 dataset
|
||||
* \param out the resultant transformed PointCloud2 dataset
|
||||
*/
|
||||
void
|
||||
transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const tf::Transform & net_transform,
|
||||
const sensor_msgs::PointCloud2 & in,
|
||||
sensor_msgs::PointCloud2 & out);
|
||||
|
||||
/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
|
||||
* \param transform the transformation to use on the points
|
||||
* \param in the input PointCloud2 dataset
|
||||
* \param out the resultant transformed PointCloud2 dataset
|
||||
*/
|
||||
void
|
||||
transformPointCloud (const Eigen::Matrix4f &transform,
|
||||
const sensor_msgs::PointCloud2 &in,
|
||||
sensor_msgs::PointCloud2 &out);
|
||||
/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
|
||||
* \param transform the transformation to use on the points
|
||||
* \param in the input PointCloud2 dataset
|
||||
* \param out the resultant transformed PointCloud2 dataset
|
||||
*/
|
||||
void
|
||||
transformPointCloud(
|
||||
const Eigen::Matrix4f & transform,
|
||||
const sensor_msgs::PointCloud2 & in,
|
||||
sensor_msgs::PointCloud2 & out);
|
||||
|
||||
/** \brief Obtain the transformation matrix from TF into an Eigen form
|
||||
* \param bt the TF transformation
|
||||
* \param out_mat the Eigen transformation
|
||||
*/
|
||||
void
|
||||
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat);
|
||||
/** \brief Obtain the transformation matrix from TF into an Eigen form
|
||||
* \param bt the TF transformation
|
||||
* \param out_mat the Eigen transformation
|
||||
*/
|
||||
void
|
||||
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat);
|
||||
}
|
||||
|
||||
#endif // PCL_ROS_TRANSFORMS_H_
|
||||
|
||||
|
||||
Reference in New Issue
Block a user