openni_octree_compression.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: Julius Kammerl (julius@kammerl.de)
00035  */
00036 
00037 
00038 #include <pcl/point_cloud.h>
00039 #include <pcl/point_types.h>
00040 #include <pcl/io/openni_grabber.h>
00041 #include <pcl/visualization/cloud_viewer.h>
00042 #include <pcl/console/parse.h>
00043 #include <pcl/common/time.h>
00044 #include <pcl/filters/passthrough.h>
00045 
00046 #include <pcl/compression/octree_pointcloud_compression.h>
00047 
00048 #include <iostream>
00049 #include <vector>
00050 #include <stdio.h>
00051 #include <sstream>
00052 #include <stdlib.h>
00053 #include "stdio.h"
00054 
00055 #include <iostream>
00056 #include <string>
00057 
00058 #include <boost/asio.hpp>
00059 
00060 using boost::asio::ip::tcp;
00061 
00062 using namespace pcl;
00063 using namespace pcl::io;
00064 
00065 using namespace std;
00066 
00067 char usage[] = "\n"
00068   "  PCL octree point cloud compression\n"
00069   "\n"
00070   "  usage: ./pcl_openni_octree_compression [mode] [profile] [parameters]\n"
00071   "\n"
00072   "  I/O: \n"
00073   "      -f file  : file name \n"
00074   "\n"
00075   "  file compression mode:\n"
00076   "      -x: encode point cloud stream to file\n"
00077   "      -d: decode from file and display point cloud stream\n"
00078   "\n"
00079   "  network streaming mode:\n"
00080   "      -s       : start server on localhost\n"
00081   "      -c host  : connect to server and display decoded cloud stream\n"
00082   "\n"
00083   "  optional compression profile: \n"
00084   "      -p profile : select compression profile:       \n"
00085   "                     -\"lowC\"  Low resolution with color\n"
00086   "                     -\"lowNC\" Low resolution without color\n"
00087   "                     -\"medC\" Medium resolution with color\n"
00088   "                     -\"medNC\" Medium resolution without color\n"
00089   "                     -\"highC\" High resolution with color\n"
00090   "                     -\"highNC\" High resolution without color\n"
00091   "\n"
00092   "  optional compression parameters:\n"
00093   "      -r prec  : point precision Hz\n"
00094   "      -o prec  : octree voxel size\n"
00095   "      -v       : enable voxel-grid downsampling\n"
00096   "      -a       : enable color coding\n"
00097   "      -i rate  : i-frame rate\n"
00098   "      -b bits  : bits/color component\n"
00099   "      -t       : output statistics\n"
00100   "      -e       : show input cloud during encoding\n"
00101   "\n"
00102   "      -minmax min-max  :: set the PassThrough min-max cutting values (default: 0-3.0)\n"
00103   "      -field  X        :: set the PassThrough field/dimension 'X' to filter data on (default: 'z')\n"
00104   "\n"
00105   "  example:\n"
00106   "      ./pcl_openni_octree_compression -x -p highC -t -f pc_compressed.pcc \n"
00107   "\n";
00108 
00109 #define FPS_CALC(_WHAT_) \
00110 do \
00111 { \
00112     static unsigned count = 0;\
00113     static double last = pcl::getTime ();\
00114     double now = pcl::getTime (); \
00115     ++count; \
00116     if (now - last >= 1.0) \
00117     { \
00118       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
00119       count = 0; \
00120       last = now; \
00121     } \
00122 }while(false)
00123 
00124 void
00125 print_usage (std::string msg)
00126 {
00127   std::cerr << msg << std::endl;
00128   std::cout << usage << std::endl;
00129 }
00130 
00131 class SimpleOpenNIViewer
00132 {
00133   public:
00134     SimpleOpenNIViewer (ostream& outputFile_arg, OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_arg) :
00135       viewer ("Input Point Cloud - PCL Compression Viewer"),
00136       outputFile_(outputFile_arg), octreeEncoder_(octreeEncoder_arg)
00137     {
00138     }
00139 
00140     void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
00141     {
00142       if (!viewer.wasStopped ())
00143       {
00144         PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA>);
00145 
00146         octreeEncoder_->encodePointCloud ( cloud, outputFile_);
00147 
00148         viewer.showCloud (cloud);
00149       }
00150     }
00151 
00152     void run ()
00153     {
00154 
00155       // create a new grabber for OpenNI devices
00156       pcl::Grabber* interface = new pcl::OpenNIGrabber();
00157 
00158       // make callback function from member function
00159       boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
00160         boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
00161 
00162       // connect callback function for desired signal. In this case its a point cloud with color values
00163       boost::signals2::connection c = interface->registerCallback (f);
00164 
00165       // start receiving point clouds
00166       interface->start ();
00167 
00168 
00169       while (!outputFile_.fail())
00170       {
00171         boost::this_thread::sleep(boost::posix_time::seconds(1));
00172       }
00173 
00174       interface->stop ();
00175     }
00176 
00177     pcl::visualization::CloudViewer viewer;
00178     ostream& outputFile_;
00179     OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_;
00180 };
00181 
00182 struct EventHelper
00183 {
00184   EventHelper (ostream& outputFile_arg, OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_arg,
00185                const std::string& field_name = "z", float min_v = 0, float max_v = 3.0) :
00186     outputFile_ (outputFile_arg), octreeEncoder_ (octreeEncoder_arg)
00187   {
00188       pass_.setFilterFieldName (field_name);
00189       pass_.setFilterLimits (min_v, max_v);
00190   }
00191 
00192   void
00193   cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
00194   {
00195     if (!outputFile_.fail ())
00196     {
00197       PointCloud<PointXYZRGBA>::Ptr cloud_out (new PointCloud<PointXYZRGBA>);
00198 
00199       // Use a PassThrough filter to clean NaNs and remove data which is not interesting
00200       pass_.setInputCloud (cloud);
00201       pass_.filter (*cloud_out);
00202  
00203       octreeEncoder_->encodePointCloud (cloud_out, outputFile_);
00204     }
00205   }
00206 
00207   void
00208   run ()
00209   {
00210     // create a new grabber for OpenNI devices
00211     pcl::Grabber* interface = new pcl::OpenNIGrabber ();
00212 
00213     // make callback function from member function
00214     boost::function<void
00215     (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&EventHelper::cloud_cb_, this, _1);
00216 
00217     // connect callback function for desired signal. In this case its a point cloud with color values
00218     boost::signals2::connection c = interface->registerCallback (f);
00219 
00220     // start receiving point clouds
00221     interface->start ();
00222 
00223     while (!outputFile_.fail ())
00224     {
00225       boost::this_thread::sleep(boost::posix_time::seconds(1));
00226     }
00227 
00228     interface->stop ();
00229   }
00230 
00231   pcl::PassThrough<PointXYZRGBA> pass_;
00232   ostream& outputFile_;
00233   OctreePointCloudCompression<PointXYZRGBA>* octreeEncoder_;
00234 };
00235 
00236 int
00237 main (int argc, char **argv)
00238 {
00239   OctreePointCloudCompression<PointXYZRGBA>* octreeCoder;
00240 
00241   pcl::io::compression_Profiles_e compressionProfile;
00242 
00243   bool showStatistics;
00244   double pointResolution;
00245   float octreeResolution;
00246   bool doVoxelGridDownDownSampling;
00247   unsigned int iFrameRate;
00248   bool doColorEncoding;
00249   unsigned int colorBitResolution;
00250 
00251   bool bShowInputCloud;
00252 
00253   // default values
00254   showStatistics = false;
00255   pointResolution = 0.001;
00256   octreeResolution = 0.01f;
00257   doVoxelGridDownDownSampling = false;
00258   iFrameRate = 30;
00259   doColorEncoding = false;
00260   colorBitResolution = 6;
00261   compressionProfile = pcl::io::MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00262 
00263   bShowInputCloud = false;
00264 
00265   std::string fileName = "pc_compressed.pcc";
00266   std::string hostName = "localhost";
00267 
00268   bool bServerFileMode;
00269   bool bEnDecode;
00270   bool validArguments;
00271 
00272   validArguments = false;
00273   bServerFileMode = false;
00274   bEnDecode = false;
00275 
00276   float min_v = 0.0f, max_v = 3.0f;
00277   pcl::console::parse_2x_arguments (argc, argv, "-minmax", min_v, max_v, false);
00278   std::string field_name ("z");
00279   pcl::console::parse_argument (argc, argv, "-field", field_name);
00280 
00281   if (pcl::console::find_argument (argc, argv, "-e")>0) 
00282     bShowInputCloud = true;
00283 
00284   if (pcl::console::find_argument (argc, argv, "-s")>0) 
00285   {
00286     bEnDecode = true;
00287     bServerFileMode = true;
00288     validArguments = true;
00289   }
00290 
00291   if (pcl::console::parse_argument (argc, argv, "-c", hostName)>0) 
00292   {
00293     bEnDecode = false;
00294     bServerFileMode = true;
00295     validArguments = true;
00296   }
00297 
00298   if (pcl::console::find_argument (argc, argv, "-x")>0) 
00299   {
00300     bEnDecode = true;
00301     bServerFileMode = false;
00302     validArguments = true;
00303   }
00304 
00305   if (pcl::console::find_argument (argc, argv, "-d")>0) 
00306   {
00307     bEnDecode = false;
00308     bServerFileMode = false;
00309     validArguments = true;
00310   }
00311 
00312   if (pcl::console::find_argument (argc, argv, "-t")>0) 
00313     showStatistics = true;
00314 
00315   if (pcl::console::find_argument (argc, argv, "-a")>0) 
00316   {
00317     doColorEncoding = true;
00318     compressionProfile = pcl::io::MANUAL_CONFIGURATION;
00319   }
00320 
00321   if (pcl::console::find_argument (argc, argv, "-v")>0) 
00322   {
00323     doVoxelGridDownDownSampling = true;
00324     compressionProfile = pcl::io::MANUAL_CONFIGURATION;
00325   }
00326 
00327   pcl::console::parse_argument (argc, argv, "-f", fileName);
00328   pcl::console::parse_argument (argc, argv, "-r", pointResolution);
00329   pcl::console::parse_argument (argc, argv, "-i", iFrameRate);
00330   pcl::console::parse_argument (argc, argv, "-o", octreeResolution);
00331   pcl::console::parse_argument (argc, argv, "-b", colorBitResolution);
00332 
00333   std::string profile;
00334   if (pcl::console::parse_argument (argc, argv, "-p", profile)>0)
00335   {
00336     if (profile == "lowC")
00337       compressionProfile = pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR;
00338     else if (profile == "lowNC")
00339       compressionProfile = pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00340     else if (profile == "medC")
00341       compressionProfile = pcl::io::MED_RES_OFFLINE_COMPRESSION_WITH_COLOR;
00342     else if (profile =="medNC")
00343       compressionProfile = pcl::io::MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00344     else if (profile == "highC")
00345       compressionProfile = pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR;
00346     else if (profile == "highNC")
00347       compressionProfile = pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR;
00348     else
00349     {
00350       print_usage ("Unknown profile parameter..\n");
00351       return -1;
00352     }
00353 
00354     if (compressionProfile != MANUAL_CONFIGURATION)
00355     {
00356       // apply selected compression profile
00357       // retrieve profile settings
00358       const pcl::io::configurationProfile_t selectedProfile = pcl::io::compressionProfiles_[compressionProfile];
00359 
00360       // apply profile settings
00361       pointResolution = selectedProfile.pointResolution;
00362       octreeResolution = float (selectedProfile.octreeResolution);
00363       doVoxelGridDownDownSampling = selectedProfile.doVoxelGridDownSampling;
00364       iFrameRate = selectedProfile.iFrameRate;
00365       doColorEncoding = selectedProfile.doColorEncoding;
00366       colorBitResolution = selectedProfile.colorBitResolution;
00367     }
00368   }
00369 
00370   if (pcl::console::find_argument (argc, argv, "-?")>0) 
00371   {
00372     print_usage ("");
00373     return 1;
00374   }
00375 
00376   if (!validArguments)
00377   {
00378     print_usage ("Please specify compression mode..\n");
00379     return -1;
00380   }
00381 
00382   octreeCoder = new OctreePointCloudCompression<PointXYZRGBA> (compressionProfile, showStatistics, pointResolution,
00383                                                                octreeResolution, doVoxelGridDownDownSampling, iFrameRate,
00384                                                                doColorEncoding, static_cast<unsigned char> (colorBitResolution));
00385 
00386 
00387   if (!bServerFileMode) 
00388   {
00389     if (bEnDecode) 
00390     {
00391       // ENCODING
00392       ofstream compressedPCFile;
00393       compressedPCFile.open (fileName.c_str(), ios::out | ios::trunc | ios::binary);
00394 
00395       if (!bShowInputCloud) 
00396       {
00397         EventHelper v (compressedPCFile, octreeCoder, field_name, min_v, max_v);
00398         v.run ();
00399       } 
00400       else
00401       {
00402         SimpleOpenNIViewer v (compressedPCFile, octreeCoder);
00403         v.run ();
00404       }
00405 
00406     } else
00407     {
00408       // DECODING
00409       ifstream compressedPCFile;
00410       compressedPCFile.open (fileName.c_str(), ios::in | ios::binary);
00411       compressedPCFile.seekg (0);
00412       compressedPCFile.unsetf (ios_base::skipws);
00413 
00414       pcl::visualization::CloudViewer viewer ("PCL Compression Viewer");
00415 
00416       while (!compressedPCFile.eof())
00417       {
00418         PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA> ());
00419         octreeCoder->decodePointCloud ( compressedPCFile, cloudOut);
00420         viewer.showCloud (cloudOut);
00421       }
00422     }
00423   } else
00424   {
00425     // switch to ONLINE profiles
00426     if (compressionProfile == pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR)
00427       compressionProfile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITH_COLOR;
00428     else if (compressionProfile == pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR)
00429       compressionProfile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
00430     else if (compressionProfile == pcl::io::MED_RES_OFFLINE_COMPRESSION_WITH_COLOR)
00431       compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
00432     else if (compressionProfile == pcl::io::MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR)
00433       compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
00434     else if (compressionProfile == pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR)
00435       compressionProfile = pcl::io::HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR;
00436     else if (compressionProfile == pcl::io::HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR)
00437       compressionProfile = pcl::io::HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
00438 
00439     if (bEnDecode)
00440     {
00441       // ENCODING
00442       try
00443       {
00444         boost::asio::io_service io_service;
00445         tcp::endpoint endpoint (tcp::v4 (), 6666);
00446         tcp::acceptor acceptor (io_service, endpoint);
00447 
00448         tcp::iostream socketStream;
00449 
00450         std::cout << "Waiting for connection.." << std::endl;
00451 
00452         acceptor.accept (*socketStream.rdbuf ());
00453 
00454         std::cout << "Connected!" << std::endl;
00455 
00456         if (!bShowInputCloud) 
00457         {
00458           EventHelper v (socketStream, octreeCoder, field_name, min_v, max_v);
00459           v.run ();
00460         } 
00461         else
00462         {
00463           SimpleOpenNIViewer v (socketStream, octreeCoder);
00464           v.run ();
00465         }
00466 
00467         std::cout << "Disconnected!" << std::endl;
00468 
00469         boost::this_thread::sleep(boost::posix_time::seconds(3));
00470 
00471       }
00472       catch (std::exception& e)
00473       {
00474         std::cerr << e.what () << std::endl;
00475       }
00476     }
00477     else
00478     {
00479       // DECODING
00480       std::cout << "Connecting to: " << hostName << ".." << std::endl;
00481 
00482       try
00483       {
00484         tcp::iostream socketStream (hostName.c_str (), "6666");
00485 
00486         std::cout << "Connected!" << std::endl;
00487 
00488         pcl::visualization::CloudViewer viewer ("Decoded Point Cloud - PCL Compression Viewer");
00489 
00490         while (!socketStream.fail()) 
00491         {
00492           FPS_CALC ("drawing");
00493           PointCloud<PointXYZRGBA>::Ptr cloudOut (new PointCloud<PointXYZRGBA> ());
00494           octreeCoder->decodePointCloud (socketStream, cloudOut);
00495           viewer.showCloud (cloudOut);
00496         }
00497 
00498       }
00499       catch (std::exception& e)
00500       {
00501         std::cout << "Exception: " << e.what () << std::endl;
00502       }
00503     }
00504   }
00505 
00506 
00507   delete (octreeCoder);
00508   return (0);
00509 }
00510 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:41