upcbarcode.cc

机器人仿真软件

      blob = NULL;
    }

    // If we dont have a blob, and we got an id, open a blob
    if (blob == NULL && id >= 0)
    {
      assert(this->blobCount < (int) (sizeof(this->blobs) / sizeof(this->blobs[0])));
      blob = this->blobs + this->blobCount;
      blob->id = id;
      blob->ax = x;
      blob->bx = x + 1;
      blob->ay = min;
      blob->by = this->stored_data.height - 2;

        /*
      if (this->out_camera_id.port)
      {
        cvRectangle(this->outSubImages + 0, cvPoint(blob->ax, blob->ay),
                    cvPoint(blob->bx, blob->by), CV_RGB(0, 0, 0), 1);
        cvRectangle(this->outSubImages + 1, cvPoint(blob->ax, blob->ay),
                    cvPoint(blob->bx, blob->by), CV_RGB(255, 255, 255), 1);
      }
        */
    }

    // If we have an open blob, and got an id, continue the blob
    else if (blob != NULL && id >= 0)
    {
      blob->bx = x + 1;
    }
  }

  WriteBlobfinderData();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Extract a bit string from the image.  Takes a vertical column in
// the image and applies an edge detector
int UPCBarcode::ExtractSymbols(int x, int symbol_max_count, int symbols[][2])
{
  int i, j, off, inc, pix;
  double fn, fv;
  int state, start, symbol_count;
  double kernel[] = {+1, +2, 0, -2, -1};

  off = x;
  inc = this->inpImage->width;

  assert(symbol_max_count >= this->inpImage->height);

  state = -1;
  start = -1;
  symbol_count = 0;

  for (i = 2, pix = off + 2 * inc; i < this->inpImage->height - 2; i++, pix += inc)
  {
   // Run an edge detector
    fn = fv = 0.0;
    for (j = -1; j <= 1; j++)
    {
      fv += kernel[j + 2] * (*cvPtr2D(this->inpImage, i + j, x));
      fn += fabs(kernel[j + 2]);
    }
    fv /= fn;
    
    // Pick the transitions
    if (state == -1)
    {
      if (fv > +this->edgeThresh)
      {
        state = 1;
        start = i;
      }
      else if (fv < -this->edgeThresh)
      {
        state = 0;
        start = i;
      }
    }
    else if (state == 0)
    {
      if (fv > +this->edgeThresh)
      {
        symbols[symbol_count][0] = i;
        symbols[symbol_count++][1] = -(i - start);
        state = 1;
        start = i;
      }
    }
    else if (state == 1)
    {
      if (fv < -this->edgeThresh)
      {
        symbols[symbol_count][0] = i;
        symbols[symbol_count++][1] = +(i - start);
        state = 0;
        start = i;
      }
    }

    // TESTING
    *cvPtr2D(this->outSubImages + 1, i, x) = 127 + 127 * state;;
    
    //fprintf(file, "%d %d %f %f %d\n", i, this->cameraData.image[pix], fv, fn, state);
  }

  if (state == 0)
  {
    symbols[symbol_count][0] = i;
    symbols[symbol_count++][1] = -(i - start);
  }
  else if (state == 1)
  {
    symbols[symbol_count][0] = i;
    symbols[symbol_count++][1] = +(i - start);
  }

  //fprintf(file, "\n\n");
  //fclose(file);

  return symbol_count;
}


