MapBuilderRequest.cc

Go to the documentation of this file.

//-*-c++-*-

#include "Shared/ProjectInterface.h"
#include "Vision/SegmentedColorGenerator.h"
#include "Vision/AprilTags/TagFamily.h"

#include "Crew/MapBuilderRequest.h"
#include "Crew/MapBuilder.h"

#include "DualCoding/ShapeBlob.h"
#include "DualCoding/ShapeCylinder.h"
#include "DualCoding/ShapeMarker.h"
#include "DualCoding/ShapeDomino.h"

using namespace std;

namespace DualCoding {

coordinate_t MapBuilderRequest::defaultMarkerHeight = 7.0f * 25.4f;  // 7 inches converted to millimeters

bool MapBuilderRequest::validateRequest() {
  return validateColors(objectColors) & validateColors(occluderColors) & validateColors(secondColors) &
    validateSift() & validateRequestType();
}

bool MapBuilderRequest::validateAdd(ShapeType_t type, std::string const &color_name) {
  if ( type >= numDataTypes ) {
    cerr << "*** Warning: MapBuilderRequest::add(...) called with invalid shape type " << type << endl;
    return false;
  }
  const color_index c = ProjectInterface::getColorIndex(color_name);
  if ( c == -1U) {
    cerr << "*** Warning: MapBuilderRequest::add(...) called with invalid color name '" << color_name << "'" << endl;
    return false;
  }
  return true;
}  

bool MapBuilderRequest:: validateColors(const map<ShapeType_t,set<color_index> > &shapes2colors) {
  color_index const numcolors = ProjectInterface::getNumColors();
  bool valid = true;
  for ( map<ShapeType_t,set<color_index> >::const_iterator shape_it = shapes2colors.begin();
  shape_it != shapes2colors.end(); shape_it++ ) {
    if ( shape_it->first == 0 || shape_it->first >= numDataTypes ) {
      cerr << "*** Warning: Mapbuilder request contains invalid shape type: " << shape_it->first << endl;
      valid = false;
    }
    for ( set<color_index>::const_iterator color_it = shape_it->second.begin();
    color_it != shape_it->second.end(); color_it++ )
      if ( *color_it > numcolors ) {
  valid = false;
  cerr << "*** Warning: MapBuilder request involving " << data_name(shape_it->first)
       << " contains invalid color index " << *color_it << endl;
      }
  }
  return valid;
}

bool MapBuilderRequest::validateSift() {
  return siftObjectNames.empty() || !siftDatabasePath.empty();
}

bool MapBuilderRequest:: validateRequestType() {
  if ( (pursueShapes || doScan || worldTargets.size()>0) &&
       getRequestType() == MapBuilderRequest::cameraMap ) {
    cout << "Warning: switching MapBuilderRequest type from cameraMap to localMap because request parameters require head movement." << endl;
    requestType = MapBuilderRequest::localMap;
  }
  if ( immediateRequest )
    if ( pursueShapes || doScan || searchArea.isValid() || worldTargets.size() > 0 || numSamples > 1 ) {
      cout << "Warning: MapBuilderRequest setting immedateRequest = false because of conflict with pursueShapes, doScan, searchArea, worldTargets, or numSamples" << endl;
      immediateRequest = false;
    }
  return true;
}

std::set<color_index> MapBuilderRequest::allColors() {
  std::set<color_index> result;
  for (color_index i = 1; i < ProjectInterface::defSegmentedColorGenerator->getNumColors(); i++ )
    if ( strcmp(ProjectInterface::getColorName(i), "black") != 0 )
      result.insert(i);
  return result;
}

void MapBuilderRequest::addObjectColor(ShapeType_t type, std::string const &color_name) {
  if ( validateAdd(type,color_name) )
    objectColors[type].insert(ProjectInterface::getColorIndex(color_name));
}

void MapBuilderRequest::addObjectColor(ShapeType_t type, rgb color) {
  objectColors[type].insert(ProjectInterface::getColorIndex(color));
}

void MapBuilderRequest::addSecondColor(ShapeType_t type, std::string const &color_name) {
  if ( validateAdd(type,color_name) )
    secondColors[type].insert(ProjectInterface::getColorIndex(color_name));
}

void MapBuilderRequest::addSecondColor(ShapeType_t type, rgb color) {
  secondColors[type].insert(ProjectInterface::getColorIndex(color));
}

void MapBuilderRequest::addAllObjectColors(ShapeType_t type) {
  objectColors[type] = allColors();
}

void MapBuilderRequest::addOccluderColor(ShapeType_t type, std::string const &color_name) {
  if ( validateAdd(type,color_name) )
    occluderColors[type].insert(ProjectInterface::getColorIndex(color_name));
}
 
void MapBuilderRequest::addMinBlobArea(std::string const &color_name, int area) {
  if ( validateAdd(blobDataType,color_name) )
    minBlobAreas[ProjectInterface::getColorIndex(color_name)] = area;
}

void MapBuilderRequest::addAllMinBlobAreas(int area) {
  for (color_index i = 1; i < ProjectInterface::defSegmentedColorGenerator->getNumColors(); i++ )
    minBlobAreas[i] = area;
}

void MapBuilderRequest::addBlobOrientation(std::string const &color_name, BlobData::BlobOrientation_t orient, coordinate_t assumedHeight){
  blobOrientations[ProjectInterface::getColorIndex(color_name)] = orient;
  assumedBlobHeights[ProjectInterface::getColorIndex(color_name)] =  assumedHeight;
}

void MapBuilderRequest::addBlobOrientation(rgb color, BlobData::BlobOrientation_t orient, coordinate_t assumedHeight){
  blobOrientations[ProjectInterface::getColorIndex(color)] = orient;
  assumedBlobHeights[ProjectInterface::getColorIndex(color)] =  assumedHeight;
}
 
void MapBuilderRequest::addCylinderHeight(std::string const &color_name, coordinate_t assumedHeight){
  assumedCylinderHeights[ProjectInterface::getColorIndex(color_name)] =  assumedHeight;
}

