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