MapBuilderRequest.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_MapBuilderRequest_h_
#define INCLUDED_MapBuilderRequest_h_

#include "Behaviors/BehaviorBase.h"
#include "Shared/fmatSpatial.h"
#include "Vision/colors.h"   // for color_index

#include <queue>

#include "DualCoding/BlobData.h"
#include "DualCoding/MarkerData.h"
#include "DualCoding/Point.h"
#include "DualCoding/ShapeTypes.h"

using namespace std;

namespace DualCoding {

class LookoutSearchRequest;

class GazePoint {
  public:
    enum GazePointType_t {
      visible,
      centered
    } type;
    Point point;
    GazePoint(GazePointType_t _type, Point _point) : type(_type), point(_point) {}
    GazePoint() : type(), point() {}
  };

class MapBuilderRequest {
  friend class MapBuilder;

public:
  enum MapBuilderRequestType_t { 
    cameraMap, 
    groundMap,
    localMap, 
    worldMap
  };

  typedef unsigned int MapBuilderVerbosity_t;
  static coordinate_t defaultMarkerHeight;

  MapBuilderRequestType_t requestType;  //!< Type of map to build
  std::map<ShapeType_t, std::set<color_index> > objectColors;   //!< For each object type, a set of object color indices
  std::map<ShapeType_t, std::set<color_index> > secondColors;   //!< For two-colored objects (the bar color for dominoes)
  std::map<ShapeType_t, std::set<color_index> > occluderColors;   //!< For each object type, a set of occluder color indices
  float minLineLength; //!< Minimum length of a line with valid endpoints, for isAdmissible() test
  float minRayLength; //!< Minimum length of a line with an invalid endpoint, for isAdmissible() test
  float minEllipseSemiMajor; //!< Minimum length of ellipse semimajor axis, for isAdmissible() test
  std::map<color_index, int> minBlobAreas;  //!< Minimum acceptable areas for blobs of specified colors, e.g., req.minBlobAreas[pink_index]=50
  std::map<color_index, BlobData::BlobOrientation_t> blobOrientations; //!< Default orientation for blobs of specified colors
  std::map<color_index, coordinate_t> assumedBlobHeights; //!< Fixed heights for pillar or poster blobs of specified colors
  std::map<color_index, coordinate_t> assumedCylinderHeights; //!< Fixed heights for cylinders of specified colors
  std::set<MarkerType_t> markerTypes; //!< Types of markers to search for
  std::map<MarkerType_t, coordinate_t> markerHeights; //!< Assumed height above ground  in mm for markers of this type
  unsigned int floorColor;
  string siftDatabasePath; //!< Path to the SIFT object database to load
  std::set<std::string> siftObjectNames;  //!< Names of objects to search for; if empty, search for everything
  std::pair<int,int> aprilTagFamily; //!< AprilTag family to use, e.g., Tag16h5 would be std::pair<int,int>(16,5) 
  fmat::Column<3> dominoDimensions; //!< length, width, and height of a domino (in mm)
  fmat::Column<3> naughtDimensions; //!< length, width, and height of a naught (in mm)
  fmat::Column<3> crossDimensions; //!< length, width, and height of a cross (in mm)
  unsigned int motionSettleTime;  //!< Time in msec to wait before taking measurements or throwing completion event after head reaches gazePt.
  int numSamples; //!< Number of camera images to combine, for noise reduction
  int sampleInterval; //!< Interval in msec between successive samples
  float maxDist; //!< Ignore objects farther than this distance
  bool clearCamera; //!< If true (default), clear the camera sketch and shape spaces at start of request.
  bool clearLocal; //!< If true (default), clear the local sketch and shape spaces at start of request.
  bool clearWorld; //!< If true (default is false), clear the world sketch and shape spaces at start of request.
  bool ignoreCamera; //!< Don't clear or import shapes from camera space; just work in local and/or world space
  bool pursueShapes; //!< If true, generate new gaze points as shapes are recognized
  bool immediateRequest; //!< If true, the current camera image is processed immediately and results are available when MapBuilder::executeRequest() returns, so there's no need to set up an event listener.  Only possible if numSamples == 1 and no head motion is required.
  bool manualHeadMotion; //!< If true, waits for !msg MoveHead before moving to next gaze point (for debugging)
  bool rawY; //!< If true, leave an intensity (Y-channel) image in camera space for debugging
  bool removePts; //!< If true, remove pending gaze points if they're visible in current image
  bool doScan; //!< If true, do a continuous scan of the area to find interest points to be examined
  AngPi dTheta; //!< Angular step for scanning
  ShapeRoot searchArea; //!< The area to search, in egocentric or allocentric coords
  std::queue<LookoutSearchRequest*> worldTargets; //!< Queue of search requests for world targets
  void (*userImageProcessing)(); //!< User routine to call after a camera image is received
  void (*userCamProcessing)(); //!< User routine to call after cam space processing
  void (*userGroundProcessing)(); //!< User routine to call after ground space processing
  void (*userLocalProcessing)(); //!< User routine to call after local space processing
  void (*userWorldProcessing)(); //!< User routine to call after world space processing
  bool (*exitTest)(); //!< If true, terminate map building and post completion event
  enum GroundPlaneAssumption_t { onStand, onLegs, onWheel, custom } groundPlaneAssumption;
  PlaneEquation customGroundPlane; //!< User-supplied ground plane
  BehaviorBase* requestingBehavior; //!< The Behavior making this request; used for posting mapbuilder events
  MapBuilderVerbosity_t setVerbosity; //!< Verbosity elements to force on
  MapBuilderVerbosity_t clearVerbosity; //!< Verbosity elements to force off
  fmat::Transform baseTransform; //!< Used by the Pilot when rotating the body to simulate a pan/tilt

private:
  std::vector<GazePoint> gazePts;
  std::vector<fmat::Transform> baseToCamMats;
  unsigned int requestID; //!< Set by mapbuilder when the request is added to its request queue
  MapBuilderVerbosity_t verbosity; //!< Merger of global default verbosity settings and set/clear for this request

public:
  //! Constructor
  MapBuilderRequest(MapBuilderRequestType_t reqtype=cameraMap)
    : requestType(reqtype), objectColors(), secondColors(), occluderColors(), 
      minLineLength(70), minRayLength(40), minEllipseSemiMajor(9),
      minBlobAreas(), blobOrientations(), assumedBlobHeights(), 
      assumedCylinderHeights(), markerTypes(), markerHeights(), floorColor(0),
      siftDatabasePath(""), siftObjectNames(), aprilTagFamily(),
      dominoDimensions(fmat::pack(200.0, 89.0, 67.0)),
      naughtDimensions(fmat::pack(90.0, 90.0, 57.0)),
      crossDimensions(fmat::pack(100.0, 68.0, 41.0)),
      motionSettleTime(1000), numSamples(1), sampleInterval(0), maxDist(1e10f), 
      clearCamera(true), clearLocal(true), clearWorld(false), ignoreCamera(false),
      pursueShapes(false), immediateRequest(false), manualHeadMotion(false), rawY(true), removePts(true), 
      doScan(false), dTheta((float)M_PI/18),
      searchArea(), worldTargets(),
      userImageProcessing(NULL), userCamProcessing(NULL), userGroundProcessing(NULL), 
      userLocalProcessing(NULL), userWorldProcessing(NULL),
      exitTest(NULL),
      groundPlaneAssumption(onLegs), customGroundPlane(), requestingBehavior(NULL),
      setVerbosity(0), clearVerbosity(0),
      baseTransform(),
      gazePts(), baseToCamMats(),
      requestID(0),
      verbosity(0) // MapBuilder will set this during execution
  {}

