simpleshape.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 simple shapes in a camera image
// Author: Andrew Howard
// Date: 15 Feb 2004
// CVS: $Id: simpleshape.cc,v 1.4.2.1 2006/06/07 16:12:39 gerkey Exp $
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_simpleshape simpleshape
 * @brief Visual shape-detection

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

The simpleshape driver looks for simple geometric shapes in a camera
image.  The user must supply a @e model of the target shape, in the
form of a binary image (such as the one shown below).

@image html simpleshape_h.gif "Sample model for the simpleshape detector"

@par Compile-time dependencies

- OpenCV

@par Requires

- This driver acquires image data from a @ref interface_camera
  interface.

@par Provides

- This driver provides detected shapes through a @ref
  interface_blobfinder interface.

- This driver also supplies processed image data through a @ref
  interface_camera interface (this data is intended mostly for
  debugging).  Note that the dimensions of the output image are twice
  that of the input image: the output image is divided into four
  parts, each showing one step in the detection process.  From
  top-to-bottom and left-to-right, these are: original image
  (monochrome), edge image, contour image, detected shapes.

@image html simpleshape_output.gif "Output image (debugging)"

@par Configuration requests

- none

@par Configuration file options

- model (string)
  - Default: NULL
  - Filename of the model image file.  This should by a binary,
    grayscale image.

- canny_thresh (float tuple) 
  - Default: [40 20]
  - Thresholds for the Canny edge detector.

- match_thresh (float tuple) 
  - Default: [0.50 20.0 0.20]
  - Match thresholds (?)

@par Example

@verbatim
driver
(
  name "simpleshape"
  requires ["camera:0"]
  provides ["blobfinder:1" "camera:1"]
  model "simpleshape_h.pgm"
)
@endverbatim

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

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

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

// Invariant feature set for a contour
class FeatureSet
{
  // Contour moments
  public: CvMoments moments;

  // Compactness
  public: double compact;

  // Elliptical variance
  public: double variance;

  // Vertices
  public: int vertexCount;
  public: int vertexString[256];
};


// Info on potential shapes.
class Shape 
{
  // Id (-1) if undetermined.
  public: int id;

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


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

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

  // Load a shape model
  private: int LoadModel();

  // Process the image
  private: int ProcessFrame();

  // Having pre-processed the image, find some shapes
  private: void FindShapes();

  // Extract a feature set for the given contour
  private: void ExtractFeatureSet(CvContour *contour, FeatureSet *feature);

  // Compute similarity measure on features
  private: int MatchFeatureSet(FeatureSet *a, FeatureSet *b);

  // Write the outgoing blobfinder data
  private: void WriteBlobfinderData();

  // Model data (this is the shape to search for)
  private: const char *modelFilename;
  private: CvMemStorage *modelStorage;
  private: CvSeq *modelContour;
  private: FeatureSet modelFeatureSet;

  // Images
  private: IplImage *inpImage;
  private: IplImage *outImage;
  private: CvMat outSubImages[4];
  private: IplImage *workImage;

  // Parameters
  private: double cannyThresh1, cannyThresh2;
  private: double matchThresh[3];

  // List of potential shapes
  private: Shape shapes[256];
  private: unsigned int shapeCount;
};


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


// a driver registration function
void SimpleShape_Register(DriverTable* table)
{
  table->AddDriver("simpleshape", SimpleShape_Init);
}


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

  // Filename for the target shape image
  this->modelFilename = cf->ReadFilename(section, "model", NULL);

  // Parameters
  this->cannyThresh1 = cf->ReadTupleFloat(section, "canny_thresh", 0, 40);
  this->cannyThresh2 = cf->ReadTupleFloat(section, "canny_thresh", 1, 20);

  this->matchThresh[0] = cf->ReadTupleFloat(section, "match_thresh", 0, 0.50);
  this->matchThresh[1] = cf->ReadTupleFloat(section, "match_thresh", 1, 20.0);
  this->matchThresh[2] = cf->ReadTupleFloat(section, "match_thresh", 2, 0.20);  

  this->shapeCount = 0;
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int SimpleShape::Setup()
{
  // Load the shape model
  if (this->LoadModel() != 0)
    return -1;

  return ImageBase::Setup();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int SimpleShape::Shutdown()
{
  ImageBase::Shutdown();
  
  // Free images
  if (this->inpImage)
    cvReleaseImage(&(this->inpImage));
  if (this->outImage)
    cvReleaseImage(&(this->outImage));

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Load a shape model
int SimpleShape::LoadModel()
{
  IplImage *img, *work;
  CvSize size;
  CvSeq *contour, *maxContour;
  double area, maxArea;

  // Load the image
  img = cvLoadImage(this->modelFilename, 0);
  if (img == NULL)
  {
    PLAYER_ERROR("failed to load model file");
    return -1;
  }

  // Create work image
  size = cvSize(img->width, img->height);
  work = cvCreateImage(size, IPL_DEPTH_8U, 1);

  // Find edges
  cvCanny(img, work, this->cannyThresh1, this->cannyThresh2);

  // Extract contours
  this->modelStorage = cvCreateMemStorage(0);
  cvFindContours(work, this->modelStorage, &contour, sizeof(CvContour), 
                 CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
  
  // Find the contour with the largest area (we will use the outer
  // contour only)
  maxArea = 0;
  maxContour = NULL;
  for (; contour != NULL; contour = contour->h_next)
  {
    area = fabs(cvContourArea(contour));
    if (area > maxArea)
    {
      maxArea = area;
      maxContour = contour;
    }
  }
  if (maxContour == NULL)
  {
    PLAYER_ERROR("no usable contours in model image");
    return -1;
  }
  this->modelContour = maxContour;

  // Record some features of the contour; we will use these to
  // recognise it later.
  this->ExtractFeatureSet((CvContour*) this->modelContour, &this->modelFeatureSet);

  // Free the image
  cvReleaseImage(&work);
  cvReleaseImage(&img);

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Look for stuff in the image.
int SimpleShape::ProcessFrame()
{
  CvSize size;
  int width, height;
  
  width = this->stored_data.width;
  height = this->stored_data.height;
  assert(width > 0 && height > 0);
    
  // Create input and output image if it doesnt exist
  size = cvSize(width, height);
  if (this->inpImage == NULL)
    this->inpImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
  size = cvSize(2 * width, 2 * height);
  if (this->outImage == NULL)
  {