node.h

Go to the documentation of this file.

/* This file is part of RGBDSLAM.
 * 
 * RGBDSLAM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * RGBDSLAM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
 */


#ifndef RGBD_SLAM_NODE_H_
#define RGBD_SLAM_NODE_H_

#ifdef HEMACLOUDS
#define PCL_NO_PRECOMPILE
#endif

#include "ros/ros.h"
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <Eigen/Core>
//#include <image_geometry/pinhole_camera_model.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CameraInfo.h>
/*
#include <pcl/registration/icp.h>
#include <pcl/registration/impl/icp.hpp>
#include <pcl/registration/registration.h>
#include <pcl/registration/impl/registration.hpp>
*/
#include "parameter_server.h"
//for ground truth
#include <tf/transform_datatypes.h>
#include <QMutex>

#ifdef USE_ICP_BIN
#include "gicp-fallback.h"
#endif

#ifdef USE_ICP_CODE
#include "gicp/gicp.h"
#include "gicp/transform.h"
#endif

#include "matching_result.h" 
#include <Eigen/StdVector>
#include "header.h"

typedef std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > std_vector_of_eigen_vector4f;
class Node {
public:
        Node(const cv::Mat& visual,
                         const cv::Mat& depth,
                         const cv::Mat& detection_mask,
       const sensor_msgs::CameraInfoConstPtr& cam_info, 
       myHeader depth_header,
                         cv::Ptr<cv::FeatureDetector> detector,
                         cv::Ptr<cv::DescriptorExtractor> extractor);
        Node(const cv::Mat visual,
                         cv::Ptr<cv::FeatureDetector> detector,
                         cv::Ptr<cv::DescriptorExtractor> extractor,
                         pointcloud_type::Ptr point_cloud,
                         const cv::Mat detection_mask = cv::Mat());
        //default constructor. TODO: still needed?
        Node(){}
        ~Node();

  MatchingResult matchNodePair(const Node* older_node);
  //MatchingResult matchNodePair2(const Node* older_node);

  void setOdomTransform(tf::StampedTransform odom);
  void setGroundTruthTransform(tf::StampedTransform gt);
  void setBase2PointsTransform(tf::StampedTransform& b2p);
  tf::StampedTransform getOdomTransform() const;
  tf::StampedTransform getGroundTruthTransform() const;
  tf::StampedTransform getBase2PointsTransform() const;

        bool getRelativeTransformationTo(const Node* target_node, 
                        std::vector<cv::DMatch>* initial_matches,
                        Eigen::Matrix4f& resulting_transformation, 
                        float& rmse,
                        std::vector<cv::DMatch>& matches) const;

#ifdef USE_ICP_BIN
        // initial_transformation: optional transformation applied to this->pc before
        // using icp
        bool getRelativeTransformationTo_ICP_bin(const Node* target_node,Eigen::Matrix4f& transformation,
                        const Eigen::Matrix4f* initial_transformation = NULL);
#endif
        
        unsigned int featureMatching(const Node* other, std::vector<cv::DMatch>* matches) const;

#ifdef USE_ICP_CODE
        bool getRelativeTransformationTo_ICP_code(const Node* target_node,
                                            Eigen::Matrix4f& transformation,
                                            const Eigen::Matrix4f& initial_transformation);
        
        static const double gicp_epsilon = 1e-3;
        static const double gicp_d_max_ = 5.0; // 10cm
        static const int gicp_max_iterations = 10;
        static const int gicp_min_point_cnt = 100;
                
        bool gicp_initialized;
        void Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m);
        void GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m);
        void gicpSetIdentity(dgc_transform_t m);
  dgc::gicp::GICPPointSet* getGICPStructure(unsigned int max_count = 0) const;
  void clearGICPStructure() const;
  protected:
    mutable dgc::gicp::GICPPointSet* gicp_point_set_;

  public:
