Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
PointCloud represents the base class in PCL for storing collections of 3D points.
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