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