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

+/*
+ * 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 <string>
 #include <string.h>
 #include <stdlib.h>
@@ -14,19 +51,16 @@
 #include <pcl/io/dinast_grabber.h>
 
 #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<pcl::PointXYZI> handler (cloud, "intensity");

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 <pcl/pcl_config.h>
 #include <pcl/point_types.h>
 #include <pcl/io/grabber.h>
+#include <pcl/common/time.h>
 #include <libusb-1.0/libusb.h>
 #include <boost/circular_buffer.hpp>
 #include <string>
@@ -60,7 +62,7 @@
 namespace pcl
 {
   /** \brief Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001)
-    * \author Marco A. Gutierrez <marcog@unex.es>
+  * \author Marco A. Gutierrez <marcog@unex.es>
   * \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<pcl::PointXYZI>::Ptr cloud);
       
-      /** \brief Read image data
-      * \param[out] the image data in PointCloud format
-      */     
-      int
-      readData(unsigned char *_image, pcl::PointCloud<pcl::PointXYZI> &cloud);
-
-      
       /** \brief the libusb context*/
       libusb_context *context;
       
@@ -189,7 +178,7 @@ namespace pcl
       /** \brief Global buffer */
       boost::circular_buffer<unsigned char> 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

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: "<<ret;
+    PCL_THROW_EXCEPTION (pcl::IOException, sstream.str());
+  }
   
   libusb_set_debug (context, 3);
-
   libusb_device **devs = NULL;
+  
   // Get the list of USB devices
   ssize_t cnt = libusb_get_device_list (context, &devs);
 
@@ -215,7 +227,11 @@ pcl::DinastGrabber::USBTxControlData (const unsigned char req_code,
   int nr_read = libusb_control_transfer (device_handle, requesttype, 
                                          req_code, value, index, buffer, uint16_t(length), timeout);
   if (nr_read != int(length))
-    PCL_THROW_EXCEPTION (pcl::IOException, "USB control data error");
+  {
+    std::stringstream sstream;
+    sstream<<"USB control data error, LIBUSB_ERROR: "<<nr_read;
+    PCL_THROW_EXCEPTION (pcl::IOException, sstream.str());
+  }
 
   return (true);
 }
@@ -229,7 +245,7 @@ pcl::DinastGrabber::getDeviceVersion ()
      PCL_THROW_EXCEPTION (pcl::IOException, "Error trying to get device version");
   }
  
-  data[21] = 0; // NULL
+  data[21] = 0;
 
   return (std::string (reinterpret_cast<const char*> (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::PointCloud<pcl::PointXYZ
   int depth_idx = 0;
   int pxl[9];
 
+  float sum = 0;
+  int count = 0;
   for (int x = 0; x < cloud->width; ++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<float>::quiet_NaN ();
-        cloud->points[depth_idx].y = std::numeric_limits<float>::quiet_NaN ();
-        cloud->points[depth_idx].z = std::numeric_limits<float>::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<float>::quiet_NaN ();
+      	cloud->points[depth_idx].y = std::numeric_limits<float>::quiet_NaN ();
+      	cloud->points[depth_idx].z = std::numeric_limits<float>::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);
 }