MapBuilderRequest.cc
Go to the documentation of this file.00001
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;
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();
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 }