#endif

  Node* copy_filtered(const Eigen::Vector3f& center, float radius) const;

  void clearFeatureInformation();
  void addPointCloud(pointcloud_type::Ptr pc_col);


  void clearPointCloud();
  void reducePointCloud(double voxelfilter_size);
        //PointCloud pc;
  int id_;         //<number of camera nodes in the graph when the node was added
        int seq_id_;      //<number of images that have been processed (even if they were not added)
        int vertex_id_;   //<id of the corresponding vertex in the g2o graph
  bool valid_tf_estimate_;      //<Flags whether the data of this node should be considered for postprocessing steps, e.g., visualization, trajectory, map creation
  bool matchable_;        //< Flags whether the data for matching is (still) available
  ros::Time timestamp_;                 //< Time of the data belonging to the node
  bool has_odometry_edge_;
  bool odometry_set_;
  pointcloud_type::Ptr pc_col;
#ifdef USE_PCL_ICP
  pointcloud_type::Ptr filtered_pc_col; //<Used for icp. May not contain NaN
#endif

        cv::Mat feature_descriptors_;         

        std_vector_of_eigen_vector4f feature_locations_3d_;  
        std::vector<float> siftgpu_descriptors;

        std::vector<cv::KeyPoint> feature_locations_2d_; 
  std::vector<std::pair<float, float> > feature_depth_stats_;
  std::vector<unsigned char> feature_matching_stats_;
#ifdef HEMACLOUDS

  std::vector<float> feature_weights_;
#endif

#ifdef  DO_FEATURE_OPTIMIZATION

  std::map<int, int> kpt_to_landmark;
#endif

  long getMemoryFootprint(bool print) const;
  void knnSearch(cv::Mat& query,
                 cv::Mat& indices,
                 cv::Mat& dists,
                 int knn, 
                 const cv::flann::SearchParams& params) const;

  myHeader header_;
  const sensor_msgs::CameraInfo& getCamInfo() const {return cam_info_;}

protected:
  const cv::flann::Index* getFlannIndex() const;
  static QMutex gicp_mutex;
  static QMutex siftgpu_mutex;
        mutable cv::flann::Index* flannIndex;
  tf::StampedTransform base2points_; 
  tf::StampedTransform ground_truth_transform_;
  tf::StampedTransform odom_transform_;        
  int initial_node_matches_;
  sensor_msgs::CameraInfo cam_info_; 
  //void computeKeypointDepthStats(const cv::Mat& depth_img, const std::vector<cv::KeyPoint> keypoints);

#ifdef USE_SIFT_GPU

        void projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
                          std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
                            const pointcloud_type::Ptr point_cloud, 
                            std::vector<float>& descriptors_in, cv::Mat& descriptors_out);
        void projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
                          std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
                          const cv::Mat& depth,
                          const sensor_msgs::CameraInfoConstPtr& cam_info,
                          std::vector<float>& descriptors_in, cv::Mat& descriptors_out);
#endif

        void projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
                   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
                   const pointcloud_type::ConstPtr point_cloud);
        void projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
                   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
                   const cv::Mat& depth,
                   const sensor_msgs::CameraInfoConstPtr& cam_info);


        void mat2dist(const Eigen::Matrix4f& t, double &dist){
                dist = sqrt(t(0,3)*t(0,3)+t(1,3)*t(1,3)+t(2,3)*t(2,3));
        }

  
  void retrieveBase2CamTransformation();
        // helper for ransac
        void computeInliersAndError(const std::vector<cv::DMatch> & initial_matches,
                              const Eigen::Matrix4f& transformation,
                              const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
                              const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& targets,
                              size_t min_inliers, //break if this can't be reached
                              std::vector<cv::DMatch>& new_inliers, //pure output var
                              double& mean_error, //pure output var //std::vector<double>& errors,
                              double squaredMaxInlierDistInM = 0.0009) const; //output var;

public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void pairwiseObservationLikelihood(const Node* newer_node, const Node* older_node, MatchingResult& mr);
void squareroot_descriptor_space(cv::Mat& feature_descriptors);
// Compute the transformation from matches using pcl::TransformationFromCorrespondences
#endif