image_grabber.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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.
 *
 *
 */

#pragma once
#ifndef __PCL_IO_IMAGE_GRABBER__
#define __PCL_IO_IMAGE_GRABBER__

#include "pcl/pcl_config.h"
#include <pcl/io/grabber.h>
#include <pcl/io/file_grabber.h>
#include <pcl/common/time_trigger.h>
#include <string>
#include <vector>
#include <pcl/conversions.h>

namespace pcl
{
  class PCL_EXPORTS ImageGrabberBase : public Grabber
  {
    public:
    ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode);

    ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat);
    ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);

    ImageGrabberBase (const ImageGrabberBase &src) : Grabber (), impl_ ()
    {
      *this = src;
    }

    ImageGrabberBase&
    operator = (const ImageGrabberBase &src)
    {
      impl_ = src.impl_;
      return (*this);
    }

    virtual ~ImageGrabberBase () throw ();

    virtual void 
    start ();
      
    virtual void 
    stop ();
      
    virtual void 
    trigger ();

    virtual bool 
    isRunning () const;
      
    virtual std::string 
    getName () const;
      
    virtual void 
    rewind ();

    virtual float 
    getFramesPerSecond () const;

    bool 
    isRepeatOn () const;

    bool
    atLastFrame () const;

    std::string
    getCurrentDepthFileName () const;

    std::string
    getPrevDepthFileName () const;

    std::string
    getDepthFileNameAtIndex (size_t idx) const;

    bool
    getTimestampAtIndex (size_t idx, uint64_t &timestamp) const;

    void
    setRGBImageFiles (const std::vector<std::string>& rgb_image_files);

    virtual void
    setCameraIntrinsics (const double focal_length_x, 
                         const double focal_length_y, 
                         const double principal_point_x, 
                         const double principal_point_y);
    
    virtual void
    getCameraIntrinsics (double &focal_length_x, 
                         double &focal_length_y, 
                         double &principal_point_x, 
                         double &principal_point_y) const;

    void
    setDepthImageUnits (float units);
    
    void
    setNumberOfThreads (unsigned int nr_threads = 0);

    protected:
    size_t
    numFrames () const;
    
    bool
    getCloudAt (size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;


    private:
    virtual void 
    publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;


    // to separate and hide the implementation from interface: PIMPL
    struct ImageGrabberImpl;
    ImageGrabberImpl* impl_;
  };

  template <typename T> class PointCloud;
  template <typename PointT> class ImageGrabber : public ImageGrabberBase, public FileGrabber<PointT>
  {
    public:
    ImageGrabber (const std::string& dir, 
                  float frames_per_second = 0, 
                  bool repeat = false, 
                  bool pclzf_mode = false);

    ImageGrabber (const std::string& depth_dir, 
                  const std::string& rgb_dir, 
                  float frames_per_second = 0, 
                  bool repeat = false);

    ImageGrabber (const std::vector<std::string>& depth_image_files, 
                  float frames_per_second = 0, 
                  bool repeat = false);
      
    virtual ~ImageGrabber () throw () {}
    
    // Inherited from FileGrabber
    const boost::shared_ptr< const pcl::PointCloud<PointT> >
    operator[] (size_t idx) const;

    // Inherited from FileGrabber
    size_t
    size () const;

    protected:
    virtual void 
    publish (const pcl::PCLPointCloud2& blob,
             const Eigen::Vector4f& origin, 
             const Eigen::Quaternionf& orientation) const;
    boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
  };

  template<typename PointT>
  ImageGrabber<PointT>::ImageGrabber (const std::string& dir, 
                                      float frames_per_second, 
                                      bool repeat, 
                                      bool pclzf_mode)
    : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
  {
    signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
  }
  
  template<typename PointT>
  ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir, 
                                      const std::string& rgb_dir, 
                                      float frames_per_second, 
                                      bool repeat)
    : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
  {
    signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
  }

  template<typename PointT>
  ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files, 
                                      float frames_per_second, 
                                      bool repeat)
    : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
  {
    signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
  }

  template<typename PointT> const boost::shared_ptr< const pcl::PointCloud<PointT> >
  ImageGrabber<PointT>::operator[] (size_t idx) const
  {
    pcl::PCLPointCloud2 blob;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    getCloudAt (idx, blob, origin, orientation);
    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
    pcl::fromPCLPointCloud2 (blob, *cloud);
    cloud->sensor_origin_ = origin;
    cloud->sensor_orientation_ = orientation;
    return (cloud);
  }

  template <typename PointT> size_t
  ImageGrabber<PointT>::size () const
  {
    return (numFrames ());
  }

  template<typename PointT> void
  ImageGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
  {
    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
    pcl::fromPCLPointCloud2 (blob, *cloud);
    cloud->sensor_origin_ = origin;
    cloud->sensor_orientation_ = orientation;

    signal_->operator () (cloud);
  }
}
#endif