rgbd_util.h

Go to the documentation of this file.

#ifndef RGBDTOOLS_RGBD_UTIL_H
#define RGBDTOOLS_RGBD_UTIL_H

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <opencv2/opencv.hpp>

#include "rgbdtools/types.h"

namespace rgbdtools {

enum DepthFitMode { 
  DEPTH_FIT_LINEAR,
  DEPTH_FIT_LINEAR_ZERO,
  DEPTH_FIT_QUADRATIC,
  DEPTH_FIT_QUADRATIC_ZERO
};
Eigen::Matrix4d m4dfromPose(Pose pose);
Pose AffineFromTRPY(double xoffset,double yoffset,double zoffset, double roll,double pitch,double yaw );
void removeInvalidMeans(
  const Vector3fVector& means,
  const BoolVector& valid,
  Vector3fVector& means_f);

void removeInvalidDistributions(
  const Vector3fVector& means,
  const Matrix3fVector& covariances,
  const BoolVector& valid,
  Vector3fVector& means_f,
  Matrix3fVector& covariances_f);

void transformMeans(
  Vector3fVector& means,
  const AffineTransform& transform);

void transformDistributions(
  Vector3fVector& means,
  Matrix3fVector& covariances,
  const AffineTransform& transform);

void pointCloudFromMeans(
  const Vector3fVector& means,
  PointCloudFeature& cloud);

void buildRegisteredDepthImage(
  const cv::Mat& intr_rect_ir,
  const cv::Mat& intr_rect_rgb,
  const cv::Mat& ir2rgb,
  const cv::Mat& depth_img_rect,
  cv::Mat& depth_img_rect_reg);

void buildPointCloud(
  const cv::Mat& depth_img_rect,
  const cv::Mat& intr_rect_ir,
  PointCloudT& cloud);

void buildPointCloud(
  const cv::Mat& depth_img_rect_reg,
  const cv::Mat& rgb_img_rect,
  const cv::Mat& intr_rect_rgb,
  PointCloudT& cloud);

void depthImageFloatTo16bit(
  const cv::Mat& depth_image_in,
  cv::Mat& depth_image_out);

void eigenAffineToOpenCVRt(
  const AffineTransform& transform,
  cv::Mat& R,
  cv::Mat& t);
  
void openCVRtToEigenAffine(
  const cv::Mat& R,
  const cv::Mat& t,
  AffineTransform& eigen_affine);

void eigenAffineToXYZRPY(
  const AffineTransform& transform, 
  float& x, float& y, float& z, 
  float& roll, float& pitch, float& yaw);
 
void XYZRPYToEigenAffine(
  float x, float y, float z, 
  float roll, float pitch, float yaw, 
  AffineTransform& t);

void getTfDifference(
  const AffineTransform& transform, 
  double& dist, double& angle);

void unwarpDepthImage(
  cv::Mat& depth_img_in,
  const cv::Mat& coeff0,
  const cv::Mat& coeff1,
  const cv::Mat& coeff2,
  int fit_mode=DEPTH_FIT_QUADRATIC);

void setRPY(
  float roll, float pitch, float yaw, 
  Matrix3f& m);

} // namespace rgbdtools

#endif // RGBDTOOLS_RGBD_UTIL_H