00001 00024 #ifndef CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H 00025 #define CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H 00026 00027 #include <ros/ros.h> 00028 #include <sensor_msgs/PointCloud2.h> 00029 #include <geometry_msgs/PoseStamped.h> 00030 #include <tf/transform_listener.h> 00031 #include <tf/transform_broadcaster.h> 00032 00033 #include "ccny_rgbd/types.h" 00034 #include "ccny_rgbd/rgbd_util.h" 00035 #include "ccny_rgbd/structures/rgbd_frame.h" 00036 #include "ccny_rgbd/features/feature_detector.h" 00037 #include "ccny_rgbd/features/orb_detector.h" 00038 #include "ccny_rgbd/features/surf_detector.h" 00039 #include "ccny_rgbd/features/gft_detector.h" 00040 #include "ccny_rgbd/features/star_detector.h" 00041 #include "ccny_rgbd/registration/motion_estimation.h" 00042 #include "ccny_rgbd/registration/motion_estimation_icp.h" 00043 #include "ccny_rgbd/registration/motion_estimation_icp_prob_model.h" 00044 00045 #include "ccny_rgbd/FeatureDetectorConfig.h" 00046 #include "ccny_rgbd/GftDetectorConfig.h" 00047 #include "ccny_rgbd/StarDetectorConfig.h" 00048 #include "ccny_rgbd/SurfDetectorConfig.h" 00049 #include "ccny_rgbd/OrbDetectorConfig.h" 00050 00051 namespace ccny_rgbd { 00052 00061 class VisualOdometry 00062 { 00063 public: 00064 00069 VisualOdometry(const ros::NodeHandle& nh, 00070 const ros::NodeHandle& nh_private); 00071 00074 virtual ~VisualOdometry(); 00075 00076 private: 00077 00078 // **** ROS-related 00079 00080 ros::NodeHandle nh_; 00081 ros::NodeHandle nh_private_; 00082 tf::TransformListener tf_listener_; 00083 tf::TransformBroadcaster tf_broadcaster_; 00084 ros::Publisher odom_publisher_; 00085 ros::Publisher cloud_publisher_; 00086 ros::Publisher path_pub_; 00087 00088 GftDetectorConfigServerPtr gft_config_server_; 00089 StarDetectorConfigServerPtr star_config_server_; 00090 SurfDetectorConfigServerPtr surf_config_server_; 00091 OrbDetectorConfigServerPtr orb_config_server_; 00092 00094 boost::shared_ptr<ImageTransport> rgb_it_; 00095 00097 boost::shared_ptr<ImageTransport> depth_it_; 00098 00100 boost::shared_ptr<RGBDSynchronizer3> sync_; 00101 00103 ImageSubFilter sub_rgb_; 00104 00106 ImageSubFilter sub_depth_; 00107 00109 CameraInfoSubFilter sub_info_; 00110 00111 // **** parameters 00112 00113 std::string fixed_frame_; 00114 std::string base_frame_; 00115 bool publish_tf_; 00116 bool publish_path_; 00117 bool publish_odom_; 00118 00127 std::string detector_type_; 00128 00135 std::string reg_type_; 00136 00141 bool publish_cloud_; 00142 00143 int queue_size_; 00144 00145 // **** variables 00146 00147 bool initialized_; 00148 int frame_count_; 00149 ros::Time init_time_; 00150 00151 tf::Transform b2c_; 00152 tf::Transform f2b_; 00153 00154 boost::shared_ptr<FeatureDetector> feature_detector_; 00155 00156 MotionEstimation * motion_estimation_; 00157 00158 PathMsg path_msg_; 00159 00160 // **** private functions 00161 00168 void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg, 00169 const ImageMsg::ConstPtr& depth_msg, 00170 const CameraInfoMsg::ConstPtr& info_msg); 00171 00174 void initParams(); 00175 00178 void resetDetector(); 00179 00183 void publishTf(const std_msgs::Header& header); 00184 00189 void publishOdom(const std_msgs::Header& header); 00190 00194 void publishPath(const std_msgs::Header& header); 00195 00200 void publishFeatureCloud(RGBDFrame& frame); 00201 00205 bool getBaseToCameraTf(const std_msgs::Header& header); 00206 00209 void gftReconfigCallback(GftDetectorConfig& config, uint32_t level); 00210 00213 void starReconfigCallback(StarDetectorConfig& config, uint32_t level); 00214 00217 void surfReconfigCallback(SurfDetectorConfig& config, uint32_t level); 00218 00221 void orbReconfigCallback(OrbDetectorConfig& config, uint32_t level); 00222 }; 00223 00224 } // namespace ccny_rgbd 00225 00226 #endif // CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H
