Point Cloud Library (PCL)  1.12.0-dev
registration_visualizer.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #pragma once
40 
41 #include <thread>
42 
43 
44 namespace pcl
45 {
46 
47 template<typename PointSource, typename PointTarget> void
49 {
50  // Create and start the rendering thread. This will open the display window.
51  viewer_thread_ = std::thread (&pcl::RegistrationVisualizer<PointSource, PointTarget>::runDisplay, this);
52 }
53 
54 
55 template<typename PointSource, typename PointTarget> void
57 {
58  // Stop the rendering thread. This will kill the display window.
59  viewer_thread_.~thread ();
60 }
61 
62 
63 template<typename PointSource, typename PointTarget> void
65 {
66  // Open 3D viewer
67  viewer_
69  viewer_->initCameraParameters ();
70 
71  // Create the handlers for the three point clouds buffers: cloud_source_, cloud_target_ and cloud_intermediate_
72  pcl::visualization::PointCloudColorHandlerCustom<PointSource> cloud_source_handler_ (cloud_source_.makeShared (),
73  255, 0, 0);
74  pcl::visualization::PointCloudColorHandlerCustom<PointTarget> cloud_target_handler_ (cloud_target_.makeShared (),
75  0, 0, 255);
76  pcl::visualization::PointCloudColorHandlerCustom<PointSource> cloud_intermediate_handler_ (cloud_intermediate_.makeShared (),
77  255, 255, 0);
78 
79  // Create the view port for displaying initial source and target point clouds
80  int v1 (0);
81  viewer_->createViewPort (0.0, 0.0, 0.5, 1.0, v1);
82  viewer_->setBackgroundColor (0, 0, 0, v1);
83  viewer_->addText ("Initial position of source and target point clouds", 10, 50, "title v1", v1);
84  viewer_->addText ("Blue -> target", 10, 30, 0.0, 0.0, 1.0, "legend target v1", v1);
85  viewer_->addText ("Red -> source", 10, 10, 1.0, 0.0, 0.0, "legend source v1", v1);
86  //
87  viewer_->addPointCloud<PointSource> (cloud_source_.makeShared (), cloud_source_handler_, "cloud source v1", v1);
88  viewer_->addPointCloud<PointTarget> (cloud_target_.makeShared (), cloud_target_handler_, "cloud target v1", v1);
89 
90  // Create the view port for displaying the registration process of source to target point cloud
91  int v2 (0);
92  viewer_->createViewPort (0.5, 0.0, 1.0, 1.0, v2);
93  viewer_->setBackgroundColor (0.1, 0.1, 0.1, v2);
94  std::string registration_port_title_ = "Registration using "+registration_method_name_;
95  viewer_->addText (registration_port_title_, 10, 90, "title v2", v2);
96 
97  viewer_->addText ("Yellow -> intermediate", 10, 50, 1.0, 1.0, 0.0, "legend intermediate v2", v2);
98  viewer_->addText ("Blue -> target", 10, 30, 0.0, 0.0, 1.0, "legend target v2", v2);
99  viewer_->addText ("Red -> source", 10, 10, 1.0, 0.0, 0.0, "legend source v2", v1);
100 
101 // viewer_->addPointCloud<PointSource> (cloud_source_.makeShared (), cloud_source_handler_, "cloud source v2", v2);
102  viewer_->addPointCloud<PointTarget> (cloud_target_.makeShared (), cloud_target_handler_, "cloud target v2", v2);
103  viewer_->addPointCloud<PointSource> (cloud_intermediate_.makeShared (), cloud_intermediate_handler_,
104  "cloud intermediate v2", v2);
105 
106  // Used to remove all old correspondences
107  std::size_t correspondeces_old_size = 0;
108 
109  // Add coordinate system to both ports
110  viewer_->addCoordinateSystem (1.0, "global");
111 
112  // The root name of correspondence lines
113  std::string line_root_ = "line";
114 
115  // Visualization loop
116  while (!viewer_->wasStopped ())
117  {
118  // Lock access to visualizer buffers
119  visualizer_updating_mutex_.lock ();
120 
121  // Updating intermediate point cloud
122  // Remove old point cloud
123  viewer_->removePointCloud ("cloud intermediate v2", v2);
124 
125  // Add the new point cloud
126  viewer_->addPointCloud<PointSource> (cloud_intermediate_.makeShared (), cloud_intermediate_handler_,
127  "cloud intermediate v2", v2);
128 
129  // Updating the correspondece lines
130 
131  std::string line_name_;
132  // Remove the old correspondeces
133  for (std::size_t correspondence_id = 0; correspondence_id < correspondeces_old_size; ++correspondence_id)
134  {
135  // Generate the line name
136  line_name_ = getIndexedName (line_root_, correspondence_id);
137 
138  // Remove the current line according to it's name
139  viewer_->removeShape (line_name_, v2);
140  }
141 
142  // Display the new correspondences lines
143  std::size_t correspondences_new_size = cloud_intermediate_indices_.size ();
144 
145 
146  const std::string correspondences_text = "Random -> correspondences " + std::to_string(correspondences_new_size);
147  viewer_->removeShape ("correspondences_size", 0);
148  viewer_->addText (correspondences_text, 10, 70, 0.0, 1.0, 0.0, "correspondences_size", v2);
149 
150  // Display entire set of correspondece lines if no maximum displayed correspondences is set
151  if( ( 0 < maximum_displayed_correspondences_ ) &&
152  (maximum_displayed_correspondences_ < correspondences_new_size) )
153  correspondences_new_size = maximum_displayed_correspondences_;
154 
155  // Actualize correspondeces_old_size
156  correspondeces_old_size = correspondences_new_size;
157 
158  // Update new correspondence lines
159  for (std::size_t correspondence_id = 0; correspondence_id < correspondences_new_size; ++correspondence_id)
160  {
161  // Generate random color for current correspondence line
162  double random_red = 255 * rand () / (RAND_MAX + 1.0);
163  double random_green = 255 * rand () / (RAND_MAX + 1.0);
164  double random_blue = 255 * rand () / (RAND_MAX + 1.0);
165 
166  // Generate the name for current line
167  line_name_ = getIndexedName (line_root_, correspondence_id);
168 
169  // Add the new correspondence line.
170  viewer_->addLine (cloud_intermediate_[cloud_intermediate_indices_[correspondence_id]],
171  cloud_target_[cloud_target_indices_[correspondence_id]],
172  random_red, random_green, random_blue,
173  line_name_, v2);
174  }
175 
176  // Unlock access to visualizer buffers
177  visualizer_updating_mutex_.unlock ();
178 
179  // Render visualizer updated buffers
180  viewer_->spinOnce (100);
181  using namespace std::chrono_literals;
182  std::this_thread::sleep_for(100ms);
183  }
184 }
185 
186 
187 template<typename PointSource, typename PointTarget> void
189  const pcl::PointCloud<PointSource> &cloud_src,
190  const pcl::Indices &indices_src,
191  const pcl::PointCloud<PointTarget> &cloud_tgt,
192  const pcl::Indices &indices_tgt)
193 {
194  // Lock local buffers
195  visualizer_updating_mutex_.lock ();
196 
197  // Update source and target point clouds if this is the first callback
198  // Here we are sure that source and target point clouds are initialized
199  if (!first_update_flag_)
200  {
201  first_update_flag_ = true;
202 
203  this->cloud_source_ = cloud_src;
204  this->cloud_target_ = cloud_tgt;
205 
206  this->cloud_intermediate_ = cloud_src;
207  }
208 
209  // Copy the intermediate point cloud and it's associates indices
210  cloud_intermediate_ = cloud_src;
211  cloud_intermediate_indices_ = indices_src;
212 
213  // Copy the intermediate indices associate to the target point cloud
214  cloud_target_indices_ = indices_tgt;
215 
216  // Unlock local buffers
217  visualizer_updating_mutex_.unlock ();
218 }
219 
220 } // namespace pcl
221 
pcl
Definition: convolution.h:46
pcl::visualization::PointCloudColorHandlerCustom
Handler for predefined user colors.
Definition: point_cloud_color_handlers.h:186
pcl::RegistrationVisualizer::startDisplay
void startDisplay()
Start the viewer thread.
Definition: registration_visualizer.hpp:48
pcl::RegistrationVisualizer::stopDisplay
void stopDisplay()
Stop the viewer thread.
Definition: registration_visualizer.hpp:56
pcl::PointCloud< PointSource >
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::visualization::PCLVisualizer
PCL Visualizer main class.
Definition: pcl_visualizer.h:93
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