  //! Copy constructor
  MapBuilderRequest(const MapBuilderRequest &req)
    : requestType(req.requestType),
      objectColors(req.objectColors), secondColors(req.secondColors), occluderColors(req.occluderColors),
      minLineLength(req.minLineLength), minRayLength(req.minRayLength),
      minEllipseSemiMajor(req.minEllipseSemiMajor),
      minBlobAreas(req.minBlobAreas), blobOrientations(req.blobOrientations), assumedBlobHeights(req.assumedBlobHeights), 
      assumedCylinderHeights(req.assumedCylinderHeights),
      markerTypes(req.markerTypes), markerHeights(req.markerHeights),
      floorColor(req.floorColor),
      siftDatabasePath(req.siftDatabasePath), siftObjectNames(req.siftObjectNames),
      aprilTagFamily(req.aprilTagFamily),
      dominoDimensions(req.dominoDimensions),
      naughtDimensions(req.naughtDimensions),
      crossDimensions(req.crossDimensions),
      motionSettleTime(req.motionSettleTime),
      numSamples(req.numSamples), sampleInterval(req.sampleInterval),
      maxDist(req.maxDist),
      clearCamera(req.clearCamera), clearLocal(req.clearLocal), clearWorld(req.clearWorld),
      ignoreCamera(req.ignoreCamera), pursueShapes(req.pursueShapes),
      immediateRequest(req.immediateRequest),
      manualHeadMotion(req.manualHeadMotion),
      rawY(req.rawY), removePts(req.removePts), 
      doScan(req.doScan), dTheta(req.dTheta),
      searchArea(req.searchArea), worldTargets(req.worldTargets),
      userImageProcessing(req.userImageProcessing), 
      userCamProcessing(req.userCamProcessing), 
      userGroundProcessing(req.userGroundProcessing), 
      userLocalProcessing(req.userLocalProcessing), 
      userWorldProcessing(req.userWorldProcessing),
      exitTest(req.exitTest),
      groundPlaneAssumption(req.groundPlaneAssumption),
      customGroundPlane(req.customGroundPlane),
      requestingBehavior(req.requestingBehavior),
      setVerbosity(req.setVerbosity), clearVerbosity(req.clearVerbosity),
      baseTransform(req.baseTransform),
      gazePts(req.gazePts), baseToCamMats(req.baseToCamMats),
      requestID(req.requestID), verbosity(req.verbosity)
      {}

