tracking.h

Go to the documentation of this file.

#ifndef TRACKING_H
#define TRACKING_H

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Range.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/stereo_camera_model.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/crop_box.h>

#include <opencv2/opencv.hpp>

#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include "frame.h"
#include "graph.h"
#include "publisher.h"

using namespace std;
using namespace boost;
namespace fs  = filesystem;

typedef pcl::PointXYZ                     PointXYZ;
typedef pcl::PointXYZRGB                  PointRGB;
typedef pcl::PointCloud<PointXYZ>         PointCloudXYZ;
typedef pcl::PointCloud<PointRGB>         PointCloudRGB;

namespace slam
{

class Publisher;
class Graph;

class Tracking
{

public:

  struct Params
  {
    string odom_topic;                
    string range_topic;               
    string camera_topic;              
    double min_range;                 
    double max_range;                 
    bool refine;                      

    // Default settings
    Params () {
      odom_topic   = "/odom";
      odom_topic   = "/range";
      camera_topic = "/usb_cam";
      min_range = 1.5;
      max_range = 3.0;
      refine = false;
    }
  };

  enum trackingState{
    NOT_INITIALIZED   = 0,
    INITIALIZING      = 1,
    WORKING           = 2,
    LOST              = 3
  };

  Tracking(Publisher* f_pub, Graph* graph);

  inline void setParams(const Params& params){params_ = params;}

  inline Params getParams() const {return params_;}

  inline Frame getCurrentFrame() const {return c_frame_;}

  void run();

protected:

  void msgsCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
                    const sensor_msgs::Range::ConstPtr& range_msg,
                    const sensor_msgs::ImageConstPtr& l_img_msg,
                    const sensor_msgs::ImageConstPtr& r_img_msg,
                    const sensor_msgs::CameraInfoConstPtr& l_info_msg,
                    const sensor_msgs::CameraInfoConstPtr& r_info_msg,
                    const sensor_msgs::PointCloud2ConstPtr& cloud_msg);

  bool getOdom2CameraTf(nav_msgs::Odometry odom_msg,
                        sensor_msgs::Image img_msg,
                        tf::StampedTransform &transform);

  bool needNewKeyFrame(double range);

  bool addFrameToMap(double range);


  PointCloudRGB::Ptr filterCloud(PointCloudRGB::Ptr in_cloud);

  void publishOverlap(PointCloudXYZ::Ptr cloud, tf::Transform movement, float overlap);

  bool refinePose(Frame c_frame, Frame p_frame, tf::Transform& out, int& num_inliers);

private:

  Params params_; 

  trackingState state_; 

  tf::StampedTransform odom2camera_; 

  tf::TransformListener tf_listener_; 

  Frame c_frame_; 

  Frame p_frame_; 

  cv::Mat camera_matrix_; 

  Publisher* f_pub_; 

  ros::Publisher pc_pub_; 

  ros::Publisher pose_pub_; 

  ros::Publisher overlapping_pub_; 

  tf::TransformBroadcaster tf_broadcaster_; 

  image_geometry::StereoCameraModel camera_model_; 

  Graph* graph_; 

  tf::Transform last_fixed_frame_pose_; 

  Eigen::Vector4f last_min_pt_, last_max_pt_; // Stores the last fixed frame minimum and maximum points

  int frame_id_; 

  int kf_id_; 

  vector<tf::Transform> odom_pose_history_; 

  tf::Transform prev_robot_pose_; 

  ros::WallTime jump_time_; 

  bool jump_detected_; 

  double secs_to_filter_; 

  // Topic sync
  typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,
                                                          sensor_msgs::Range,
                                                          sensor_msgs::Image,
                                                          sensor_msgs::Image,
                                                          sensor_msgs::CameraInfo,
                                                          sensor_msgs::CameraInfo,
                                                          sensor_msgs::PointCloud2> SyncPolicy;
  typedef message_filters::Synchronizer<SyncPolicy> Sync;

};

} // namespace

#endif // TRACKING_H