#include <QMatrix4x4>
Go to the source code of this file.
Functions | |
bool | asyncFrameDrop (ros::Time depth, ros::Time rgb) |
Return true if frames should be dropped because they are asynchronous. | |
cv::DescriptorExtractor * | createDescriptorExtractor (const std::string &descriptorType) |
Create an object to extract features at keypoints. The Exctractor is passed to the Node constructor and must be the same for each node. | |
cv::FeatureDetector * | createDetector (const std::string &detectorType) |
pointcloud_type * | createXYZRGBPointCloud (const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &rgb_msg, const sensor_msgs::CameraInfoConstPtr &cam_info) |
Creates a pointcloud from rgb8 or mono8 coded images + float depth. | |
void | depthToCV8UC1 (const cv::Mat &float_img, cv::Mat &mono8_img) |
Convert the CV_32FC1 image to CV_8UC1 with a fixed scale factor. | |
g2o::SE3Quat | eigen2G2O (const Eigen::Matrix4d &eigen_mat) |
Conversion Function. | |
double | errorFunction (const Eigen::Vector4f &x1, const double x1_depth_cov, const Eigen::Vector4f &x2, const double x2_depth_cov, const Eigen::Matrix4f &tf_1_to_2) |
double | errorFunction2 (const Eigen::Vector4f &x1, const Eigen::Vector4f &x2, const Eigen::Matrix4f &tf_1_to_2) |
QMatrix4x4 | g2o2QMatrix (const g2o::SE3Quat se3) |
Conversion Function. | |
tf::Transform | g2o2TF (const g2o::SE3Quat se3) |
Conversion Function. | |
float | getMinDepthInNeighborhood (const cv::Mat &depth, cv::Point2f center, float diameter) |
bool | isBigTrafo (const Eigen::Matrix4f &t) |
bool | isBigTrafo (const g2o::SE3Quat &t) |
void | logTransform (QTextStream &out, const tf::Transform &t, double timestamp, const char *label=NULL) |
Write Transformation to textstream. | |
void | mat2components (const Eigen::Matrix4f &t, double &roll, double &pitch, double &yaw, double &dist) |
get euler angles and translation from 4x4 homogenous | |
void | mat2dist (const Eigen::Matrix4f &t, double &dist) |
get translation-distance from 4x4 homogenous | |
void | mat2RPY (const Eigen::Matrix4f &t, double &roll, double &pitch, double &yaw) |
get euler angles from 4x4 homogenous | |
std::string | openCVCode2String (unsigned int code) |
Return the macro string for the cv::Mat type integer. | |
geometry_msgs::Point | pointInWorldFrame (const Eigen::Vector4f &point3d, g2o::SE3Quat transf) |
void | printMatrixInfo (cv::Mat &image, std::string name=std::string("")) |
Print Type and size of image. | |
void | printQMatrix4x4 (const char *name, const QMatrix4x4 &m) |
void | printTransform (const char *name, const tf::Transform t) |
g2o::SE3Quat | tf2G2O (const tf::Transform t) |
Conversion Function. | |
void | transformAndAppendPointCloud (const pointcloud_type &cloud_in, pointcloud_type &cloud_to_append_to, const tf::Transform transformation, float Max_Depth) |
Helper function to aggregate pointclouds in a single coordinate frame. |
bool asyncFrameDrop | ( | ros::Time | depth, |
ros::Time | rgb | ||
) |
cv::DescriptorExtractor* createDescriptorExtractor | ( | const std::string & | descriptorType | ) |
Create an object to extract features at keypoints. The Exctractor is passed to the Node constructor and must be the same for each node.
cv::FeatureDetector* createDetector | ( | const std::string & | detectorType | ) |
Creates Feature Detector Objects accordingt to the type. Possible detectorTypes: FAST, STAR, SIFT, SURF, GFTT FAST and SURF are the self-adjusting versions (see http://opencv.willowgarage.com/documentation/cpp/features2d_common_interfaces_of_feature_detectors.html#DynamicAdaptedFeatureDetector)
pointcloud_type* createXYZRGBPointCloud | ( | const sensor_msgs::ImageConstPtr & | depth_msg, |
const sensor_msgs::ImageConstPtr & | rgb_msg, | ||
const sensor_msgs::CameraInfoConstPtr & | cam_info | ||
) |
void depthToCV8UC1 | ( | const cv::Mat & | float_img, |
cv::Mat & | mono8_img | ||
) |
g2o::SE3Quat eigen2G2O | ( | const Eigen::Matrix4d & | eigen_mat | ) |
double errorFunction | ( | const Eigen::Vector4f & | x1, |
const double | x1_depth_cov, | ||
const Eigen::Vector4f & | x2, | ||
const double | x2_depth_cov, | ||
const Eigen::Matrix4f & | tf_1_to_2 | ||
) |
double errorFunction2 | ( | const Eigen::Vector4f & | x1, |
const Eigen::Vector4f & | x2, | ||
const Eigen::Matrix4f & | tf_1_to_2 | ||
) |
QMatrix4x4 g2o2QMatrix | ( | const g2o::SE3Quat | se3 | ) |
tf::Transform g2o2TF | ( | const g2o::SE3Quat | se3 | ) |
float getMinDepthInNeighborhood | ( | const cv::Mat & | depth, |
cv::Point2f | center, | ||
float | diameter | ||
) |
bool isBigTrafo | ( | const Eigen::Matrix4f & | t | ) |
bool isBigTrafo | ( | const g2o::SE3Quat & | t | ) |
void logTransform | ( | QTextStream & | out, |
const tf::Transform & | t, | ||
double | timestamp, | ||
const char * | label = NULL |
||
) |
void mat2components | ( | const Eigen::Matrix4f & | t, |
double & | roll, | ||
double & | pitch, | ||
double & | yaw, | ||
double & | dist | ||
) |
void mat2dist | ( | const Eigen::Matrix4f & | t, |
double & | dist | ||
) |
void mat2RPY | ( | const Eigen::Matrix4f & | t, |
double & | roll, | ||
double & | pitch, | ||
double & | yaw | ||
) |
std::string openCVCode2String | ( | unsigned int | code | ) |
geometry_msgs::Point pointInWorldFrame | ( | const Eigen::Vector4f & | point3d, |
g2o::SE3Quat | transf | ||
) |
void printMatrixInfo | ( | cv::Mat & | image, |
std::string | name = std::string("") |
||
) |
void printQMatrix4x4 | ( | const char * | name, |
const QMatrix4x4 & | m | ||
) |
void printTransform | ( | const char * | name, |
const tf::Transform | t | ||
) |
g2o::SE3Quat tf2G2O | ( | const tf::Transform | t | ) |
void transformAndAppendPointCloud | ( | const pointcloud_type & | cloud_in, |
pointcloud_type & | cloud_to_append_to, | ||
const tf::Transform | transformation, | ||
float | max_Depth | ||
) |
Helper function to aggregate pointclouds in a single coordinate frame.
Helper function to aggregate pointclouds in a single coordinate frame.
cloud_in | the input point cloud |
cloud_to_append_to | the transformed cloud will be appended to this one |
transform | a tf::Transform stating the transformation of cloud_to_append_to relative to cloud_in |