Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id$ 00035 * 00036 */ 00037 00038 #ifndef PCL_REGISTRATION_VISUALIZER_H_ 00039 #define PCL_REGISTRATION_VISUALIZER_H_ 00040 00041 // PCL 00042 #include <pcl/registration/registration.h> 00043 #include <pcl/visualization/pcl_visualizer.h> 00044 00045 namespace pcl 00046 { 00047 /** \brief @b RegistrationVisualizer represents the base class for rendering 00048 * the intermediate positions ocupied by the source point cloud during it's registration 00049 * to the target point cloud. A registration algorithm is considered as input and 00050 * it's covergence is rendered. 00051 * \author Gheorghe Lisca 00052 * \ingroup visualization 00053 */ 00054 template<typename PointSource, typename PointTarget> 00055 class RegistrationVisualizer 00056 { 00057 00058 public: 00059 /** \brief Empty constructor. */ 00060 RegistrationVisualizer () : 00061 viewer_ (), 00062 viewer_thread_ (), 00063 registration_method_name_ (), 00064 update_visualizer_ (), 00065 first_update_flag_ (), 00066 cloud_source_ (), 00067 cloud_target_ (), 00068 visualizer_updating_mutex_ (), 00069 cloud_intermediate_ (), 00070 cloud_intermediate_indices_ (), 00071 cloud_target_indices_ (), 00072 maximum_displayed_correspondences_ (0) 00073 {} 00074 00075 /** \brief Set the registration algorithm whose intermediate steps will be rendered. 00076 * The method creates the local callback function pcl::RegistrationVisualizer::update_visualizer_ and 00077 * binds it to the local biffers update function pcl::RegistrationVisualizer::updateIntermediateCloud(). 00078 * The local callback function pcl::RegistrationVisualizer::update_visualizer_ is then linked to 00079 * the pcl::Registration::update_visualizer_ callback function. 00080 * \param registration represents the registration method whose intermediate steps will be rendered. 00081 */ 00082 bool 00083 setRegistration (pcl::Registration<PointSource, PointTarget> ®istration) 00084 { 00085 // Update the name of the registration method to be desplayed 00086 registration_method_name_ = registration.getClassName(); 00087 00088 // Create the local callback function and bind it to the local function resposable for updating 00089 // the local buffers 00090 update_visualizer_ = boost::bind (&RegistrationVisualizer<PointSource, PointTarget>::updateIntermediateCloud, 00091 this, _1, _2, _3, _4); 00092 00093 // Register the local callback function to the registration algorithm callback function 00094 registration.registerVisualizationCallback (this->update_visualizer_); 00095 00096 // Flag that no visualizer update was done. It indicates to visualizer update function to copy 00097 // the registration input source and the target point clouds in the next call. 00098 visualizer_updating_mutex_.lock (); 00099 00100 first_update_flag_ = false; 00101 00102 visualizer_updating_mutex_.unlock (); 00103 00104 return true; 00105 } 00106 00107 /** \brief Start the viewer thread 00108 */ 00109 void 00110 startDisplay (); 00111 00112 /** \brief Stop the viewer thread 00113 */ 00114 void 00115 stopDisplay (); 00116 00117 /** \brief Updates visualizer local buffers cloud_intermediate, cloud_intermediate_indices, cloud_target_indices with 00118 * the newest registration intermediate results. 00119 * \param cloud_src represents the initial source point cloud 00120 * \param indices_src represents the incices of the intermediate source points used for the estimation of rigid transformation 00121 * \param cloud_tgt represents the target point cloud 00122 * \param indices_tgt represents the incices of the target points used for the estimation of rigid transformation 00123 */ 00124 void 00125 updateIntermediateCloud (const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src, 00126 const pcl::PointCloud<PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt); 00127 00128 /** \brief Set maximum number of corresponcence lines whch will be rendered. */ 00129 inline void 00130 setMaximumDisplayedCorrespondences (const int maximum_displayed_correspondences) 00131 { 00132 // This method is usualy called form other thread than visualizer thread 00133 // therefore same visualizer_updating_mutex_ will be used 00134 00135 // Lock maximum_displayed_correspondences_ 00136 visualizer_updating_mutex_.lock (); 00137 00138 // Update maximum_displayed_correspondences_ 00139 maximum_displayed_correspondences_ = maximum_displayed_correspondences; 00140 00141 // Unlock maximum_displayed_correspondences_ 00142 visualizer_updating_mutex_.unlock(); 00143 } 00144 00145 /** \brief Return maximum number of corresponcence lines which are rendered. */ 00146 inline size_t 00147 getMaximumDisplayedCorrespondences() 00148 { 00149 return maximum_displayed_correspondences_; 00150 } 00151 00152 private: 00153 /** \brief Initialize and run the visualization loop. This function will be runned in the internal thread viewer_thread_ */ 00154 void 00155 runDisplay (); 00156 00157 /** \brief Return the string obtained by concatenating a root_name and an id */ 00158 inline std::string 00159 getIndexedName (std::string &root_name, size_t &id) 00160 { 00161 std::stringstream id_stream_; 00162 id_stream_ << id; 00163 std::string indexed_name_ = root_name + id_stream_.str (); 00164 return indexed_name_; 00165 } 00166 00167 /** \brief The registration viewer. */ 00168 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_; 00169 00170 /** \brief The thread running the runDisplay() function. */ 00171 boost::thread viewer_thread_; 00172 00173 /** \brief The name of the registration method whose intermediate results are rendered. */ 00174 std::string registration_method_name_; 00175 00176 /** \brief Callback function linked to pcl::Registration::update_visualizer_ */ 00177 boost::function<void 00178 (const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &indices_src, const pcl::PointCloud< 00179 PointTarget> &cloud_tgt, const std::vector<int> &indices_tgt)> update_visualizer_; 00180 00181 /** \brief Updates source and target point clouds only for the first update call. */ 00182 bool first_update_flag_; 00183 00184 /** \brief The local buffer for source point cloud. */ 00185 pcl::PointCloud<PointSource> cloud_source_; 00186 00187 /** \brief The local buffer for target point cloud. */ 00188 pcl::PointCloud<PointTarget> cloud_target_; 00189 00190 /** \brief The mutex used for the sincronization of updating and rendering of the local buffers. */ 00191 boost::mutex visualizer_updating_mutex_; 00192 00193 /** \brief The local buffer for intermediate point cloud obtained during registration process. */ 00194 pcl::PointCloud<PointSource> cloud_intermediate_; 00195 00196 /** \brief The indices of intermediate points used for computation of rigid transformation. */ 00197 std::vector<int> cloud_intermediate_indices_; 00198 00199 /** \brief The indices of target points used for computation of rigid transformation. */ 00200 std::vector<int> cloud_target_indices_; 00201 00202 /** \brief The maximum number of displayed correspondences. */ 00203 size_t maximum_displayed_correspondences_; 00204 00205 }; 00206 } 00207 00208 #include <pcl/visualization/impl/registration_visualizer.hpp> 00209 00210 #endif //#ifndef PCL_REGISTRATION_VISUALIZER_H_