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/features/range_image_border_extractor.h"
00012 #include "pcl/keypoints/narf_keypoint.h"
00013 #include "pcl/console/parse.h"
00014
00015 using namespace pcl;
00016 using namespace pcl::visualization;
00017
00018 std::string device_id = "#1";
00019
00020 float angular_resolution = 0.5f;
00021 float support_size = 0.2f;
00022 bool set_unseen_to_max_range = true;
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 void
00038 depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
00039 {
00040 if (depth_image_mutex.try_lock ())
00041 {
00042 depth_image_ptr = depth_image;
00043 depth_image_mutex.unlock ();
00044 received_new_depth_data = true;
00045 }
00046 }
00047 };
00048
00049
00050 void
00051 printUsage (const char* progName)
00052 {
00053 cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
00054 << "Options:\n"
00055 << "-------------------------------------------\n"
00056 << "-d <device_id> set the device id (default \""<<device_id<<"\")\n"
00057 << "-r <float> angular resolution in degrees (default "<<rad2deg (angular_resolution)<<")\n"
00058 << "-s <float> support size for the interest points (diameter of the used sphere in meters) (default "<<support_size<<")\n"
00059 << "-h this help\n"
00060 << "\n\n";
00061 }
00062
00063 int main (int argc, char** argv)
00064 {
00065
00066
00067
00068 if (pcl::console::find_argument (argc, argv, "-h") >= 0)
00069 {
00070 printUsage (argv[0]);
00071 return 0;
00072 }
00073 if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
00074 cout << "Using device id \""<<device_id<<"\".\n";
00075 if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
00076 cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
00077 if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
00078 cout << "Setting support size to "<<angular_resolution<<"m.\n";
00079 angular_resolution = deg2rad (angular_resolution);
00080
00081 RangeImageVisualizer range_image_widget ("Range Image");
00082
00083 PCLVisualizer viewer ("3D Viewer");
00084 viewer.addCoordinateSystem (1.0f);
00085 viewer.setBackgroundColor (1, 1, 1);
00086
00087 viewer.initCameraParameters ();
00088 viewer.camera_.pos[0] = 0.0;
00089 viewer.camera_.pos[1] = -0.3;
00090 viewer.camera_.pos[2] = -2.0;
00091 viewer.camera_.focal[0] = 0.0;
00092 viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
00093 viewer.camera_.focal[2] = 1.0;
00094 viewer.camera_.view[0] = 0.0;
00095 viewer.camera_.view[1] = -1.0;
00096 viewer.camera_.view[2] = 0.0;
00097 viewer.updateCamera();
00098
00099 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00100 if (driver.getNumberDevices () > 0)
00101 {
00102 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00103 {
00104 cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
00105 << ", product: " << driver.getProductName (deviceIdx) << ", connected: "
00106 << (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
00107 << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
00108 }
00109 }
00110 else
00111 {
00112 cout << "\nNo devices connected.\n\n";
00113 return 1;
00114 }
00115
00116 pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
00117 EventHelper event_helper;
00118
00119 boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&) > f_depth_image =
00120 boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
00121 boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
00122
00123 cout << "Starting grabber\n";
00124 interface->start ();
00125 cout << "Done\n";
00126
00127 boost::shared_ptr<RangeImagePlanar> range_image_planar_ptr (new RangeImagePlanar);
00128 RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
00129
00130 RangeImageBorderExtractor range_image_border_extractor;
00131 NarfKeypoint narf_keypoint_detector;
00132 narf_keypoint_detector.setRangeImageBorderExtractor(&range_image_border_extractor);
00133 narf_keypoint_detector.getParameters().support_size = support_size;
00134
00135
00136
00137 PointCloud<PointXYZ>::Ptr keypoints_cloud_ptr(new PointCloud<PointXYZ>);
00138 PointCloud<PointXYZ>& keypoints_cloud = *keypoints_cloud_ptr;
00139
00140 while (true)
00141 {
00142 viewer.spinOnce (10);
00143 range_image_widget.spinOnce ();
00144 usleep (10000);
00145
00146 bool got_new_range_image = false;
00147 if (received_new_depth_data && depth_image_mutex.try_lock ())
00148 {
00149 received_new_depth_data = false;
00150
00151
00152
00153 const unsigned short* depth_map = depth_image_ptr->getDepthMetaData().Data();
00154 int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
00155 float center_x = width/2, center_y = height/2;
00156 float focal_length_x = depth_image_ptr->getFocalLength(), focal_length_y = focal_length_x;
00157 float original_angular_resolution = asinf (0.5f*float(width)/float(focal_length_x)) / (0.5f*float(width));
00158 float desired_angular_resolution = angular_resolution;
00159 range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
00160 focal_length_x, focal_length_y, desired_angular_resolution);
00161 depth_image_mutex.unlock ();
00162 got_new_range_image = !range_image_planar.points.empty();
00163 }
00164
00165 if (!got_new_range_image)
00166 continue;
00167
00168
00169
00170
00171 if (set_unseen_to_max_range)
00172 range_image_planar.setUnseenToMaxRange();
00173 narf_keypoint_detector.setRangeImage(&range_image_planar);
00174 PointCloud<int> keypoint_indices;
00175 narf_keypoint_detector.compute(keypoint_indices);
00176 std::cout << "Found "<<keypoint_indices.points.size()<<" key points.\n";
00177
00178
00179
00180
00181 range_image_widget.showRangeImage (range_image_planar, 0.0f, 10.0f);
00182 for (size_t i=0; i<keypoint_indices.points.size(); ++i)
00183 range_image_widget.markPoint(keypoint_indices.points[i]%range_image_planar.width,
00184 keypoint_indices.points[i]/range_image_planar.width,
00185 pcl::visualization::Vector3ub (0,255,0));
00186
00187
00188
00189
00190 PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud (range_image_planar_ptr, 0, 0, 0);
00191 if (!viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image"))
00192 viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
00193
00194 keypoints_cloud.points.resize(keypoint_indices.points.size());
00195 for (size_t i=0; i<keypoint_indices.points.size(); ++i)
00196 keypoints_cloud.points[i].getVector3fMap() = range_image_planar.points[keypoint_indices.points[i]].getVector3fMap();
00197 PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler_keypoints (keypoints_cloud_ptr, 0, 255, 0);
00198 if (!viewer.updatePointCloud(keypoints_cloud_ptr, color_handler_keypoints, "keypoints"))
00199 viewer.addPointCloud(keypoints_cloud_ptr, color_handler_keypoints, "keypoints");
00200 viewer.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
00201 }
00202
00203 interface->stop ();
00204 }