Statistics
| Branch: | Revision:

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