map_keyframe_based.h

Go to the documentation of this file.

#ifndef UCL_DRONE_SIMPLE_MAP_H
#define UCL_DRONE_SIMPLE_MAP_H
#define PCL_NO_PRECOMPILE

#define USE_PROFILING
#include <ucl_drone/profiling.h>

// #define DEBUG_PROJECTION  // if defined print relative errors of projection for the target

/* Header files */
#include <ucl_drone/ucl_drone.h>

#include <ros/package.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>

// vision
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>

/* Point Cloud library */
#include <pcl/visualization/point_cloud_geometry_handlers.h>

#include <pcl/common/common_headers.h>
#include <pcl/point_types.h>  // pcl::PointXYZRGB
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>  // pcl::fromROSMsg
#include <pcl_ros/point_cloud.h>              // pcl::PointCloud

/* Boost */
#include <boost/shared_ptr.hpp>
#include <boost/thread/thread.hpp>

/* Messages */
//#include <sensor_msgs/PointCloud2.h>  // sensor_msgs::PointCloud2
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Empty.h>

/* ucl_drone */
#include <ucl_drone/PointXYZRGBSIFT.h>  // pcl::PointXYZRGBSIFT
#include <ucl_drone/Pose3D.h>
#include <ucl_drone/ProcessedImageMsg.h>
#include <ucl_drone/TargetDetected.h>
#include <ucl_drone/map/projection_2D.h>
#include <ucl_drone/opencv_utils.h>
#include <ucl_drone/read_from_launch.h>

#include <ucl_drone/map/frame.h>
#include <ucl_drone/map/keyframe.h>

// to print the pointcloud at the console
void cloud_debug(pcl::PointCloud< pcl::PointXYZRGBSIFT >::ConstPtr cloud);

class Map
{
private:
  /* Attributes */

  ros::NodeHandle nh;

  bool processedImgReceived;   
  ucl_drone::Pose3D PnP_pose;  
  bool tracking_lost;  

  bool do_search;      
  bool stop_if_lost;   

  /* Subscribers */
  ros::Subscriber processed_image_sub;  
  std::string processed_image_channel_in;
  ros::Subscriber reset_pose_sub;  
  std::string reset_pose_channel;
  ros::Subscriber end_reset_pose_sub;  
  std::string end_reset_pose_channel;

  /* Publishers */
  ros::Publisher pose_PnP_pub;  
  std::string pose_PnP_channel;
  ros::Publisher pose_correction_pub;  

  std::string pose_correction_channel;
  ros::Publisher target_pub;  
  std::string target_channel_out;

  /* Tresholds for PnP */
  int threshold_lost;          

  int threshold_new_keyframe;  

  double threshold_new_keyframe_percentage;  


  void processedImageCb(const ucl_drone::ProcessedImageMsg::ConstPtr processed_image_in);

  bool doPnP(Frame current_frame, ucl_drone::Pose3D& PnP_pose, std::vector< int >& inliers,
             KeyFrame* ref_keyframe);

  bool closestKeyFrame(const ucl_drone::Pose3D& pose, int& keyframe_ID, Frame current_frame);

  // Measure
  ucl_drone::ProcessedImageMsg::ConstPtr lastProcessedImgReceived;  

  /* Services Definition */

  cv::Mat camera_matrix_K;

  cv::Mat tvec;  
  cv::Mat rvec;  

  Frame previous_frame;          
  KeyFrame* reference_keyframe;  

  cv::Mat cam_plane_top;
  cv::Mat cam_plane_bottom;
  cv::Mat cam_plane_left;
  cv::Mat cam_plane_right;

  void init_planes();

  void resetPoseCb(const std_msgs::Empty& msg);

  void endResetPoseCb(const std_msgs::Empty& msg);

  bool newKeyFrameNeeded(int number_of_common_keypoints);
  bool newKeyFrameNeeded(int number_of_common_keypoints, KeyFrame* reference_keyframe_candidate);

public:
  bool pending_reset;  

  pcl::PointCloud< pcl::PointXYZRGBSIFT >::Ptr cloud;

  boost::shared_ptr< pcl::visualization::PCLVisualizer > visualizer;

  Map();

  ~Map();

  void getDescriptors(const ucl_drone::Pose3D& pose, cv::Mat& descriptors, std::vector< int >& idx,
                      bool only_visible = true);

  void getDescriptors(const std::vector< int >& idx, cv::Mat& descriptors);

  void getVisibleKeyFrames(const ucl_drone::Pose3D& pose,
                           std::vector< std::vector< int > >& keyframes_ID);

  void getVisiblePoints(const ucl_drone::Pose3D& pose, std::vector< int >& idx);

  void targetDetectedPublisher();

  void publishPoseVisual(ucl_drone::Pose3D poseFrame, ucl_drone::Pose3D posePnP);
};

#endif /* UCL_DRONE_SIMPLE_MAP_H */