Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MapBuilderRequest.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include "Shared/ProjectInterface.h"
00004 #include "Vision/SegmentedColorGenerator.h"
00005 #include "Vision/AprilTags/TagFamily.h"
00006 
00007 #include "Crew/MapBuilderRequest.h"
00008 #include "Crew/MapBuilder.h"
00009 
00010 #include "DualCoding/ShapeBlob.h"
00011 #include "DualCoding/ShapeCylinder.h"
00012 #include "DualCoding/ShapeMarker.h"
00013 #include "DualCoding/ShapeDomino.h"
00014 
00015 using namespace std;
00016 
00017 namespace DualCoding {
00018 
00019 coordinate_t MapBuilderRequest::defaultMarkerHeight = 7.0f * 25.4f;  // 7 inches converted to millimeters
00020 
00021 bool MapBuilderRequest::validateRequest() {
00022   return validateColors(objectColors) & validateColors(occluderColors) & validateColors(secondColors) &
00023     validateSift() & validateRequestType();
00024 }
00025 
00026 bool MapBuilderRequest::validateAdd(ShapeType_t type, std::string const &color_name) {
00027   if ( type >= numDataTypes ) {
00028     cerr << "*** Warning: MapBuilderRequest::add(...) called with invalid shape type " << type << endl;
00029     return false;
00030   }
00031   const color_index c = ProjectInterface::getColorIndex(color_name);
00032   if ( c == -1U) {
00033     cerr << "*** Warning: MapBuilderRequest::add(...) called with invalid color name '" << color_name << "'" << endl;
00034     return false;
00035   }
00036   return true;
00037 }  
00038 
00039 bool MapBuilderRequest:: validateColors(const map<ShapeType_t,set<color_index> > &shapes2colors) {
00040   color_index const numcolors = ProjectInterface::getNumColors();
00041   bool valid = true;
00042   for ( map<ShapeType_t,set<color_index> >::const_iterator shape_it = shapes2colors.begin();
00043   shape_it != shapes2colors.end(); shape_it++ ) {
00044     if ( shape_it->first == 0 || shape_it->first >= numDataTypes ) {
00045       cerr << "*** Warning: Mapbuilder request contains invalid shape type: " << shape_it->first << endl;
00046       valid = false;
00047     }
00048     for ( set<color_index>::const_iterator color_it = shape_it->second.begin();
00049     color_it != shape_it->second.end(); color_it++ )
00050       if ( *color_it > numcolors ) {
00051   valid = false;
00052   cerr << "*** Warning: MapBuilder request involving " << data_name(shape_it->first)
00053        << " contains invalid color index " << *color_it << endl;
00054       }
00055   }
00056   return valid;
00057 }
00058 
00059 bool MapBuilderRequest::validateSift() {
00060   return siftObjectNames.empty() || !siftDatabasePath.empty();
00061 }
00062 
00063 bool MapBuilderRequest:: validateRequestType() {
00064   if ( (pursueShapes || doScan || worldTargets.size()>0) &&
00065        getRequestType() == MapBuilderRequest::cameraMap ) {
00066     cout << "Warning: switching MapBuilderRequest type from cameraMap to localMap because request parameters require head movement." << endl;
00067     requestType = MapBuilderRequest::localMap;
00068   }
00069   if ( immediateRequest )
00070     if ( pursueShapes || doScan || searchArea.isValid() || worldTargets.size() > 0 || numSamples > 1 ) {
00071       cout << "Warning: MapBuilderRequest setting immedateRequest = false because of conflict with pursueShapes, doScan, searchArea, worldTargets, or numSamples" << endl;
00072       immediateRequest = false;
00073     }
00074   return true;
00075 }
00076 
00077 std::set<color_index> MapBuilderRequest::allColors() {
00078   std::set<color_index> result;
00079   for (color_index i = 1; i < ProjectInterface::defSegmentedColorGenerator->getNumColors(); i++ )
00080     if ( strcmp(ProjectInterface::getColorName(i), "black") != 0 )
00081       result.insert(i);
00082   return result;
00083 }
00084 
00085 void MapBuilderRequest::addObjectColor(ShapeType_t type, std::string const &color_name) {
00086   if ( validateAdd(type,color_name) )
00087     objectColors[type].insert(ProjectInterface::getColorIndex(color_name));
00088 }
00089 
00090 void MapBuilderRequest::addObjectColor(ShapeType_t type, rgb color) {
00091   objectColors[type].insert(ProjectInterface::getColorIndex(color));
00092 }
00093 
00094 void MapBuilderRequest::addSecondColor(ShapeType_t type, std::string const &color_name) {
00095   if ( validateAdd(type,color_name) )
00096     secondColors[type].insert(ProjectInterface::getColorIndex(color_name));
00097 }
00098 
00099 void MapBuilderRequest::addSecondColor(ShapeType_t type, rgb color) {
00100   secondColors[type].insert(ProjectInterface::getColorIndex(color));
00101 }
00102 
00103 void MapBuilderRequest::addAllObjectColors(ShapeType_t type) {
00104   objectColors[type] = allColors();
00105 }
00106 
00107 void MapBuilderRequest::addOccluderColor(ShapeType_t type, std::string const &color_name) {
00108   if ( validateAdd(type,color_name) )
00109     occluderColors[type].insert(ProjectInterface::getColorIndex(color_name));
00110 }
00111  
00112 void MapBuilderRequest::addMinBlobArea(std::string const &color_name, int area) {
00113   if ( validateAdd(blobDataType,color_name) )
00114     minBlobAreas[ProjectInterface::getColorIndex(color_name)] = area;
00115 }
00116 
00117 void MapBuilderRequest::addAllMinBlobAreas(int area) {
00118   for (color_index i = 1; i < ProjectInterface::defSegmentedColorGenerator->getNumColors(); i++ )
00119     minBlobAreas[i] = area;
00120 }
00121 
00122 void MapBuilderRequest::addBlobOrientation(std::string const &color_name, BlobData::BlobOrientation_t orient, coordinate_t assumedHeight){
00123   blobOrientations[ProjectInterface::getColorIndex(color_name)] = orient;
00124   assumedBlobHeights[ProjectInterface::getColorIndex(color_name)] =  assumedHeight;
00125 }
00126 
00127 void MapBuilderRequest::addBlobOrientation(rgb color, BlobData::BlobOrientation_t orient, coordinate_t assumedHeight){
00128   blobOrientations[ProjectInterface::getColorIndex(color)] = orient;
00129   assumedBlobHeights[ProjectInterface::getColorIndex(color)] =  assumedHeight;
00130 }
00131  
00132 void MapBuilderRequest::addCylinderHeight(std::string const &color_name, coordinate_t assumedHeight){
00133   assumedCylinderHeights[ProjectInterface::getColorIndex(color_name)] =  assumedHeight;
00134 }
00135 
00136   void MapBuilderRequest::addCylinderHeight(rgb color, coordinate_t assumedHeight){
00137   assumedCylinderHeights[ProjectInterface::getColorIndex(color)] =  assumedHeight;
00138 }
00139 
00140 void MapBuilderRequest::addMarkerType(MarkerType_t type) {
00141   markerTypes.insert(type);
00142 }
00143 
00144 void MapBuilderRequest::addSiftObject(string const &name) {
00145   siftObjectNames.insert(name);
00146 }
00147 
00148 void MapBuilderRequest::addAttributes(const ShapeRoot &shape) {
00149   ShapeType_t type = shape->getType();
00150   rgb color = shape->getColor();
00151   color_index cindex = ProjectInterface::getColorIndex(color);
00152   addObjectColor(type,color);
00153   switch ( type ) {
00154   case blobDataType: {
00155     const Shape<BlobData> &blob = ShapeRootTypeConst(shape,BlobData);
00156     blobOrientations[cindex] = blob->orientation;
00157     assumedBlobHeights[cindex] = blob->assumedHeight;
00158     break;}
00159   case markerDataType:
00160     addMarkerType(ShapeRootTypeConst(shape,MarkerData)->getMarkerType());
00161     break;
00162   case cylinderDataType:
00163     assumedCylinderHeights[cindex] = ShapeRootTypeConst(shape,CylinderData)->getHeight();
00164     break;
00165   case aprilTagDataType:
00166     setAprilTagFamily();  // *** should really check the family of the tag itself
00167     break;
00168   case dominoDataType:
00169     addSecondColor(type, ShapeRootTypeConst(shape,DominoData)->getLineColor());
00170   default:
00171     break;
00172   }
00173 }
00174 
00175 void MapBuilderRequest::setAprilTagFamily() {
00176   int const notFound = 100000;
00177   int bits = notFound, minimumHammingDistance;
00178   for ( std::map<std::pair<int,int>,AprilTags::TagFamily*>::const_iterator it = AprilTags::TagFamily::tagFamilyRegistry.begin();
00179   it != AprilTags::TagFamily::tagFamilyRegistry.end(); it++ )
00180     if ( it->first.first < bits ) {
00181       bits = it->first.first;
00182       minimumHammingDistance = it->first.second;
00183     }
00184   if ( bits == notFound ) {
00185     cerr << "*** Warning: MapBuilderRequest::setAprilTagFamily called but there are no tag families available!" << endl;
00186     return;
00187   }
00188   aprilTagFamily = std::pair<int,int>(bits,minimumHammingDistance);
00189 }
00190 
00191 void MapBuilderRequest::setAprilTagFamily(int bits, int minimumHammingDistance) {
00192   aprilTagFamily = std::pair<int,int>(bits,minimumHammingDistance);
00193   if ( AprilTags::TagFamily::tagFamilyRegistry[aprilTagFamily] == NULL )
00194     cerr << "*** Error: MapBuilderRequest::setAprilTagFamily cannot find requested "
00195    << bits << "h" << minimumHammingDistance << " tag family!" << endl;
00196 }
00197 
00198 void MapBuilderRequest::addAgentRecognition() {
00199   addObjectColor(agentDataType, "red");
00200 }
00201 
00202 
00203 } // namespace

DualCoding 5.1CVS
Generated Mon May 9 04:56:26 2016 by Doxygen 1.6.3