upcbarcode.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey et al
 *                      [e-mail]    
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: Driver for detecting UPC barcodes from camera
// Author: Andrew Howard
// Date: 15 Feb 2004
// CVS: $Id: upcbarcode.cc,v 1.17.2.1 2006/06/07 16:12:39 gerkey Exp $
//
// Theory of operation:
//   TODO
//
// Requires:
//   Camera device.
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_upcbarcode upcbarcode
 * @brief UPC barcode detection

@todo This driver is currently disabled because it needs to be updated to
the Player 2.0 API.

The upcbarcode driver searches for standard, single-digit UPC barcodes
in a camera image (a sample barcode is shown below)

@image html upc.jpg "Sample UPC barcode (one digit)"

@par Compile-time dependencies

- OpenCV

@par Provides

- @ref interface_blobfinder : outputs blob information on detected 
  barcodes
- @ref interface_camera : passes through image data from underlying 
  camera device (optional)

@par Requires

- @ref interface_camera

@par Configuration requests

- none

@par Configuration file options

- edgeThresh (float)
  - Default: 20
  - Edge threshold
- bit_width (length)
  - Default: 0.08 m
  - Width of a single bit
- bit_count (integer)
  - Default: 3
  - Number of bits per digit
- guardMin (integer)
  - Default: 3
  - Minimum height of bit (pixels)
- guardTol (length)
  - Default: 0.2 m
  - Height tolerance for bit (ratio)
- digit_errFirst (float)
  - Default: 0.5
  - Error threshold on the best bit
- digit_errSecond (float)
  - Default: 1.0
  - Error threshold on the second-best bit

@par Example

@verbatim
driver
(
  name "upcbarcode"
  provides ["blobfinder:0"]
  requires ["camera:0"]
)
@endverbatim

@author Andrew Howard

*/

/** @} */

#include "../../base/imagebase.h"

#include <opencv/cv.h>
#include <opencv/highgui.h>

// Info on potential blobs.
struct blob_t
{
  // Id (-1) if undetermined.
  int id;

  // Blob bounding coords
  int ax, ay, bx, by;
};


// Driver for detecting laser retro-reflectors.
class UPCBarcode : public ImageBase
{
  // Constructor
  public: UPCBarcode( ConfigFile* cf, int section);

    // Setup/shutdown routines.
    virtual int Setup();
    virtual int Shutdown();

  // Look for barcodes in the image.  
  private: int ProcessFrame();

  // Extract a bit string from the image.  
  private: int ExtractSymbols(int x, int symbol_max_count, int symbols[][2]);

  // Extract a code from a symbol string.
  private: int ExtractCode(int symbol_count, int symbols[][2], int *min, int *max);
  
  // Write the device data (the data going back to the client).
  private: void WriteBlobfinderData();

  // Write the device data (the data going back to the client).
  //private: void WriteCameraData();


  // Image processing
  private: double edgeThresh;
  
  // Barcode tolerances
  private: int barcount;
  private: double barwidth;
  private: double guardMin, guardTol;
  private: double errFirst, errSecond;

 
  // Images
  private: IplImage *inpImage;
  private: IplImage *outImage;
  private: CvMat outSubImages[4];
  
  // Output camera stuff
  private: player_camera_data_t outCameraData;

  // List of currently tracked blobs.
  private: int blobCount;
  private: blob_t blobs[256];
};


// Initialization function
Driver* UPCBarcode_Init( ConfigFile* cf, int section)
{
  return ((Driver*) (new UPCBarcode( cf, section)));
}


// a driver registration function
void UPCBarcode_Register(DriverTable* table)
{
  table->AddDriver("upcbarcode", UPCBarcode_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
UPCBarcode::UPCBarcode( ConfigFile* cf, int section)
	: ImageBase(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_BLOBFINDER_CODE)
{
  // Image workspace
  this->inpImage = NULL;
  this->outImage = NULL;

  // Image processing
  this->edgeThresh = cf->ReadFloat(section, "edgeThresh", 20);
  
  // Default blobfinder properties.
  this->barwidth = cf->ReadLength(section, "bit_width", 0.08);
  this->barcount = cf->ReadInt(section, "bit_count", 3);

  // Barcode properties: minimum height (pixels), height tolerance (ratio).
  this->guardMin = cf->ReadInt(section, "guardMin", 3);
  this->guardTol = cf->ReadLength(section, "guardTol", 0.20);

  // Error threshold on the first and second best digits
  this->errFirst = cf->ReadFloat(section, "digit_errFirst", 0.5);
  this->errSecond = cf->ReadFloat(section, "digit_errSecond", 1.0);

  // Reset blob list.
  this->blobCount = 0;

  return;
}

////////////////////////////////////////////////////////////////////////////////
// Setup the device (called by server thread).
int UPCBarcode::Setup()
{
  return ImageBase::Setup();
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int UPCBarcode::Shutdown()
{
  ImageBase::Shutdown();

  if (this->inpImage)
    cvReleaseImage(&(this->inpImage));
  if (this->outImage)
    cvReleaseImage(&(this->outImage));
  inpImage = outImage = NULL;

  return 0;
}



////////////////////////////////////////////////////////////////////////////////
// Look for barcodes in the image.  This looks for verical barcodes,
// and assumes barcodes are not placed above each other.
int UPCBarcode::ProcessFrame()
{
  int x, step_x;
  int id, min, max;
  int symbol_count;
  int symbols[PLAYER_CAMERA_IMAGE_HEIGHT][2];
  blob_t *blob;

  int width, height;

  width = this->stored_data.width;
  height = this->stored_data.height;

  // Create input image if it doesnt exist
  if (this->inpImage == NULL)
    this->inpImage = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);

  // Copy pixels into input image
  switch (this->stored_data.format)
  {
    case PLAYER_CAMERA_FORMAT_MONO8:
    {
      // Copy pixels to input image (grayscale)
      assert(this->inpImage->imageSize >= (int) this->stored_data.image_count);
      memcpy(this->inpImage->imageData, this->stored_data.image, this->inpImage->imageSize);
      break;
    }
    default:
    {
      PLAYER_WARN1("image format [%d] is not supported", this->stored_data.format);
      return -1;
    }
  }

  // Create an output image if we dont have one already.
  if (this->outImage == NULL)
  {
    this->outImage = cvCreateImage(cvSize(2 * width, height), IPL_DEPTH_8U, 1);
    cvGetSubRect(this->outImage, this->outSubImages + 0, cvRect(0, 0, width, height));
    cvGetSubRect(this->outImage, this->outSubImages + 1, cvRect(width, 0, width, height));
  }

  // Copy original image to output
/*  if (this->out_camera_id.port)
  {
    cvSetZero(this->outImage);
    cvCopy(this->inpImage, this->outSubImages + 0);
  }*/

  step_x = 16;
  
  this->blobCount = 0;
  blob = NULL;

  // Process image columns
  for (x = 0; x < width; x += step_x)
  {
    // Extract raw symbols
    symbol_count = this->ExtractSymbols(x, sizeof(symbols) / sizeof(symbols[0]), symbols);

    // Identify barcode
    id = this->ExtractCode(symbol_count, symbols, &min, &max);

    if (id >= 0)
      printf("%d %d %d\n", x, min, id);

    // If we have an open blob, and didnt get the same id, close the blob
    if (blob != NULL && id != blob->id)
    {
      this->blobCount++;