Functions
misc.h File Reference
#include <QMatrix4x4>
Include dependency graph for misc.h:
This graph shows which files directly or indirectly include this file:

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_typecreateXYZRGBPointCloud (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.

Function Documentation

bool asyncFrameDrop ( ros::Time  depth,
ros::Time  rgb 
)

Return true if frames should be dropped because they are asynchronous.

Definition at line 400 of file misc.cpp.

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 
)

Creates a pointcloud from rgb8 or mono8 coded images + float depth.

Definition at line 438 of file misc.cpp.

void depthToCV8UC1 ( const cv::Mat &  float_img,
cv::Mat &  mono8_img 
)

Convert the CV_32FC1 image to CV_8UC1 with a fixed scale factor.

Definition at line 364 of file misc.cpp.

g2o::SE3Quat eigen2G2O ( const Eigen::Matrix4d &  eigen_mat)

Conversion Function.

Definition at line 261 of file misc.cpp.

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 
)

Definition at line 514 of file misc.cpp.

double errorFunction2 ( const Eigen::Vector4f &  x1,
const Eigen::Vector4f &  x2,
const Eigen::Matrix4f &  tf_1_to_2 
)

Definition at line 574 of file misc.cpp.

QMatrix4x4 g2o2QMatrix ( const g2o::SE3Quat  se3)

Conversion Function.

Definition at line 56 of file misc.cpp.

tf::Transform g2o2TF ( const g2o::SE3Quat  se3)

Conversion Function.

Definition at line 67 of file misc.cpp.

float getMinDepthInNeighborhood ( const cv::Mat &  depth,
cv::Point2f  center,
float  diameter 
)

Definition at line 629 of file misc.cpp.

bool isBigTrafo ( const Eigen::Matrix4f &  t)

Definition at line 185 of file misc.cpp.

bool isBigTrafo ( const g2o::SE3Quat &  t)

Definition at line 202 of file misc.cpp.

void logTransform ( QTextStream &  out,
const tf::Transform t,
double  timestamp,
const char *  label = NULL 
)

Write Transformation to textstream.

Definition at line 50 of file misc.cpp.

void mat2components ( const Eigen::Matrix4f &  t,
double &  roll,
double &  pitch,
double &  yaw,
double &  dist 
)

get euler angles and translation from 4x4 homogenous

Definition at line 173 of file misc.cpp.

void mat2dist ( const Eigen::Matrix4f &  t,
double &  dist 
)

get translation-distance from 4x4 homogenous

Definition at line 164 of file misc.cpp.

void mat2RPY ( const Eigen::Matrix4f &  t,
double &  roll,
double &  pitch,
double &  yaw 
)

get euler angles from 4x4 homogenous

Definition at line 168 of file misc.cpp.

std::string openCVCode2String ( unsigned int  code)

Return the macro string for the cv::Mat type integer.

Definition at line 378 of file misc.cpp.

geometry_msgs::Point pointInWorldFrame ( const Eigen::Vector4f &  point3d,
g2o::SE3Quat  transf 
)

Definition at line 155 of file misc.cpp.

void printMatrixInfo ( cv::Mat &  image,
std::string  name = std::string("") 
)

Print Type and size of image.

Definition at line 396 of file misc.cpp.

void printQMatrix4x4 ( const char *  name,
const QMatrix4x4 &  m 
)

Definition at line 37 of file misc.cpp.

void printTransform ( const char *  name,
const tf::Transform  t 
)

Definition at line 45 of file misc.cpp.

g2o::SE3Quat tf2G2O ( const tf::Transform  t)

Conversion Function.

Definition at line 253 of file misc.cpp.

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.

Parameters:
cloud_inthe input point cloud
cloud_to_append_tothe transformed cloud will be appended to this one
transforma tf::Transform stating the transformation of cloud_to_append_to relative to cloud_in
Note:
The density of the point cloud is lost, since density implies that the origin is the point of view
Can not(?) be used with cloud_in equal to cloud_to_append_to

Definition at line 97 of file misc.cpp.

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


rgbdslam
Author(s): Felix Endres, Juergen Hess, Nikolas Engelhard
autogenerated on Wed Dec 26 2012 15:53:09