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