visual_odometry.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H
#define CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H

#include <ros/ros.h>
#include <ros/publisher.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <pcl_ros/point_cloud.h>
#include <rgbdtools/rgbdtools.h>


#include "ccny_rgbd/types.h"
#include "ccny_rgbd/util.h"
#include "ccny_rgbd/FeatureDetectorConfig.h"
#include "ccny_rgbd/GftDetectorConfig.h"
#include "ccny_rgbd/StarDetectorConfig.h"
#include "ccny_rgbd/SurfDetectorConfig.h"
#include "ccny_rgbd/OrbDetectorConfig.h"
#include "ccny_rgbd/Save.h"
#include "ccny_rgbd/Load.h"
namespace ccny_rgbd {

class VisualOdometry
{
  public:

    VisualOdometry(const ros::NodeHandle& nh, 
                   const ros::NodeHandle& nh_private);
    
    virtual ~VisualOdometry();

  private:

    // **** ROS-related

    ros::NodeHandle nh_;                
    ros::NodeHandle nh_private_;        
    tf::TransformListener tf_listener_; 
    tf::TransformBroadcaster tf_broadcaster_; 
    ros::Publisher odom_publisher_;           
    ros::Publisher pose_stamped_publisher_;   
    ros::Publisher path_pub_;                 

    ros::Publisher feature_cloud_publisher_;     
    ros::Publisher feature_cov_publisher_;  
    ros::Publisher model_cloud_publisher_;       
    ros::Publisher model_cov_publisher_;         
    ros::ServiceServer reset_pos_;
    FILE * diagnostics_file_;           
    std::string diagnostics_file_name_; 
    bool save_diagnostics_;              
    bool verbose_;                      
    
    GftDetectorConfigServerPtr gft_config_server_;    
    StarDetectorConfigServerPtr star_config_server_;  
    SurfDetectorConfigServerPtr surf_config_server_;  
    OrbDetectorConfigServerPtr orb_config_server_;    
        
    boost::shared_ptr<ImageTransport> rgb_it_;
    
    boost::shared_ptr<ImageTransport> depth_it_;
    
    boost::shared_ptr<RGBDSynchronizer3> sync_;
          
    ImageSubFilter      sub_rgb_;
    
    ImageSubFilter      sub_depth_;  
   
    CameraInfoSubFilter sub_info_;

    // **** parameters 

    std::string fixed_frame_; 
    std::string base_frame_;  
    bool publish_tf_;         
    bool publish_path_;       
    bool publish_odom_;       
    bool publish_pose_;       

    bool publish_feature_cloud_;
    bool publish_feature_cov_; 

    bool publish_model_cloud_;
    bool publish_model_cov_;    
    
    std::string detector_type_;
    
    std::string reg_type_;

    bool publish_cloud_; 
    
    int queue_size_;  
    
    // **** variables

    bool initialized_; 
    int  frame_count_; 
    ros::Time init_time_; 

    tf::Transform b2c_;  
    tf::Transform f2b_;  

    boost::shared_ptr<rgbdtools::FeatureDetector> feature_detector_; 

    rgbdtools::MotionEstimationICPProbModel motion_estimation_; 
  
    PathMsg path_msg_; 

    // **** private functions
    
    void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg,
                      const ImageMsg::ConstPtr& depth_msg,
                      const CameraInfoMsg::ConstPtr& info_msg);

    void initParams();
    bool resetposSrvCallback(Save::Request& request,Save::Response& response);

    void resetDetector();
    
    void publishTf(const std_msgs::Header& header);
    
    void publishOdom(const std_msgs::Header& header); 

    void publishPoseStamped(const std_msgs::Header& header); 

    void publishPath(const std_msgs::Header& header);
    
    void publishFeatureCloud(rgbdtools::RGBDFrame& frame);

    void publishFeatureCovariances(rgbdtools::RGBDFrame& frame);

    void publishModelCloud();

    void publishModelCovariances();

    bool getBaseToCameraTf(const std_msgs::Header& header);
    
    void gftReconfigCallback(GftDetectorConfig& config, uint32_t level);
    
    void starReconfigCallback(StarDetectorConfig& config, uint32_t level);
    
    void surfReconfigCallback(SurfDetectorConfig& config, uint32_t level);
    
    void orbReconfigCallback(OrbDetectorConfig& config, uint32_t level);

    void diagnostics(
      int n_features, int n_valid_features, int n_model_pts,
      double d_frame, double d_features, double d_reg, double d_total);
      
    void configureMotionEstimation();
};

} // namespace ccny_rgbd

#endif // CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H