util.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_RGBD_UTIL_H
#define CCNY_RGBD_RGBD_UTIL_H

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <tf/transform_datatypes.h>
#include <pcl/filters/voxel_grid.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

#include <rgbdtools/rgbdtools.h>

#include "ccny_rgbd/types.h"

namespace ccny_rgbd {

void getTfDifference(const tf::Transform& motion, double& dist, double& angle);

void getTfDifference(
  const tf::Transform& a, 
  const tf::Transform b, 
  double& dist, double& angle);

bool tfGreaterThan(const tf::Transform& a, double dist, double angle);

tf::Transform tfFromEigen(Eigen::Matrix4f trans);

Eigen::Matrix4f eigenFromTf(const tf::Transform& tf);
int getmaxheight(PathMsg path_msg);
int getminheight(PathMsg path_msg);

tf::Transform tfFromEigenAffine(const AffineTransform& trans);
AffineTransform eigenAffineFromTf(const tf::Transform& tf);

void tfToEigenRt(
  const tf::Transform& tf, 
  Matrix3f& R, 
  Vector3f& t);

void tfToOpenCVRt(
  const tf::Transform& transform,
  cv::Mat& R,
  cv::Mat& t);

void openCVRtToTf(
  const cv::Mat& R,
  const cv::Mat& t,
  tf::Transform& transform);

void tfToXYZRPY(
  const tf::Transform& t,
  double& x,    double& y,     double& z,
  double& roll, double& pitch, double& yaw);

void convertCameraInfoToMats(
  const CameraInfoMsg::ConstPtr camera_info_msg,
  cv::Mat& intr,
  cv::Mat& dist);

void convertMatToCameraInfo(
  const cv::Mat& intr,
  CameraInfoMsg& camera_info_msg);

double getMsDuration(const ros::WallTime& start);

void createRGBDFrameFromROSMessages(
  const ImageMsg::ConstPtr& rgb_msg,
  const ImageMsg::ConstPtr& depth_msg,
  const CameraInfoMsg::ConstPtr& info_msg,
  rgbdtools::RGBDFrame& frame);

void pathEigenAffineToROS(
  const AffineTransformVector& path,
  PathMsg& path_msg);

void pathROSToEigenAffine(
  const PathMsg& path_msg,
  AffineTransformVector& path);

} // namespace ccny_rgbd

#endif // CCNY_RGBD_RGBD_UTIL_H