Point Cloud Library (PCL) 1.12.1
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
44namespace pcl
45{
46
47template<typename PointSource, typename PointTarget> void
49{
50 // Create and start the rendering thread. This will open the display window.
52}
53
54
55template<typename PointSource, typename PointTarget> void
57{
58 // Stop the rendering thread. This will kill the display window.
59 viewer_thread_.~thread ();
60}
61
62
63template<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
187template<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
RegistrationVisualizer represents the base class for rendering the intermediate positions occupied by...
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...
void startDisplay()
Start the viewer thread.
void stopDisplay()
Stop the viewer thread.
PCL Visualizer main class.
shared_ptr< PCLVisualizer > Ptr
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133