#include <ros/ros.h>#include <tf/transform_datatypes.h>#include <Eigen/Core>#include <QString>#include <QMatrix4x4>#include <ctime>#include <limits>#include <algorithm>#include "parameter_server.h"#include <cv.h>#include "scoped_timer.h"#include "header.h"#include "g2o/types/slam3d/se3quat.h"#include "g2o/types/slam3d/vertex_se3.h"#include <pcl_ros/transforms.h>#include "pcl/common/io.h"#include "pcl/common/distances.h"#include "aorb.h"#include <omp.h>#include "misc2.h"#include "point_types.h"#include <boost/math/distributions/chi_squared.hpp>#include <numeric>#include "feature_adjuster.h"#include <sensor_msgs/Image.h>#include <sensor_msgs/CameraInfo.h>#include <pcl/ros/conversions.h>#include <math.h>
Include dependency graph for misc.cpp:
Go to the source code of this file.
Defines | |
| #define | LOG_SQRT_2_PI = 0.9189385332 |
| #define | SQRT_2 1.41421 |
| #define | SQRT_2_PI 2.5066283 |
Functions | |
| bool | asyncFrameDrop (ros::Time depth, ros::Time rgb) |
| Return true if frames should be dropped because they are asynchronous. | |
| double | cdf (double x, double mu, double sigma) |
| pointcloud_type * | createXYZRGBPointCloud (const cv::Mat &depth_img, const cv::Mat &rgb_img, const sensor_msgs::CameraInfoConstPtr &cam_info) |
| 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 (cv::Mat &depth_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::Matrix4d &transformation) |
| QMatrix4x4 | g2o2QMatrix (const g2o::SE3Quat se3) |
| Conversion Function. | |
| tf::Transform | g2o2TF (const g2o::SE3Quat se3) |
| Conversion Function. | |
| static void | getCameraIntrinsics (float &fx, float &fy, float &cx, float &cy, const sensor_msgs::CameraInfo &cam_info) |
| static void | getCameraIntrinsicsInverseFocalLength (float &fxinv, float &fyinv, float &cx, float &cy, const sensor_msgs::CameraInfo &cam_info) |
| void | getColor (const point_type &p, unsigned char &r, unsigned char &g, unsigned char &b) |
| float | getMinDepthInNeighborhood (const cv::Mat &depth, cv::Point2f center, float diameter) |
| bool | isBigTrafo (const Eigen::Isometry3d &t) |
| bool | isBigTrafo (const Eigen::Matrix4f &t) |
| bool | isBigTrafo (const g2o::SE3Quat &t) |
| bool | isSmallTrafo (const Eigen::Isometry3d &t, double seconds) |
| bool | isSmallTrafo (const g2o::SE3Quat &t, double seconds) |
| Computes whether the motion per time is bigger than the parameters max_translation_meter and max_rotation_degree define. | |
| void | logTransform (QTextStream &out, const tf::Transform &t, double timestamp, const char *label) |
| 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 affine matrix (helper for isBigTrafo) | |
| bool | observation_criterion_met (unsigned int inliers, unsigned int outliers, unsigned int all, double &quality) |
| Quality is output param. | |
| void | observationLikelihood (const Eigen::Matrix4f &proposed_transformation, pointcloud_type::Ptr new_pc, pointcloud_type::Ptr old_pc, const sensor_msgs::CameraInfo &old_cam_info, double &likelihood, double &confidence, unsigned int &inliers, unsigned int &outliers, unsigned int &occluded, unsigned int &all) |
| std::string | openCVCode2String (unsigned int code) |
| Return the macro string for the cv::Mat type integer. | |
| void | overlay_edges (cv::Mat visual, cv::Mat depth, cv::Mat &visual_edges, cv::Mat &depth_edges) |
| Overlay the monochrom edges and depth jumps. | |
| geometry_msgs::Point | pointInWorldFrame (const Eigen::Vector4f &point3d, const g2o::VertexSE3::EstimateType &transf) |
| void | printMatrixInfo (const cv::Mat &image, std::string name) |
| Print Type and size of image. | |
| void | printQMatrix4x4 (const char *name, const QMatrix4x4 &m) |
| void | printTransform (const char *name, const tf::StampedTransform t) |
| Print tf::Transform via ROS_INFO. | |
| void | printTransform (const char *name, const tf::Transform t) |
| Print tf::Transform via ROS_INFO. | |
| double | rejectionSignificance (const Eigen::Matrix4f &proposed_transformation, pointcloud_type::Ptr new_pc, pointcloud_type::Ptr old_pc) |
| int | round (float d) |
| g2o::SE3Quat | tf2G2O (const tf::Transform t) |
| Conversion Function. | |
| void | trafoSize (const Eigen::Isometry3d &t, double &angle, double &dist) |
| void | transformAndAppendPointCloud (const pointcloud_type &cloud_in, pointcloud_type &cloud_to_append_to, const tf::Transform transformation, float max_Depth, int) |
| Apply an affine transform defined by an Eigen Transform. | |
Define Documentation
| #define LOG_SQRT_2_PI = 0.9189385332 |
Function Documentation
| bool asyncFrameDrop | ( | ros::Time | depth, |
| ros::Time | rgb | ||
| ) |
| pointcloud_type* createXYZRGBPointCloud | ( | const cv::Mat & | depth_img, |
| const cv::Mat & | rgb_img, | ||
| const sensor_msgs::CameraInfoConstPtr & | cam_info | ||
| ) |
| pointcloud_type* createXYZRGBPointCloud | ( | const sensor_msgs::ImageConstPtr & | depth_msg, |
| const sensor_msgs::ImageConstPtr & | rgb_msg, | ||
| const sensor_msgs::CameraInfoConstPtr & | cam_info | ||
| ) |
| void depthToCV8UC1 | ( | cv::Mat & | depth_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::Matrix4d & | transformation | ||
| ) |
| QMatrix4x4 g2o2QMatrix | ( | const g2o::SE3Quat | se3 | ) |
| tf::Transform g2o2TF | ( | const g2o::SE3Quat | se3 | ) |
| static void getCameraIntrinsics | ( | float & | fx, |
| float & | fy, | ||
| float & | cx, | ||
| float & | cy, | ||
| const sensor_msgs::CameraInfo & | cam_info | ||
| ) | [static] |
| static void getCameraIntrinsicsInverseFocalLength | ( | float & | fxinv, |
| float & | fyinv, | ||
| float & | cx, | ||
| float & | cy, | ||
| const sensor_msgs::CameraInfo & | cam_info | ||
| ) | [static] |
| void getColor | ( | const point_type & | p, |
| unsigned char & | r, | ||
| unsigned char & | g, | ||
| unsigned char & | b | ||
| ) |
| float getMinDepthInNeighborhood | ( | const cv::Mat & | depth, |
| cv::Point2f | center, | ||
| float | diameter | ||
| ) |
| bool isBigTrafo | ( | const Eigen::Isometry3d & | t | ) |
| bool isBigTrafo | ( | const Eigen::Matrix4f & | t | ) |
| bool isBigTrafo | ( | const g2o::SE3Quat & | t | ) |
| bool isSmallTrafo | ( | const Eigen::Isometry3d & | t, |
| double | seconds | ||
| ) |
| bool isSmallTrafo | ( | const g2o::SE3Quat & | t, |
| double | seconds | ||
| ) |
| void logTransform | ( | QTextStream & | out, |
| const tf::Transform & | t, | ||
| double | timestamp, | ||
| const char * | label | ||
| ) |
| 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 | ||
| ) |
| bool observation_criterion_met | ( | unsigned int | inliers, |
| unsigned int | outliers, | ||
| unsigned int | all, | ||
| double & | quality | ||
| ) |
| void observationLikelihood | ( | const Eigen::Matrix4f & | proposed_transformation, |
| pointcloud_type::Ptr | new_pc, | ||
| pointcloud_type::Ptr | old_pc, | ||
| const sensor_msgs::CameraInfo & | old_cam_info, | ||
| double & | likelihood, | ||
| double & | confidence, | ||
| unsigned int & | inliers, | ||
| unsigned int & | outliers, | ||
| unsigned int & | occluded, | ||
| unsigned int & | all | ||
| ) |
| std::string openCVCode2String | ( | unsigned int | code | ) |
| void overlay_edges | ( | cv::Mat | visual, |
| cv::Mat | depth, | ||
| cv::Mat & | visual_edges, | ||
| cv::Mat & | depth_edges | ||
| ) |
| geometry_msgs::Point pointInWorldFrame | ( | const Eigen::Vector4f & | point3d, |
| const g2o::VertexSE3::EstimateType & | transf | ||
| ) |
| void printMatrixInfo | ( | const cv::Mat & | image, |
| std::string | name | ||
| ) |
| void printQMatrix4x4 | ( | const char * | name, |
| const QMatrix4x4 & | m | ||
| ) |
| void printTransform | ( | const char * | name, |
| const tf::StampedTransform | t | ||
| ) |
Print tf::Transform via ROS_INFO.
| void printTransform | ( | const char * | name, |
| const tf::Transform | t | ||
| ) |
Print tf::Transform via ROS_INFO.
| double rejectionSignificance | ( | const Eigen::Matrix4f & | proposed_transformation, |
| pointcloud_type::Ptr | new_pc, | ||
| pointcloud_type::Ptr | old_pc | ||
| ) |
This function computes the p-value of the null hypothesis that the transformation is the true one. It is too sensitive to outliers
TODO: Compute correctly transformed new sigma in old_z direction
Very unlikely to be inlier as bigger than 3*std_dev
Compute covariance statistics also based on depth variance in the neighborhood
| g2o::SE3Quat tf2G2O | ( | const tf::Transform | t | ) |
| void trafoSize | ( | const Eigen::Isometry3d & | t, |
| double & | angle, | ||
| double & | dist | ||
| ) |
| void transformAndAppendPointCloud | ( | const pointcloud_type & | cloud_in, |
| pointcloud_type & | cloud_to_append_to, | ||
| const tf::Transform | transformation, | ||
| float | max_Depth, | ||
| int | |||
| ) |
Apply an affine transform defined by an Eigen Transform.
- Parameters:
-
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
- Note:
- The density of the point cloud is lost, if parameter preserve_raster_on_save is set, as NaNs will be copied to keep raster structure
- Can not(?) be used with cloud_in equal to cloud_to_append_to