#include "ccny_rgbd/util.h"
Go to the source code of this file.
Namespaces | |
namespace | ccny_rgbd |
Functions | |
void | ccny_rgbd::convertCameraInfoToMats (const CameraInfoMsg::ConstPtr camera_info_msg, cv::Mat &intr, cv::Mat &dist) |
Create OpenCV matrices from a CameraInfoMsg. | |
void | ccny_rgbd::convertMatToCameraInfo (const cv::Mat &intr, CameraInfoMsg &camera_info_msg) |
Create CameraInfoMsg from OpenCV matrices (assumes no distortion) | |
void | ccny_rgbd::createRGBDFrameFromROSMessages (const ImageMsg::ConstPtr &rgb_msg, const ImageMsg::ConstPtr &depth_msg, const CameraInfoMsg::ConstPtr &info_msg, rgbdtools::RGBDFrame &frame) |
AffineTransform | ccny_rgbd::eigenAffineFromTf (const tf::Transform &tf) |
Eigen::Matrix4f | ccny_rgbd::eigenFromTf (const tf::Transform &tf) |
Converts an tf::Transform transform to an Eigen transform. | |
int | ccny_rgbd::getmaxheight (PathMsg path_msg) |
int | ccny_rgbd::getminheight (PathMsg path_msg) |
double | ccny_rgbd::getMsDuration (const ros::WallTime &start) |
Returns the duration, in ms, from a given time. | |
void | ccny_rgbd::getTfDifference (const tf::Transform &motion, double &dist, double &angle) |
Given a transform, calculates the linear and angular distance between it and identity. | |
void | ccny_rgbd::getTfDifference (const tf::Transform &a, const tf::Transform b, double &dist, double &angle) |
Given two transformss, calculates the linear and angular distance between them, or the linear and angular components of a.inv() * b. | |
void | ccny_rgbd::openCVRtToTf (const cv::Mat &R, const cv::Mat &t, tf::Transform &transform) |
Creates a tf transform from a 3x3 OpenCV rotation matrix and a 3x1 OpenCV translation vector. | |
void | ccny_rgbd::pathEigenAffineToROS (const AffineTransformVector &path, PathMsg &path_msg) |
Copies over the poses from a Eigen vector to a ROS message. Assumes the message is already correctly resized, and preserves the headers of each pose in the message. | |
void | ccny_rgbd::pathROSToEigenAffine (const PathMsg &path_msg, AffineTransformVector &path) |
copies over the poses from a ROS message Eigen vector. The eigen vector will be cleared and resized appropriately. | |
void | ccny_rgbd::removeInvalidDistributions (const Vector3fVector &means, const Matrix3fVector &covariances, const BoolVector &valid, Vector3fVector &means_f, Matrix3fVector &covariances_f) |
void | ccny_rgbd::removeInvalidMeans (const Vector3fVector &means, const BoolVector &valid, Vector3fVector &means_f) |
tf::Transform | ccny_rgbd::tfFromEigen (Eigen::Matrix4f trans) |
Converts an Eigen transform to a tf::Transform. | |
tf::Transform | ccny_rgbd::tfFromEigenAffine (const AffineTransform &trans) |
bool | ccny_rgbd::tfGreaterThan (const tf::Transform &a, double dist, double angle) |
Given a transfom (possibly computed as a difference between two transforms) checks if either its angular or linar component exceeds a threshold. | |
void | ccny_rgbd::tfToEigenRt (const tf::Transform &tf, Matrix3f &R, Vector3f &t) |
Decomposes a tf into an Eigen 3x3 rotation matrix and Eigen 3x1 rotation vector. | |
void | ccny_rgbd::tfToOpenCVRt (const tf::Transform &transform, cv::Mat &R, cv::Mat &t) |
Decomposes a tf::Transform into a 3x3 OpenCV rotation matrix and a 3x1 OpenCV translation vector. | |
void | ccny_rgbd::tfToXYZRPY (const tf::Transform &t, double &x, double &y, double &z, double &roll, double &pitch, double &yaw) |
Decomposes a tf::Transform into x, y, z, roll, pitch, yaw. |