feature_viewer.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_RGBD_FEATURE_VIEWER_H
#define CCNY_RGBD_RGBD_FEATURE_VIEWER_H

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <dynamic_reconfigure/server.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl_ros/point_cloud.h>
#include <rgbdtools/rgbdtools.h>

#include "ccny_rgbd/types.h"
#include "ccny_rgbd/util.h"
#include "ccny_rgbd/FeatureDetectorConfig.h"
#include "ccny_rgbd/GftDetectorConfig.h"
#include "ccny_rgbd/StarDetectorConfig.h"
#include "ccny_rgbd/SurfDetectorConfig.h"
#include "ccny_rgbd/OrbDetectorConfig.h"

namespace ccny_rgbd {

class FeatureViewer
{ 
  public:

    FeatureViewer(const ros::NodeHandle& nh, 
                  const ros::NodeHandle& nh_private);
    
    virtual ~FeatureViewer();

  private:

    // **** ROS-related

    ros::NodeHandle nh_;                
    ros::NodeHandle nh_private_;        

    ros::Publisher cloud_publisher_;        
    ros::Publisher covariances_publisher_;  
    
    FeatureDetectorConfigServer config_server_; 
    
    GftDetectorConfigServerPtr gft_config_server_;    
    StarDetectorConfigServerPtr star_config_server_;  
    SurfDetectorConfigServerPtr surf_config_server_;    
    OrbDetectorConfigServerPtr orb_config_server_;  
    
    boost::shared_ptr<ImageTransport> rgb_it_;
    
    boost::shared_ptr<ImageTransport> depth_it_;
    
    boost::shared_ptr<RGBDSynchronizer3> sync_;
          
    ImageSubFilter      sub_rgb_;
    
    ImageSubFilter      sub_depth_;  
   
    CameraInfoSubFilter sub_info_;

    // **** parameters 

    std::string detector_type_;
  
    int queue_size_;  
  
    bool show_keypoints_; 
    
    bool publish_cloud_; 
    
    bool publish_covariances_;
 
    // **** variables

    boost::mutex mutex_; 
    int  frame_count_; 

    rgbdtools::FeatureDetectorPtr feature_detector_; 

    // **** private functions
    
    void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg,
                      const ImageMsg::ConstPtr& depth_msg,
                      const CameraInfoMsg::ConstPtr& info_msg);

    void initParams();
    
    void resetDetector();
    
    void publishFeatureCloud(rgbdtools::RGBDFrame& frame);
    
    void publishFeatureCovariances(rgbdtools::RGBDFrame& frame);
        
    void showKeypointImage(rgbdtools::RGBDFrame& frame);
    
    void reconfigCallback(FeatureDetectorConfig& config, uint32_t level);
    
    void gftReconfigCallback(GftDetectorConfig& config, uint32_t level);
    
    void starReconfigCallback(StarDetectorConfig& config, uint32_t level);
    
    void surfReconfigCallback(SurfDetectorConfig& config, uint32_t level);
    
    void orbReconfigCallback(OrbDetectorConfig& config, uint32_t level);
};

} // namespace ccny_rgbd

#endif // CCNY_RGBD_RGBD_FEATURE_VIEWER_H