00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef RGBD_SLAM_NODE_H_
00019 #define RGBD_SLAM_NODE_H_
00020
00021 #ifdef HEMACLOUDS
00022 #define PCL_NO_PRECOMPILE
00023 #endif
00024
00025 #include "ros/ros.h"
00026 #include <vector>
00027 #include <opencv2/core/core.hpp>
00028 #include <opencv2/features2d/features2d.hpp>
00029 #include <Eigen/Core>
00030
00031 #include <sensor_msgs/PointCloud2.h>
00032 #include <sensor_msgs/CameraInfo.h>
00033
00034
00035
00036
00037
00038
00039 #include "parameter_server.h"
00040
00041 #include <tf/transform_datatypes.h>
00042 #include <QMutex>
00043
00044 #ifdef USE_ICP_BIN
00045 #include "gicp-fallback.h"
00046 #endif
00047
00048 #ifdef USE_ICP_CODE
00049 #include "gicp/gicp.h"
00050 #include "gicp/transform.h"
00051 #endif
00052
00053 #include "matching_result.h"
00054 #include <Eigen/StdVector>
00055 #include "header.h"
00056
00057 typedef std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > std_vector_of_eigen_vector4f;
00059 class Node {
00060 public:
00064 Node(const cv::Mat& visual,
00065 const cv::Mat& depth,
00066 const cv::Mat& detection_mask,
00067 const sensor_msgs::CameraInfoConstPtr& cam_info,
00068 myHeader depth_header,
00069 cv::Ptr<cv::FeatureDetector> detector,
00070 cv::Ptr<cv::DescriptorExtractor> extractor);
00074 Node(const cv::Mat visual,
00075 cv::Ptr<cv::FeatureDetector> detector,
00076 cv::Ptr<cv::DescriptorExtractor> extractor,
00077 pointcloud_type::Ptr point_cloud,
00078 const cv::Mat detection_mask = cv::Mat());
00079
00080 Node(){}
00082 ~Node();
00083
00085 MatchingResult matchNodePair(const Node* older_node);
00086
00087
00089 void setOdomTransform(tf::StampedTransform odom);
00091 void setGroundTruthTransform(tf::StampedTransform gt);
00093 void setBase2PointsTransform(tf::StampedTransform& b2p);
00095 tf::StampedTransform getOdomTransform() const;
00097 tf::StampedTransform getGroundTruthTransform() const;
00099 tf::StampedTransform getBase2PointsTransform() const;
00100
00102 bool getRelativeTransformationTo(const Node* target_node,
00103 std::vector<cv::DMatch>* initial_matches,
00104 Eigen::Matrix4f& resulting_transformation,
00105 float& rmse,
00106 std::vector<cv::DMatch>& matches) const;
00107
00108 #ifdef USE_ICP_BIN
00109
00110
00111 bool getRelativeTransformationTo_ICP_bin(const Node* target_node,Eigen::Matrix4f& transformation,
00112 const Eigen::Matrix4f* initial_transformation = NULL);
00113 #endif
00114
00117 unsigned int featureMatching(const Node* other, std::vector<cv::DMatch>* matches) const;
00118
00119 #ifdef USE_ICP_CODE
00120 bool getRelativeTransformationTo_ICP_code(const Node* target_node,
00121 Eigen::Matrix4f& transformation,
00122 const Eigen::Matrix4f& initial_transformation);
00123
00124 static const double gicp_epsilon = 1e-3;
00125 static const double gicp_d_max_ = 5.0;
00126 static const int gicp_max_iterations = 10;
00127 static const int gicp_min_point_cnt = 100;
00128
00129 bool gicp_initialized;
00130 void Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m);
00131 void GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m);
00132 void gicpSetIdentity(dgc_transform_t m);
00133 dgc::gicp::GICPPointSet* getGICPStructure(unsigned int max_count = 0) const;
00134 void clearGICPStructure() const;
00135 protected:
00136 mutable dgc::gicp::GICPPointSet* gicp_point_set_;
00137
00138 public:
00139 #endif
00140
00142 Node* copy_filtered(const Eigen::Vector3f& center, float radius) const;
00143
00144 void clearFeatureInformation();
00145 void addPointCloud(pointcloud_type::Ptr pc_col);
00146
00147
00149 void clearPointCloud();
00151 void reducePointCloud(double voxelfilter_size);
00152
00154 int id_;
00155 int seq_id_;
00156 int vertex_id_;
00157 bool valid_tf_estimate_;
00158 bool matchable_;
00159 ros::Time timestamp_;
00160 bool has_odometry_edge_;
00161 bool odometry_set_;
00162 pointcloud_type::Ptr pc_col;
00163 #ifdef USE_PCL_ICP
00164 pointcloud_type::Ptr filtered_pc_col;
00165 #endif
00166
00167 cv::Mat feature_descriptors_;
00168
00170 std_vector_of_eigen_vector4f feature_locations_3d_;
00171 std::vector<float> siftgpu_descriptors;
00172
00174 std::vector<cv::KeyPoint> feature_locations_2d_;
00176 std::vector<std::pair<float, float> > feature_depth_stats_;
00178 std::vector<unsigned char> feature_matching_stats_;
00179 #ifdef HEMACLOUDS
00180
00181 std::vector<float> feature_weights_;
00182 #endif
00183
00184 #ifdef DO_FEATURE_OPTIMIZATION
00185
00186 std::map<int, int> kpt_to_landmark;
00187 #endif
00188
00189 long getMemoryFootprint(bool print) const;
00190 void knnSearch(cv::Mat& query,
00191 cv::Mat& indices,
00192 cv::Mat& dists,
00193 int knn,
00194 const cv::flann::SearchParams& params) const;
00195
00196 myHeader header_;
00197 const sensor_msgs::CameraInfo& getCamInfo() const {return cam_info_;}
00198
00199 protected:
00200 const cv::flann::Index* getFlannIndex() const;
00201 static QMutex gicp_mutex;
00202 static QMutex siftgpu_mutex;
00203 mutable cv::flann::Index* flannIndex;
00204 tf::StampedTransform base2points_;
00205 tf::StampedTransform ground_truth_transform_;
00206 tf::StampedTransform odom_transform_;
00207 int initial_node_matches_;
00208 sensor_msgs::CameraInfo cam_info_;
00209
00210
00211 #ifdef USE_SIFT_GPU
00212
00213 void projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
00214 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00215 const pointcloud_type::Ptr point_cloud,
00216 std::vector<float>& descriptors_in, cv::Mat& descriptors_out);
00218 void projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
00219 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00220 const cv::Mat& depth,
00221 const sensor_msgs::CameraInfoConstPtr& cam_info,
00222 std::vector<float>& descriptors_in, cv::Mat& descriptors_out);
00223 #endif
00224
00225 void projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
00226 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00227 const pointcloud_type::ConstPtr point_cloud);
00229 void projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
00230 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00231 const cv::Mat& depth,
00232 const sensor_msgs::CameraInfoConstPtr& cam_info);
00233
00234
00236 void mat2dist(const Eigen::Matrix4f& t, double &dist){
00237 dist = sqrt(t(0,3)*t(0,3)+t(1,3)*t(1,3)+t(2,3)*t(2,3));
00238 }
00239
00240
00242 void retrieveBase2CamTransformation();
00243
00244 void computeInliersAndError(const std::vector<cv::DMatch> & initial_matches,
00245 const Eigen::Matrix4f& transformation,
00246 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
00247 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& targets,
00248 size_t min_inliers,
00249 std::vector<cv::DMatch>& new_inliers,
00250 double& mean_error,
00251 double squaredMaxInlierDistInM = 0.0009) const;
00252
00253 public:
00254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00255 };
00256
00257 void pairwiseObservationLikelihood(const Node* newer_node, const Node* older_node, MatchingResult& mr);
00259 void squareroot_descriptor_space(cv::Mat& feature_descriptors);
00260
00261 #endif