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