rgb_navigator.h
Go to the documentation of this file.
00001 
00024 #ifndef CCNY_RGBD_KEYFRAME_MAPPER_H
00025 #define CCNY_RGBD_KEYFRAME_MAPPER_H
00026 
00027 #include <iostream>
00028 #include <fstream>
00029 #include <ros/ros.h>
00030 #include <ros/publisher.h>
00031 #include <pcl/point_cloud.h>
00032 #include <pcl_ros/point_cloud.h>
00033 #include <pcl_ros/transforms.h>
00034 #include <pcl/filters/passthrough.h>
00035 #include <tf/transform_listener.h>
00036 #include <visualization_msgs/Marker.h>
00037 #include <boost/regex.hpp>
00038 #include <boost/filesystem.hpp>
00039 #include <octomap/octomap.h>
00040 #include <octomap/OcTree.h>
00041 #include <octomap/ColorOcTree.h>
00042 #include <rgbdtools/rgbdtools.h>
00043 
00044 #include "ccny_rgbd/types.h"
00045 #include "ccny_rgbd/util.h"
00046 #include "ccny_rgbd/GenerateGraph.h"
00047 #include "ccny_rgbd/SolveGraph.h"
00048 #include "ccny_rgbd/AddManualKeyframe.h"
00049 #include "ccny_rgbd/PublishKeyframe.h"
00050 #include "ccny_rgbd/PublishKeyframes.h"
00051 #include "ccny_rgbd/Save.h"
00052 #include "ccny_rgbd/Load.h"
00053 
00054 namespace ccny_rgbd {
00055 
00069 class RGBNavigator
00070 {
00071   public:
00072 
00077     RGBNavigator(const ros::NodeHandle& nh,
00078                    const ros::NodeHandle& nh_private);
00079     
00082     virtual ~RGBNavigator();
00083 
00086     void initParams();
00087 
00102     bool publishKeyframesSrvCallback(
00103       PublishKeyframes::Request& request,
00104       PublishKeyframes::Response& response);
00105 
00110     bool publishKeyframeSrvCallback(
00111       PublishKeyframe::Request& request,
00112       PublishKeyframe::Response& response);
00113 
00119     bool saveKeyframesSrvCallback(
00120       Save::Request& request,
00121       Save::Response& response);
00122 
00131     bool savePcdMapSrvCallback(
00132       Save::Request& request,
00133       Save::Response& response);
00134     
00143     bool saveOctomapSrvCallback(
00144       Save::Request& request,
00145       Save::Response& response);
00146     
00152     bool loadKeyframesSrvCallback(
00153       Load::Request& request,
00154       Load::Response& response);
00155 
00158     bool addManualKeyframeSrvCallback(
00159       AddManualKeyframe::Request& request,
00160       AddManualKeyframe::Response& response);
00161     
00165      bool generateGraphSrvCallback(
00166       GenerateGraph::Request& request,
00167       GenerateGraph::Response& response);
00168 
00174     bool solveGraphSrvCallback(
00175       SolveGraph::Request& request,
00176       SolveGraph::Response& response);
00177     
00178   protected:
00179     std::vector<std::vector<double> > aruco_pos;
00180     std::vector<int> aruco_kfs;
00181     ros::NodeHandle nh_;          
00182     ros::NodeHandle nh_private_;  
00183     
00184     std::string fixed_frame_;     
00185     
00186     int queue_size_;  
00187     
00188     double max_range_;  
00189     double max_stdev_;  
00190 
00191     rgbdtools::KeyframeVector keyframes_;    
00192     
00199     virtual void RGBCallback(const ImageMsg::ConstPtr& rgb_msg,
00200                               const CameraInfoMsg::ConstPtr& info_msg);
00201 
00202   private:
00203 
00204     ros::Publisher keyframes_pub_;    
00205     ros::Publisher poses_pub_;        
00206     ros::Publisher kf_assoc_pub_;     
00207     ros::Publisher path_pub_;         
00208     
00210     ros::ServiceServer generate_graph_service_;
00211     
00213     ros::ServiceServer solve_graph_service_;   
00214     
00216     ros::ServiceServer pub_keyframe_service_;
00217     
00219     ros::ServiceServer pub_keyframes_service_;
00220     
00222     ros::ServiceServer save_kf_service_;
00223     
00225     ros::ServiceServer save_pcd_map_service_;
00226     
00228     ros::ServiceServer save_octomap_service_;
00229     
00231     ros::ServiceServer load_kf_service_;
00232     
00234     ros::ServiceServer add_manual_keyframe_service_;
00235 
00236     tf::TransformListener tf_listener_; 
00237 
00239     boost::shared_ptr<ImageTransport> rgb_it_;
00240     
00242     boost::shared_ptr<ImageTransport> depth_it_;
00243     
00245     boost::shared_ptr<RGBSynchronizer> sync_;
00246           
00248     ImageSubFilter      sub_rgb_;
00249     
00251     ImageSubFilter      sub_depth_;  
00252    
00254     CameraInfoSubFilter sub_info_;
00255     
00256     // params
00257     double pcd_map_res_; 
00258     double octomap_res_;  
00259     double kf_dist_eps_;  
00260     double kf_angle_eps_; 
00261     bool octomap_with_color_; 
00262     double max_map_z_;   
00263     double min_map_z_;   
00264 
00265     // state vars
00266     bool manual_add_;   
00267 
00268     int rgbd_frame_index_;
00269 
00270     rgbdtools::KeyframeGraphDetector graph_detector_;  
00271     rgbdtools::KeyframeGraphSolverG2O graph_solver_;    
00272     rgbdtools::KeyframeGraphSolverISAM solver_;
00273     rgbdtools::KeyframeAssociationVector associations_; 
00274     
00275     PathMsg path_msg_;    
00276     PathMsg aruco_path;
00284     bool processFrame(const rgbdtools::RGBDFrame& frame, const AffineTransform& pose);
00285     
00291     void addKeyframe(const rgbdtools::RGBDFrame& frame, const AffineTransform& pose);
00292 
00296     void publishKeyframeData(int i);
00297     
00301     void publishKeyframePose(int i);
00302         
00305     void publishKeyframeAssociations();
00306     
00309     void publishKeyframePoses();
00310     void publishArucoPos();
00311 
00314     void publishPath();
00315     
00321     bool savePcdMap(const std::string& path);
00322            
00326     void buildPcdMap(PointCloudT& map_cloud);
00327     void buildFullCloud(PointCloudT& map_cloud);
00328 
00334     bool saveOctomap(const std::string& path);
00335     
00336     
00340     void buildColorOctomap(octomap::ColorOcTree& tree);
00341         
00346     static inline octomap::pose6d poseTfToOctomap(
00347       const tf::Pose& poseTf)
00348     {
00349       return octomap::pose6d(
00350               pointTfToOctomap(poseTf.getOrigin()),
00351               quaternionTfToOctomap(poseTf.getRotation()));
00352     }
00353    
00358     static inline octomap::point3d pointTfToOctomap(
00359       const tf::Point& ptTf)
00360     {
00361       return octomap::point3d(ptTf.x(), ptTf.y(), ptTf.z());
00362     }
00363    
00368     static inline octomath::Quaternion quaternionTfToOctomap(
00369       const tf::Quaternion& qTf)
00370     {
00371       return octomath::Quaternion(qTf.w(), qTf.x(), qTf.y(), qTf.z());
00372     }
00373     
00374     bool savePath(const std::string& filepath);
00375     bool savePathTUMFormat(const std::string& filepath);
00376     void filter_aruco();
00377     bool loadPath(const std::string& filepath);
00378     
00379     void updatePathFromKeyframePoses();
00380 };
00381 
00382 } // namespace ccny_rgbd
00383 
00384 #endif // CCNY_RGBD_KEYFRAME_MAPPER_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ccny_rgbd
Author(s): Ivan Dryanovski
autogenerated on Tue Aug 27 2013 10:34:02