swissranger.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* author: Radu Bogdan Rusu <rusu@cs.tum.edu> */
00031 
00032 #include "swissranger.h"
00033 
00035 #define SR_EXCEPT(except, msg) \
00036   { \
00037     char buf[100]; \
00038     snprintf(buf, 100, "[SwissRanger::%s]: " msg, __FUNCTION__); \
00039     throw except(buf); \
00040   }
00041 
00043 #define SR_EXCEPT_ARGS(except, msg, ...) \
00044   { \
00045     char buf[100]; \
00046     snprintf(buf, 100, "[SwissRanger::%s]: " msg, __FUNCTION__, __VA_ARGS__); \
00047     throw except(buf); \
00048   }
00049 
00050 swissranger::SwissRanger::SwissRanger () : srCam_ (NULL)
00051 {
00052   // Set the intrinsic camera parameters (found after calibration)
00053   intrinsic_  = cvCreateMat (3, 3, CV_64FC1);
00054   distortion_ = cvCreateMat (1, 4, CV_64FC1);
00055   cvmSet (intrinsic_, 0, 0, 250.8230); cvmSet (intrinsic_, 0, 1, 0.0);      cvmSet (intrinsic_, 0, 2, 83.3768);
00056   cvmSet (intrinsic_, 1, 0, 0.0);      cvmSet (intrinsic_, 1, 1, 252.9221); cvmSet (intrinsic_, 1, 2, 72.6264);
00057   cvmSet (intrinsic_, 2, 0, 0.0);      cvmSet (intrinsic_, 2, 1, 0.0);      cvmSet (intrinsic_, 2, 2, 1.0);
00058   cvmSet (distortion_, 0, 0, -0.0612); cvmSet (distortion_, 0, 1, 0.1706);  cvmSet (distortion_, 0, 2, 0); cvmSet (distortion_, 0, 3, 0);
00059   
00060   pcd_filter_ = false;
00061   undistort_distance_ = undistort_amplitude_ = undistort_confidence_ = false;
00062 }
00063 
00064 swissranger::SwissRanger::~SwissRanger ()
00065 {
00066   cvReleaseMat (&intrinsic_);
00067   cvReleaseMat (&distortion_);
00068 }
00069 
00071 // Set up the device
00072 int
00073   swissranger::SwissRanger::open ()
00074 {
00075   // ---[ Open the camera ]---
00076   int res = SR_OpenUSB (&srCam_, 0);          //returns the device ID used in other calls
00077   if (res <= 0)
00078   {
00079     SR_EXCEPT(swissranger::Exception, "Failed to open camera port!");
00080     return (-1);
00081   }
00082   
00083   device_id_   = getDeviceString ();
00084   lib_version_ = getLibraryVersion ();
00085   // ---[ Get the number of rows, cols, ... ]---
00086   rows_ = SR_GetRows (srCam_);
00087   cols_ = SR_GetCols (srCam_);
00088   inr_  = SR_GetImageList (srCam_, &imgEntryArray_);
00089   ROS_INFO ("[SwissRanger::open] Number of images available: %d", inr_);
00090   modulation_freq_  = SR_GetModulationFrequency (srCam_);
00091   integration_time_ = SR_GetIntegrationTime (srCam_);
00092 
00093   if ( (cols_ != SR_COLS) || (rows_ != SR_ROWS) || (inr_ < 1) || (imgEntryArray_ == 0) )
00094   {
00095     SR_Close (srCam_);
00096     SR_EXCEPT_ARGS(swissranger::Exception, "Invalid data image (%dx%d) received from camera!", cols_, rows_);
00097     return (-1);
00098   }
00099 
00100   // ---[ Set the acquisition mode ]---
00101   SR_SetMode (srCam_, MODE);
00102 
00103   // Points array
00104   size_t buffer_size = rows_ * cols_ * 3 * sizeof (float);
00105   buffer_ = (float*)malloc (buffer_size);
00106   memset (buffer_, 0xaf, buffer_size);
00107 
00108   xp_ = buffer_;
00109   yp_ = &xp_[rows_*cols_];
00110   zp_ = &yp_[rows_*cols_];
00111 
00112   return (0);
00113 
00114 }
00115 
00117 // Shutdown the device
00118 int
00119   swissranger::SwissRanger::close ()
00120 {
00121   if (srCam_ == NULL)
00122     return (-1);
00123 
00124   // ---[ Close the camera ]---
00125   int res = SR_Close (srCam_);
00126 
00127   // ---[ Free the allocated memory buffer ]---
00128   free (buffer_);
00129 
00130   if (res != 0)
00131     return (-1);
00132   return (0);
00133 }
00134 
00136 // Rotate an image buffer with 180 degrees
00137 void
00138   swissranger::SwissRanger::rotateImage180 (uint8_t *img, uint8_t *rot_img, int width, int height)
00139 {
00140   for (int u = 0; u < height; u++)
00141   {
00142     int nu_ = (height - u - 1) * width;
00143     int u_  =           u      * width;
00144     for (int v = 0; v < width; v++)
00145     {
00146       int nv = width - v - 1;
00147       rot_img[(nu_ + nv) * 2 + 0] = img[(u_ + v) * 2 + 0];
00148       rot_img[(nu_ + nv) * 2 + 1] = img[(u_ + v) * 2 + 1];
00149     }
00150   }
00151 }
00152 
00153 
00155 // Undistort an image based on the intrinsic camera parameters
00156 void
00157   swissranger::SwissRanger::undistort (uint8_t *img, uint8_t *un_img, int width, int height)
00158 {
00159   CvMat *src = cvCreateMatHeader (height, width, CV_8UC2);
00160   cvSetData (src, img, width * 2);
00161   CvMat *dst = cvCreateMatHeader (height, width, CV_8UC2);
00162   cvSetData (dst, un_img, width * 2);
00163   
00164   cvUndistort2 (src, dst, intrinsic_, distortion_);
00165 }
00166 
00167 
00169 #define _sqr(x) ((x)*(x))
00170 #define _sqr_sum_3d(x) (_sqr(x[0])+_sqr(x[1])+_sqr(x[2]))
00171 #define _dot(x,y) ((x[0])*(y[0])+(x[1])*(y[1])+(x[2])*(y[2]))
00172 double
00173   swissranger::SwissRanger::getAngle (float px, float py, float pz, float qx, float qy, float qz)
00174 {
00175   float dir_a[3], dir_b[3];
00176   dir_a[0] = cvmGet (intrinsic_, 0, 2) - px; dir_a[1] = cvmGet (intrinsic_, 1, 2) - py; dir_a[2] = -pz;
00177   dir_b[0] = cvmGet (intrinsic_, 0, 2) - qx; dir_b[1] = cvmGet (intrinsic_, 1, 2) - qy; dir_b[2] = -qz;
00178   
00179   double norm_a = sqrt (_sqr_sum_3d (dir_a));
00180   double norm_b = sqrt (_sqr_sum_3d (dir_b));
00181   return (acos (_dot (dir_a, dir_b) / (norm_a * norm_b)));
00182 }
00183 
00185 // Get contours
00186 void
00187   swissranger::SwissRanger::contours (uint8_t *img, uint8_t *con_img,
00188                                       int width, int height, int threshold)
00189 {
00190   CvMat *src = cvCreateMatHeader (height, width, CV_8UC1);
00191   cvSetData (src, img + width * height, width);
00192 
00193   CvMat *dst = cvCreateMatHeader (height, width, CV_8UC1);
00194   cvSetData (dst, con_img + width * height, width);
00195   
00196   cvCanny (src, dst, 5, 2);
00197 
00198   // Convert source to GRAY
00199 //  IplImage *monoImg = cvCreateImage (cvGetSize (srcImg), IPL_DEPTH_8U, 1);
00200 //  cvCvtColor (srcImg, monoImg, CV_RGB2GRAY);
00201 
00202   // Clone source
00203 //  IplImage *tmpImg = cvCloneImage (monoImg);
00204 
00205   // Threshold the source image. This needful for cvFindContours ()
00206 /*  cvThreshold (monoImg, tmpImg, threshold, 255, CV_THRESH_BINARY);
00207 
00208 //  cvReleaseImage (&monoImg);
00209 
00210   // Find all contours
00211   CvMemStorage* stor = cvCreateMemStorage (0);
00212   CvSeq* cont = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00213   cvFindContours (tmpImg, stor, &cont, sizeof (CvContour),
00214                   CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint (0, 0));
00215 //  cvZero (tmpImg);
00216 
00217   // This cycle draw all contours and approximate it by ellipses
00218   for (;cont; cont = cont->h_next)
00219   {
00220     cvDrawContours (dstImg, cont, CV_RGB (255, 255, 255), CV_RGB (255, 255,255), 0, 1, 8, cvPoint (0, 0));
00221   }
00222 
00223 //  cvShowImage ("test", tmpImg);
00224 //  cvWaitKey (0);
00225   cvReleaseImage (&tmpImg);*/
00226 }
00227 
00229 // Read data from the device
00230 void
00231   swissranger::SwissRanger::readData (std_msgs::PointCloud &cloud, std_msgs::ImageArray &images)
00232 {
00233   if (srCam_ == NULL)
00234     SR_EXCEPT(swissranger::Exception, "Read attempted on NULL camera port!");
00235 
00236   int res;
00237 
00238   inr_  = SR_GetImageList (srCam_, &imgEntryArray_);
00239   res = SR_Acquire (srCam_);
00240 
00241   size_t image_size = imgEntryArray_->width * imgEntryArray_->height * 2;
00242 
00243   // Pointers to data
00244   uint8_t *distance_image   = (unsigned char*)SR_GetImage (srCam_, 0);//(unsigned char*)imgEntryArray_->data;
00245   uint8_t *amplitude_image  = (unsigned char*)SR_GetImage (srCam_, 1);//(unsigned char*)imgEntryArray_->data + (image_size + sizeof (ImgEntry));
00246   uint8_t *confidence_image = (unsigned char*)SR_GetImage (srCam_, 2);//(unsigned char*)imgEntryArray_->data + (image_size + sizeof (ImgEntry)) * 2;
00247   
00248   // Stores a copy of the rotated distance image
00249   uint8_t *img_tmp = (uint8_t*)malloc (image_size);
00250   memcpy (img_tmp, distance_image, image_size);
00251 
00252   if (undistort_distance_)
00253   {
00254     // Rotate the distance image with 180 on spot
00255     rotateImage180 (distance_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height);
00256     // Undistort the distance image
00257     undistort (img_tmp, distance_image, imgEntryArray_->width, imgEntryArray_->height);
00258   }
00259   else
00260     // Rotate the distance image with 180 on spot
00261     rotateImage180 (img_tmp, distance_image, imgEntryArray_->width, imgEntryArray_->height);
00262 
00263   memcpy (img_tmp, amplitude_image, image_size);
00264   
00265   if (undistort_amplitude_)
00266   {
00267     // Rotate the amplitude image with 180 on spot
00268     rotateImage180 (amplitude_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height);
00269     // Undistort the amplitude image
00270     undistort (img_tmp, amplitude_image, imgEntryArray_->width, imgEntryArray_->height);
00271   }
00272   else
00273     // Rotate the amplitude image with 180 on spot
00274     rotateImage180 (img_tmp, amplitude_image, imgEntryArray_->width, imgEntryArray_->height);
00275 
00276   memcpy (img_tmp, confidence_image, image_size);
00277   
00278   if (undistort_confidence_)
00279   {
00280     // Rotate the confidence image with 180 on spot
00281     rotateImage180 (confidence_image, img_tmp, imgEntryArray_->width, imgEntryArray_->height);
00282     //contours (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height, 1);
00283     // Undistort the confidence image
00284     undistort (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height);
00285   }
00286   else
00287     // Rotate the confidence image with 180 on spot
00288     rotateImage180 (img_tmp, confidence_image, imgEntryArray_->width, imgEntryArray_->height);
00289   
00290   // Points array
00291   res = SR_CoordTrfFlt (srCam_, xp_, yp_, zp_, sizeof (float), sizeof (float), sizeof (float));
00292 
00293   // Filter points
00294   cloud.set_pts_size (imgEntryArray_->width * imgEntryArray_->height);
00295   cloud.set_chan_size (2);
00296   if (pcd_filter_)                    // if in-driver filtering is enabled, do not send the point confidence anymore, but rather the original point index
00297     cloud.chan[0].name = "pid";
00298   else
00299     cloud.chan[0].name = "con";
00300   cloud.chan[0].set_vals_size (imgEntryArray_->width * imgEntryArray_->height);
00301   cloud.chan[1].name = "i";
00302   cloud.chan[1].set_vals_size (imgEntryArray_->width * imgEntryArray_->height);
00303   
00304   int nr_pts = 0;
00305   for (int u = 0; u < imgEntryArray_->height; u++)
00306   {
00307     int nu = imgEntryArray_->width * u;
00308     for (int v = 0; v < imgEntryArray_->width; v++)
00309     {
00310       int i = nu + v;
00311       
00312       if (pcd_filter_)                // is in-driver filtering enabled ?
00313       {
00314         unsigned short conf_val = (confidence_image[i * 2 + 0] << 0) + (confidence_image[i * 2 + 1] << 8);
00315         unsigned short dist_val = (distance_image[i * 2 + 0] << 0) + (distance_image[i * 2 + 1] << 8);
00316         if ( (conf_val > 0xffff * 3 / 4) && (dist_val < 0xff00) )
00317         {
00318           if (getAngle (xp_[i], yp_[i], zp_[i], xp_[i+1], yp_[i+1], zp_[i+1]) > 0.0004)
00319             continue;
00320         }
00321       }
00322 
00323       // Save the XYZ coordinates
00324       cloud.pts[nr_pts].x = xp_[i];
00325       cloud.pts[nr_pts].y = yp_[i];
00326       cloud.pts[nr_pts].z = zp_[i];
00327 
00328       if (pcd_filter_)                // is in-driver filtering enabled ?
00329         cloud.chan[0].vals[nr_pts] = i;
00330       else
00331         cloud.chan[0].vals[nr_pts] = (confidence_image[i * 2 + 0] << 0) + (confidence_image[i * 2 + 1] << 8);
00332       
00333       cloud.chan[1].vals[nr_pts] = (amplitude_image[i * 2 + 0] << 0) + (amplitude_image[i * 2 + 1] << 8); //amplitude_image[i*2 + 1];
00334       nr_pts++;
00335     }
00336   }
00337   cloud.pts.resize (nr_pts);
00338   cloud.chan[0].vals.resize (nr_pts);
00339   cloud.chan[1].vals.resize (nr_pts);
00340   
00341   // Fill in the ROS PointCloud message
00342   /*for (int i = 0; i < imgEntryArray_->width * imgEntryArray_->height; i++)
00343   {
00344     cloud.pts[i].x = xp_[i];
00345     cloud.pts[i].y = yp_[i];
00346     cloud.pts[i].z = zp_[i];
00347     cloud.chan[0].vals[i] = amplitude_image[i*2 + 1];
00348   }*/
00349   
00350   images.set_images_size (3);
00351   
00352   images.images[0].width  = cols_;
00353   images.images[0].height = rows_;
00354   images.images[0].colorspace  = "mono16";
00355   images.images[0].compression = "raw";
00356   images.images[0].label       = "sr4k-distance";
00357   images.images[0].set_data_size (image_size);
00358   memcpy (&(images.images[0].data[0]), distance_image, images.images[0].get_data_size ());
00359 
00360   images.images[1].width  = cols_;
00361   images.images[1].height = rows_;
00362   //if (MODE & AM_CONV_GRAY)
00363   images.images[1].colorspace  = "mono16";
00364   images.images[1].compression = "raw";
00365   images.images[1].label       = "sr4k-intensity";
00366   images.images[1].set_data_size (image_size);
00367   memcpy (&(images.images[1].data[0]), amplitude_image, images.images[1].get_data_size ());
00368 
00369   images.images[2].width  = cols_;
00370   images.images[2].height = rows_;
00371   images.images[2].colorspace  = "mono16";
00372   images.images[2].compression = "raw";
00373   images.images[2].label       = "sr4k-confidence";
00374   images.images[2].set_data_size (image_size);
00375   memcpy (&(images.images[2].data[0]), confidence_image, images.images[2].get_data_size ());
00376 
00377   free (img_tmp);
00378   return;
00379 }
00380 
00382 int
00383   swissranger::SwissRanger::setAutoIllumination (bool on)
00384 {
00385   int res;
00386   if (on)
00387 //    res = SR_SetAutoIllumination (srCam_, 5, 255, 10, 45);
00388     res = SR_SetAutoExposure (srCam_, 20, 20, 5, 100);
00389   else
00390     res = SR_SetAutoExposure (srCam_, 255, 0, 0, 0);
00391   return (res);
00392 }
00393 
00395 int
00396   swissranger::SwissRanger::setIntegrationTime (int time)
00397 {
00398   // ---[ Set integration time
00399   return (SR_SetIntegrationTime (srCam_, time));
00400 }
00401 
00403 int
00404   swissranger::SwissRanger::getIntegrationTime ()
00405 {
00406   // ---[ Set integration time
00407   return (SR_GetIntegrationTime (srCam_));
00408 }
00409 
00411 int
00412   swissranger::SwissRanger::setModulationFrequency (int freq)
00413 {
00414   // ---[ Set modulation frequency
00415   return (SR_SetModulationFrequency (srCam_, (ModulationFrq)freq));
00416 }
00417 
00419 int
00420   swissranger::SwissRanger::getModulationFrequency ()
00421 {
00422   // ---[ Set modulation frequency
00423   return (SR_GetModulationFrequency (srCam_));
00424 }
00425 
00427 int
00428   swissranger::SwissRanger::setAmplitudeThreshold (int thresh)
00429 {
00430   // ---[ Set amplitude threshold
00431   return (SR_SetAmplitudeThreshold (srCam_, thresh));
00432 }
00433 
00435 int
00436   swissranger::SwissRanger::getAmplitudeThreshold ()
00437 {
00438   // ---[ Set amplitude threshold
00439   return (SR_GetAmplitudeThreshold (srCam_));
00440 }
00441 
00443 // Obtain the device product name
00444 std::string
00445   swissranger::SwissRanger::getDeviceString ()
00446 {
00447   char *buf = new char[256];
00448   int *buflen = new int;
00449   SR_GetDeviceString (srCam_, buf, *buflen);
00450 
00451   // VendorID:0x%04x, ProductID:0x%04x, Manufacturer:'%s', Product:'%s'
00452   std::string sensor (buf);
00453   std::string::size_type loc = sensor.find ("Product:", 0);
00454   if (loc != std::string::npos)
00455   {
00456     sensor = sensor.substr (loc + 9, *buflen);
00457     loc = sensor.find ("'", 0);
00458     if (loc != std::string::npos)
00459       sensor = sensor.substr (0, loc);
00460   }
00461   else
00462     sensor = "";
00463 
00464   delete buflen;
00465   delete [] buf;
00466   return (sensor);
00467 }
00468 
00470 // Obtain the libusbSR library version
00471 std::string
00472   swissranger::SwissRanger::getLibraryVersion ()
00473 {
00474   unsigned short version[4];
00475   char buf[80];
00476   SR_GetVersion (version);
00477   snprintf (buf, sizeof (buf), "%d.%d.%d.%d", version[3], version[2], version[1], version[0]);
00478   return (std::string (buf));
00479 }


composite_swissranger
Author(s): Radu Bogdan Rusu (rusu@cs.tum.edu)
autogenerated on Mon Oct 6 2014 08:11:48