registration_visualizer.h

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */
 
#pragma once
 
// PCL
#include <pcl/registration/registration.h>
#include <pcl/visualization/pcl_visualizer.h>
 
#include <mutex>
#include <thread>
 
namespace pcl
{
  /** \brief @b RegistrationVisualizer represents the base class for rendering
   * the intermediate positions occupied by the source point cloud during it's registration
   * to the target point cloud. A registration algorithm is considered as input and
   * it's convergence is rendered.
   * \author Gheorghe Lisca
   * \ingroup visualization
   */
  template<typename PointSource, typename PointTarget, typename Scalar = float>
  class RegistrationVisualizer
  {
 
    public:
      /** \brief Empty constructor. */
      RegistrationVisualizer () : 
        update_visualizer_ (),
        cloud_source_ (),
        cloud_target_ (),
        cloud_intermediate_ ()
      {}
 
      ~RegistrationVisualizer ()
      {
          stopDisplay();
      }
 
      /** \brief Set the registration algorithm whose intermediate steps will be rendered.
       * The method creates the local callback function pcl::RegistrationVisualizer::update_visualizer_ and
       * binds it to the local buffers update function pcl::RegistrationVisualizer::updateIntermediateCloud().
       * The local callback function pcl::RegistrationVisualizer::update_visualizer_ is then linked to
       * the pcl::Registration::update_visualizer_ callback function.
       * \param registration represents the registration method whose intermediate steps will be rendered.
       */
      bool
      setRegistration (pcl::Registration<PointSource, PointTarget, Scalar> &registration)
      {
        // Update the name of the registration method to be displayed
        registration_method_name_ = registration.getClassName();
 
        // Create the local callback function and bind it to the local function responsible for updating
        // the local buffers
        update_visualizer_ = [this] (const pcl::PointCloud<PointSource>& cloud_src, const pcl::Indices& indices_src,
                                     const pcl::PointCloud<PointTarget>& cloud_tgt, const pcl::Indices& indices_tgt)
        {
          updateIntermediateCloud (cloud_src, indices_src, cloud_tgt, indices_tgt);
        };
 
        // Flag that no visualizer update was done. It indicates to visualizer update function to copy
        // the registration input source and the target point clouds in the next call.
        visualizer_updating_mutex_.lock ();
 
        first_update_flag_ = false;
 
        visualizer_updating_mutex_.unlock ();
 
        // Register the local callback function to the registration algorithm callback function
        registration.registerVisualizationCallback (this->update_visualizer_);
 
        return true;
      }
 
      /** \brief Start the viewer thread
       */
      void
      startDisplay ();
 
      /** \brief Stop the viewer thread
       */
      void
      stopDisplay ();
 
      /** \brief Updates visualizer local buffers cloud_intermediate, cloud_intermediate_indices, cloud_target_indices with
       * the newest registration intermediate results.
       * \param cloud_src represents the initial source point cloud
       * \param indices_src represents the indices of the intermediate source points used for the estimation of rigid transformation
       * \param cloud_tgt represents the target point cloud
       * \param indices_tgt represents the indices of the target points used for the estimation of rigid transformation
       */
      void
      updateIntermediateCloud (const pcl::PointCloud<PointSource> &cloud_src, const pcl::Indices &indices_src,
                               const pcl::PointCloud<PointTarget> &cloud_tgt, const pcl::Indices &indices_tgt);
 
      /** \brief Set maximum number of correspondence lines which will be rendered. */
      inline void
      setMaximumDisplayedCorrespondences (const int maximum_displayed_correspondences)
      {
        // This method is usually called form other thread than visualizer thread
        // therefore same visualizer_updating_mutex_ will be used
 
        // Lock maximum_displayed_correspondences_
        visualizer_updating_mutex_.lock ();
 
        // Update maximum_displayed_correspondences_
        maximum_displayed_correspondences_ = maximum_displayed_correspondences;
 
        // Unlock maximum_displayed_correspondences_
        visualizer_updating_mutex_.unlock();
      }
 
      /** \brief Return maximum number of correspondence lines which are rendered. */
      inline std::size_t
      getMaximumDisplayedCorrespondences()
      {
        return maximum_displayed_correspondences_;
      }
 
    private:
      /** \brief Initialize and run the visualization loop. This function will run in the internal thread viewer_thread_ */
      void
      runDisplay ();
 
      /** \brief Return the string obtained by concatenating a root_name and an id */
      inline std::string
      getIndexedName (std::string &root_name, std::size_t &id)
      {
        return root_name + std::to_string(id);
      }
 
      /** \brief The registration viewer. */
      pcl::visualization::PCLVisualizer::Ptr viewer_;
 
      /** \brief The thread running the runDisplay() function. */
      std::thread viewer_thread_;
 
      /** \brief The name of the registration method whose intermediate results are rendered. */
      std::string registration_method_name_;
 
      /** \brief Callback function linked to pcl::Registration::update_visualizer_ */
      std::function<void
      (const pcl::PointCloud<PointSource> &cloud_src, const pcl::Indices &indices_src, const pcl::PointCloud<
          PointTarget> &cloud_tgt, const pcl::Indices &indices_tgt)> update_visualizer_;
 
      /** \brief Updates source and target point clouds only for the first update call. */
      bool first_update_flag_{false};
 
      /** \brief The local buffer for source point cloud. */
      pcl::PointCloud<PointSource> cloud_source_;
 
      /** \brief The local buffer for target point cloud. */
      pcl::PointCloud<PointTarget> cloud_target_;
 
      /** \brief The mutex used for the synchronization of updating and rendering of the local buffers. */
      std::mutex visualizer_updating_mutex_;
 
      /** \brief The local buffer for intermediate point cloud obtained during registration process. */
      pcl::PointCloud<PointSource> cloud_intermediate_;
 
      /** \brief The indices of intermediate points used for computation of rigid transformation. */
      pcl::Indices cloud_intermediate_indices_;
 
      /** \brief The indices of target points used for computation of rigid transformation. */
      pcl::Indices cloud_target_indices_;
 
      /** \brief The maximum number of displayed correspondences. */
      std::size_t maximum_displayed_correspondences_{0};
 
    };
}
 
#include <pcl/visualization/impl/registration_visualizer.hpp>