← Back to branch summary

~shadowrobot/sr-taco/feature-sr-pcl-tracking

« back to all changes in this revision

Viewing changes to sr_taco_openni/src/TacoOpenNI.cpp

  • Committer: markpitchless
  • Date: 2012-11-22 16:55:13 UTC
  • Revision ID: markpitchless@gmail.com-20121122165513-lnzecneqvya8b2m8
Add a z filter

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_taco_openni/src/TacoOpenNI.cpp
34
34
    {
35
35
        nh_home.param<string>("camera", camera, "camera");
36
36
        nh_home.param<double>("downsampling_grid_size", downsampling_grid_size_, 0.01);
 
37
        nh_home.param<double>("filter_z_min", filter_z_min_, 0.0);
 
38
        nh_home.param<double>("filter_z_max", filter_z_max_, 10.0);
 
39
 
37
40
        saliency_map_spatial = boost::shared_ptr<sensor_msgs::Image>(new sensor_msgs::Image());
38
41
 
39
42
        // Setup a callback with the point cloud and camera info in sync.
64
67
                       const sensor_msgs::CameraInfo::ConstPtr& info)
65
68
    {
66
69
        CloudPtr input_cloud(new Cloud);
 
70
        CloudPtr tmp_cloud(new Cloud);
67
71
        pcl::fromROSMsg(*cloud, *input_cloud);
68
72
        ROS_INFO_STREAM_ONCE("input_cloud: " << *input_cloud);
69
73
 
 
74
        // Do some z filtering, to reduce number points and focus on foreground
 
75
        pcl::PassThrough<PointType> pass;
 
76
        pass.setFilterFieldName ("z");
 
77
        pass.setFilterLimits (filter_z_min_, filter_z_max_);
 
78
        pass.setKeepOrganized (false);
 
79
        pass.setInputCloud (input_cloud);
 
80
        pass.filter (*tmp_cloud);
 
81
 
70
82
        // Downsample the point cloud to speed up processing
71
83
        target_cloud_.reset(new Cloud);
72
84
        pcl::VoxelGrid<PointType> grid;
73
 
        grid.setInputCloud(input_cloud);
 
85
        grid.setInputCloud(tmp_cloud);
74
86
        grid.setLeafSize(downsampling_grid_size_, downsampling_grid_size_, downsampling_grid_size_);
75
87
        grid.filter(*target_cloud_);
 
88
 
76
89
        ROS_INFO_STREAM_ONCE("target_cloud: " << *target_cloud_);
77
90
 
78
91
        // No foveation yet so pub the same cloud twice