  void MapBuilderRequest::addCylinderHeight(rgb color, coordinate_t assumedHeight){
  assumedCylinderHeights[ProjectInterface::getColorIndex(color)] =  assumedHeight;
}

void MapBuilderRequest::addMarkerType(MarkerType_t type) {
  markerTypes.insert(type);
}

void MapBuilderRequest::addSiftObject(string const &name) {
  siftObjectNames.insert(name);
}

void MapBuilderRequest::addAttributes(const ShapeRoot &shape) {
  ShapeType_t type = shape->getType();
  rgb color = shape->getColor();
  color_index cindex = ProjectInterface::getColorIndex(color);
  addObjectColor(type,color);
  switch ( type ) {
  case blobDataType: {
    const Shape<BlobData> &blob = ShapeRootTypeConst(shape,BlobData);
    blobOrientations[cindex] = blob->orientation;
    assumedBlobHeights[cindex] = blob->assumedHeight;
    break;}
  case markerDataType:
    addMarkerType(ShapeRootTypeConst(shape,MarkerData)->getMarkerType());
    break;
  case cylinderDataType:
    assumedCylinderHeights[cindex] = ShapeRootTypeConst(shape,CylinderData)->getHeight();
    break;
  case aprilTagDataType:
    setAprilTagFamily();  // *** should really check the family of the tag itself
    break;
  case dominoDataType:
    addSecondColor(type, ShapeRootTypeConst(shape,DominoData)->getLineColor());
  default:
    break;
  }
}

void MapBuilderRequest::setAprilTagFamily() {
  int const notFound = 100000;
  int bits = notFound, minimumHammingDistance;
  for ( std::map<std::pair<int,int>,AprilTags::TagFamily*>::const_iterator it = AprilTags::TagFamily::tagFamilyRegistry.begin();
  it != AprilTags::TagFamily::tagFamilyRegistry.end(); it++ )
    if ( it->first.first < bits ) {
      bits = it->first.first;
      minimumHammingDistance = it->first.second;
    }
  if ( bits == notFound ) {
    cerr << "*** Warning: MapBuilderRequest::setAprilTagFamily called but there are no tag families available!" << endl;
    return;
  }
  aprilTagFamily = std::pair<int,int>(bits,minimumHammingDistance);
}

void MapBuilderRequest::setAprilTagFamily(int bits, int minimumHammingDistance) {
  aprilTagFamily = std::pair<int,int>(bits,minimumHammingDistance);
  if ( AprilTags::TagFamily::tagFamilyRegistry[aprilTagFamily] == NULL )
    cerr << "*** Error: MapBuilderRequest::setAprilTagFamily cannot find requested "
   << bits << "h" << minimumHammingDistance << " tag family!" << endl;
}

void MapBuilderRequest::addAgentRecognition() {
  addObjectColor(agentDataType, "red");
}


} // namespace