Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 //////////////////////////////////////////////////////////////////////////////////////////////
40 template<typename PointSource, typename PointTarget> void
42 {
43  // Create and start the rendering thread. This will open the display window.
44  viewer_thread_ = boost::thread (&pcl::RegistrationVisualizer<PointSource, PointTarget>::runDisplay, this);
45 }
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template<typename PointSource, typename PointTarget> void
50 {
51  // Stop the rendering thread. This will kill the display window.
52  viewer_thread_.~thread ();
53 }
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////
56 template<typename PointSource, typename PointTarget> void
58 {
59  // Open 3D viewer
60  viewer_
61  = boost::shared_ptr<pcl::visualization::PCLVisualizer> (new pcl::visualization::PCLVisualizer ("3D Viewer"));
62  viewer_->initCameraParameters ();
63 
64  // Create the handlers for the three point clouds buffers: cloud_source_, cloud_target_ and cloud_intermediate_
65  pcl::visualization::PointCloudColorHandlerCustom<PointSource> cloud_source_handler_ (cloud_source_.makeShared (),
66  255, 0, 0);
67  pcl::visualization::PointCloudColorHandlerCustom<PointTarget> cloud_target_handler_ (cloud_target_.makeShared (),
68  0, 0, 255);
69  pcl::visualization::PointCloudColorHandlerCustom<PointSource> cloud_intermediate_handler_ (cloud_intermediate_.makeShared (),
70  255, 255, 0);
71 
72  // Create the view port for displaying initial source and target point clouds
73  int v1 (0);
74  viewer_->createViewPort (0.0, 0.0, 0.5, 1.0, v1);
75  viewer_->setBackgroundColor (0, 0, 0, v1);
76  viewer_->addText ("Initial position of source and target point clouds", 10, 50, "title v1", v1);
77  viewer_->addText ("Blue -> target", 10, 30, 0.0, 0.0, 1.0, "legend target v1", v1);
78  viewer_->addText ("Red -> source", 10, 10, 1.0, 0.0, 0.0, "legend source v1", v1);
79  //
80  viewer_->addPointCloud<PointSource> (cloud_source_.makeShared (), cloud_source_handler_, "cloud source v1", v1);
81  viewer_->addPointCloud<PointTarget> (cloud_target_.makeShared (), cloud_target_handler_, "cloud target v1", v1);
82 
83  // Create the view port for displaying the registration process of source to target point cloud
84  int v2 (0);
85  viewer_->createViewPort (0.5, 0.0, 1.0, 1.0, v2);
86  viewer_->setBackgroundColor (0.1, 0.1, 0.1, v2);
87  std::string registration_port_title_ = "Registration using "+registration_method_name_;
88  viewer_->addText (registration_port_title_, 10, 90, "title v2", v2);
89 
90  viewer_->addText ("Yellow -> intermediate", 10, 50, 1.0, 1.0, 0.0, "legend intermediate v2", v2);
91  viewer_->addText ("Blue -> target", 10, 30, 0.0, 0.0, 1.0, "legend target v2", v2);
92  viewer_->addText ("Red -> source", 10, 10, 1.0, 0.0, 0.0, "legend source v2", v1);
93 
94 // viewer_->addPointCloud<PointSource> (cloud_source_.makeShared (), cloud_source_handler_, "cloud source v2", v2);
95  viewer_->addPointCloud<PointTarget> (cloud_target_.makeShared (), cloud_target_handler_, "cloud target v2", v2);
96  viewer_->addPointCloud<PointSource> (cloud_intermediate_.makeShared (), cloud_intermediate_handler_,
97  "cloud intermediate v2", v2);
98 
99  // Used to remove all old correspondences
100  size_t correspondeces_old_size = 0;
101 
102  // Add coordinate system to both ports
103  viewer_->addCoordinateSystem (1.0, "global");
104 
105  // The root name of correspondence lines
106  std::string line_root_ = "line";
107 
108  // Visualization loop
109  while (!viewer_->wasStopped ())
110  {
111  // Lock access to visualizer buffers
112  visualizer_updating_mutex_.lock ();
113 
114  // Updating intermediate point cloud
115  // Remove old point cloud
116  viewer_->removePointCloud ("cloud intermediate v2", v2);
117 
118  // Add the new point cloud
119  viewer_->addPointCloud<PointSource> (cloud_intermediate_.makeShared (), cloud_intermediate_handler_,
120  "cloud intermediate v2", v2);
121 
122  // Updating the correspondece lines
123 
124  std::string line_name_;
125  // Remove the old correspondeces
126  for (size_t correspondence_id = 0; correspondence_id < correspondeces_old_size; ++correspondence_id)
127  {
128  // Generate the line name
129  line_name_ = getIndexedName (line_root_, correspondence_id);
130 
131  // Remove the current line according to it's name
132  viewer_->removeShape (line_name_, v2);
133  }
134 
135  // Display the new correspondences lines
136  size_t correspondences_new_size = cloud_intermediate_indices_.size ();
137 
138 
139  std::stringstream stream_;
140  stream_ << "Random -> correspondences " << correspondences_new_size;
141  viewer_->removeShape ("correspondences_size", 0);
142  viewer_->addText (stream_.str(), 10, 70, 0.0, 1.0, 0.0, "correspondences_size", v2);
143 
144  // Display entire set of correspondece lines if no maximum displayed correspondences is set
145  if( ( 0 < maximum_displayed_correspondences_ ) &&
146  (maximum_displayed_correspondences_ < correspondences_new_size) )
147  correspondences_new_size = maximum_displayed_correspondences_;
148 
149  // Actualize correspondeces_old_size
150  correspondeces_old_size = correspondences_new_size;
151 
152  // Update new correspondence lines
153  for (size_t correspondence_id = 0; correspondence_id < correspondences_new_size; ++correspondence_id)
154  {
155  // Generate random color for current correspondence line
156  double random_red = 255 * rand () / (RAND_MAX + 1.0);
157  double random_green = 255 * rand () / (RAND_MAX + 1.0);
158  double random_blue = 255 * rand () / (RAND_MAX + 1.0);
159 
160  // Generate the name for current line
161  line_name_ = getIndexedName (line_root_, correspondence_id);
162 
163  // Add the new correspondence line.
164  viewer_->addLine (cloud_intermediate_[cloud_intermediate_indices_[correspondence_id]],
165  cloud_target_[cloud_target_indices_[correspondence_id]],
166  random_red, random_green, random_blue,
167  line_name_, v2);
168  }
169 
170  // Unlock access to visualizer buffers
171  visualizer_updating_mutex_.unlock ();
172 
173  // Render visualizer updated buffers
174  viewer_->spinOnce (100);
175  boost::this_thread::sleep (boost::posix_time::microseconds (100000));
176 
177  }
178 }
179 
180 //////////////////////////////////////////////////////////////////////////////////////////////
181 template<typename PointSource, typename PointTarget> void
183  const pcl::PointCloud<PointSource> &cloud_src,
184  const std::vector<int> &indices_src,
185  const pcl::PointCloud<PointTarget> &cloud_tgt,
186  const std::vector<int> &indices_tgt)
187 {
188  // Lock local buffers
189  visualizer_updating_mutex_.lock ();
190 
191  // Update source and target point clouds if this is the first callback
192  // Here we are sure that source and target point clouds are initialized
193  if (!first_update_flag_)
194  {
195  first_update_flag_ = true;
196 
197  this->cloud_source_ = cloud_src;
198  this->cloud_target_ = cloud_tgt;
199 
200  this->cloud_intermediate_ = cloud_src;
201  }
202 
203  // Copy the intermediate point cloud and it's associates indices
204  cloud_intermediate_ = cloud_src;
205  cloud_intermediate_indices_ = indices_src;
206 
207  // Copy the intermediate indices associate to the target point cloud
208  cloud_target_indices_ = indices_tgt;
209 
210  // Unlock local buffers
211  visualizer_updating_mutex_.unlock ();
212 }
void startDisplay()
Start the viewer thread.
void stopDisplay()
Stop the viewer thread.
PCL Visualizer main class.
RegistrationVisualizer represents the base class for rendering the intermediate positions occupied by...
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...