示例#1
0
  StereoVslamNode(const std::string& vocab_tree_file, const std::string& vocab_weights_file,
                  const std::string& calonder_trees_file)
    : it_(nh_), sync_(3),
      vslam_system_(vocab_tree_file, vocab_weights_file),
      detector_(new vslam_system::AnyDetector)
  {
    // Use calonder descriptor
    typedef cv::CalonderDescriptorExtractor<float> Calonder;
    vslam_system_.frame_processor_.setFrameDescriptor(new Calonder(calonder_trees_file));
    
    // Advertise outputs
    cam_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/cameras", 1);
    point_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/points", 1);
    vo_tracks_pub_ = it_.advertiseCamera("/vslam/vo_tracks/image", 1);
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/vo", 1);
    pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/vslam/pointcloud", 1);

    // Synchronize inputs
    l_image_sub_.subscribe(it_, "left/image_rect", 1);
    l_info_sub_ .subscribe(nh_, "left/camera_info", 1);
    r_image_sub_.subscribe(it_, "right/image_rect", 1);
    r_info_sub_ .subscribe(nh_, "right/camera_info", 1);
    sync_.connectInput(l_image_sub_, l_info_sub_, r_image_sub_, r_info_sub_);
    sync_.registerCallback( boost::bind(&StereoVslamNode::imageCb, this, _1, _2, _3, _4) );

    // Dynamic reconfigure for parameters
    ReconfigureServer::CallbackType f = boost::bind(&StereoVslamNode::configCb, this, _1, _2);
    reconfigure_server_.setCallback(f);

  }
    /////////////////////////////////////////////////////////////////
    // Constructor
    PedestrianDetectorHOG(ros::NodeHandle nh): 
      nh_(nh), 
      local_nh_("~"), 
      it_(nh_), 
      stereo_sync_(4),
      //      cam_model_(NULL), 
      counter(0) 
    {
      
      hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
      
      // Get parameters from the server
      local_nh_.param("hit_threshold",hit_threshold_,0.0);
      local_nh_.param("group_threshold",group_threshold_,2);
      local_nh_.param("use_depth", use_depth_, true);
      local_nh_.param("use_height",use_height_,true);
      local_nh_.param("max_height_m",max_height_m_,2.2);
      local_nh_.param("ground_frame",ground_frame_,std::string("base_link"));
      local_nh_.param("do_display", do_display_, true);
       
      // Advertise a 3d position measurement for each head.
      people_pub_ = nh_.advertise<people_msgs::PositionMeasurement>("people_tracker_measurements",1);

      // Subscribe to tf & the images  
      if (use_depth_) {  

	tf_.setExtrapolationLimit(ros::Duration().fromSec(0.01));

	std::string left_topic = nh_.resolveName("stereo") + "/left/image";
	std::string disp_topic = nh_.resolveName("stereo") + "/disparity";
	std::string left_info_topic = nh_.resolveName("stereo") + "/left/camera_info";
	std::string right_info_topic = nh_.resolveName("stereo") + "/right/camera_info";
	left_sub_.subscribe(it_, left_topic, 10);
	disp_sub_.subscribe(it_, disp_topic, 10);
	left_info_sub_.subscribe(nh_, left_info_topic, 10);
	right_info_sub_.subscribe(nh_, right_info_topic, 10);
	stereo_sync_.connectInput(left_sub_, left_info_sub_, disp_sub_, right_info_sub_);
	stereo_sync_.registerCallback(boost::bind(&PedestrianDetectorHOG::imageCBAll, this, _1, _2, _3, _4));
      }
      else {
	std::string topic = nh_.resolveName("image");
	image_sub_ = it_.subscribe("image",10,&PedestrianDetectorHOG::imageCB,this);
      }

    }