Go to the documentation of this file.00001
00002
00003
00004 #include <iostream>
00005 using namespace std;
00006 #include <pcl/io/openni_grabber.h>
00007 #include "pcl/io/pcd_io.h"
00008 #include "pcl/common/file_io.h"
00009 #include "pcl/common/common_headers.h"
00010 #include "pcl/visualization/pcl_visualizer.h"
00011 #include "pcl/console/parse.h"
00012
00013 using namespace pcl;
00014 using namespace pcl::visualization;
00015
00016
00017 typedef pcl::PointXYZRGB PointType;
00018
00019 boost::mutex mutex_;
00020 pcl::PointCloud<PointType>::ConstPtr point_cloud_ptr;
00021 bool new_cloud = false;
00022 bool use_binary_format = true;
00023
00024 string filename_prefix = "openni";
00025
00026 float max_fps = 5.0;
00027
00028
00029 struct EventHelper
00030 {
00031 void
00032 cloud_cb (const pcl::PointCloud<PointType>::ConstPtr & cloud)
00033 {
00034 if (mutex_.try_lock ())
00035 {
00036 point_cloud_ptr = cloud;
00037 static int counter = 0;
00038 static double start_time = getTime(),
00039 last_write_time = 0.0;
00040 double current_time = getTime();
00041
00042
00043 if (current_time-last_write_time > 1.0f/max_fps)
00044 {
00045 ++counter;
00046 cout << "w" << std::flush;
00047 stringstream filename_stream;
00048 filename_stream << filename_prefix << "_" << setfill ('0') << setw (6) << counter << ".pcd";
00049 pcl::io::savePCDFile (filename_stream.str(), *cloud, use_binary_format);
00050 last_write_time = current_time;
00051 }
00052 else
00053 {
00054 cout << "s"<<std::flush;
00055 }
00056 new_cloud = true;
00057 mutex_.unlock ();
00058 }
00059
00060 }
00061 };
00062
00063 void printUsage(const char* progName)
00064 {
00065 cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
00066 << "Options:\n"
00067 << "-------------------------------------------\n"
00068 << "-p <string> filename prefix (default: "<<filename_prefix<<")\n"
00069 << "-h this help\n"
00070 << "\n\n";
00071 }
00072
00073 int main (int argc, char** argv)
00074 {
00075
00076
00077
00078 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00079 {
00080 printUsage (argv[0]);
00081 return 0;
00082 }
00083 if (pcl::console::parse (argc, argv, "-p", filename_prefix) >= 0)
00084 cout << "Setting filename prefix to \""<<filename_prefix<<"\".\n";
00085
00086 PCLVisualizer viewer("3D Viewer");
00087 viewer.addCoordinateSystem(1.0f);
00088 viewer.setBackgroundColor (1, 1, 1);
00089
00090 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
00091 if (driver.getNumberDevices() > 0)
00092 {
00093 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx)
00094 {
00095 cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName(deviceIdx) << ", product: " << driver.getProductName(deviceIdx)
00096 << ", connected: " << (int) driver.getBus(deviceIdx) << " @ " << (int) driver.getAddress(deviceIdx) << ", serial number: \'" << driver.getSerialNumber(deviceIdx) << "\'" << endl;
00097 }
00098
00099 }
00100 else
00101 cout << "No devices connected." << endl;
00102
00103 std::string device_id = "#1";
00104 pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
00105 EventHelper h;
00106 boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &h, _1);
00107 boost::signals2::connection c1 = interface->registerCallback (f);
00108 cout << "Starting grabber\n";
00109 interface->start ();
00110 cout << "Done\n";
00111
00112
00113 while(!viewer.wasStopped())
00114 {
00115 viewer.spinOnce(100);
00116 usleep(100000);
00117
00118 if (new_cloud && mutex_.try_lock ())
00119 {
00120 new_cloud = false;
00121 if (point_cloud_ptr)
00122 {
00123 cout << "v" << std::flush;
00124
00126
00127 static bool first_point_cloud = true;
00128 PointCloudColorHandlerCustom<PointType> color_handler_cloud(point_cloud_ptr, 0, 0, 0);
00129 if (first_point_cloud)
00130 {
00131 viewer.addPointCloud<PointType> (point_cloud_ptr, color_handler_cloud, "point cloud");
00132 first_point_cloud = false;
00133 }
00134 else
00135 {
00136 viewer.updatePointCloud<PointType> (point_cloud_ptr, color_handler_cloud, "point cloud");
00137 }
00138 }
00139 mutex_.unlock ();
00140 }
00141 }
00142
00143 interface->stop ();
00144 }
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00188
00196
00197
00198
00199
00200
00201
00202
00222
00224
00225
00226
00227
00228
00229