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 
41 void
43 {
45 
46  //ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
47  pub_output_ = advertise<PointCloudIn> (*pnh_, "output", max_queue_size_);
48  pub_normals_ = advertise<NormalCloudOut> (*pnh_, "normals", max_queue_size_);
49 
50  //if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
51  if (!pnh_->getParam ("search_radius", search_radius_))
52  {
53  //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 ());
54  NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ());
55  return;
56  }
57  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
58  {
59  NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
60  return;
61  }
62 
63  // Enable the dynamic reconfigure service
64  srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_);
65  dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
66  srv_->setCallback (f);
67 
68  // ---[ Optional parameters
69  pnh_->getParam ("use_indices", use_indices_);
70 
71  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
72  " - use_indices : %s",
73  getName ().c_str (),
74  (use_indices_) ? "true" : "false");
75 
77 }
78 
80 void
82 {
83  // If we're supposed to look for PointIndices (indices)
84  if (use_indices_)
85  {
86  // Subscribe to the input using a filter
87  sub_input_filter_.subscribe (*pnh_, "input", 1);
88  // If indices are enabled, subscribe to the indices
89  sub_indices_filter_.subscribe (*pnh_, "indices", 1);
90 
91  if (approximate_sync_)
92  {
93  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
94  // surface not enabled, connect the input-indices duo and register
95  sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
96  sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
97  }
98  else
99  {
100  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_);
101  // surface not enabled, connect the input-indices duo and register
102  sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
103  sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
104  }
105  }
106  else
107  // Subscribe in an old fashion to input only (no filters)
108  sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ()));
109 }
110 
112 void
114 {
115  if (use_indices_)
116  {
117  sub_input_filter_.unsubscribe ();
118  sub_indices_filter_.unsubscribe ();
119  }
120  else
121  sub_input_.shutdown ();
122 }
123 
125 void
127  const PointIndicesConstPtr &indices)
128 {
129  // No subscribers, no work
130  if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
131  return;
132 
133  // Output points have the same type as the input, they are only smoothed
134  PointCloudIn output;
135 
136  // Normals are also estimated and published on a separate topic
137  NormalCloudOut::Ptr normals (new NormalCloudOut ());
138 
139  // If cloud is given, check if it's valid
140  if (!isValid (cloud))
141  {
142  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
143  output.header = cloud->header;
144  pub_output_.publish (ros_ptr(output.makeShared ()));
145  return;
146  }
147  // If indices are given, check if they are valid
148  if (indices && !isValid (indices, "indices"))
149  {
150  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
151  output.header = cloud->header;
152  pub_output_.publish (ros_ptr(output.makeShared ()));
153  return;
154  }
155 
157  if (indices)
158  NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
159  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
160  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
161  getName ().c_str (),
162  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 (),
163  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
164  else
165  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 ());
167 
168  // Reset the indices and surface pointers
169  impl_.setInputCloud (pcl_ptr(cloud));
170 
171  IndicesPtr indices_ptr;
172  if (indices)
173  indices_ptr.reset (new std::vector<int> (indices->indices));
174 
175  impl_.setIndices (indices_ptr);
176 
177  // Initialize the spatial locator
178 
179  // Do the reconstructon
180  //impl_.process (output);
181 
182  // Publish a Boost shared ptr const data
183  // Enforce that the TF frame and the timestamp are copied
184  output.header = cloud->header;
185  pub_output_.publish (ros_ptr(output.makeShared ()));
186  normals->header = cloud->header;
187  pub_normals_.publish (ros_ptr(normals));
188 }
189 
191 void
192 pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t /*level*/)
193 {
194  // \Note Zoli, shouldn't this be implemented in MLS too?
195  /*if (k_ != config.k_search)
196  {
197  k_ = config.k_search;
198  NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_);
199  }*/
200  if (search_radius_ != config.search_radius)
201  {
202  search_radius_ = config.search_radius;
203  NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
204  impl_.setSearchRadius (search_radius_);
205  }
206  if (spatial_locator_type_ != config.spatial_locator)
207  {
208  spatial_locator_type_ = config.spatial_locator;
209  NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
210  }
211  if (use_polynomial_fit_ != config.use_polynomial_fit)
212  {
213  use_polynomial_fit_ = config.use_polynomial_fit;
214  NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
215 #if PCL_VERSION_COMPARE(<, 1, 9, 0)
216  impl_.setPolynomialFit (use_polynomial_fit_);
217 #else
218  if (use_polynomial_fit_)
219  {
220  NODELET_WARN ("[config_callback] use_polynomial_fit is deprecated, use polynomial_order instead!");
221  if (impl_.getPolynomialOrder () < 2)
222  {
223  impl_.setPolynomialOrder (2);
224  }
225  }
226  else
227  {
228  impl_.setPolynomialOrder (0);
229  }
230 #endif
231  }
232  if (polynomial_order_ != config.polynomial_order)
233  {
234  polynomial_order_ = config.polynomial_order;
235  NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
236  impl_.setPolynomialOrder (polynomial_order_);
237  }
238  if (gaussian_parameter_ != config.gaussian_parameter)
239  {
240  gaussian_parameter_ = config.gaussian_parameter;
241  NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
242  impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
243  }
244 }
245 
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
pcl_ros::MovingLeastSquares::subscribe
virtual void subscribe()
LazyNodelet connection routine.
Definition: moving_least_squares.cpp:81
boost::shared_ptr< const PointCloudIn >
pcl_ros::MovingLeastSquares::srv_
boost::shared_ptr< dynamic_reconfigure::Server< MLSConfig > > srv_
Pointer to a dynamic reconfigure service.
Definition: moving_least_squares.h:107
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
MovingLeastSquares
pcl_ros::MovingLeastSquares MovingLeastSquares
Definition: moving_least_squares.cpp:246
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::PCLNodelet::use_indices_
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:95
pcl_ros::MovingLeastSquares
MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation....
Definition: moving_least_squares.h:60
NODELET_WARN
#define NODELET_WARN(...)
pcl_ros::MovingLeastSquares::PointCloudIn
pcl::PointCloud< PointIn > PointCloudIn
Definition: moving_least_squares.h:65
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
f
f
moving_least_squares.h
fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
pcl_ros::MovingLeastSquares::unsubscribe
virtual void unsubscribe()
Definition: moving_least_squares.cpp:113
pcl_ros::PCLNodelet::max_queue_size_
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:128
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
pcl_ros::PCLNodelet::IndicesPtr
pcl::IndicesPtr IndicesPtr
Definition: pcl_nodelet.h:90
pcl_ros::PCLNodelet::pub_output_
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:125
pcl_ros::PCLNodelet::onInit
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
pcl_ros::MovingLeastSquares::config_callback
void config_callback(MLSConfig &config, uint32_t level)
Dynamic reconfigure callback.
Definition: moving_least_squares.cpp:192
pcl_ros::MovingLeastSquares::pub_normals_
ros::Publisher pub_normals_
The output PointCloud (containing the normals) publisher.
Definition: moving_least_squares.h:139
nodelet::Nodelet
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
pcl_ros::MovingLeastSquares::search_radius_
double search_radius_
The nearest neighbors search radius for each point.
Definition: moving_least_squares.h:81
pcl::pcl_ptr
boost::shared_ptr< T > pcl_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:392
class_list_macros.hpp
nodelet::Nodelet::getName
const std::string & getName() const
pcl_ros::MovingLeastSquares::onInit
virtual void onInit()
Nodelet initialization routine.
Definition: moving_least_squares.cpp:42
pcl_ros::MovingLeastSquares::NormalCloudOut
pcl::PointCloud< NormalOut > NormalCloudOut
Definition: moving_least_squares.h:68
pcl_ros::MovingLeastSquares::spatial_locator_type_
int spatial_locator_type_
Parameter for the spatial locator tree. By convention, the values represent: 0: ANN (Approximate Near...
Definition: moving_least_squares.h:104
pcl_ros::MovingLeastSquares::input_indices_callback
void input_indices_callback(const PointCloudInConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback.
Definition: moving_least_squares.cpp:126
NODELET_DEBUG
#define NODELET_DEBUG(...)
pcl::ros_ptr
boost::shared_ptr< T > ros_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:354


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Fri Jul 12 2024 02:54:40