  //! Assignment operator
  MapBuilderRequest& operator=(const MapBuilderRequest &req) {
    requestType = req.requestType;
    objectColors = req.objectColors;
    secondColors = req.secondColors;
    occluderColors = req.occluderColors;
    minLineLength = req.minLineLength;
    minRayLength = req.minRayLength;
    minEllipseSemiMajor = req.minEllipseSemiMajor;
    minBlobAreas = req.minBlobAreas;
    blobOrientations = req.blobOrientations;
    assumedBlobHeights = req.assumedBlobHeights;
    markerTypes = req.markerTypes;
    markerHeights = req.markerHeights;
    floorColor = req.floorColor;
    siftDatabasePath = req.siftDatabasePath;
    siftObjectNames = req.siftObjectNames;
    aprilTagFamily = req.aprilTagFamily;
    dominoDimensions = req.dominoDimensions;
    naughtDimensions = req.naughtDimensions;
    crossDimensions = req.crossDimensions;
    motionSettleTime = req.motionSettleTime;
    numSamples = req.numSamples;
    sampleInterval = req.sampleInterval;
    maxDist = req.maxDist;
    clearCamera = req.clearCamera;
    clearLocal = req.clearLocal;
    clearWorld = req.clearWorld;
    ignoreCamera = req.ignoreCamera;
    pursueShapes = req.pursueShapes;
    immediateRequest = req.immediateRequest;
    manualHeadMotion = req.manualHeadMotion;
    rawY = req.rawY;
    removePts = req.removePts;
    doScan = req.doScan;
    dTheta = req.dTheta;
    searchArea = req.searchArea;
    worldTargets = req.worldTargets;
    userImageProcessing = req.userImageProcessing;
    userCamProcessing = req.userCamProcessing;
    userGroundProcessing = req.userGroundProcessing;
    userLocalProcessing = req.userLocalProcessing;
    userWorldProcessing = req.userWorldProcessing;
    exitTest = req.exitTest;
    groundPlaneAssumption = req.groundPlaneAssumption;
    customGroundPlane = req.customGroundPlane;
    requestingBehavior = req.requestingBehavior;
    setVerbosity = req.setVerbosity;
    clearVerbosity = req.clearVerbosity;
    baseTransform = req.baseTransform;
    gazePts = req.gazePts;
    baseToCamMats = req.baseToCamMats;
    requestID = req.requestID;
    verbosity = req.verbosity;
    return *this;
  }

  virtual ~MapBuilderRequest() {} //!< Destructor

  MapBuilderRequestType_t getRequestType() const { return requestType; }

  //! Set of all non-zero/non-unsaturated colors (useful for requesting markers of all possible colors)
  std::set<color_index> allColors();

  //! Shortcut for specifying an objectColors entry
  void addObjectColor(ShapeType_t type, std::string const &color_name);
  
  //! Shortcut for specifying an objectColors entry
  void addObjectColor(ShapeType_t type, rgb color);

  //! Shortcut for specifying a secondColors entry
  void addSecondColor(ShapeType_t type, std::string const &color_name);
  
  //! Shortcut for specifying a secondColors entry
  void addSecondColor(ShapeType_t type, rgb color);

  //! Shortcut for specifying all object colors
  void addAllObjectColors(ShapeType_t type);

  //! Shortcut for specifying an occluderColors entry
  void addOccluderColor(ShapeType_t type, std::string const &color_name);
  
  //! Shortcut for specifying a minimum blob area
  void addMinBlobArea(std::string const &color_name, int area);

  //! Shortcut for specifying a minimum blob area for all blob colors
  void addAllMinBlobAreas(int area);

  //! Shortcut for specifying a blob's orientation and (for poster orientations) assumed height
  void addBlobOrientation(std::string const &color_name, BlobData::BlobOrientation_t orient, coordinate_t assumedHeight=0);
  
  //! Shortcut for specifying a blob's orientation and (for poster orientations) assumed height
  void addBlobOrientation(rgb color, BlobData::BlobOrientation_t orient, coordinate_t assumedHeight=0);
  
  //! Shortcut for specifying a cylinder's height
  void addCylinderHeight(std::string const &color_name, coordinate_t assumedHeight=0);
  
  //! Shortcut for specifying a cylinder's height
  void addCylinderHeight(rgb color, coordinate_t assumedHeight=0);

  //! Shortcut for specifying a marker type to look for
  void addMarkerType(MarkerType_t type);

  //! Shortcut for specifying a SIFT object type to look for
  void addSiftObject(string const &name);

  //! Look for shapes that could match this one
  void addAttributes(const ShapeRoot &shape);

  //! Shortcut for requesting AprilTag extraction using the smallest available tag family
  void setAprilTagFamily();

  //! Shortcut for selecting a specific AprilTag family
  void setAprilTagFamily(int bits, int minimumHammingDistance);

  //! Shortcut for requesting agent recognition (seeing other robots)
  void addAgentRecognition();

protected:
  bool validateRequest(); //!< check object types, colors, etc.
  static bool validateColors(const map<ShapeType_t,set<color_index> > &shapes2colors);
  bool validateSift();
  bool validateRequestType();
  static bool validateAdd(ShapeType_t type, std::string const &color_name);

};

} // namespace

#endif