laservisualbarcode.cc

机器人仿真平台,和stage配合运行

        this->FitLaserFiducial(data, i - (int) mn, i - 1, pose);

        // Match this fiducial against the ones we are already tracking.
        this->MatchLaserFiducial(time, pose);
      }
      
      mn = 0.0;
      mr = 0.0;
      mb = 0.0;
      mrr = 0.0;
      mbb = 0.0;
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Find the line of best fit for the given segment of the laser scan.
// Fills in the pose and pose of the reflector relative to the laser.
void LaserVisualBarcode::FitLaserFiducial(player_laser_data_t *data,
                                          int first, int last, double pose[3])
{
  int i;
  double r, b;
  double mn, mr, mb;

  mn = 0.0;
  mr = 1e6;
  mb = 0.0;

  for (i = first; i <= last; i++)
  {
    r = (double) (data->ranges[i]) / 1000;
    b = (double) (data->min_angle + i * data->resolution) / 100.0 * M_PI / 180;

    if (r < mr)
      mr = r;
    mn += 1.0;
    mb += b;
  }

  mr += this->barwidth / 2;
  mb /= mn;

  pose[0] = mr * cos(mb);
  pose[1] = mr * sin(mb);
  pose[2] = mb;

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Match a new laser fiducial against the ones we are already
// tracking.  The pose is relative to the laser.
void LaserVisualBarcode::MatchLaserFiducial(double time, double pose[3])
{
  int i;
  double dx, dy, dr;
  double mindr;
  fiducial_t *fiducial;
  fiducial_t *minfiducial;
  
  // Observations must be at least this close to the existing
  // fiducial.
  mindr = this->max_dist;
  minfiducial = NULL;

  // Find the existing fiducial which is closest to the new
  // observation.
  for (i = 0; i < this->fiducial_count; i++)
  {
    fiducial = this->fiducials + i;

    dx = pose[0] - fiducial->pose[0];
    dy = pose[1] - fiducial->pose[1];
    dr = sqrt(dx * dx + dy * dy);
    if (dr < mindr)
    {
      mindr = dr;
      minfiducial = fiducial;
    }
  }

  // If we didnt find a matching fiducial, add a new one.
  if (minfiducial == NULL)
  {
    if (this->fiducial_count < ARRAYSIZE(this->fiducials))
    {
      minfiducial = this->fiducials + this->fiducial_count++;
      minfiducial->id = -1;
      minfiducial->pose[0] = pose[0];
      minfiducial->pose[1] = pose[1];
      minfiducial->pose[2] = pose[2];
      minfiducial->laser_time = time;
      minfiducial->ptz_select_time = -1;
      minfiducial->ptz_lockon_time = -1;
      minfiducial->id_time = -1;
    }
  }

  // Otherwise, update the existing fiducial.
  else
  {
    minfiducial->pose[0] = pose[0];
    minfiducial->pose[1] = pose[1];
    minfiducial->pose[2] = pose[2];
    minfiducial->laser_time = time;
  }
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Retire fiducials we havent seen for a while.
void LaserVisualBarcode::RetireLaserFiducials(double time, player_laser_data_t *data)
{
  int i;
  fiducial_t *fiducial;
  
  // Remove any old fiducials.
  for (i = 0; i < this->fiducial_count; i++)
  {
    fiducial = this->fiducials + i;

    if (time - fiducial->laser_time > this->retire_time)
    {
      if (this->ptz_fiducial == fiducial)
        this->ptz_fiducial = NULL;
      memmove(fiducial, fiducial + 1, (this->fiducial_count - i - 1) * sizeof(fiducial_t));
      this->fiducial_count--;
      i--;
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Update the PTZ to point at one of the laser reflectors.
int LaserVisualBarcode::UpdatePtz()
{
  player_ptz_data_t data;
  size_t size;
  uint32_t timesec, timeusec;
  double time;
  
  // Get the ptz data.
  size = this->ptz->GetData(this,(unsigned char*) &data, sizeof(data), &timesec, &timeusec);
  time = (double) timesec + ((double) timeusec) * 1e-6;
  
  // Dont do anything if this is old data.
  if (time == this->ptz_time)
    return 0;
  this->ptz_time = time;
  
  // Do some byte swapping on the ptz data.
  data.pan = ntohs(data.pan);
  data.tilt = ntohs(data.tilt);
  data.zoom = ntohs(data.zoom);

  // Pick a fiducial to look at.
  this->SelectPtzTarget(time, &data);

  // Point the fiducial
  this->ServoPtz(time, &data);

  return 1;
}


////////////////////////////////////////////////////////////////////////////////
// Select a target fiducial for the PTZ to inspect.
// This algorithm picks the one that we havent looked at for a long time.
void LaserVisualBarcode::SelectPtzTarget(double time, player_ptz_data_t *data)
{
  int i;
  double t, maxt;
  fiducial_t *fiducial;

  // Consider the currently selected target for a while to
  // give the blobfinder time to identify it.
  if (this->ptz_fiducial != NULL)
  {
    if (time - this->ptz_fiducial->ptz_select_time < this->max_ptz_attention)
      return;
  }

  // Find one we havent looked at for while.
  this->ptz_fiducial = NULL;
  maxt = -1;
  
  for (i = 0; i < this->fiducial_count; i++)
  {
    fiducial = this->fiducials + i;

    t = time - fiducial->ptz_select_time;
    if (t > maxt)
    {
      maxt = t;
      this->ptz_fiducial = fiducial;
    }
  }

  if (this->ptz_fiducial)
  {
    this->ptz_fiducial->ptz_select_time = time;
    this->ptz_fiducial->ptz_lockon_time = -1;
  }
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Servo the PTZ to a target fiducial.
void LaserVisualBarcode::ServoPtz(double time, player_ptz_data_t *data)
{
  double dx, dy, r, pan, tilt, zoom;
  fiducial_t *fiducial;
  player_ptz_cmd_t cmd;
  double maxtilt;
  double deadpan, deadzoom;

  // Max tilt value.
  maxtilt = 5 * M_PI / 180;

  // Deadband values.
  deadpan = 2;
  deadzoom = 2;
  
  fiducial = this->ptz_fiducial;
  if (fiducial == NULL)
  {
    r = 0;
    pan = 0;
    tilt = 0;
    zoom = M_PI;
  }
  else
  {
    // Compute range and bearing of fiducial relative to camera.
    // TODO: account for camera geometry.
    dx = fiducial->pose[0];
    dy = fiducial->pose[1];
    r = sqrt(dx * dx + dy * dy);
    pan = atan2(dy, dx);
    zoom = 8 * atan2(this->barwidth / 2, r);

    // See if we have locked on yet.
    if (fiducial->ptz_lockon_time < 0)
      if (fabs(pan * 180 / M_PI - data->pan) < deadpan &&
          fabs(zoom * 180 / M_PI - data->zoom) < deadzoom)
        fiducial->ptz_lockon_time = time;

    // If we havent locked on yet...
    if (fiducial->ptz_lockon_time < 0)
      tilt = 0;
    else
      tilt = maxtilt * sin((time - fiducial->ptz_lockon_time) /
                           this->max_ptz_attention * 2 * M_PI);
  }
  
  // Compose the command packet to send to the PTZ device.
  cmd.pan = htons(((int16_t) (pan * 180 / M_PI)));
  cmd.tilt = htons(((int16_t) (tilt * 180 / M_PI)));
  cmd.zoom = htons(((int16_t) (zoom * 180 / M_PI)));
  this->ptz->PutCommand(this,(unsigned char*) &cmd, sizeof(cmd));

  // Compute the dimensions of the image at the range of the target fiducial.
  this->zoomwidth = 2 * r * tan(data->zoom * M_PI / 180 / 2);
  this->zoomheight = 3.0 / 4.0 * this->zoomwidth;

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Process any new blobfinder data.
int LaserVisualBarcode::UpdateBlobfinder()
{
  int i, ch, id;
  player_blobfinder_data_t data;
  player_blobfinder_header_elt_t *channel;
  player_blobfinder_blob_elt_t *blob;
  size_t size;
  uint32_t timesec, timeusec;
  double time;
  
  // Get the blobfinder data.
  size = this->blobfinder->GetData(this,(unsigned char*) &data, sizeof(data), &timesec, &timeusec);
  time = (double) timesec + ((double) timeusec) * 1e-6;
  
  // Dont do anything if this is old data.
  if (time == this->blobfinder_time)
    return 0;
  this->blobfinder_time = time;

  // Do some byte-swapping
  data.width = ntohs(data.width);
  data.height = ntohs(data.height);
  for (ch = 0; ch < PLAYER_BLOBFINDER_MAX_CHANNELS; ch++)
  {
    channel = data.header + ch;
    channel->index = ntohs(channel->index);
    channel->num = ntohs(channel->num);
    
    for (i = channel->index; i < channel->index + channel->num; i++)
    {
      blob = data.blobs + i;
      blob->x = ntohs(blob->x);
      blob->y = ntohs(blob->y);
      blob->left = ntohs(blob->left);
      blob->right = ntohs(blob->right);
      blob->top = ntohs(blob->top);
      blob->bottom = ntohs(blob->bottom);
      blob->area = ntohl(blob->area);
    }
  }

  // Extract valid blobs.
  this->FindBlobs(time, &data);

  // Search for fiducials.
  id = this->FindVisualFiducials(time, &data, 0, NULL);

  // Assign id to fiducial we are currently looking at.
  if (id >= 0 && this->ptz_fiducial)
  {
    if (this->ptz_fiducial->ptz_lockon_time >= 0)
    {
      this->ptz_fiducial->id = id;
      this->ptz_fiducial->id_time = time;
    }
  }
  
  return 1;
}


////////////////////////////////////////////////////////////////////////////////
// Find blobs with valid properties.
void LaserVisualBarcode::FindBlobs(double time, player_blobfinder_data_t *data)
{
  int i, ch;
  blob_t *nblob;
  player_blobfinder_header_elt_t *channel;
  player_blobfinder_blob_elt_t *blob;
  int width, height;
  int minx, maxx, miny, maxy;
  int minwidth, maxwidth, minheight, maxheight;
  int minarea, maxarea;
  double tol;

  // Allowable tolerance (fractional error).
  tol = 0.5;

  // Compute expect width and height at the current range and zoom.
  width = (int) (this->barwidth / this->zoomwidth * data->width);
  height = (int) (this->barheight / this->zoomheight * data->height);
      
  // Set limits
  minx = (int) ((1 - tol) * data->width / 2);
  maxx = (int) ((1 + tol) * data->width / 2);
  miny = 0;
  maxy = data->height;
  minwidth = (int) ((1 - tol) * width);
  maxwidth = (int) ((1 + tol) * width);
  minheight = (int) ((1 - tol) * height);
  maxheight = (int) ((1 + tol) * height);
  minarea = 50;
  maxarea = maxwidth * maxheight;

  /*
  printf("zoom %.3f %.3f\n", this->zoomwidth, this->zoomheight);
  printf("image %d %d\n", data->width, data->height);
  printf("w, h %d %d\n", width, height);
  */
  
  this->blob_count = 0;
  for (ch = 0; ch < PLAYER_BLOBFINDER_MAX_CHANNELS; ch++)
  {
    channel = data->header + ch;
    
    for (i = channel->index; i < channel->index + channel->num; i++)
    {
      blob = data->blobs + i;

      // Test the blob properties.
      if (blob->x < minx || blob->x > maxx)
        continue;
      if (blob->y < miny || blob->y > maxy)
        continue;
      if ((blob->right - blob->left) < minwidth || (blob->right - blob->left) > maxwidth)
        continue;
      if ((blob->bottom - blob->top) < minheight || (blob->bottom - blob->top) > maxheight)
        continue;
      if ((int) blob->area < minarea || (int) blob->area > maxarea)
        continue;

      /*
      printf("%d %d : %d %d : %d\n", blob->x, blob->y,
             blob->right - blob->left, blob->bottom - blob->top, blob->area);
      */

      // Add to valid blob list.
      if (this->blob_count < ARRAYSIZE(this->blobs))
      {
        nblob = this->blobs + this->blob_count++;
        nblob->ch = ch;
        nblob->x = blob->x;
        nblob->y = blob->y;
      }
    }
  }
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Do a recursive depth-first search of the blob list for fiducals.
int LaserVisualBarcode::FindVisualFiducials(double time, player_blobfinder_data_t *data,
                                            int depth, blob_t *prevblob)
{
  int i, id;
  blob_t *blob;
  double dx, dy;
  double tol;
  double width, height;

  // Allowable tolerance (fractional error).
  tol = 0.5;

  // Compute expected width and height at the current range and zoom.
  width = (int) (this->barwidth / this->zoomwidth * data->width);
  height = (int) (this->barheight / this->zoomheight * data->height);

  for (i = 0; i < this->blob_count; i++)
  {
    blob = this->blobs + i;

    if (depth > 0)
    {
      dx = blob->x - prevblob->x;
      dy = blob->y - prevblob->y;

      if (fabs(dx) > (1 - tol) * width)
        continue;
      if (dy < (1 - tol) * height)
        continue;
      if (dy > (1 + tol) * height)
        continue;
    }

    if (depth == this->barcount - 1)
    {
      id = blob->ch;
      return id;
    }
    
    id = this->FindVisualFiducials(time, data, depth + 1, blob);
    if (id >= 0)
    {
      id = 10 * id + blob->ch;
      return id;
    }
  }

  return -1;
}


////////////////////////////////////////////////////////////////////////////////
// Update the device data (the data going back to the client).
void LaserVisualBarcode::UpdateData()
{
  int i;
  double r, b, o;
  uint32_t timesec, timeusec;
  fiducial_t *fiducial;
  player_fiducial_data_t data;

  data.count = 0;
  for (i = 0; i < this->fiducial_count; i++)
  {
    fiducial = this->fiducials + i;

    // Only report fiducials that where seen in the most recent laser
    // scan.
    if (fiducial->laser_time != this->laser_time)
      continue;
    
    r = sqrt(fiducial->pose[0] * fiducial->pose[0] +
             fiducial->pose[1] * fiducial->pose[1]);
    b = atan2(fiducial->pose[1], fiducial->pose[0]);
    o = fiducial->pose[2];

    data.fiducials[data.count].id = htons(((int16_t) fiducial->id));
    data.fiducials[data.count].pose[0] = htons(((int16_t) (1000 * r)));
    data.fiducials[data.count].pose[1] = htons(((int16_t) (180 * b / M_PI)));
    data.fiducials[data.count].pose[2] = htons(((int16_t) (180 * o / M_PI)));
    data.count++;
  }
  data.count = htons(data.count);
  
  // Compute the data timestamp (from laser).
  timesec = (uint32_t) this->laser_time;
  timeusec = (uint32_t) (fmod(this->laser_time, 1.0) * 1e6);
  
  // Copy data to server.
  PutData((unsigned char*) &data, sizeof(data), timesec, timeusec);
}