00001 00024 #ifndef CCNY_RGBD_RGBD_FEATURE_VIEWER_H 00025 #define CCNY_RGBD_RGBD_FEATURE_VIEWER_H 00026 00027 #include <ros/ros.h> 00028 #include <sensor_msgs/PointCloud2.h> 00029 #include <visualization_msgs/Marker.h> 00030 #include <dynamic_reconfigure/server.h> 00031 #include <opencv2/imgproc/imgproc.hpp> 00032 #include <opencv2/highgui/highgui.hpp> 00033 00034 #include "ccny_rgbd/types.h" 00035 #include "ccny_rgbd/rgbd_util.h" 00036 #include "ccny_rgbd/structures/rgbd_frame.h" 00037 #include "ccny_rgbd/features/feature_detector.h" 00038 #include "ccny_rgbd/features/orb_detector.h" 00039 #include "ccny_rgbd/features/surf_detector.h" 00040 #include "ccny_rgbd/features/gft_detector.h" 00041 #include "ccny_rgbd/features/star_detector.h" 00042 00043 #include "ccny_rgbd/FeatureDetectorConfig.h" 00044 #include "ccny_rgbd/GftDetectorConfig.h" 00045 #include "ccny_rgbd/StarDetectorConfig.h" 00046 #include "ccny_rgbd/SurfDetectorConfig.h" 00047 #include "ccny_rgbd/OrbDetectorConfig.h" 00048 00049 namespace ccny_rgbd { 00050 00054 class FeatureViewer 00055 { 00056 public: 00057 00062 FeatureViewer(const ros::NodeHandle& nh, 00063 const ros::NodeHandle& nh_private); 00064 00067 virtual ~FeatureViewer(); 00068 00069 private: 00070 00071 // **** ROS-related 00072 00073 ros::NodeHandle nh_; 00074 ros::NodeHandle nh_private_; 00075 00076 ros::Publisher cloud_publisher_; 00077 ros::Publisher covariances_publisher_; 00078 00079 FeatureDetectorConfigServer config_server_; 00080 00081 GftDetectorConfigServerPtr gft_config_server_; 00082 StarDetectorConfigServerPtr star_config_server_; 00083 SurfDetectorConfigServerPtr surf_config_server_; 00084 OrbDetectorConfigServerPtr orb_config_server_; 00085 00087 boost::shared_ptr<ImageTransport> rgb_it_; 00088 00090 boost::shared_ptr<ImageTransport> depth_it_; 00091 00093 boost::shared_ptr<RGBDSynchronizer3> sync_; 00094 00096 ImageSubFilter sub_rgb_; 00097 00099 ImageSubFilter sub_depth_; 00100 00102 CameraInfoSubFilter sub_info_; 00103 00104 // **** parameters 00105 00114 std::string detector_type_; 00115 00116 int queue_size_; 00117 00122 bool show_keypoints_; 00123 00128 bool publish_cloud_; 00129 00134 bool publish_covariances_; 00135 00136 // **** variables 00137 00138 boost::mutex mutex_; 00139 int frame_count_; 00140 00141 FeatureDetectorPtr feature_detector_; 00142 00143 // **** private functions 00144 00151 void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg, 00152 const ImageMsg::ConstPtr& depth_msg, 00153 const CameraInfoMsg::ConstPtr& info_msg); 00154 00157 void initParams(); 00158 00161 void resetDetector(); 00162 00167 void publishFeatureCloud(RGBDFrame& frame); 00168 00173 void publishFeatureCovariances(RGBDFrame& frame); 00174 00179 void showKeypointImage(RGBDFrame& frame); 00180 00183 void reconfigCallback(FeatureDetectorConfig& config, uint32_t level); 00184 00187 void gftReconfigCallback(GftDetectorConfig& config, uint32_t level); 00188 00191 void starReconfigCallback(StarDetectorConfig& config, uint32_t level); 00192 00195 void surfReconfigCallback(SurfDetectorConfig& config, uint32_t level); 00196 00199 void orbReconfigCallback(OrbDetectorConfig& config, uint32_t level); 00200 }; 00201 00202 } //namespace ccny_rgbd 00203 00204 #endif // CCNY_RGBD_RGBD_FEATURE_VIEWER_H
