rgbd_image_proc.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_RGBD_IMAGE_PROC_H
#define CCNY_RGBD_RGBD_IMAGE_PROC_H

#include <ros/ros.h>
#include <boost/filesystem.hpp>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <dynamic_reconfigure/server.h>

#include "ccny_rgbd/types.h"
#include "ccny_rgbd/rgbd_util.h"
#include "ccny_rgbd/proc_util.h"
#include "ccny_rgbd/RGBDImageProcConfig.h"

namespace ccny_rgbd {

class RGBDImageProc 
{
  typedef RGBDImageProcConfig ProcConfig;
  typedef dynamic_reconfigure::Server<ProcConfig> ProcConfigServer;
  
  public:

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

    void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg,
                      const ImageMsg::ConstPtr& depth_msg,
                      const CameraInfoMsg::ConstPtr& rgb_info_msg,
                      const CameraInfoMsg::ConstPtr& depth_info_msg);

  private:

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

    boost::shared_ptr<RGBDSynchronizer4> sync_; 
       
    ImageTransport rgb_image_transport_;    
    ImageTransport depth_image_transport_;  
    
    ImageSubFilter sub_rgb_;   
    ImageSubFilter sub_depth_; 

    CameraInfoSubFilter sub_rgb_info_;   
    CameraInfoSubFilter sub_depth_info_; 
    
    ImagePublisher rgb_publisher_;      
    ImagePublisher depth_publisher_;    
    ros::Publisher info_publisher_;     
    ros::Publisher cloud_publisher_;    
    
    ProcConfigServer config_server_;    
    
    // parameters
    
    int queue_size_;          
    
    std::string calib_path_;  
    bool unwarp_;             
    bool publish_cloud_;      
    
    double scale_;             
    
    std::string calib_extr_filename_;
    
    std::string calib_warp_filename_;
   
    // **** state variables
    
    bool initialized_;      
    boost::mutex mutex_;    
    
    // **** calibration
    
    int fit_mode_;        
    
    cv::Size size_in_; 
    
    cv::Mat coeff_0_, coeff_1_, coeff_2_;   
    
    cv::Mat coeff_0_rect_, coeff_1_rect_, coeff_2_rect_;  
    
    cv::Mat ir2rgb_;    
   
    cv::Mat intr_rect_rgb_, intr_rect_depth_;  
    
    CameraInfoMsg rgb_rect_info_msg_;   
    
    CameraInfoMsg depth_rect_info_msg_; 
    
    cv::Mat map_rgb_1_,   map_rgb_2_;
    
    cv::Mat map_depth_1_, map_depth_2_;
    
    void initMaps(
      const CameraInfoMsg::ConstPtr& rgb_info_msg,
      const CameraInfoMsg::ConstPtr& depth_info_msg);
    
    bool loadCalibration();   
    
    bool loadUnwarpCalibration();

    void reconfigCallback(ProcConfig& config, uint32_t level);
};

} //namespace ccny_rgbd

#endif // CCNY_RGBD_RGBD_IMAGE_PROC_H