00001
00002
00003
00004 #include <iostream>
00005 using namespace std;
00006 #include <pcl/io/openni_grabber.h>
00007 #include "pcl/range_image/range_image_planar.h"
00008 #include "pcl/common/common_headers.h"
00009 #include "pcl/visualization/range_image_visualizer.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 std::string device_id = "#1";
00017
00018 float angular_resolution = -1.0f;
00019
00020
00021
00022
00023
00024 boost::mutex depth_image_mutex,
00025 ir_image_mutex,
00026 image_mutex;
00027 pcl::PointCloud<pcl::PointXYZ>::ConstPtr point_cloud_ptr;
00028 boost::shared_ptr<openni_wrapper::DepthImage> depth_image_ptr;
00029 boost::shared_ptr<openni_wrapper::IRImage> ir_image_ptr;
00030 boost::shared_ptr<openni_wrapper::Image> image_ptr;
00031
00032 bool received_new_depth_data = false,
00033 received_new_ir_image = false,
00034 received_new_image = false;
00035 struct EventHelper
00036 {
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 void
00049 depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
00050 {
00051 if (depth_image_mutex.try_lock ())
00052 {
00053 depth_image_ptr = depth_image;
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 depth_image_mutex.unlock ();
00066 received_new_depth_data = true;
00067 }
00068 }
00069
00070 void
00071 ir_image_cb (const boost::shared_ptr<openni_wrapper::IRImage>& ir_image)
00072 {
00073 if (ir_image_mutex.try_lock ())
00074 {
00075 ir_image_ptr = ir_image;
00076 ir_image_mutex.unlock ();
00077 received_new_ir_image = true;
00078 }
00079 }
00080
00081 void
00082 image_cb (const boost::shared_ptr<openni_wrapper::Image>& image)
00083 {
00084 if (image_mutex.try_lock ())
00085 {
00086 image_ptr = image;
00087 image_mutex.unlock ();
00088 received_new_image = true;
00089 }
00090 }
00091 };
00092
00093
00094 void
00095 printUsage (const char* progName)
00096 {
00097 cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
00098 << "Options:\n"
00099 << "-------------------------------------------\n"
00100 << "-r <float> angular resolution in degrees (default "<<rad2deg (angular_resolution)<<")\n"
00101 << "-d <device_id> set the device id (default \""<<device_id<<"\")\n"
00102 << "-h this help\n"
00103 << "\n\n";
00104 }
00105
00106 int main (int argc, char** argv)
00107 {
00108
00109
00110
00111 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00112 {
00113 printUsage (argv[0]);
00114 return 0;
00115 }
00116 if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
00117 cout << "Using device id \""<<device_id<<"\".\n";
00118 if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00119 cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00120 angular_resolution = deg2rad (angular_resolution);
00121
00122 RangeImageVisualizer range_image_widget ("Range Image"),
00123 ir_image_widget ("IR Image"),
00124 rgb_image_widget ("RGB Image");
00125
00126 PCLVisualizer viewer ("3D Viewer");
00127 viewer.addCoordinateSystem (1.0f);
00128 viewer.setBackgroundColor (1, 1, 1);
00129
00130 viewer.initCameraParameters ();
00131 viewer.camera_.pos[0] = 0.0;
00132 viewer.camera_.pos[1] = -0.3;
00133 viewer.camera_.pos[2] = -2.0;
00134 viewer.camera_.focal[0] = 0.0;
00135 viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
00136 viewer.camera_.focal[2] = 1.0;
00137 viewer.camera_.view[0] = 0.0;
00138 viewer.camera_.view[1] = -1.0;
00139 viewer.camera_.view[2] = 0.0;
00140 viewer.updateCamera();
00141
00142 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00143 if (driver.getNumberDevices () > 0)
00144 {
00145 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00146 {
00147 cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
00148 << ", product: " << driver.getProductName (deviceIdx) << ", connected: "
00149 << (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
00150 << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
00151 }
00152 }
00153 else
00154 {
00155 cout << "\nNo devices connected.\n\n";
00156 return 1;
00157 }
00158
00159 pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
00160 EventHelper event_helper;
00161
00162
00163
00164
00165 boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&) > f_depth_image =
00166 boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
00167 boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
00168
00169 boost::function<void (const boost::shared_ptr<openni_wrapper::IRImage>&) > f_ir_image =
00170 boost::bind (&EventHelper::ir_image_cb, &event_helper, _1);
00171
00172
00173 boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&) > f_image =
00174 boost::bind (&EventHelper::image_cb, &event_helper, _1);
00175 boost::signals2::connection c_image = interface->registerCallback (f_image);
00176
00177 cout << "Starting grabber\n";
00178 interface->start ();
00179 cout << "Done\n";
00180
00181 boost::shared_ptr<RangeImagePlanar> range_image_planar_ptr (new RangeImagePlanar);
00182 RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
00183
00184 while (true)
00185 {
00186 viewer.spinOnce (10);
00187 ImageWidgetWX::spinOnce ();
00188 usleep (10000);
00189
00190 bool got_new_range_image = false;
00191 if (received_new_depth_data && depth_image_mutex.try_lock ())
00192 {
00193 received_new_depth_data = false;
00194
00195 unsigned long time_stamp = depth_image_ptr->getTimeStamp ();
00196 int frame_id = depth_image_ptr->getFrameID ();
00197 const unsigned short* depth_map = depth_image_ptr->getDepthMetaData().Data();
00198 int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
00199 float center_x = width/2, center_y = height/2;
00200 float focal_length_x = depth_image_ptr->getFocalLength(), focal_length_y = focal_length_x;
00201 float original_angular_resolution = asinf (0.5f*float(width)/float(focal_length_x)) / (0.5f*float(width));
00202 float desired_angular_resolution = angular_resolution;
00203 range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
00204 focal_length_x, focal_length_y, desired_angular_resolution);
00205
00206 depth_image_mutex.unlock ();
00207 got_new_range_image = !range_image_planar.points.empty();
00208
00209
00210
00211
00212
00213
00214
00215 depth_image_mutex.unlock ();
00216 }
00217
00218 if (received_new_ir_image && ir_image_mutex.try_lock ())
00219 {
00220 received_new_ir_image = false;
00221
00222 unsigned long time_stamp = ir_image_ptr->getTimeStamp ();
00223 int frame_id = ir_image_ptr->getFrameID ();
00224 const unsigned short* ir_map = ir_image_ptr->getMetaData().Data();
00225 int width = ir_image_ptr->getWidth (), height = ir_image_ptr->getHeight ();
00226 float center_x = width/2, center_y = height/2;
00227
00228
00229 cout << "IR image: " << PVARC(time_stamp)<<PVARC(frame_id)
00230 << PVARC(width)<<PVARC(height)<<PVARC(center_x)<<PVARC(center_y)
00231
00232 << "\n";
00233 ir_image_widget.setShortImage(ir_map, width, height, "IR Image", 0, 1000);
00234
00235 ir_image_mutex.unlock ();
00236 }
00237
00238 if (received_new_image && image_mutex.try_lock ())
00239 {
00240 received_new_image = false;
00241
00242 unsigned long time_stamp = image_ptr->getTimeStamp ();
00243 int frame_id = image_ptr->getFrameID ();
00244 openni_wrapper::Image::Encoding encoding = image_ptr->getEncoding();
00245
00246
00247
00248
00249
00250
00251 const unsigned char* color_map = image_ptr->getMetaData().Data();
00252 int width = image_ptr->getWidth (), height = image_ptr->getHeight ();
00253 float center_x = width/2, center_y = height/2;
00254
00255
00256
00257 if (encoding==openni_wrapper::Image::RGB)
00258 rgb_image_widget.setRGBImage(color_map, width, height);
00259 else if(encoding==openni_wrapper::Image::BAYER_GRBG)
00260 rgb_image_widget.setBayerGRBGImage(color_map, width, height);
00261
00262
00263
00264
00266
00267
00268 image_mutex.unlock ();
00269 }
00270
00271 if (!got_new_range_image)
00272 continue;
00273
00274 cout << "." << std::flush;
00275
00277 static bool first_range_image = true;
00278 range_image_widget.setRangeImage (range_image_planar, 0.0f, 10.0f);
00279
00280 PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud (range_image_planar_ptr, 0, 0, 0);
00281 if (first_range_image)
00282 {
00283 viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
00284 first_range_image = false;
00285 }
00286 else
00287 {
00288 viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
00289 }
00290 }
00291
00292 interface->stop ();
00293 }