Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl/io/openni_grabber.h>
00040 #include <pcl/visualization/cloud_viewer.h>
00041 #include <pcl/io/openni_camera/openni_driver.h>
00042 #include <pcl/filters/color.h>
00043 #include <pcl/console/parse.h>
00044 #include <pcl/common/time.h>
00045
00046 #define FPS_CALC(_WHAT_) \
00047 do \
00048 { \
00049 static unsigned count = 0;\
00050 static double last = pcl::getTime ();\
00051 double now = pcl::getTime (); \
00052 ++count; \
00053 if (now - last >= 1.0) \
00054 { \
00055 std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \
00056 count = 0; \
00057 last = now; \
00058 } \
00059 }while(false)
00060
00061
00062 template <typename PointType>
00063 class OpenNIPassthrough
00064 {
00065 public:
00066 typedef pcl::PointCloud<PointType> Cloud;
00067 typedef typename Cloud::Ptr CloudPtr;
00068 typedef typename Cloud::ConstPtr CloudConstPtr;
00069
00070 OpenNIPassthrough (pcl::OpenNIGrabber& grabber, unsigned char red, unsigned char green, unsigned char blue, unsigned char radius)
00071 : viewer ("PCL OpenNI ColorFilter Viewer")
00072 , grabber_(grabber)
00073 {
00074 boost::function<void (const CloudConstPtr&)> f = boost::bind (&OpenNIPassthrough::cloud_cb_, this, _1);
00075 boost::signals2::connection c = grabber_.registerCallback (f);
00076
00077 std::vector<bool> lookup(1<<24, false);
00078 fillLookup (lookup, red, green, blue, radius);
00079 unsigned set = 0;
00080 for (unsigned i = 0; i < (1<<24); ++i)
00081 if (lookup[i])
00082 ++set;
00083
00084 cout << "used colors: " << set << endl;
00085
00086 color_filter_.setLookUpTable (lookup);
00087 }
00088
00089 void fillLookup (std::vector<bool>& lookup, unsigned char red, unsigned char green, unsigned char blue, unsigned radius)
00090 {
00091 unsigned radius_sqr = radius * radius;
00092 pcl::RGB color;
00093 for (color.rgba = 0; color.rgba < (1<<24); ++color.rgba)
00094 {
00095 unsigned dist = (unsigned(color.r) - unsigned(red)) * (unsigned(color.r) - unsigned(red)) +
00096 (unsigned(color.g) - unsigned(green)) * (unsigned(color.g) - unsigned(green)) +
00097 (unsigned(color.b) - unsigned(blue)) * (unsigned(color.b) - unsigned(blue));
00098 if (dist < radius_sqr)
00099 lookup [color.rgba] = true;
00100 else
00101 lookup [color.rgba] = false;
00102 }
00103 }
00104
00105 void
00106 cloud_cb_ (const CloudConstPtr& cloud)
00107 {
00108 boost::mutex::scoped_lock lock (mtx_);
00109 FPS_CALC ("computation");
00110
00111 cloud_color_.reset (new Cloud);
00112
00113 color_filter_.setInputCloud (cloud);
00114 color_filter_.filter (*cloud_color_);
00115 cloud_ = cloud;
00116 }
00117
00118 void
00119 run ()
00120 {
00121
00122 grabber_.start ();
00123
00124 while (!viewer.wasStopped ())
00125 {
00126 if (cloud_color_)
00127 {
00128 boost::mutex::scoped_lock lock (mtx_);
00129
00130 FPS_CALC ("visualization");
00131 CloudPtr temp_cloud;
00132 temp_cloud.swap (cloud_color_);
00133 viewer.showCloud (temp_cloud);
00134 }
00135 }
00136
00137 grabber_.stop ();
00138 }
00139
00140 pcl::ColorFilter<PointType> color_filter_;
00141 pcl::visualization::CloudViewer viewer;
00142 pcl::OpenNIGrabber& grabber_;
00143 std::string device_id_;
00144 boost::mutex mtx_;
00145 CloudConstPtr cloud_;
00146 CloudPtr cloud_color_;
00147 };
00148
00149 void
00150 usage (char ** argv)
00151 {
00152 std::cout << "usage: " << argv[0] << " <device_id> [-rgb <red> <green> <blue> [-radius <radius>] ]\n\n" << std::endl;
00153
00154 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
00155 if (driver.getNumberDevices () > 0)
00156 {
00157 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00158 {
00159 cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
00160 << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00161 cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl
00162 << " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << endl
00163 << " <serial-number> (only in Linux and for devices which provide serial numbers)" << endl;
00164 }
00165 }
00166 else
00167 cout << "No devices connected." << endl;
00168 }
00169
00170 int
00171 main (int argc, char ** argv)
00172 {
00173 if (argc < 2)
00174 {
00175 usage (argv);
00176 return 1;
00177 }
00178
00179 std::string arg (argv[1]);
00180
00181 if (arg == "--help" || arg == "-h")
00182 {
00183 usage (argv);
00184 return 1;
00185 }
00186
00187 unsigned char red = 0, green = 0, blue = 0;
00188 int rr, gg, bb;
00189 unsigned char radius = 442;
00190 int rad;
00191
00192 if (pcl::console::parse_3x_arguments (argc, argv, "-rgb", rr, gg, bb, true) != -1 )
00193 {
00194 cout << "-rgb present" << endl;
00195 int idx = pcl::console::parse_argument (argc, argv, "-radius", rad);
00196 if (idx != -1)
00197 {
00198 if (rad > 0)
00199 radius = rad;
00200 }
00201 if (rr >= 0 && rr < 256)
00202 red = (unsigned char) rr;
00203 if (gg >= 0 && gg < 256)
00204 green = (unsigned char) gg;
00205 if (bb >= 0 && bb < 256)
00206 blue = (unsigned char) bb;
00207 }
00208
00209 pcl::OpenNIGrabber grabber (arg);
00210
00211 if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
00212 {
00213 OpenNIPassthrough<pcl::PointXYZRGBA> v (grabber, red, green, blue, radius);
00214 v.run ();
00215 }
00216 else
00217 {
00218 cout << "device does not provide rgb stream" << endl;
00219 }
00220
00221 return (0);
00222 }