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);
      }

    }
    PlanarCoordinator () {
	ROS_DEBUG("Instantiating a PlanarCoordinator Class");
	// define subscribers, synchronizer, and the corresponding
	// callback:
	robot_kinect_sub.subscribe(node_, "robot_kinect_position", 10);
	mass_kinect_sub.subscribe(node_, "object1_position", 10);
	sync = new message_filters::TimeSynchronizer<PointPlus, PointPlus>
	    (robot_kinect_sub, mass_kinect_sub, 10);
	sync->registerCallback(boost::bind(
				   &PlanarCoordinator::callback, this, _1, _2));
	// define publisher
	config_pub = node_.advertise<PlanarSystemConfig> ("meas_config", 100);

	// wait for service, and get the starting config:
	ROS_INFO("Waiting for get_ref_config service:");
	ROS_INFO("Will not continue until available...");
	if(ros::service::waitForService("get_ref_config"))
	{
	    // service available:
	    PlanarSystemService::Request req;
	    PlanarSystemService::Response resp;
	    req.index = 0; //initial config:
	    ros::service::call("get_ref_config", req, resp);
	    q0 = resp.config;
	    ROS_DEBUG("Initial config:");
	    ROS_DEBUG("xm=%f, ym=%f, xr=%f, r=%f\r\n",q0.xm,q0.ym,q0.xr,q0.r);
	}
	else
	{
	    ROS_ERROR("Could not get service, shutting down");
	    ros::shutdown();
	}

	// set all default variables:
	calibrated_flag = false;
	calibrate_count = 0;
	
	
	return;
    }