From 72c02d2bef68b5aeb4c0a97da5bd6e31e197246c Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 4 Oct 2012 18:05:13 +0000 Subject: [PATCH] Some fixes on point cloud grabbing git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@7455 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- apps/src/dinast_grabber_example.cpp | 65 +++++++++--- io/include/pcl/io/dinast_grabber.h | 33 ++---- io/src/dinast_grabber.cpp | 151 ++++++++++------------------ 3 files changed, 114 insertions(+), 135 deletions(-) diff --git a/apps/src/dinast_grabber_example.cpp b/apps/src/dinast_grabber_example.cpp index f027544a7..8478c0ab2 100644 --- a/apps/src/dinast_grabber_example.cpp +++ b/apps/src/dinast_grabber_example.cpp @@ -1,3 +1,40 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + #include #include #include @@ -14,19 +51,16 @@ #include #define FPS_CALC(_WHAT_) \ -do \ +static unsigned count = 0;\ +static double last = pcl::getTime ();\ +double now = pcl::getTime (); \ +++count; \ +if (now - last >= 1.0) \ { \ - static unsigned count = 0;\ - static double last = pcl::getTime ();\ - double now = pcl::getTime (); \ - ++count; \ - if (now - last >= 1.0) \ - { \ - std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \ - count = 0; \ - last = now; \ - } \ -}while(false) + std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \ + count = 0; \ + last = now; \ +} \ void savePGM (const unsigned char *image, const std::string& filename) @@ -46,7 +80,7 @@ savePGM (const unsigned char *image, const std::string& filename) { for (int j = 0; j < IMAGE_WIDTH; ++j) { - fprintf (file, "%3d ", (int)*image++); + fprintf (file, "%3d ", int(*image++)); } fprintf (file, "\n"); } @@ -78,7 +112,8 @@ main (int argc, char** argv) grabber.openDevice(); - std::cerr << "Device version/revision number: " << grabber.getDeviceVersion () << std::endl; + + std::cerr << "Device version/revision number: " << grabber.getDeviceVersion () << "Hz"<< std::endl; grabber.start (); @@ -93,8 +128,8 @@ main (int argc, char** argv) grabber.getData(image, cloud); - FPS_CALC ("grabber + visualization"); + vis_img.showMonoImage (image, IMAGE_WIDTH, IMAGE_HEIGHT); pcl::visualization::PointCloudColorHandlerGenericField handler (cloud, "intensity"); diff --git a/io/include/pcl/io/dinast_grabber.h b/io/include/pcl/io/dinast_grabber.h index 3bac2a7bf..615d1f4c2 100644 --- a/io/include/pcl/io/dinast_grabber.h +++ b/io/include/pcl/io/dinast_grabber.h @@ -2,7 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * @@ -16,7 +16,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its + * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -34,12 +34,14 @@ * POSSIBILITY OF SUCH DAMAGE. * */ + #ifndef __PCL_IO_DINAST_GRABBER__ #define __PCL_IO_DINAST_GRABBER__ #include #include #include +#include #include #include #include @@ -60,7 +62,7 @@ namespace pcl { /** \brief Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001) - * \author Marco A. Gutierrez + * \author Marco A. Gutierrez * \ingroup io */ class PCL_EXPORTS DinastGrabber: public Grabber @@ -91,7 +93,7 @@ namespace pcl getFramesPerSecond () const; - /** \brief Find a Dinast 3D camera device and open de device handler + /** \brief Find the specified Dinast 3D camera device * \param[in] device_position Number corresponding to the device to grab * \param[in] id_vendor the ID of the camera vendor (should be 0x18d1) * \param[in] id_product the ID of the product (should be 0x1402) @@ -101,12 +103,12 @@ namespace pcl const int id_vendor = 0x18d1, const int id_product = 0x1402); - /** \brief Claims the interface for the already finded device + /** \brief Claims the interface for an already founded device */ void openDevice (); - /** \brief Close the given the device of the DINAST camera + /** \brief Close the DINAST camera device */ void closeDevice (); @@ -157,26 +159,13 @@ namespace pcl */ int readImage (unsigned char *image); - - /** \brief Read image data - * \param[out] the image data in unsigned short format - */ - int - readImage (unsigned char *image1, unsigned char *image2); - /** \brief Read XYZI Point Cloud - * \return the XYZ point cloud from the camera + /** \brief Obtains the image and the corresponding XYZI Point Cloud + * \param[out] the image data in unsigned short format and the corresponding point cloud from the camera */ void getData(unsigned char *image, pcl::PointCloud::Ptr cloud); - /** \brief Read image data - * \param[out] the image data in PointCloud format - */ - int - readData(unsigned char *_image, pcl::PointCloud &cloud); - - /** \brief the libusb context*/ libusb_context *context; @@ -189,7 +178,7 @@ namespace pcl /** \brief Global buffer */ boost::circular_buffer g_buffer; - // Bulk endpoint address value + /** \brief Bulk endpoint address value */ unsigned char bulk_ep; // Since there is no header after the first image, we need to save the state diff --git a/io/src/dinast_grabber.cpp b/io/src/dinast_grabber.cpp index 31ff86430..763aea857 100644 --- a/io/src/dinast_grabber.cpp +++ b/io/src/dinast_grabber.cpp @@ -2,7 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * @@ -16,7 +16,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its + * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -83,8 +83,14 @@ pcl::DinastGrabber::isRunning () const float pcl::DinastGrabber::getFramesPerSecond () const { - - return (0); + static unsigned count = 0; + static double last = pcl::getTime (); + double now = pcl::getTime (); + float rate = 1/float(now - last); + count = 0; + last = now; + + return (rate); } void @@ -92,12 +98,18 @@ pcl::DinastGrabber::findDevice ( int device_position, const int id_vendor, const { int device_position_counter=0; - if (libusb_init (&context) != 0) - return; + //Initialize libusb + int ret=libusb_init (&context); + std::stringstream sstream; + if (ret != 0) + { + sstream << "libusb initialization fialure, LIBUSB_ERROR: "< (data))); } @@ -239,14 +255,16 @@ pcl::DinastGrabber::start () { unsigned char ctrl_buf[3]; if (!USBTxControlData (CMD_READ_START, ctrl_buf, 1)) - return; + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start the USB data reading"); running_ = true; } void pcl::DinastGrabber::stop () { - + unsigned char ctrl_buf[3]; + if (!USBTxControlData (CMD_READ_START, ctrl_buf, 1)) + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start the USB data reading"); running_=false; } @@ -362,120 +380,57 @@ pcl::DinastGrabber::getData (unsigned char *image, pcl::PointCloudwidth; ++x) { for (int y = 0; y < cloud->height; ++y, ++depth_idx) { - float xc = (float)(x - 160); - float yc = (float)(y - 120); + unsigned char pixel = image[x + IMAGE_WIDTH * y]; //image[depth_idx]; + + float xc = float(x - 160); + float yc = float(y - 120); double r1 = sqrt (xc * xc + yc * yc); - float r2 = r1 * r1; - float r3 = r1 * r2; + double r2 = r1 * r1; + double r3 = r1 * r2; double A = -2e-5 * r3 + 0.004 * r2 + 0.1719 * r1 + 350.03; double B = -2e-9 * r3 + 3e-7 * r2 - 1e-5 * r1 - 0.01; - // Low pass filtering - /// @todo Try a bilateral filter to avoid blurring over depth boundaries - int measure = 0; - if ((y > 0) && (y < (IMAGE_HEIGHT - 1)) && (x > 0) && (x < (IMAGE_WIDTH - 1))) - { - int ipx = x + IMAGE_WIDTH * y; -#if 1 - pxl[0] = image[ipx]; - pxl[1] = image[ipx-1]; - pxl[2] = image[ipx+1]; - pxl[3] = image[ipx - IMAGE_WIDTH]; - pxl[4] = image[ipx - IMAGE_WIDTH - 1]; - pxl[5] = image[ipx - IMAGE_WIDTH + 1]; - pxl[6] = image[ipx + IMAGE_WIDTH]; - pxl[7] = image[ipx + IMAGE_WIDTH - 1]; - pxl[8] = image[ipx + IMAGE_WIDTH + 1]; - - for (int ii = 0; ii < 9; ii++) - measure += pxl[ii]; - measure /= 9; -#else - // No blurring - measure = image[ipx]; -#endif - } - if (measure > 255) - measure = 255; // saturation for display - - unsigned char pixel = measure;//image[depth_idx]; - if (pixel < 1) - { - cloud->points[depth_idx].x = std::numeric_limits::quiet_NaN (); - cloud->points[depth_idx].y = std::numeric_limits::quiet_NaN (); - cloud->points[depth_idx].z = std::numeric_limits::quiet_NaN (); - cloud->points[depth_idx].intensity = pixel; - continue; - } + /// Compute distance if (pixel > A) pixel = A; - float dy = y*0.1; - float dist = (log((double)pixel/A)/B-dy)*(7E-07*r3 - 0.0001*r2 + 0.004*r1 + 0.9985)*1.5; + float dist = (log(double(pixel/A))/B-dy)*(7E-07*r3 - 0.0001*r2 + 0.004*r1 + 0.9985)*1.5; float dist_2d = r1; - +// static const float dist_max_2d = 1 / 212.60291; static const float dist_max_2d = 1 / 160.0; /// @todo Why not 200? //static const double dist_max_2d = 1 / 200.0; +// static const double FOV = 40. * M_PI / 180.0; // diagonal FOV? static const double FOV = 64.0 * M_PI / 180.0; // diagonal FOV? - - float theta_colati = FOV * r1 * dist_max_2d; + float theta_colati = FOV * r1 * dist_max_2d; float c_theta = cos (theta_colati); float s_theta = sin (theta_colati); - float c_ksai = ((double)(x - 160.)) / r1; - float s_ksai = ((double)(y - 120.)) / r1; - + float c_ksai = (double(x - 160.)) / r1; + float s_ksai = (double(y - 120.)) / r1; cloud->points[depth_idx].x = (dist * s_theta * c_ksai) / 500.0 + 0.5; //cartesian x cloud->points[depth_idx].y = (dist * s_theta * s_ksai) / 500.0 + 0.5; //cartesian y cloud->points[depth_idx].z = (dist * c_theta); //cartesian z - /// @todo This looks weird, can it cause artifacts? if (cloud->points[depth_idx].z < 0.01) cloud->points[depth_idx].z = 0.01; - cloud->points[depth_idx].z /= 500.0; cloud->points[depth_idx].intensity = pixel; - } - } -} - - -int -pcl::DinastGrabber::readImage (unsigned char *image1, unsigned char *image2) -{ - // Read data in two image blocks until we get a header - int data_adr = -1; - // Read at least two images in synchronous mode - int actual_length; - int res = libusb_bulk_transfer (device_handle, bulk_ep, raw_buffer, - RGB16_LENGTH * 2 + SYNC_PACKET, &actual_length, 0); - if (res != 0 || actual_length == 0) - { - memset (&image1[0], 0x00, IMAGE_SIZE); - PCL_THROW_EXCEPTION (pcl::IOException, "USB read error!"); - } - - for (size_t i = 0; i < actual_length; ++i) - { - if ((raw_buffer[i+0] == 0xAA) && (raw_buffer[i+1] == 0xAA) && - (raw_buffer[i+2] == 0x44) && (raw_buffer[i+3] == 0x44) && - (raw_buffer[i+4] == 0xBB) && (raw_buffer[i+5] == 0xBB) && - (raw_buffer[i+6] == 0x77) && (raw_buffer[i+7] == 0x77)) - { - data_adr = int (i) + SYNC_PACKET; - break; + + //get rid of the noise + if(cloud->points[depth_idx].z > 0.8 or cloud->points[depth_idx].z < 0.02) + { + cloud->points[depth_idx].x = std::numeric_limits::quiet_NaN (); + cloud->points[depth_idx].y = std::numeric_limits::quiet_NaN (); + cloud->points[depth_idx].z = std::numeric_limits::quiet_NaN (); + cloud->points[depth_idx].intensity = pixel; + } } } - // An image found. Copy it from the buffer into the user given buffer - for (int i = 0; i < IMAGE_SIZE; ++i) - image1[i] = raw_buffer[data_adr + i]; - for (int i = 0; i < IMAGE_SIZE; ++i) - image2[i] = raw_buffer[data_adr + IMAGE_SIZE + i]; - - return (IMAGE_SIZE); } -- GitLab