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_