Point Cloud Library (PCL)  1.7.1
registration_visualizer.h
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$
35  *
36  */
37 
38 #ifndef PCL_REGISTRATION_VISUALIZER_H_
39 #define PCL_REGISTRATION_VISUALIZER_H_
40 
41 // PCL
42 #include <pcl/registration/registration.h>
43 #include <pcl/visualization/pcl_visualizer.h>
44 
45 namespace pcl
46 {
47  /** \brief @b RegistrationVisualizer represents the base class for rendering
48  * the intermediate positions ocupied by the source point cloud during it's registration
49  * to the target point cloud. A registration algorithm is considered as input and
50  * it's covergence is rendered.
51  * \author Gheorghe Lisca
52  * \ingroup visualization
53  */
54  template<typename PointSource, typename PointTarget>
56  {
57 
58  public:
59  /** \brief Empty constructor. */
61  viewer_ (),
62  viewer_thread_ (),
63  registration_method_name_ (),
64  update_visualizer_ (),
65  first_update_flag_ (),
66  cloud_source_ (),
67  cloud_target_ (),
68  visualizer_updating_mutex_ (),
69  cloud_intermediate_ (),
70  cloud_intermediate_indices_ (),
71  cloud_target_indices_ (),
72  maximum_displayed_correspondences_ (0)
73  {}
74 
75  /** \brief Set the registration algorithm whose intermediate steps will be rendered.
76  * The method creates the local callback function pcl::RegistrationVisualizer::update_visualizer_ and
77  * binds it to the local biffers update function pcl::RegistrationVisualizer::updateIntermediateCloud().
78  * The local callback function pcl::RegistrationVisualizer::update_visualizer_ is then linked to
79  * the pcl::Registration::update_visualizer_ callback function.
80  * \param registration represents the registration method whose intermediate steps will be rendered.
81  */
82  bool
84  {
85  // Update the name of the registration method to be desplayed
86  registration_method_name_ = registration.getClassName();
87 
88  // Create the local callback function and bind it to the local function resposable for updating
89  // the local buffers
91  this, _1, _2, _3, _4);
92 
93  // Register the local callback function to the registration algorithm callback function
94  registration.registerVisualizationCallback (this->update_visualizer_);
95 
96  // Flag that no visualizer update was done. It indicates to visualizer update function to copy
97  // the registration input source and the target point clouds in the next call.
98  visualizer_updating_mutex_.lock ();
99 
100  first_update_flag_ = false;
101 
102  visualizer_updating_mutex_.unlock ();
103 
104  return true;
105  }
106 
107  /** \brief Start the viewer thread
108  */
109  void
110  startDisplay ();
111 
112  /** \brief Stop the viewer thread
113  */
114  void
115  stopDisplay ();
116 
117  /** \brief Updates visualizer local buffers cloud_intermediate, cloud_intermediate_indices, cloud_target_indices with
118  * the newest registration intermediate results.
119  * \param cloud_src represents the initial source point cloud
120  * \param indices_src represents the incices of the intermediate source points used for the estimation of rigid transformation
121  * \param cloud_tgt represents the target point cloud
122  * \param indices_tgt represents the incices of the target points used for the estimation of rigid transformation
123  */
124  void
125  updateIntermediateCloud (const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src,
126  const pcl::PointCloud<PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt);
127 
128  /** \brief Set maximum number of corresponcence lines whch will be rendered. */
129  inline void
130  setMaximumDisplayedCorrespondences (const int maximum_displayed_correspondences)
131  {
132  // This method is usualy called form other thread than visualizer thread
133  // therefore same visualizer_updating_mutex_ will be used
134 
135  // Lock maximum_displayed_correspondences_
136  visualizer_updating_mutex_.lock ();
137 
138  // Update maximum_displayed_correspondences_
139  maximum_displayed_correspondences_ = maximum_displayed_correspondences;
140 
141  // Unlock maximum_displayed_correspondences_
142  visualizer_updating_mutex_.unlock();
143  }
144 
145  /** \brief Return maximum number of corresponcence lines which are rendered. */
146  inline size_t
148  {
149  return maximum_displayed_correspondences_;
150  }
151 
152  private:
153  /** \brief Initialize and run the visualization loop. This function will be runned in the internal thread viewer_thread_ */
154  void
155  runDisplay ();
156 
157  /** \brief Return the string obtained by concatenating a root_name and an id */
158  inline std::string
159  getIndexedName (std::string &root_name, size_t &id)
160  {
161  std::stringstream id_stream_;
162  id_stream_ << id;
163  std::string indexed_name_ = root_name + id_stream_.str ();
164  return indexed_name_;
165  }
166 
167  /** \brief The registration viewer. */
168  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
169 
170  /** \brief The thread running the runDisplay() function. */
171  boost::thread viewer_thread_;
172 
173  /** \brief The name of the registration method whose intermediate results are rendered. */
174  std::string registration_method_name_;
175 
176  /** \brief Callback function linked to pcl::Registration::update_visualizer_ */
177  boost::function<void
178  (const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud<
179  PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt)> update_visualizer_;
180 
181  /** \brief Updates source and target point clouds only for the first update call. */
182  bool first_update_flag_;
183 
184  /** \brief The local buffer for source point cloud. */
185  pcl::PointCloud<PointSource> cloud_source_;
186 
187  /** \brief The local buffer for target point cloud. */
188  pcl::PointCloud<PointTarget> cloud_target_;
189 
190  /** \brief The mutex used for the sincronization of updating and rendering of the local buffers. */
191  boost::mutex visualizer_updating_mutex_;
192 
193  /** \brief The local buffer for intermediate point cloud obtained during registration process. */
194  pcl::PointCloud<PointSource> cloud_intermediate_;
195 
196  /** \brief The indices of intermediate points used for computation of rigid transformation. */
197  std::vector<int> cloud_intermediate_indices_;
198 
199  /** \brief The indices of target points used for computation of rigid transformation. */
200  std::vector<int> cloud_target_indices_;
201 
202  /** \brief The maximum number of displayed correspondences. */
203  size_t maximum_displayed_correspondences_;
204 
205  };
206 }
207 
208 #include <pcl/visualization/impl/registration_visualizer.hpp>
209 
210 #endif //#ifndef PCL_REGISTRATION_VISUALIZER_H_
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:418
bool setRegistration(pcl::Registration< PointSource, PointTarget > &registration)
Set the registration algorithm whose intermediate steps will be rendered.
void setMaximumDisplayedCorrespondences(const int maximum_displayed_correspondences)
Set maximum number of corresponcence lines whch will be rendered.
size_t getMaximumDisplayedCorrespondences()
Return maximum number of corresponcence lines which are rendered.
void stopDisplay()
Stop the viewer thread.
void startDisplay()
Start the viewer thread.
RegistrationVisualizer represents the base class for rendering the intermediate positions ocupied by ...
RegistrationVisualizer()
Empty constructor.
bool registerVisualizationCallback(boost::function< FunctionSignature > &visualizerCallback)
Register the user callback function which will be called from registration thread in order to update ...
Definition: registration.h:371
void updateIntermediateCloud(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &indices_src, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &indices_tgt)
Updates visualizer local buffers cloud_intermediate, cloud_intermediate_indices, cloud_target_indices...