particle_filter.h

#pragma once
 
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
 
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/PointIndices.h>
#include <pcl/pcl_macros.h>
 
#include <pcl/gpu/kinfu/pixel_rgb.h>
#include <pcl/tracking/particle_filter.h>
 
#include "internal.h"
 
namespace pcl
{
  namespace gpu
  { 
    class ParticleFilterGPUTracker
    {
    public:
      /** \brief Point type supported */
      using PointType = pcl::PointXYZ;
      //using NormalType = pcl::Normal;
      using PixelRGB = pcl::RGB;
      
      using StateXYZ = pcl::PointXYZ;
      using StateRPY = pcl::PointXYZ;
      
      using StateType = pcl::tracking::ParticleXYZRPY;
 
      /** \brief Empty constructor. */
      ParticleFilterGPUTracker ()
      //: ParticleFilterTracker<PointInT, StateT> ()
      {
        tracker_name_ = "ParticleFilterGPUTracker";
      }
      
      /** \brief set the number of the particles.
      * \param particle_num the number of the particles.
      */
      inline void
        setParticleNum (const int particle_num) { particle_num_ = particle_num; }
      
      /** \brief get the number of the particles. */
      inline int
        getParticleNum () const { return particle_num_; }
 
       /** \brief set a pointer to a reference dataset to be tracked.
       * \param ref a pointer to a PointCloud message
       */
      inline void
        setReferenceCloud (const DeviceArray2D<PointType> &ref) { ref_ = ref; }
      
      /** \brief get a pointer to a reference dataset to be tracked. */
      inline DeviceArray2D<PointType> const
        getReferenceCloud () { return ref_; }
 
      int
        cols ();
 
      int
        rows ();
 
      virtual bool 
        operator() (const DeviceArray2D<PointType>& input, const DeviceArray2D<PixelRGB>& input_colors)
      {
 
      }
 
      virtual void
        setMotion (StateType motion)
      { motion_ = motion; }
            
      virtual StateType
        getResult();            
 
    protected:
      std::string tracker_name_;
 
      virtual bool 
      initCompute()
      {
 
        //pcl::device::initParticles(particle_num_, particle_xyz_, particle_rpy_, particle_weight_ );
      }
      
      virtual void 
      computeTracking()
      {
 
      }           
      
      virtual void
        allocateBuffers()
      {
        particles_.create( particle_num_ );       
 
        random_number_generator_.create( particle_num_ );
        
      }
      
      // reference point cloud
      DeviceArray2D<PointType> ref_;
 
      DeviceArray2D<PixelRGB> ref_colors_;
 
      //DeviceArray2D<NormalType> ref_normals_;
 
      // input point cloud
      DeviceArray2D<PointType> input_;
 
      DeviceArray2D<PixelRGB> input_colors_;
 
      //DeviceArray2D<NormalType> input_normals_;
            
      //StateCloud particles_;
      DeviceArray<StateType> particles_;
 
      // random number generate state
      DeviceArray<curandState> rng_states;
            
      int particle_num_;
 
      std::vector<float> step_noise_covariance_;
 
      std::vector<float> initial_noise_covariance_;
        
      std::vector<float> initial_noise_mean_;
 
      StateType motion_;
 
      float motion_ratio_;
 
      bool use_colors_;
 
      StateType representative_state_;      
 
      /** \brief Height of input depth image. */
      int rows_;
      /** \brief Width of input depth image. */
      int cols_;
 
    };
  }
}