shapetracker.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: shapetracker.cc,v 1.14.2.1 2006/06/07 16:12:39 gerkey Exp $
//
// Theory of operation:
//   TODO
//
// Requires:
//   Camera device.
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_shapetracker shapetracker
    @brief Tracks shapes in camera images.

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

@todo Document this driver

@author Andrew Howard
*/
/** @} */

#include "player.h"

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

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

#define winName "ShapeTracker"

//#include "orientation.h"

// Info on potential shapes.
class Shape 
{
  public: Shape() : id(-1), ax(0),ay(0),bx(0),by(0) {}

  // Id (-1) if undetermined.
  public: int id;

  // Shape bounding coords
  public: int ax, ay, bx, by;
};



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

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

  private: void FindShapes();
  private: void KalmanFilter();

  private: void CreateBinaryImage(IplImage *src, IplImage *dest);

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

  // Calculate angle between three points
  private: double CalcAngle( CvPoint *pt1, CvPoint *pt2, CvPoint *pt0);

  private: void ContrastStretch( IplImage *src, IplImage *gray );

  private: IplImage *mainImage;
  private: IplImage *workImage;

  // Histogram attributes
  private: CvHistogram *hist;
  private: int histSize;
  private: unsigned char lut[256];
  private: CvMat *lutMat; 

  private: double threshold;
  private: int vertices;

  private: Shape shapes[256];
  private: unsigned int shapeCount;

  // Kalmna filters used to track a shape
  private: CvKalman *kalmanX;
  private: CvKalman *kalmanY;
  private: int kalmanFirst;

  private: CvPoint orientPoint;
  private: double trackVelocityX;
  private: double trackVelocityY;
  private: double trackHeading;

};


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


// a driver registration function
void ShapeTracker_Register(DriverTable* table)
{
  table->AddDriver("shapetracker", ShapeTracker_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
ShapeTracker::ShapeTracker( ConfigFile* cf, int section)
	: ImageBase(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_BLOBFINDER_CODE)
{
  this->mainImage = NULL;
  this->workImage = NULL;
  this->hist = NULL;

  this->threshold = 80;
  this->vertices = 8;

  this->shapeCount = 0;
}


////////////////////////////////////////////////////////////////////////////////
// Find all the shapes in the image
void ShapeTracker::FindShapes()
{
  double s, t1;
  CvRect rect;
  int i;

  CvMemStorage *storage = cvCreateMemStorage(0);
  CvSeq *contour = 0, *result = 0;

  // Find contours on a binary image
  cvFindContours(this->workImage, storage, &contour, sizeof(CvContour), 
      CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

  for(; contour != 0; contour = contour->h_next)
  {    
    // Approximates polygonal curves with desired precision
    result = cvApproxPoly(contour, sizeof(CvContour), storage, 
        CV_POLY_APPROX_DP, cvContourPerimeter(contour)*0.02, 0);

    printf("contour %p %d\n", contour, result->total);
    
  
    if ( result->total > 4 &&
        (result->total/2.0 == (int)(result->total/2)) &&
        fabs(cvContourArea(result, CV_WHOLE_SEQ)) > 50)
    {
      s = 0;
      for(i = 0; i < result->total +1; i++)
      {
        if(i >= 2)
        {
          t1 = fabs(this->CalcAngle(
                (CvPoint*)cvGetSeqElem(result, i ),
                (CvPoint*)cvGetSeqElem(result, i-2),
                (CvPoint*)cvGetSeqElem(result, i-1)));
          s = s > t1 ? s : t1;
          
        }
      }
      if(s < 0.5)
      {
        rect = cvBoundingRect(result,0);

        if (rect.x >5 && rect.y>5)/* && 
            rect.width < this->cameraData.width-5 && 
            rect.height < this->cameraData.height-5)*/
        {
          this->shapes[this->shapeCount].id = result->total;
          this->shapes[this->shapeCount].ax = rect.x;
          this->shapes[this->shapeCount].ay = rect.y;
          this->shapes[this->shapeCount].bx = rect.x + rect.width;
          this->shapes[this->shapeCount].by = rect.y + rect.height;

          this->shapeCount = (this->shapeCount+1) % 255;
          //cvDrawContours(this->workImage, result, 255, 0, 0, 3, 8);
          //cvSaveImage("workContours.jpg", this->workImage);
         // printf("Total[%d] XY[%d %d]\n",result->total, rect.x, rect.y);

        }

        //cvDrawContours(this->mainImage, result, 255, 0, 0, 5, 8);

        //this->orientPoint = getcentralpoint(this->mainImage, result);
      }
    }
  }

  cvReleaseMemStorage(&storage);
}


////////////////////////////////////////////////////////////////////////////////
// Run a Kalman filter 
void ShapeTracker::KalmanFilter()
{
  /*CvMoments moments;
  CvPoint pt3, pt4;
  int xmean, ymean;


  float xmeanp, ymeanp, xmeanvp, ymeanvp;
  //float orientation = 0.0;
  float vx, vy;

  //Calculates all moments up to third order of a polygon or rasterized shape
  cvMoments(this->workImage, &moments, 1);

  xmean = (int)(moments.m10/moments.m00);
  ymean = (int)(moments.m01/moments.m00);

  if ((xmean != 0) && (ymean != 0))
  {
    CvPoint tmpOrient;

    // Retrieves central moment from moment state structure
    float cm02 = cvGetCentralMoment(&moments, 0, 2);
    float cm20 = cvGetCentralMoment(&moments, 2, 0);
    float cm11 = cvGetCentralMoment(&moments, 1, 1);

    tmpOrient.x = xmean;
    tmpOrient.y = ymean;

    this->trackHeading = getorientation(tmpOrient, this->orientPoint);

    //fprintf(stderr, " heading %f\n", heading);
    //orientation = atan2(cm20 -cm02, 2*cm11)*180.0/(2*M_PI);
    if(this->kalmanFirst == 1)
    {
      fprintf(stderr, " x %f %f\n", (float)xmean, (float)ymean);
      this->kalmanX->state_post->data.fl[0] = (float) xmean;
      this->kalmanY->state_post->data.fl[1] = (float) 0.0;

      kalmany->state_post->data.fl[0] = (float) ymean;
      kalmany->state_post->data.fl[1] = (float) 0.0;
      this->kalmanFirst = 0;
    }
  }


  // If we have no x and y point, assume center of image
  if (xmean == 0 && ymean == 0)
  {
    xmean = (this->cameraData.width/2);
    ymean = (this->cameraData.height/2);
  }


  if (this->kalmanFirst == 0)