String Series::toJson() const
{
PTree pt;
pt.put("id", id);
pt.put("key", key);
pt.put("name", name);
PTree ptags;
BOOST_FOREACH(const std::string tag, tags) {
PTree t;
t.put("", tag);
ptags.push_back(std::make_pair("", t));
}
void SurfaceSample::saveToXML(const std::string& filename, const std::vector<SurfaceSample>& samples) {
PTree tree;
// save all the samples!
BOOST_FOREACH(const SurfaceSample& sample, samples) {
PTree node;
node.put("Pos", XMLHelpers::QToString(Q(3, sample.transform.P()[0], sample.transform.P()[1], sample.transform.P()[2])));
RPY<> rpy(sample.transform.R());
node.put("RPY", XMLHelpers::QToString(Q(3, rpy[0], rpy[1], rpy[2])));
node.put("GraspW", boost::lexical_cast<std::string>(sample.graspW));
tree.add_child("SurfaceSamples.Sample", node);
}
/**
* @brief Constructor
*
* Register subscriptions.
*
* @param nh node handle reference
* @todo determine ideal queue size for subscribers and sync policy
*/
SurfaceTrackingNode(ros::NodeHandle &nh)
: it_(nh),
name_in_cld_("/camera/depth_registered/points"),
name_in_img_("/camera/rgb/image_color"),
name_in_seg_("/ni/depth_segmentation/depth_segmentation/map_image_gray"),
name_out_("/ni/depth_segmentation/surfaces/image"),
#if USE_IMAGE_TRANSPORT_SUBSCRIBER_FILTER
img_sub_(it_, name_in_img_, 30),
img_sub_seg_(it_, name_in_seg_, 30),
#else // USE_IMAGE_TRANSPORT_SUBSCRIBER_FILTER
img_sub_(nh, name_in_img_, 30),
img_sub_seg_(nh, name_in_seg_, 30),
#endif // USE_IMAGE_TRANSPORT_SUBSCRIBER_FILTER
cloud_sub_(nh, name_in_cld_, 30)
{
using namespace message_filters; // Subscriber, sync_policies
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
int queue_size = 30;
sync_ptr_ = new Synchronizer<MySyncPolicy>(MySyncPolicy(queue_size),
cloud_sub_,
img_sub_,
img_sub_seg_);
sync_ptr_->registerCallback(
boost::bind(
&SurfaceTrackingNode::callback, this, _1, _2, _3)
);
{ // 0
// Instantiate DepthMap layer
LayerConfig cfg;
LayerIONames io;
io.Input(DepthMap::KEY_INPUT_STIMULUS, name_in_cld_);
io.Output(DepthMap::KEY_OUTPUT_RESPONSE, "depth_map");
layers_.push_back(LayerFactoryNI::CreateShared("DepthMap", cfg, io));
}
{ // 1
// Instantiate Depth Gradient layer
LayerConfig cfg;
PTree p;
p.put(DepthGradient::PARAM_GRAD_WEIGHT, 0.5f);
cfg.Params(p);
LayerIONames io;
io.Input(DepthGradient::KEY_INPUT_STIMULUS, "depth_map");
io.Output(DepthGradient::KEY_OUTPUT_GRAD_X, "depth_grad_x");
io.Output(DepthGradient::KEY_OUTPUT_GRAD_Y, "depth_grad_y");
layers_.push_back(LayerFactoryNI::CreateShared("DepthGradient", cfg, io));
}
{ // 2
// Instantiate Depth gradient smoothing layer
// applied on vertical gradient component
LayerConfig cfg;
PTree p;
p.put(DepthGradientSmoothing::PARAM_APERTURE_SIZE, 5);
p.put(DepthGradientSmoothing::PARAM_BAND_1, 5);
p.put(DepthGradientSmoothing::PARAM_BAND_2, 14);
p.put(DepthGradientSmoothing::PARAM_FILTER_MODE, 2);
p.put(DepthGradientSmoothing::PARAM_MAX, 0.04f);
p.put(DepthGradientSmoothing::PARAM_SMOOTH_CENTER, 128);
p.put(DepthGradientSmoothing::PARAM_SMOOTH_FACTOR, 3);
p.put(DepthGradientSmoothing::PARAM_SMOOTH_MODE, 2 );
cfg.Params(p);
LayerIONames io;
io.Input(DepthGradientSmoothing::KEY_INPUT_STIMULUS, "depth_grad_y");
io.Output(DepthGradientSmoothing::KEY_OUTPUT_RESPONSE, "depth_grad_y_smooth");
layers_.push_back(LayerFactoryNI::CreateShared("DepthGradientSmoothing", cfg, io));
}
{ // 3
// Instantiate layer for rectifying smoothed gradient
// apply thresholds on raw gradient and have them reflect on smoothed component
LayerConfig cfg;
PTree p;
p.put(DepthGradientRectify::PARAM_MAX_GRAD, 0.04f); // paper = 0.04
cfg.Params(p);
LayerIONames io;
io.Input(DepthGradientRectify::KEY_INPUT_GRAD_X, "depth_grad_x");
io.Input(DepthGradientRectify::KEY_INPUT_GRAD_Y, "depth_grad_y");
io.Input(DepthGradientRectify::KEY_INPUT_GRAD_SMOOTH, "depth_grad_y_smooth");
io.Output(DepthGradientRectify::KEY_OUTPUT_RESPONSE, "depth_grad_y_smooth_r");
layers_.push_back(LayerFactoryNI::CreateShared("DepthGradientRectify", cfg, io));
}
{
// Instantiate Depth segmentation layer
// applied on smoothed vertical gradient component
LayerConfig cfg;
PTree params;
params.put(DepthSegmentation::PARAM_MAX_GRAD, 0.003f); // paper = 0.003
cfg.Params(params);
LayerIONames io;
io.Input(DepthSegmentation::KEY_INPUT_STIMULUS, "depth_grad_y_smooth_r");
io.Output(DepthSegmentation::KEY_OUTPUT_RESPONSE, "depth_seg_raw");
layers_.push_back(LayerFactoryNI::CreateShared("DepthSegmentation", cfg, io));
}
{
// Instantiate Map Area Filter layer for smoothing surfaces
// by merging small-sized surfaces together
// then merging them with largest neighbor
LayerConfig cfg;
PTree params;
params.put(MapAreaFilter::PARAM_TAU_SIZE, 200); // @todo adapt this threshold to higher resolution input
cfg.Params(params);
LayerIONames io;
io.Input(MapAreaFilter::KEY_INPUT_STIMULUS, "depth_seg_raw");
io.Output(MapAreaFilter::KEY_OUTPUT_RESPONSE, "map_gray_");
layers_.push_back(LayerFactoryNI::CreateShared("MapAreaFilter", cfg, io));
}
// {
// LayerConfig cfg;
// PTree p;
// p.put(SurfaceTracking::PARAM_HIST_BINS, 8);
// p.put(SurfaceTracking::PARAM_WEIGHT_COLOR, 0.4f);
// p.put(SurfaceTracking::PARAM_WEIGHT_POS, 0.1f);
// p.put(SurfaceTracking::PARAM_WEIGHT_SIZE, 0.5f);
// p.put(SurfaceTracking::PARAM_MAX_COLOR, 0.3f);
// p.put(SurfaceTracking::PARAM_MAX_POS, 0.15f);
// p.put(SurfaceTracking::PARAM_MAX_SIZE, 0.3f);
// p.put(SurfaceTracking::PARAM_MAX_DIST, 1.6f);
// cfg.Params(p);
// LayerIONames io;
// io.Input(SurfaceTracking::KEY_INPUT_BGR_IMAGE, name_in_img_);
// io.Input(SurfaceTracking::KEY_INPUT_CLOUD, name_in_cld_);
// io.Input(SurfaceTracking::KEY_INPUT_MAP, "map_gray_");
// io.Output(SurfaceTracking::KEY_OUTPUT_RESPONSE, name_out_);
// //io.Output(SurfaceTracking::KEY_OUTPUT_RESPONSE, "name_out_2");
// layers_.push_back(LayerFactoryNI::CreateShared("SurfaceTracking", cfg, io));
// }
{
LayerConfig cfg;
PTree p;
p.put(SurfaceTracking::PARAM_HIST_BINS, 8);
p.put(SurfaceTracking::PARAM_WEIGHT_COLOR, 0.4f);
p.put(SurfaceTracking::PARAM_WEIGHT_POS, 0.1f);
p.put(SurfaceTracking::PARAM_WEIGHT_SIZE, 0.5f);
p.put(SurfaceTracking::PARAM_MAX_COLOR, 0.3f);
p.put(SurfaceTracking::PARAM_MAX_POS, 0.15f);
p.put(SurfaceTracking::PARAM_MAX_SIZE, 0.3f);
p.put(SurfaceTracking::PARAM_MAX_DIST, 1.6f);
cfg.Params(p);
LayerIONames io;
io.Input(SurfaceTracking::KEY_INPUT_BGR_IMAGE, name_in_img_);
io.Input(SurfaceTracking::KEY_INPUT_CLOUD, name_in_cld_);
io.Input(SurfaceTracking::KEY_INPUT_MAP, name_in_seg_);
io.Output(SurfaceTracking::KEY_OUTPUT_RESPONSE, name_out_);
layers_.push_back(LayerFactoryNI::CreateShared("SurfaceTracking", cfg, io));
}
img_pub_ = it_.advertise(name_out_, 1);
}
BOOST_FOREACH(const MapSS::value_type &attr, attributes) {
pattributes.put(attr.first, attr.second);
}
int main() {
PTree<string> pt;
// normal get
string name("Alice");
pt.put("person.name", name);
string nameOut = pt.get("person.name"); // assignment syntax
assert(name == nameOut);
string nameEx(pt.get<string>("person.name")); // explicit syntax, need convert explicitly
assert(name == nameEx);
assert(pt.isValidPath("person.name"));
// const get
const PTree<string>& cpt = pt;
string nameOutC = cpt.get("person.name");
assert(name == nameOutC);
string nameExC;
nameExC = cpt.get<string>("person.name");
assert(name == nameExC);
const string& nameRefC = cpt.get("person.name");
assert(name == nameRefC);
// ref get
string& nameRef = pt.get("person.name");
nameRef = "Bob";
nameOut = pt.get<string>("person.name");
assert(nameRef == nameOut);
// other types
int age = 25;
pt.put("person.age", age);
int ageOut(pt.get("person.age"));
assert(age == ageOut);
float height = 1.65;
pt.put("person.height", height);
float heightOut(pt.get("person.height"));
assert(height == heightOut);
Car car;
car.color = "black";
car.price = 50000;
pt.put("person.car", car);
Car carOut(pt.get("person.car"));
assert(car.color == carOut.color);
assert(car.price == carOut.price);
// push as vector
int income = 100;
pt.push("person.deposit", income++);
pt.push("person.deposit", income++);
pt.push("person.deposit", income++);
const std::vector<int>& sum = pt.get("person.deposit");
assert(sum.size() == 3);
// del
string error;
pt.del("person.name");
try {
pt.get("person.name");
} catch (const PTreeError& e) {
error = e.what();
cout << "catched PTreeError: " << error << endl;
}
assert(!error.empty());
assert(!pt.isValidPath("person.name"));
cout << "all test passed" << endl;
return 0;
}
