root / rgbdslam / src / misc.h @ 9240aaa3
History | View | Annotate | Download (2.96 KB)
1 | 9240aaa3 | Alex | /* This file is part of RGBDSLAM.
|
---|---|---|---|
2 | *
|
||
3 | * RGBDSLAM is free software: you can redistribute it and/or modify
|
||
4 | * it under the terms of the GNU General Public License as published by
|
||
5 | * the Free Software Foundation, either version 3 of the License, or
|
||
6 | * (at your option) any later version.
|
||
7 | *
|
||
8 | * RGBDSLAM is distributed in the hope that it will be useful,
|
||
9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
11 | * GNU General Public License for more details.
|
||
12 | *
|
||
13 | * You should have received a copy of the GNU General Public License
|
||
14 | * along with RGBDSLAM. If not, see <http://www.gnu.org/licenses/>.
|
||
15 | */
|
||
16 | #include <QMatrix4x4> |
||
17 | |||
18 | void printTransform(const char* name, const tf::Transform t) ; |
||
19 | |||
20 | void printQMatrix4x4(const char* name, const QMatrix4x4& m); |
||
21 | |||
22 | QMatrix4x4 g2o2QMatrix(const g2o::SE3Quat se3) ;
|
||
23 | |||
24 | tf::Transform g2o2TF(const g2o::SE3Quat se3) ;
|
||
25 | |||
26 | void logTransform(QTextStream& out, const tf::Transform& t, double timestamp, const char* label = NULL); |
||
27 | |||
28 | void transformAndAppendPointCloud (const pointcloud_type &cloud_in, pointcloud_type &cloud_to_append_to, |
||
29 | const tf::Transform transformation, float Max_Depth); |
||
30 | |||
31 | |||
32 | geometry_msgs::Point pointInWorldFrame(const Eigen::Vector4f& point3d, g2o::SE3Quat transf);
|
||
33 | // true if translation > 10cm or largest euler-angle>5 deg
|
||
34 | // used to decide if the camera has moved far enough to generate a new nodes
|
||
35 | bool isBigTrafo(const Eigen::Matrix4f& t); |
||
36 | bool isBigTrafo(const g2o::SE3Quat& t); |
||
37 | |||
38 | /// get euler angles from 4x4 homogenous
|
||
39 | void mat2RPY(const Eigen::Matrix4f& t, double& roll, double& pitch, double& yaw); |
||
40 | /// get translation-distance from 4x4 homogenous
|
||
41 | void mat2dist(const Eigen::Matrix4f& t, double &dist); |
||
42 | |||
43 | //bool overlappingViews(LoadedEdge3D edge);
|
||
44 | //bool triangleRayIntersection(Eigen::Vector3d triangle1,Eigen::Vector3d triangle2, Eigen::Vector3d ray_origin, Eigen::Vector3d ray);
|
||
45 | |||
46 | g2o::SE3Quat eigen2G2O(const Eigen::Matrix4d eigen_mat);
|
||
47 | void mat2components(const Eigen::Matrix4f& t, double& roll, double& pitch, double& yaw, double& dist); |
||
48 | |||
49 | /// Creates Feature Detector Objects accordingt to the type.
|
||
50 | /// Possible detectorTypes: FAST, STAR, SIFT, SURF, GFTT
|
||
51 | /// FAST and SURF are the self-adjusting versions (see http://opencv.willowgarage.com/documentation/cpp/features2d_common_interfaces_of_feature_detectors.html#DynamicAdaptedFeatureDetector)
|
||
52 | cv::FeatureDetector* createDetector( const std::string& detectorType );
|
||
53 | /// Create an object to extract features at keypoints. The Exctractor is passed to the Node constructor and must be the same for each node.
|
||
54 | cv::DescriptorExtractor* createDescriptorExtractor( const std::string& descriptorType );
|
||
55 | ///Convert the CV_32FC1 image to CV_8UC1 with a fixed scale factor
|
||
56 | void depthToCV8UC1(const cv::Mat& float_img, cv::Mat& mono8_img); |
||
57 | |||
58 | ///Return the macro string for the cv::Mat type integer
|
||
59 | std::string openCVCode2String(unsigned int code); |
||
60 | |||
61 | ///Print Type and size of image
|
||
62 | void printMatrixInfo(cv::Mat& image); |