root / rgbdslam / src / node.h @ 9240aaa3
History | View | Annotate | Download (7.19 KB)
1 |
/* 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
|