////////////////////////////////////////////////////////////////////////////////
// Extract a code from a symbol string.
int UPCBarcode::ExtractCode(int symbol_count, int symbols[][2], int *miny, int *maxy)
{
  int i, j, k;
  double a, b, c;
  double mean, min, max, wm, wo;
  int best_digit, best_miny;
  double best_err;
  double err[10];

  // These are UPC the mark-space patterns for digits.  From:
  // http://www.ee.washington.edu/conselec/Sp96/projects/ajohnson/proposal/project.htm
  double digits[][4] =
    {
      {-3,+2,-1,+1}, // 0
      {-2,+2,-2,+1}, // 1
      {-2,+1,-2,+2}, // 2
      {-1,+4,-1,+1}, // 3
      {-1,+1,-3,+2}, // 4
      {-1,+2,-3,+1}, // 5
      {-1,+1,-1,+4}, // 6
      {-1,+3,-1,+2}, // 7
      {-1,+2,-1,+3}, // 8
      {-3,+1,-1,+2}, // 9
    };

  best_digit = -1;
  best_miny = INT_MAX;
      
  // Note that each code has seven symbols in it, not counting the
  // initial space.
  for (i = 0; i < symbol_count - 7; i++)
  {
    /*
    for (j = 0; j < 7; j++)
      printf("%+d", symbols[i + j]);
    printf("\n");
    */

    a = symbols[i][1];
    b = symbols[i + 1][1];
    c = symbols[i + 2][1];

    // Look for a start guard: +N-N+N
    if (a > this->guardMin && b < -this->guardMin && c > this->guardMin)
    {
      mean = (a - b + c) / 3.0;
      min = MIN(a, MIN(-b, c));
      max = MAX(a, MAX(-b, c));
      assert(mean > 0);

      if ((mean - min) / mean > this->guardTol)
        continue;
      if ((max - mean) / mean > this->guardTol)
        continue;

      //printf("guard %d %.2f\n", i, mean);

      best_err = this->errFirst;
      best_digit = -1;
      best_miny = INT_MAX;
      
      // Read the code digit (4 symbols) and compare against the known
      // digit patterns
      for (k = 0; k < (int) (sizeof(digits) / sizeof(digits[0])); k++)
      {
        err[k] = 0;        
        for (j = 0; j < 4; j++)
        {
          wm = digits[k][j];
          wo = symbols[i + 3 + j][1] / mean;
          err[k] += fabs(wo - wm);
          //printf("digit %d = %.3f %.3f\n", k, wm, wo);
        }
        //printf("digit %d = %.3f\n", k, err[k]);

        if (err[k] < best_err)
        {
          best_err = err[k];
          best_digit = k;
          best_miny = symbols[i][0];
        }
      }

      // Id is good if it fits one and *only* one pattern.  So find the
      // second best digit and make sure it has a much higher error.
      for (k = 0; k < (int) (sizeof(digits) / sizeof(digits[0])); k++)
      {
        if (k == best_digit)
          continue;
        if (err[k] < this->errSecond)
        {
          best_digit = -1;
          break;
        }
      }

      // Stop if we found a valid digit
      if (best_digit >= 0)
        break;
    }    
  }

  //if (best_digit >= 0)
  //  printf("best = %d\n", best_digit);

  *miny = best_miny;
  
  return best_digit;
}


////////////////////////////////////////////////////////////////////////////////
// Update the device data (the data going back to the client).
void UPCBarcode::WriteBlobfinderData()
{
  int i;
  blob_t *blob;
//  size_t size;
  player_blobfinder_data_t data;

  data.width = (this->stored_data.width);
  data.height = (this->stored_data.height);

  data.blobs_count = (this->blobCount);
    
  for (i = 0; i < this->blobCount; i++)
  {
    blob = this->blobs + i;

    data.blobs[i].id = blob->id;  // TODO
    data.blobs[i].color = 0;  // TODO
    data.blobs[i].area = ((int) ((blob->bx - blob->ax) * (blob->by - blob->ay)));
    data.blobs[i].x = ((int) ((blob->bx + blob->ax) / 2));
    data.blobs[i].y = ((int) ((blob->by + blob->ay) / 2));
    data.blobs[i].left = ((int) (blob->ax));
    data.blobs[i].right = ((int) (blob->ay));
    data.blobs[i].top = ((int) (blob->bx));
    data.blobs[i].bottom = ((int) (blob->by));
    data.blobs[i].range = (0);
  }
    
  // Copy data to server.
  Publish(device_addr,NULL,PLAYER_MSGTYPE_DATA,PLAYER_BLOBFINDER_DATA_BLOBS,&data,sizeof(data));
  // Copy data to server
/*  size = sizeof(data) - sizeof(data.blobs) + this->blobCount * sizeof(data.blobs[0]);
  PutMsg(blobfinder_id, NULL, PLAYER_MSGTYPE_DATA, 0, (unsigned char*) &data, size, &this->cameraTime);		
  */
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Write camera data; this is a little bit naughty: we re-use the
// input camera data, but modify the pixels
/*void UPCBarcode::WriteCameraData()
{
  size_t size;
  
  if (this->camera_id.port == 0)
    return;
  if (this->outImage == NULL)
    return;

  // Do some byte swapping
  this->outCameraData.width = htons(this->outImage->width);
  this->outCameraData.height = htons(this->outImage->height);
  this->outCameraData.bpp = 8;
  this->outCameraData.format = PLAYER_CAMERA_FORMAT_MONO8;
  this->outCameraData.compression = PLAYER_CAMERA_COMPRESS_RAW;
  this->outCameraData.image_size = htonl(this->outImage->imageSize);

  // Copy in the pixels
  memcpy(this->outCameraData.image, this->outImage->imageData, this->outImage->imageSize);

  // Compute message size
  size = sizeof(this->outCameraData) - sizeof(this->outCameraData.image)
    + this->outImage->imageSize;

  // Copy data to server
  //this->PutData(this->out_camera_id, &this->outCameraData, size, &this->cameraTime);
  PutMsg(out_camera_id, NULL, PLAYER_MSGTYPE_DATA, 0, &this->outCameraData, size, &this->cameraTime);		
  
  return;
}*/