openni_change_viewer.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *      
00034  * Author: Nico Blodow (blodow@cs.tum.edu), Julius Kammerl (julius@kammerl.de)
00035  */
00036 
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl/io/openni_grabber.h>
00040 #include <pcl/io/openni_grabber.h>
00041 #include <pcl/visualization/cloud_viewer.h>
00042 #include <pcl/octree/octree.h>
00043 #include <pcl/filters/extract_indices.h>
00044 
00045 #include <pcl/console/parse.h>
00046 
00047 enum 
00048 {
00049   REDDIFF_MODE,
00050   ONLYDIFF_MODE,
00051   MODE_COUNT
00052 };
00053 
00054 class OpenNIChangeViewer
00055 {
00056   public:
00057     OpenNIChangeViewer (double resolution, int mode, int noise_filter)
00058       : viewer ("PCL OpenNI Viewer")
00059     {
00060       octree = new pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA>(resolution);
00061       mode_ = mode;
00062       noise_filter_ = noise_filter;
00063     }
00064 
00065     void 
00066     cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
00067     {
00068       std::cerr << cloud->points.size() << " -- ";
00069 
00070       // assign point cloud to octree
00071       octree->setInputCloud (cloud);
00072 
00073       // add points from cloud to octree
00074       octree->addPointsFromInputCloud ();
00075 
00076       std::cerr << octree->getLeafCount() << " -- ";
00077       boost::shared_ptr<std::vector<int> > newPointIdxVector (new std::vector<int>);
00078 
00079       // get a vector of new points, which did not exist in previous buffer
00080       octree->getPointIndicesFromNewVoxels (*newPointIdxVector, noise_filter_);
00081 
00082       std::cerr << newPointIdxVector->size() << std::endl;
00083 
00084       pcl::PointCloud<pcl::PointXYZRGBA>::Ptr filtered_cloud;
00085 
00086       switch (mode_) 
00087       {
00088         case REDDIFF_MODE:
00089           filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA> (*cloud));
00090           filtered_cloud->points.reserve(newPointIdxVector->size());
00091 
00092           for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
00093             filtered_cloud->points[*it].rgba = 255<<16;
00094 
00095           if (!viewer.wasStopped())
00096             viewer.showCloud (filtered_cloud);
00097 
00098           break;
00099         case ONLYDIFF_MODE:
00100           filtered_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
00101 
00102           filtered_cloud->points.reserve(newPointIdxVector->size());
00103 
00104           for (std::vector<int>::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++)
00105             filtered_cloud->points.push_back(cloud->points[*it]);
00106 
00107 
00108           if (!viewer.wasStopped())
00109             viewer.showCloud (filtered_cloud);
00110           break;
00111       }
00112       
00113       // switch buffers - reset tree
00114       octree->switchBuffers ();
00115     }
00116     
00117     void 
00118     run ()
00119     {
00120       pcl::Grabber* interface = new pcl::OpenNIGrabber();
00121 
00122       boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = 
00123         boost::bind (&OpenNIChangeViewer::cloud_cb_, this, _1);
00124 
00125       boost::signals2::connection c = interface->registerCallback (f);
00126       
00127       interface->start ();
00128       
00129       while (!viewer.wasStopped())
00130       {
00131         boost::this_thread::sleep(boost::posix_time::seconds(1));
00132       }
00133 
00134       interface->stop ();
00135     }
00136 
00137     pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA> *octree;
00138     pcl::visualization::CloudViewer viewer;
00139 
00140     int mode_;
00141     int noise_filter_;
00142 };
00143 
00144 int 
00145 main (int argc, char* argv[])
00146 {
00147 
00148   std::cout << "Syntax is " << argv[0] << " [-r octree resolution] [-d] [-n noise_filter intensity] \n";
00149 
00150   int mode = REDDIFF_MODE;
00151   int noise_filter = 7;
00152   double resolution = 0.01;
00153 
00154   pcl::console::parse_argument (argc, argv, "-r", resolution);
00155 
00156   pcl::console::parse_argument (argc, argv, "-n", noise_filter);
00157 
00158   if (pcl::console::find_argument (argc, argv, "-d")>0) {
00159     mode = ONLYDIFF_MODE;
00160   }
00161 
00162 
00163   OpenNIChangeViewer v (resolution, mode, noise_filter);
00164   v.run ();
00165   return 0;
00166 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:59