rgbd_frame.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_RGBD_FRAME_H
#define CCNY_RGBD_RGBD_FRAME_H

#include <vector>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>
#include <boost/filesystem.hpp>
#include <pcl/io/pcd_io.h>

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

namespace ccny_rgbd {

class RGBDFrame
{
  public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    RGBDFrame();

    RGBDFrame(const ImageMsg::ConstPtr& rgb_msg,
              const ImageMsg::ConstPtr& depth_msg,
              const CameraInfoMsg::ConstPtr& info_msg);
        
    std_msgs::Header header; 
    cv::Mat rgb_img;         
    cv::Mat depth_img;       

    image_geometry::PinholeCameraModel model;
    
    KeypointVector keypoints;         
    cv::Mat        descriptors;       

    BoolVector     kp_valid;          
    Vector3fVector kp_means;          
    Matrix3fVector kp_covariances;    

    int n_valid_keypoints;            

    void computeDistributions(
      double max_z = 5.5,
      double max_stdev_z = 0.03);    

    void constructFeaturePointCloud(PointCloudFeature& cloud);

    void constructDensePointCloud(PointCloudT& cloud,
                                  double max_z = 5.5,
                                  double max_stdev_z = 0.03) const;
    
    static bool save(const RGBDFrame& frame, const std::string& path);

    static bool load(RGBDFrame& frame, const std::string& path);
    
  protected:

    static const double Z_STDEV_CONSTANT = 0.001425;
    
    double getVarZ(double z) const;
    
    double getStdDevZ(double z) const;

    void getGaussianDistribution(int u, int v, double& z_mean, double& z_var) const;
    
    void getGaussianMixtureDistribution(int u, int v, double& z_mean, double& z_var) const;
};

} // namespace ccny_rgbd

#endif // CCNY_RGBD_RGBD_FRAME_H