/* Parse and emit initializer code for target variable in statements such as
* int b[] = {0, 1, 2, 3}. Generate a series of assignment operations on
* references to target variable.
*/
static struct block *initializer(struct block *block, struct var target)
{
assert(target.kind == DIRECT);
/* Do not care about cv-qualifiers here. */
target.type = unwrapped(target.type);
if (peek().token == '{') {
block = object_initializer(block, target);
} else {
block = assignment_expression(block);
if (!target.symbol->depth && block->expr.kind != IMMEDIATE) {
error("Initializer must be computable at load time.");
exit(1);
}
if (target.kind == DIRECT && !target.type->size) {
assert(!target.offset);
assert(is_string(block->expr));
assert(is_array(block->expr.type));
/* Complete type based on string literal. Evaluation does not have
* the required context to do this logic. */
((struct symbol *) target.symbol)->type.size =
block->expr.type->size;
target.type = block->expr.type;
}
eval_assign(block, target, block->expr);
}
return block;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "gaussian_tracker");
ros::NodeHandle nh("~");
/* ------------------------------ */
/* - Parameters - */
/* ------------------------------ */
// tracker's main parameter container
dbot::GaussianTrackerBuilder::Parameters params;
// camera data
dbot::CameraData::Resolution resolution;
std::string camera_info_topic;
std::string depth_image_topic;
int downsampling_factor;
// object data
std::string object_package;
std::string object_directory;
std::vector<std::string> object_meshes;
// parameter shorthand prefix
std::string pre = "gaussian_filter/";
/* ------------------------------ */
/* - Read out parameters - */
/* ------------------------------ */
// get object parameters
/// \todo nh.getParam does not check whether the parameter exists in the
/// config file. this is dangerous, we should use ri::read instead
nh.getParam("object/meshes", object_meshes);
nh.getParam("object/package", object_package);
nh.getParam("object/directory", object_directory);
params.ori.package_path(ros::package::getPath(object_package));
params.ori.directory(object_directory);
params.ori.meshes(object_meshes);
// get filter parameters
nh.getParam(pre + "unscented_transform/alpha", params.ut_alpha);
nh.getParam(pre + "moving_average_update_rate",
params.moving_average_update_rate);
nh.getParam(pre + "center_object_frame", params.center_object_frame);
nh.getParam(pre + "observation/tail_weight",
params.observation.tail_weight);
nh.getParam(pre + "observation/bg_depth", params.observation.bg_depth);
nh.getParam(pre + "observation/fg_noise_std",
params.observation.fg_noise_std);
nh.getParam(pre + "observation/bg_noise_std",
params.observation.bg_noise_std);
nh.getParam(pre + "observation/uniform_tail_max",
params.observation.uniform_tail_max);
nh.getParam(pre + "observation/uniform_tail_min",
params.observation.uniform_tail_min);
// state transition parameters
nh.getParam(pre + "object_transition/linear_sigma_x",
params.object_transition.linear_sigma_x);
nh.getParam(pre + "object_transition/linear_sigma_y",
params.object_transition.linear_sigma_y);
nh.getParam(pre + "object_transition/linear_sigma_z",
params.object_transition.linear_sigma_z);
nh.getParam(pre + "object_transition/angular_sigma_x",
params.object_transition.angular_sigma_x);
nh.getParam(pre + "object_transition/angular_sigma_y",
params.object_transition.angular_sigma_y);
nh.getParam(pre + "object_transition/angular_sigma_z",
params.object_transition.angular_sigma_z);
nh.getParam(pre + "object_transition/velocity_factor",
params.object_transition.velocity_factor);
params.object_transition.part_count = object_meshes.size();
// camera parameters
nh.getParam("camera_info_topic", camera_info_topic);
nh.getParam("depth_image_topic", depth_image_topic);
nh.getParam("downsampling_factor", downsampling_factor);
nh.getParam("resolution/width", resolution.width);
nh.getParam("resolution/height", resolution.height);
/* ------------------------------ */
/* - Setup camera data - */
/* ------------------------------ */
// setup camera data
auto camera_data_provider = std::shared_ptr<dbot::CameraDataProvider>(
new dbot::RosCameraDataProvider(nh,
camera_info_topic,
depth_image_topic,
resolution,
downsampling_factor,
2.0));
auto camera_data = std::make_shared<dbot::CameraData>(camera_data_provider);
// finally, set number of pixels
params.observation.sensors = camera_data->pixels();
/* ------------------------------ */
/* - Initialize interactively - */
/* ------------------------------ */
opi::InteractiveMarkerInitializer object_initializer(
camera_data->frame_id(),
params.ori.package(),
params.ori.directory(),
params.ori.meshes(),
{},
true);
if (!object_initializer.wait_for_object_poses())
{
ROS_INFO("Setting object poses was interrupted.");
return 0;
}
auto initial_ros_poses = object_initializer.poses();
std::vector<Tracker::State> initial_poses;
initial_poses.push_back(Tracker::State(params.ori.count_meshes()));
int i = 0;
for (auto& ros_pose : initial_ros_poses)
{
initial_poses[0].component(i++) = ri::to_pose_velocity_vector(ros_pose);
}
/* ------------------------------ */
/* - Create the tracker - */
/* ------------------------------ */
auto tracker_builder =
dbot::GaussianTrackerBuilder(params, camera_data);
auto tracker = tracker_builder.build();
tracker->initialize(initial_poses);
/* ------------------------------ */
/* - Tracker publisher - */
/* ------------------------------ */
int object_color[3];
nh.getParam(pre + "object_color/R", object_color[0]);
nh.getParam(pre + "object_color/G", object_color[1]);
nh.getParam(pre + "object_color/B", object_color[2]);
auto tracker_publisher = dbot::ObjectStatePublisher(
params.ori, object_color[0], object_color[1], object_color[2]);
/* ------------------------------ */
/* - Create and run tracker - */
/* - node - */
/* ------------------------------ */
dbot::ObjectTrackerRos<dbot::GaussianTracker>
ros_object_tracker(tracker, camera_data, params.ori.count_meshes());
ros::Subscriber subscriber =
nh.subscribe(depth_image_topic,
1,
&dbot::ObjectTrackerRos<
dbot::GaussianTracker>::update_obsrv,
&ros_object_tracker);
while (ros::ok())
{
if (ros_object_tracker.run_once())
{
tracker_publisher.publish(
ros_object_tracker.current_state_messages());
}
ros::spinOnce();
}
ros::spin();
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "particle_tracker");
ros::NodeHandle nh("~");
/* ---------------------------------------------------------------------- */
/* Roa-Blackwellized Coordinate Particle Filter Object Tracker */
/* */
/* Ingredients: */
/* - ObjectTrackerRos */
/* - Tracker */
/* - Rbc Particle Filter Algorithm */
/* - Objects tate transition model */
/* - Observation model */
/* - Object model */
/* - Camera data */
/* - Tracker publisher to advertise the estimated state */
/* */
/* Construnction of the tracker will utilize few builders and factories. */
/* For that, we need the following builders/factories: */
/* - Object state transition model builder */
/* - Observation model builder to build GPU or CPU based models */
/* - Filter builder */
/* ---------------------------------------------------------------------- */
// parameter shorthand prefix
std::string pre = "particle_filter/";
/* ------------------------------ */
/* - Create the object model - */
/* ------------------------------ */
// get object parameters
std::string object_package;
std::string object_directory;
std::vector<std::string> object_meshes;
/// \todo nh.getParam does not check whether the parameter exists in the
/// config file. this is dangerous, we should use ri::read instead
nh.getParam("object/meshes", object_meshes);
nh.getParam("object/package", object_package);
nh.getParam("object/directory", object_directory);
// Use the ORI to load the object model usign the
// SimpleWavefrontObjectLoader
dbot::ObjectResourceIdentifier ori;
ori.package_path(ros::package::getPath(object_package));
ori.directory(object_directory);
ori.meshes(object_meshes);
auto object_model_loader = std::shared_ptr<dbot::ObjectModelLoader>(
new dbot::SimpleWavefrontObjectModelLoader(ori));
// Load the model usign the simple wavefront load and center the frames
// of all object part meshes
bool center_object_frame;
nh.getParam(pre + "center_object_frame", center_object_frame);
auto object_model = std::make_shared<dbot::ObjectModel>(
object_model_loader, center_object_frame);
/* ------------------------------ */
/* - Setup camera data - */
/* ------------------------------ */
int downsampling_factor;
std::string camera_info_topic;
std::string depth_image_topic;
dbot::CameraData::Resolution resolution;
nh.getParam("camera_info_topic", camera_info_topic);
nh.getParam("depth_image_topic", depth_image_topic);
nh.getParam("downsampling_factor", downsampling_factor);
nh.getParam("resolution/width", resolution.width);
nh.getParam("resolution/height", resolution.height);
auto camera_data_provider = std::shared_ptr<dbot::CameraDataProvider>(
new dbot::RosCameraDataProvider(nh,
camera_info_topic,
depth_image_topic,
resolution,
downsampling_factor,
60.0));
// Create camera data from the RosCameraDataProvider which takes the data
// from a ros camera topic
auto camera_data = std::make_shared<dbot::CameraData>(camera_data_provider);
/* ------------------------------ */
/* - Few types we will be using - */
/* ------------------------------ */
typedef osr::FreeFloatingRigidBodiesState<> State;
typedef dbot::ParticleTracker Tracker;
typedef dbot::ParticleTrackerBuilder<Tracker> TrackerBuilder;
typedef TrackerBuilder::TransitionBuilder TransitionBuilder;
typedef TrackerBuilder::SensorBuilder SensorBuilder;
/* ------------------------------ */
/* - State transition function - */
/* ------------------------------ */
// We will use a linear observation model built by the object transition
// model builder. The linear model will generate a random walk.
dbot::ObjectTransitionBuilder<State>::Parameters params_state;
// state transition parameters
nh.getParam(pre + "object_transition/linear_sigma_x",
params_state.linear_sigma_x);
nh.getParam(pre + "object_transition/linear_sigma_y",
params_state.linear_sigma_y);
nh.getParam(pre + "object_transition/linear_sigma_z",
params_state.linear_sigma_z);
nh.getParam(pre + "object_transition/angular_sigma_x",
params_state.angular_sigma_x);
nh.getParam(pre + "object_transition/angular_sigma_y",
params_state.angular_sigma_y);
nh.getParam(pre + "object_transition/angular_sigma_z",
params_state.angular_sigma_z);
nh.getParam(pre + "object_transition/velocity_factor",
params_state.velocity_factor);
params_state.part_count = object_meshes.size();
auto state_trans_builder = std::shared_ptr<TransitionBuilder>(
new dbot::ObjectTransitionBuilder<State>(params_state));
/* ------------------------------ */
/* - Observation model - */
/* ------------------------------ */
dbot::RbSensorBuilder<State>::Parameters params_obsrv;
nh.getParam(pre + "use_gpu", params_obsrv.use_gpu);
if (params_obsrv.use_gpu)
{
nh.getParam(pre + "gpu/sample_count", params_obsrv.sample_count);
}
else
{
nh.getParam(pre + "cpu/sample_count", params_obsrv.sample_count);
}
nh.getParam(pre + "observation/occlusion/p_occluded_visible",
params_obsrv.occlusion.p_occluded_visible);
nh.getParam(pre + "observation/occlusion/p_occluded_occluded",
params_obsrv.occlusion.p_occluded_occluded);
nh.getParam(pre + "observation/occlusion/initial_occlusion_prob",
params_obsrv.occlusion.initial_occlusion_prob);
nh.getParam(pre + "observation/kinect/tail_weight",
params_obsrv.kinect.tail_weight);
nh.getParam(pre + "observation/kinect/model_sigma",
params_obsrv.kinect.model_sigma);
nh.getParam(pre + "observation/kinect/sigma_factor",
params_obsrv.kinect.sigma_factor);
params_obsrv.delta_time = 1. / 30.;
// gpu only parameters
nh.getParam(pre + "gpu/use_custom_shaders",
params_obsrv.use_custom_shaders);
nh.getParam(pre + "gpu/vertex_shader_file",
params_obsrv.vertex_shader_file);
nh.getParam(pre + "gpu/fragment_shader_file",
params_obsrv.fragment_shader_file);
nh.getParam(pre + "gpu/geometry_shader_file",
params_obsrv.geometry_shader_file);
auto sensor_builder =
std::shared_ptr<SensorBuilder>(new dbot::RbSensorBuilder<State>(
object_model, camera_data, params_obsrv));
/* ------------------------------ */
/* - Create Filter & Tracker - */
/* ------------------------------ */
TrackerBuilder::Parameters params_tracker;
params_tracker.evaluation_count = params_obsrv.sample_count;
nh.getParam(pre + "moving_average_update_rate",
params_tracker.moving_average_update_rate);
nh.getParam(pre + "max_kl_divergence", params_tracker.max_kl_divergence);
nh.getParam(pre + "center_object_frame",
params_tracker.center_object_frame);
auto tracker_builder = dbot::ParticleTrackerBuilder<Tracker>(
state_trans_builder, sensor_builder, object_model, params_tracker);
auto tracker = tracker_builder.build();
dbot::ObjectTrackerRos<Tracker> ros_object_tracker(
tracker, camera_data, ori.count_meshes());
/* ------------------------------ */
/* - Initialize interactively - */
/* ------------------------------ */
opi::InteractiveMarkerInitializer object_initializer(
camera_data->frame_id(),
ori.package(),
ori.directory(),
ori.meshes(),
{},
true);
if (!object_initializer.wait_for_object_poses())
{
ROS_INFO("Setting object poses was interrupted.");
return 0;
}
auto initial_ros_poses = object_initializer.poses();
std::vector<Tracker::State> initial_poses;
initial_poses.push_back(Tracker::State(ori.count_meshes()));
int i = 0;
for (auto& ros_pose : initial_ros_poses)
{
initial_poses[0].component(i++) = ri::to_pose_velocity_vector(ros_pose);
}
tracker->initialize(initial_poses);
/* ------------------------------ */
/* - Tracker publisher - */
/* ------------------------------ */
int object_color[3];
nh.getParam(pre + "object_color/R", object_color[0]);
nh.getParam(pre + "object_color/G", object_color[1]);
nh.getParam(pre + "object_color/B", object_color[2]);
auto tracker_publisher = dbot::ObjectStatePublisher(
ori, object_color[0], object_color[1], object_color[2]);
/* ------------------------------ */
/* - Run the tracker - */
/* ------------------------------ */
ros::Subscriber subscriber =
nh.subscribe(depth_image_topic,
1,
&dbot::ObjectTrackerRos<Tracker>::update_obsrv,
&ros_object_tracker);
(void)subscriber;
ros::AsyncSpinner spinner(2);
spinner.start();
while (ros::ok())
{
if (ros_object_tracker.run_once())
{
tracker_publisher.publish(
ros_object_tracker.current_state_messages());
}
}
return 0;
}
