tracking.h
Go to the documentation of this file.
00001 
00006 #ifndef TRACKING_H
00007 #define TRACKING_H
00008 
00009 #include <ros/ros.h>
00010 #include <nav_msgs/Odometry.h>
00011 #include <sensor_msgs/Image.h>
00012 #include <sensor_msgs/Range.h>
00013 #include <message_filters/subscriber.h>
00014 #include <message_filters/time_synchronizer.h>
00015 #include <message_filters/sync_policies/approximate_time.h>
00016 #include <image_transport/subscriber_filter.h>
00017 #include <image_geometry/stereo_camera_model.h>
00018 #include <tf/transform_datatypes.h>
00019 #include <tf/transform_listener.h>
00020 #include <tf/transform_broadcaster.h>
00021 
00022 #include <pcl_ros/point_cloud.h>
00023 #include <pcl_ros/transforms.h>
00024 #include <pcl/point_types.h>
00025 #include <pcl/common/common.h>
00026 #include <pcl/filters/filter.h>
00027 #include <pcl/filters/approximate_voxel_grid.h>
00028 #include <pcl/filters/crop_box.h>
00029 
00030 #include <opencv2/opencv.hpp>
00031 
00032 #include <boost/filesystem.hpp>
00033 #include <boost/lexical_cast.hpp>
00034 
00035 #include "frame.h"
00036 #include "graph.h"
00037 #include "publisher.h"
00038 
00039 using namespace std;
00040 using namespace boost;
00041 namespace fs  = filesystem;
00042 
00043 typedef pcl::PointXYZ                     PointXYZ;
00044 typedef pcl::PointXYZRGB                  PointRGB;
00045 typedef pcl::PointCloud<PointXYZ>         PointCloudXYZ;
00046 typedef pcl::PointCloud<PointRGB>         PointCloudRGB;
00047 
00048 namespace slam
00049 {
00050 
00051 class Publisher;
00052 class Graph;
00053 
00054 class Tracking
00055 {
00056 
00057 public:
00058 
00059   struct Params
00060   {
00061     string odom_topic;                
00062     string range_topic;               
00063     string camera_topic;              
00064     double min_range;                 
00065     double max_range;                 
00066     bool refine;                      
00067 
00068     // Default settings
00069     Params () {
00070       odom_topic   = "/odom";
00071       odom_topic   = "/range";
00072       camera_topic = "/usb_cam";
00073       min_range = 1.5;
00074       max_range = 3.0;
00075       refine = false;
00076     }
00077   };
00078 
00079   enum trackingState{
00080     NOT_INITIALIZED   = 0,
00081     INITIALIZING      = 1,
00082     WORKING           = 2,
00083     LOST              = 3
00084   };
00085 
00090   Tracking(Publisher* f_pub, Graph* graph);
00091 
00095   inline void setParams(const Params& params){params_ = params;}
00096 
00099   inline Params getParams() const {return params_;}
00100 
00103   inline Frame getCurrentFrame() const {return c_frame_;}
00104 
00107   void run();
00108 
00109 protected:
00110 
00120   void msgsCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
00121                     const sensor_msgs::Range::ConstPtr& range_msg,
00122                     const sensor_msgs::ImageConstPtr& l_img_msg,
00123                     const sensor_msgs::ImageConstPtr& r_img_msg,
00124                     const sensor_msgs::CameraInfoConstPtr& l_info_msg,
00125                     const sensor_msgs::CameraInfoConstPtr& r_info_msg,
00126                     const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
00127 
00134   bool getOdom2CameraTf(nav_msgs::Odometry odom_msg,
00135                         sensor_msgs::Image img_msg,
00136                         tf::StampedTransform &transform);
00137 
00141   bool needNewKeyFrame(double range);
00142 
00146   bool addFrameToMap(double range);
00147 
00148 
00153   PointCloudRGB::Ptr filterCloud(PointCloudRGB::Ptr in_cloud);
00154 
00161   void publishOverlap(PointCloudXYZ::Ptr cloud, tf::Transform movement, float overlap);
00162 
00170   bool refinePose(Frame c_frame, Frame p_frame, tf::Transform& out, int& num_inliers);
00171 
00172 private:
00173 
00174   Params params_; 
00175 
00176   trackingState state_; 
00177 
00178   tf::StampedTransform odom2camera_; 
00179 
00180   tf::TransformListener tf_listener_; 
00181 
00182   Frame c_frame_; 
00183 
00184   Frame p_frame_; 
00185 
00186   cv::Mat camera_matrix_; 
00187 
00188   Publisher* f_pub_; 
00189 
00190   ros::Publisher pc_pub_; 
00191 
00192   ros::Publisher pose_pub_; 
00193 
00194   ros::Publisher overlapping_pub_; 
00195 
00196   tf::TransformBroadcaster tf_broadcaster_; 
00197 
00198   image_geometry::StereoCameraModel camera_model_; 
00199 
00200   Graph* graph_; 
00201 
00202   tf::Transform last_fixed_frame_pose_; 
00203 
00204   Eigen::Vector4f last_min_pt_, last_max_pt_; // Stores the last fixed frame minimum and maximum points
00205 
00206   int frame_id_; 
00207 
00208   int kf_id_; 
00209 
00210   vector<tf::Transform> odom_pose_history_; 
00211 
00212   tf::Transform prev_robot_pose_; 
00213 
00214   ros::WallTime jump_time_; 
00215 
00216   bool jump_detected_; 
00217 
00218   double secs_to_filter_; 
00219 
00220   // Topic sync
00221   typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,
00222                                                           sensor_msgs::Range,
00223                                                           sensor_msgs::Image,
00224                                                           sensor_msgs::Image,
00225                                                           sensor_msgs::CameraInfo,
00226                                                           sensor_msgs::CameraInfo,
00227                                                           sensor_msgs::PointCloud2> SyncPolicy;
00228   typedef message_filters::Synchronizer<SyncPolicy> Sync;
00229 
00230 };
00231 
00232 } // namespace
00233 
00234 #endif // TRACKING_H


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jun 6 2019 21:40:57