moving_least_squares.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: moving_least_squares.cpp 36097 2011-02-20 14:18:58Z marton $
35  *
36  */
37 
40 #include <pcl/io/io.h>
42 void
44 {
46 
47  //ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
48  pub_output_ = advertise<PointCloudIn> (*pnh_, "output", max_queue_size_);
49  pub_normals_ = advertise<NormalCloudOut> (*pnh_, "normals", max_queue_size_);
50 
51  //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
52  if (!pnh_->getParam ("search_radius", search_radius_))
53  {
54  //NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
55  NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ());
56  return;
57  }
58  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
59  {
60  NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
61  return;
62  }
63 
64  // Enable the dynamic reconfigure service
65  srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_);
66  dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
67  srv_->setCallback (f);
68 
69  // ---[ Optional parameters
70  pnh_->getParam ("use_indices", use_indices_);
71 
72  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
73  " - use_indices : %s",
74  getName ().c_str (),
75  (use_indices_) ? "true" : "false");
76 
78 }
79 
81 void
83 {
84  // If we're supposed to look for PointIndices (indices)
85  if (use_indices_)
86  {
87  // Subscribe to the input using a filter
88  sub_input_filter_.subscribe (*pnh_, "input", 1);
89  // If indices are enabled, subscribe to the indices
90  sub_indices_filter_.subscribe (*pnh_, "indices", 1);
91 
93  {
94  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
95  // surface not enabled, connect the input-indices duo and register
97  sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
98  }
99  else
100  {
101  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_);
102  // surface not enabled, connect the input-indices duo and register
104  sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
105  }
106  }
107  else
108  // Subscribe in an old fashion to input only (no filters)
109  sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ()));
110 }
111 
113 void
115 {
116  if (use_indices_)
117  {
120  }
121  else
122  sub_input_.shutdown ();
123 }
124 
126 void
128  const PointIndicesConstPtr &indices)
129 {
130  // No subscribers, no work
132  return;
133 
134  // Output points have the same type as the input, they are only smoothed
135  PointCloudIn output;
136 
137  // Normals are also estimated and published on a separate topic
138  NormalCloudOut::Ptr normals (new NormalCloudOut ());
139 
140  // If cloud is given, check if it's valid
141  if (!isValid (cloud))
142  {
143  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
144  output.header = cloud->header;
145  pub_output_.publish (output.makeShared ());
146  return;
147  }
148  // If indices are given, check if they are valid
149  if (indices && !isValid (indices, "indices"))
150  {
151  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
152  output.header = cloud->header;
153  pub_output_.publish (output.makeShared ());
154  return;
155  }
156 
158  if (indices)
159  NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
160  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
161  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
162  getName ().c_str (),
163  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
164  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
165  else
166  NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
168 
169  // Reset the indices and surface pointers
170  impl_.setInputCloud (cloud);
171 
172  IndicesPtr indices_ptr;
173  if (indices)
174  indices_ptr.reset (new std::vector<int> (indices->indices));
175 
176  impl_.setIndices (indices_ptr);
177 
178  // Initialize the spatial locator
179 
180  // Do the reconstructon
181  //impl_.process (output);
182 
183  // Publish a Boost shared ptr const data
184  // Enforce that the TF frame and the timestamp are copied
185  output.header = cloud->header;
186  pub_output_.publish (output.makeShared ());
187  normals->header = cloud->header;
188  pub_normals_.publish (normals);
189 }
190 
192 void
193 pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
194 {
195  // \Note Zoli, shouldn't this be implemented in MLS too?
196  /*if (k_ != config.k_search)
197  {
198  k_ = config.k_search;
199  NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
200  }*/
201  if (search_radius_ != config.search_radius)
202  {
203  search_radius_ = config.search_radius;
204  NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
205  impl_.setSearchRadius (search_radius_);
206  }
207  if (spatial_locator_type_ != config.spatial_locator)
208  {
209  spatial_locator_type_ = config.spatial_locator;
210  NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
211  }
212  if (use_polynomial_fit_ != config.use_polynomial_fit)
213  {
214  use_polynomial_fit_ = config.use_polynomial_fit;
215  NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
216  impl_.setPolynomialFit (use_polynomial_fit_);
217  }
218  if (polynomial_order_ != config.polynomial_order)
219  {
220  polynomial_order_ = config.polynomial_order;
221  NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
222  impl_.setPolynomialOrder (polynomial_order_);
223  }
224  if (gaussian_parameter_ != config.gaussian_parameter)
225  {
226  gaussian_parameter_ = config.gaussian_parameter;
227  NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
228  impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
229  }
230 }
231 
ros::Subscriber sub_input_
The input PointCloud subscriber.
pcl::PointCloud< PointIn > PointCloudIn
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloudIn, PointIndices > > > sync_input_indices_e_
Synchronized input, and indices.
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:203
void publish(const boost::shared_ptr< M > &message) const
f
std::string resolveName(const std::string &name, bool remap=true) const
int polynomial_order_
The order of the polynomial to be fit.
const std::string & getName() const
double gaussian_parameter_
How &#39;flat&#39; should the neighbor weighting gaussian be (the smaller, the more local the fit)...
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:121
void config_callback(MLSConfig &config, uint32_t level)
Dynamic reconfigure callback.
pcl::MovingLeastSquares< PointIn, NormalOut > impl_
The PCL implementation used.
void input_indices_callback(const PointCloudInConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback.
int spatial_locator_type_
Parameter for the spatial locator tree. By convention, the values represent: 0: ANN (Approximate Near...
pcl_ros::MovingLeastSquares MovingLeastSquares
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
Definition: pcl_nodelet.h:140
virtual void subscribe()
LazyNodelet connection routine.
ros::NodeHandle & getMTPrivateNodeHandle() const
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
double search_radius_
The nearest neighbors search radius for each point.
boost::shared_ptr< ros::NodeHandle > pnh_
PointCloudIn::ConstPtr PointCloudInConstPtr
bool use_polynomial_fit_
The number of K nearest neighbors to use for each point.
virtual void onInit()
Nodelet initialization routine.
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.
boost::shared_ptr< dynamic_reconfigure::Server< MLSConfig > > srv_
Pointer to a dynamic reconfigure service.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloudIn, PointIndices > > > sync_input_indices_a_
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet)
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
ros::Publisher pub_normals_
The output PointCloud (containing the normals) publisher.
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: pcl_nodelet.h:130
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:127
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:124
#define NODELET_DEBUG(...)
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:94
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:83
pcl::PointCloud< NormalOut > NormalCloudOut
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.
Definition: pcl_nodelet.h:118


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Jun 5 2019 19:52:52