Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 ////////////////////////////////////////////////////////////////////////////////////////////// 00040 template<typename PointSource, typename PointTarget> void 00041 pcl::RegistrationVisualizer<PointSource, PointTarget>::startDisplay () 00042 { 00043 // Create and start the rendering thread. This will open the display window. 00044 viewer_thread_ = boost::thread (&pcl::RegistrationVisualizer<PointSource, PointTarget>::runDisplay, this); 00045 } 00046 00047 ////////////////////////////////////////////////////////////////////////////////////////////// 00048 template<typename PointSource, typename PointTarget> void 00049 pcl::RegistrationVisualizer<PointSource, PointTarget>::stopDisplay () 00050 { 00051 // Stop the rendering thread. This will kill the display window. 00052 viewer_thread_.~thread (); 00053 } 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////// 00056 template<typename PointSource, typename PointTarget> void 00057 pcl::RegistrationVisualizer<PointSource, PointTarget>::runDisplay () 00058 { 00059 // Open 3D viewer 00060 viewer_ 00061 = boost::shared_ptr<pcl::visualization::PCLVisualizer> (new pcl::visualization::PCLVisualizer ("3D Viewer")); 00062 viewer_->initCameraParameters (); 00063 00064 // Create the handlers for the three point clouds buffers: cloud_source_, cloud_target_ and cloud_intermediate_ 00065 pcl::visualization::PointCloudColorHandlerCustom<PointSource> cloud_source_handler_ (cloud_source_.makeShared (), 00066 255, 0, 0); 00067 pcl::visualization::PointCloudColorHandlerCustom<PointTarget> cloud_target_handler_ (cloud_target_.makeShared (), 00068 0, 0, 255); 00069 pcl::visualization::PointCloudColorHandlerCustom<PointSource> cloud_intermediate_handler_ (cloud_intermediate_.makeShared (), 00070 255, 255, 0); 00071 00072 // Create the view port for displaying initial source and target point clouds 00073 int v1 (0); 00074 viewer_->createViewPort (0.0, 0.0, 0.5, 1.0, v1); 00075 viewer_->setBackgroundColor (0, 0, 0, v1); 00076 viewer_->addText ("Initial position of source and target point clouds", 10, 50, "title v1", v1); 00077 viewer_->addText ("Blue -> target", 10, 30, 0.0, 0.0, 1.0, "legend target v1", v1); 00078 viewer_->addText ("Red -> source", 10, 10, 1.0, 0.0, 0.0, "legend source v1", v1); 00079 // 00080 viewer_->addPointCloud<PointSource> (cloud_source_.makeShared (), cloud_source_handler_, "cloud source v1", v1); 00081 viewer_->addPointCloud<PointTarget> (cloud_target_.makeShared (), cloud_target_handler_, "cloud target v1", v1); 00082 00083 // Create the view port for displaying the registration process of source to target point cloud 00084 int v2 (0); 00085 viewer_->createViewPort (0.5, 0.0, 1.0, 1.0, v2); 00086 viewer_->setBackgroundColor (0.1, 0.1, 0.1, v2); 00087 std::string registration_port_title_ = "Registration using "+registration_method_name_; 00088 viewer_->addText (registration_port_title_, 10, 90, "title v2", v2); 00089 00090 viewer_->addText ("Yellow -> intermediate", 10, 50, 1.0, 1.0, 0.0, "legend intermediate v2", v2); 00091 viewer_->addText ("Blue -> target", 10, 30, 0.0, 0.0, 1.0, "legend target v2", v2); 00092 viewer_->addText ("Red -> source", 10, 10, 1.0, 0.0, 0.0, "legend source v2", v1); 00093 00094 // viewer_->addPointCloud<PointSource> (cloud_source_.makeShared (), cloud_source_handler_, "cloud source v2", v2); 00095 viewer_->addPointCloud<PointTarget> (cloud_target_.makeShared (), cloud_target_handler_, "cloud target v2", v2); 00096 viewer_->addPointCloud<PointSource> (cloud_intermediate_.makeShared (), cloud_intermediate_handler_, 00097 "cloud intermediate v2", v2); 00098 00099 // Used to remove all old correspondences 00100 size_t correspondeces_old_size = 0; 00101 00102 // Add coordinate system to both ports 00103 viewer_->addCoordinateSystem (1.0); 00104 00105 // The root name of correspondence lines 00106 std::string line_root_ = "line"; 00107 00108 // Visualization loop 00109 while (!viewer_->wasStopped ()) 00110 { 00111 // Lock access to visualizer buffers 00112 visualizer_updating_mutex_.lock (); 00113 00114 // Updating intermediate point cloud 00115 // Remove old point cloud 00116 viewer_->removePointCloud ("cloud intermediate v2", v2); 00117 00118 // Add the new point cloud 00119 viewer_->addPointCloud<PointSource> (cloud_intermediate_.makeShared (), cloud_intermediate_handler_, 00120 "cloud intermediate v2", v2); 00121 00122 // Updating the correspondece lines 00123 00124 std::string line_name_; 00125 // Remove the old correspondeces 00126 for (size_t correspondence_id = 0; correspondence_id < correspondeces_old_size; ++correspondence_id) 00127 { 00128 // Generate the line name 00129 line_name_ = getIndexedName (line_root_, correspondence_id); 00130 00131 // Remove the current line according to it's name 00132 viewer_->removeShape (line_name_, v2); 00133 } 00134 00135 // Display the new correspondences lines 00136 size_t correspondences_new_size = cloud_intermediate_indices_.size (); 00137 00138 00139 std::stringstream stream_; 00140 stream_ << "Random -> correspondences " << correspondences_new_size; 00141 viewer_->removeShape ("correspondences_size", 0); 00142 viewer_->addText (stream_.str(), 10, 70, 0.0, 1.0, 0.0, "correspondences_size", v2); 00143 00144 // Display entire set of correspondece lines if no maximum displayed correspondences is set 00145 if( ( 0 < maximum_displayed_correspondences_ ) && 00146 (maximum_displayed_correspondences_ < correspondences_new_size) ) 00147 correspondences_new_size = maximum_displayed_correspondences_; 00148 00149 // Actualize correspondeces_old_size 00150 correspondeces_old_size = correspondences_new_size; 00151 00152 // Update new correspondence lines 00153 for (size_t correspondence_id = 0; correspondence_id < correspondences_new_size; ++correspondence_id) 00154 { 00155 // Generate random color for current correspondence line 00156 double random_red = 255 * rand () / (RAND_MAX + 1.0); 00157 double random_green = 255 * rand () / (RAND_MAX + 1.0); 00158 double random_blue = 255 * rand () / (RAND_MAX + 1.0); 00159 00160 // Generate the name for current line 00161 line_name_ = getIndexedName (line_root_, correspondence_id); 00162 00163 // Add the new correspondence line. 00164 viewer_->addLine (cloud_intermediate_[cloud_intermediate_indices_[correspondence_id]], 00165 cloud_target_[cloud_target_indices_[correspondence_id]], 00166 random_red, random_green, random_blue, 00167 line_name_, v2); 00168 } 00169 00170 // Unlock access to visualizer buffers 00171 visualizer_updating_mutex_.unlock (); 00172 00173 // Render visualizer updated buffers 00174 viewer_->spinOnce (100); 00175 boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 00176 00177 } 00178 } 00179 00180 ////////////////////////////////////////////////////////////////////////////////////////////// 00181 template<typename PointSource, typename PointTarget> void 00182 pcl::RegistrationVisualizer<PointSource, PointTarget>::updateIntermediateCloud ( 00183 const pcl::PointCloud<PointSource> &cloud_src, 00184 const std::vector<int> &indices_src, 00185 const pcl::PointCloud<PointTarget> &cloud_tgt, 00186 const std::vector<int> &indices_tgt) 00187 { 00188 // Lock local buffers 00189 visualizer_updating_mutex_.lock (); 00190 00191 // Update source and target point clouds if this is the first callback 00192 // Here we are sure that source and target point clouds are initialized 00193 if (!first_update_flag_) 00194 { 00195 first_update_flag_ = true; 00196 00197 this->cloud_source_ = cloud_src; 00198 this->cloud_target_ = cloud_tgt; 00199 00200 this->cloud_intermediate_ = cloud_src; 00201 } 00202 00203 // Copy the intermediate point cloud and it's associates indices 00204 cloud_intermediate_ = cloud_src; 00205 cloud_intermediate_indices_ = indices_src; 00206 00207 // Copy the intermediate indices associate to the target point cloud 00208 cloud_target_indices_ = indices_tgt; 00209 00210 // Unlock local buffers 00211 visualizer_updating_mutex_.unlock (); 00212 }