root / rgbdslam / src / node.h @ 9240aaa3
History | View | Annotate | Download (7.19 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 | |||
17 | |||
18 | #ifndef RGBD_SLAM_NODE_H_
|
||
19 | #define RGBD_SLAM_NODE_H_
|
||
20 | |||
21 | |||
22 | #include "ros/ros.h" |
||
23 | #include <vector> |
||
24 | #include <opencv2/core/core.hpp> |
||
25 | #include <opencv2/features2d/features2d.hpp> |
||
26 | #include <Eigen/Core> |
||
27 | //#include <image_geometry/pinhole_camera_model.h>
|
||
28 | #include <sensor_msgs/PointCloud2.h> |
||
29 | #include <pcl/registration/icp.h> |
||
30 | #include <pcl/registration/registration.h> |
||
31 | #include "parameter_server.h" |
||
32 | //for ground truth
|
||
33 | #include <tf/transform_datatypes.h> |
||
34 | |||
35 | // ICP_1 for external binary
|
||
36 | //#define USE_ICP_BIN
|
||
37 | |||
38 | // ICP_2 for included function
|
||
39 | //#define USE_ICP_CODE
|
||
40 | |||
41 | //#define USE_SIFT_GPU
|
||
42 | |||
43 | #ifdef USE_ICP_BIN
|
||
44 | #include "gicp-fallback.h" |
||
45 | #endif
|
||
46 | |||
47 | #ifdef USE_ICP_CODE
|
||
48 | #include "../gicp/gicp.h" |
||
49 | #include "../gicp/transform.h" |
||
50 | #endif
|
||
51 | |||
52 | #include "matching_result.h" |
||
53 | #include <Eigen/StdVector> |
||
54 | |||
55 | //!Holds the data for one graph node and provides functionality to compute relative transformations to other Nodes.
|
||
56 | class Node { |
||
57 | public:
|
||
58 | ///Visual must be CV_8UC1, depth CV_32FC1,
|
||
59 | ///id must correspond to the hogman vertex id
|
||
60 | ///detection_mask must be CV_8UC1 with non-zero
|
||
61 | ///at potential keypoint locations
|
||
62 | Node(const cv::Mat visual,
|
||
63 | cv::Ptr<cv::FeatureDetector> detector, |
||
64 | cv::Ptr<cv::DescriptorExtractor> extractor, |
||
65 | pointcloud_type::Ptr point_cloud, |
||
66 | const cv::Mat detection_mask = cv::Mat());
|
||
67 | //default constructor. TODO: still needed?
|
||
68 | Node(){} |
||
69 | ///Delete the flannIndex if built
|
||
70 | ~Node(); |
||
71 | |||
72 | ///Compare the features of two nodes and compute the transformation
|
||
73 | MatchingResult matchNodePair(const Node* older_node);
|
||
74 | |||
75 | ///Transform, e.g., from MoCap
|
||
76 | void setGroundTruthTransform(tf::StampedTransform gt);
|
||
77 | ///Transform, e.g., from kinematics
|
||
78 | void setBase2PointsTransform(tf::StampedTransform b2p);
|
||
79 | ///Transform, e.g., from MoCap
|
||
80 | tf::StampedTransform getGroundTruthTransform(); |
||
81 | ///Transform, e.g., from kinematics
|
||
82 | tf::StampedTransform getBase2PointsTransform(); |
||
83 | |||
84 | ///Compute the relative transformation between the nodes
|
||
85 | bool getRelativeTransformationTo(const Node* target_node, |
||
86 | std::vector<cv::DMatch>* initial_matches, |
||
87 | Eigen::Matrix4f& resulting_transformation, |
||
88 | float& rmse,
|
||
89 | std::vector<cv::DMatch>& matches) const;
|
||
90 | |||
91 | #ifdef USE_ICP_BIN
|
||
92 | // initial_transformation: optional transformation applied to this->pc before
|
||
93 | // using icp
|
||
94 | bool getRelativeTransformationTo_ICP_bin(const Node* target_node,Eigen::Matrix4f& transformation, |
||
95 | const Eigen::Matrix4f* initial_transformation = NULL); |
||
96 | #endif
|
||
97 | |||
98 | #ifdef USE_ICP_CODE
|
||
99 | bool getRelativeTransformationTo_ICP_code(const Node* target_node,Eigen::Matrix4f& transformation, |
||
100 | const Eigen::Matrix4f* initial_transformation = NULL); |
||
101 | #endif
|
||
102 | |||
103 | ///Send own pointcloud with given frame, publisher and timestamp
|
||
104 | void publish(const char* frame, ros::Time timestamp, ros::Publisher pub); |
||
105 | |||
106 | void buildFlannIndex();
|
||
107 | int findPairsFlann(const Node* other, std::vector<cv::DMatch>* matches) const; |
||
108 | |||
109 | #ifdef USE_ICP_CODE
|
||
110 | |||
111 | dgc::gicp::GICPPointSet* gicp_point_set; |
||
112 | |||
113 | static const double gicp_epsilon = 1e-4; |
||
114 | static const double gicp_d_max = 0.10; // 10cm |
||
115 | static const unsigned int gicp_max_iterations = 50; |
||
116 | static const unsigned int gicp_point_cnt = 10000; |
||
117 | |||
118 | bool gicp_initialized;
|
||
119 | |||
120 | void Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m); |
||
121 | void GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m); |
||
122 | void gicpSetIdentity(dgc_transform_t m);
|
||
123 | void createGICPStructures(unsigned int max_count = 1000); |
||
124 | |||
125 | #endif
|
||
126 | |||
127 | |||
128 | |||
129 | //!erase the points from the cloud to save memory
|
||
130 | void clearPointCloud();
|
||
131 | //PointCloud pc;
|
||
132 | ///pointcloud_type centrally defines what the pc is templated on
|
||
133 | pointcloud_type::Ptr pc_col; |
||
134 | |||
135 | cv::Mat feature_descriptors_; ///<descriptor definitions
|
||
136 | std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > feature_locations_3d_; ///<backprojected 3d descriptor locations relative to cam position in homogeneous coordinates (last dimension is 1.0)
|
||
137 | std::vector<cv::KeyPoint> feature_locations_2d_; ///<Where in the image are the descriptors
|
||
138 | unsigned int id_; ///must correspond to the hogman vertex id |
||
139 | void clearFeatureInformation();
|
||
140 | |||
141 | protected:
|
||
142 | |||
143 | cv::flann::Index* flannIndex; |
||
144 | tf::StampedTransform base2points_; //!<contains the transformation from the base (defined on param server) to the point_cloud
|
||
145 | tf::StampedTransform ground_truth_transform_;//!<contains the transformation from the mocap system
|
||
146 | int initial_node_matches_;
|
||
147 | |||
148 | /** remove invalid keypoints (NaN or outside the image) and return the backprojection of valid ones*/
|
||
149 | #ifdef USE_SIFT_GPU
|
||
150 | //remove also unused descriptors
|
||
151 | void projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
|
||
152 | std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d, |
||
153 | const pointcloud_type::Ptr point_cloud, float* descriptors_in, cv::Mat& descriptors_out); |
||
154 | #endif
|
||
155 | void projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
|
||
156 | std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d, |
||
157 | const pointcloud_type::ConstPtr point_cloud);
|
||
158 | |||
159 | // helper for ransac
|
||
160 | // check for distances only if max_dist_cm > 0
|
||
161 | template<class InputIterator> |
||
162 | Eigen::Matrix4f getTransformFromMatches(const Node* other_node,
|
||
163 | InputIterator iter_begin, |
||
164 | InputIterator iter_end, |
||
165 | bool& valid,
|
||
166 | float max_dist_m = -1) const; |
||
167 | //std::vector<cv::DMatch> const* matches,
|
||
168 | //pcl::TransformationFromCorrespondences& tfc);
|
||
169 | |||
170 | ///Get the norm of the translational part of an affine matrix (Helper for isBigTrafo)
|
||
171 | void mat2dist(const Eigen::Matrix4f& t, double &dist){ |
||
172 | dist = sqrt(t(0,3)*t(0,3)+t(1,3)*t(1,3)+t(2,3)*t(2,3)); |
||
173 | } |
||
174 | |||
175 | |||
176 | ///Retrieves and stores the transformation from base to point cloud at capturing time
|
||
177 | void retrieveBase2CamTransformation();
|
||
178 | // helper for ransac
|
||
179 | void computeInliersAndError(const std::vector<cv::DMatch>& initial_matches, |
||
180 | const Eigen::Matrix4f& transformation,
|
||
181 | const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
|
||
182 | const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& targets,
|
||
183 | std::vector<cv::DMatch>& new_inliers, //output var
|
||
184 | double& mean_error, std::vector<double>& errors, |
||
185 | double squaredMaxInlierDistInM = 0.0009) const; //output var; |
||
186 | |||
187 | public:
|
||
188 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
189 | }; |
||
190 | #endif |