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 "gicpfallback.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 nonzero

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 = 1e4; 
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
