Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MapBuilder.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #include "Events/EventRouter.h"
00003 #include "Events/TextMsgEvent.h"
00004 #include "Events/LookoutEvents.h"
00005 #include "Motion/HeadPointerMC.h"
00006 #include "Motion/Kinematics.h"
00007 #include "Shared/mathutils.h"
00008 #include "Shared/Measures.h"
00009 #include "Shared/Config.h"
00010 #include "Shared/MarkScope.h"
00011 
00012 #include "DualCoding/ShapeRoot.h"
00013 #include "DualCoding/ShapeLine.h"
00014 #include "DualCoding/ShapeEllipse.h"
00015 #include "DualCoding/ShapeBlob.h"
00016 #include "DualCoding/ShapeCylinder.h"
00017 #include "DualCoding/ShapePolygon.h"
00018 #include "DualCoding/ShapeSphere.h"
00019 #include "DualCoding/ShapeTarget.h"
00020 #include "DualCoding/ShapeMarker.h"
00021 #include "DualCoding/ShapeSift.h"
00022 #include "DualCoding/ShapeAprilTag.h"
00023 #include "DualCoding/ShapeDomino.h"
00024 #include "DualCoding/ShapeNaught.h"
00025 #include "DualCoding/ShapeCross.h"
00026 #include "DualCoding/Sketch.h"    // for NEW_SKETCH
00027 #include "DualCoding/visops.h"
00028 #include "DualCoding/VRmixin.h"
00029 
00030 #include "Crew/LookoutRequests.h"
00031 #include "Crew/Lookout.h"
00032 #include "Crew/MapBuilder.h"
00033 
00034 #include "Vision/SIFT/API/SiftTekkotsu.h"
00035 #include "Vision/AprilTags/TagDetector.h"
00036 #include "Vision/AprilTags/TagDetection.h"
00037 
00038 using namespace std;
00039 
00040 namespace DualCoding {
00041 
00042 inline float distSq(const fmat::Column<4>& vec) {
00043   return vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2];
00044 }
00045 
00046 MapBuilder::MapBuilder() : 
00047   BehaviorBase("MapBuilder"),
00048   camSkS(VRmixin::getCamSkS()), camShS(camSkS.getDualSpace()),
00049   groundShS(VRmixin::getGroundShS()),
00050   localSkS(VRmixin::getLocalSkS()), localShS(localSkS.getDualSpace()),
00051   worldSkS(VRmixin::getWorldSkS()), worldShS(worldSkS.getDualSpace()),
00052   xres(camSkS.getWidth()), yres(camSkS.getHeight()), 
00053   ground_plane(),
00054   theAgent(VRmixin::theAgent),
00055   localToWorldMatrix(fmat::Transform::identity()),
00056   worldToLocalMatrix(fmat::Transform::identity()), 
00057   badGazePoints(), 
00058   requests(), curReq(NULL), idCounter(0), maxDistSq(0), siftMatchers(),
00059   pointAtID(Lookout::invalid_LO_ID), scanID(Lookout::invalid_LO_ID),
00060   nextGazePoint() {}
00061 
00062 void MapBuilder::preStart() {
00063   BehaviorBase::preStart();
00064   if ( verbosity & MBVstart )
00065     cout << "MapBuilder::start()\n";
00066 
00067   camSkS.clear(); camShS.clear();
00068   groundShS.clear();
00069   localSkS.clear(); localShS.clear();
00070   worldSkS.clear(); worldShS.clear();
00071   badGazePoints.clear();
00072 
00073   erouter->addListener(this,EventBase::textmsgEGID);  // listen for commands to move to next gaze point
00074   erouter->addListener(this,EventBase::lookoutEGID);  // listen for Lookout completion events
00075 }
00076 
00077 bool MapBuilder::retain = true;
00078 
00079 MapBuilder::MapBuilderVerbosity_t MapBuilder::verbosity =
00080   (-1U & ~(MBVevents | MBVcomplete
00081 #ifdef TGT_HAS_WHEELS
00082            | MBVgroundPlane
00083 #endif
00084            | MBVgazePointQueue | MBVskipShape));
00085 
00086 /*
00087   Since MapBuilder is constructed as static from VRmixin, the
00088   destructor doesn't get called until the robot shuts down.  We have
00089   to do everything assumed to be done in destructor in doStop, such as
00090   clearing the request queue and setting parameters to the initial
00091   values as set in the constructor.
00092 */
00093 void MapBuilder::stop() {
00094   cout << "MapBuilder::stop()\n";
00095   while(!requests.empty()) {
00096     delete requests.front();
00097     requests.pop();
00098   }
00099   curReq = NULL;
00100   BehaviorBase::stop();
00101 }
00102 
00103 void MapBuilder::executeRequest(BehaviorBase *requestingBehavior, const MapBuilderRequest &req) {
00104   MapBuilderRequest newreq(req);
00105   newreq.requestingBehavior = requestingBehavior;
00106   executeRequest(newreq);
00107 }
00108 
00109 unsigned int MapBuilder::executeRequest(const MapBuilderRequest& req, unsigned int *req_id) {
00110   VRmixin::requireCrew("MapBuilder");
00111   MapBuilderRequest *newreq = new MapBuilderRequest(req);
00112   if ( !newreq->validateRequest() ) {
00113     cout << "*** Request rejected by MapBuilder" << endl;
00114     return 0;
00115   }
00116   const unsigned int this_request  = ++idCounter;
00117   newreq->requestID = this_request;
00118   if ( req_id != NULL ) *req_id = this_request;
00119   requests.push(newreq);
00120   executeRequest();
00121   return this_request;
00122 }
00123 
00124 void MapBuilder::executeRequest() {
00125   if ( curReq != NULL || requests.empty() ) return;
00126   curReq = requests.front();
00127   curReq->verbosity = (verbosity & ~curReq->clearVerbosity) | curReq->setVerbosity;
00128   if ( curReq->verbosity & MBVexecute )
00129     cout << "MapBuilder::executeRequest: execute " << curReq->requestID << endl;
00130   erouter->postEvent(EventBase::mapbuilderEGID, curReq->requestID, EventBase::activateETID,0);  
00131 
00132   calculateGroundPlane();
00133   maxDistSq = curReq->maxDist*curReq->maxDist;
00134   badGazePoints.clear();
00135  
00136   if ( curReq->clearCamera && ! curReq->ignoreCamera) {
00137     camShS.clear();
00138     camSkS.clear();
00139     curReq->gazePts.clear();
00140     curReq->baseToCamMats.clear();
00141   }
00142   if ( curReq->clearLocal ) {
00143     localShS.clear();
00144     localSkS.clear();
00145   }
00146   if ( curReq->clearWorld ) {
00147     worldShS.clear();
00148     worldSkS.clear();
00149   }
00150 
00151   if ( curReq->immediateRequest )
00152     grabCameraImageAndGo();
00153   else if ( ! curReq->searchArea.isValid() && curReq->worldTargets.size() == 0 )
00154     storeImage(false);
00155   else {
00156     setInitialGazePts();
00157     if ( curReq->doScan == true )
00158       return; // wait for Lookout to finish scanning
00159     else if ( curReq->worldTargets.size() > 0 )
00160       doNextSearch();
00161     else if ( determineNextGazePoint() )
00162       moveToNextGazePoint();
00163     else
00164       requestComplete();
00165   }
00166 }
00167   
00168 //================================================================
00169 
00170 /*
00171   There are three kinds of events for MapBuilder::doEvent to handle:
00172 
00173   1. TextMsg event saying to move to the next gaze point; only needed
00174   when we've asserted manual control of gaze point sequencing for
00175   debugging.
00176 
00177   2. Lookout event announcing that a "point at" request is complete and
00178   an image has been stored.  We can now go ahead and process that image.
00179 
00180   3. Lookout event announcing that a "scan" request is complete and gaze
00181   points have been stored.  We can now start examining those gaze points.
00182 */
00183 
00184 void MapBuilder::doEvent() {
00185   if ( curReq == NULL) return;
00186   if ( curReq->verbosity & MBVevents )
00187     cout << "MapBuilder got event " << event->getName() << endl;
00188 
00189   switch ( event->getGeneratorID() ) {
00190   case EventBase::textmsgEGID:
00191     if ( strcmp(dynamic_cast<const TextMsgEvent&>(*event).getText().c_str(),"MoveHead") == 0 )
00192       moveToNextGazePoint(true);
00193     break;
00194 
00195   case EventBase::lookoutEGID:
00196     if ( event->getSourceID() == pointAtID )
00197       processImage(dynamic_cast<const LookoutSketchEvent&>(*event));
00198     else if ( event->getSourceID() == scanID ) {
00199       const vector<Point>& pts = dynamic_cast<const LookoutScanEvent*>(event)->getTasks().front()->data;
00200       cout << " doScan found " << pts.size() << " interest points." << endl;
00201       for (vector<Point>::const_iterator it = pts.begin(); it != pts.end(); it++)
00202         curReq->gazePts.push_back(GazePoint(GazePoint::visible, *it));
00203     }
00204     else {
00205       cout << "MapBuilder: unexpected Lookout event " << event->getDescription()
00206            << ",   current pointAtID=" << pointAtID << ", scanID=" << scanID << endl;
00207       return;
00208     }
00209     // we've dealt with the event (processed an image or did a scan); now see what we should do next
00210     if ( requestExitTest() )
00211       requestComplete();
00212     else if ( curReq->worldTargets.size() > 0 )
00213       doNextSearch();
00214     else if ( determineNextGazePoint() )
00215       moveToNextGazePoint();
00216     else
00217       requestComplete();
00218     break;
00219 
00220   default:
00221     cout << "MapBuilder received unexpected event: " << event->getDescription() << endl;
00222   }
00223 }
00224 
00225 void MapBuilder::processImage(const LookoutSketchEvent &e) {
00226   const fmat::Transform& camToBase = curReq->baseTransform * e.toBaseMatrix;
00227   const fmat::Transform baseToCam = camToBase.rigidInverse();
00228   if ( ! curReq->ignoreCamera ) {
00229     if ( curReq->clearCamera ) {
00230       camSkS.clear(false); // don't clear retained sketches
00231       camShS.clear();
00232     }
00233     if ( curReq->rawY ) {
00234       NEW_SKETCH(rawY, uchar, VRmixin::sketchFromRawY());
00235     }
00236     NEW_SKETCH(camFrame, uchar, e.getSketch());
00237     if (curReq->userImageProcessing != NULL) (*curReq->userImageProcessing)();
00238     getCameraShapes(camFrame);
00239     if (curReq->userCamProcessing != NULL) (*curReq->userCamProcessing)();
00240     if (curReq->getRequestType() > MapBuilderRequest::cameraMap) {
00241       projectToGround(camToBase);
00242       if (curReq->userGroundProcessing != NULL) (*curReq->userGroundProcessing)();
00243       filterGroundShapes(baseToCam);
00244     }
00245   }
00246   switch ( curReq->getRequestType() ) {
00247   case MapBuilderRequest::cameraMap:
00248   case MapBuilderRequest::groundMap:
00249     break;
00250   case MapBuilderRequest::localMap:
00251     if ( ! curReq->ignoreCamera )
00252       extendLocal(baseToCam);
00253     if (curReq->userLocalProcessing != NULL) (*curReq->userLocalProcessing)();
00254     break;
00255   case MapBuilderRequest::worldMap:
00256     if ( ! curReq->ignoreCamera )
00257       extendLocal(baseToCam);
00258     if (curReq->userLocalProcessing != NULL) (*curReq->userLocalProcessing)();
00259     extendWorld(baseToCam);
00260     if (curReq->userWorldProcessing != NULL) (*curReq->userWorldProcessing)();
00261   }
00262 }
00263 
00264 bool MapBuilder::determineNextGazePoint() {
00265   if ( (curReq->verbosity & MBVgazePointQueue) ) {
00266     std::cout << "GazePoint queue: ";
00267     if ( curReq->gazePts.empty() )
00268       std::cout << "empty";
00269     else
00270       for (std::vector<GazePoint>::const_iterator it = curReq->gazePts.begin();
00271            it != curReq->gazePts.end(); it++ )
00272         std::cout << " " << it->point;
00273     std::cout << std::endl;
00274   }
00275   if (curReq->getRequestType() == MapBuilderRequest::worldMap) {
00276     // worldMap
00277     worldShS.applyTransform(worldToLocalMatrix,egocentric);
00278     bool b = determineNextGazePoint(worldShS.allShapes()) || determineNextGazePoint(curReq->gazePts);
00279     worldShS.applyTransform(localToWorldMatrix,allocentric); // transform back
00280     return b;
00281   }
00282   else // localMap
00283     return determineNextGazePoint(localShS.allShapes()) || determineNextGazePoint(curReq->gazePts);
00284 }
00285 
00286   
00287 bool MapBuilder::determineNextGazePoint(const vector<ShapeRoot>& shapes) {
00288   if ( ! curReq->pursueShapes )
00289     return false;
00290   HeadPointerMC headpointer_mc;
00291   for (vector<ShapeRoot>::const_iterator it = shapes.begin();
00292        it != shapes.end(); it++) {
00293     // look for invalid endpoints of lines / polygons
00294     if ((*it)->isType(lineDataType) || (*it)->isType(polygonDataType)) {
00295       const Shape<LineData>& ld = ShapeRootTypeConst(*it,LineData);
00296       const Shape<PolygonData>& pd = ShapeRootTypeConst(*it,PolygonData);
00297       bool isLine = (*it)->isType(lineDataType);
00298       EndPoint p[2] = { isLine ? ld->end1Pt(): pd->end1Pt(), isLine ? ld->end2Pt() : pd->end2Pt()};
00299       for (int i = 0; i < 2; i++) {
00300         if ( !p[i].isValid() && !isBadGazePoint(p[i]) ) {
00301           cout << "Next gazepoint at endpoint" << (i+1) << " of shape id " 
00302                << (*it)->getId() << " at " << p[i] << endl;
00303           if ( !headpointer_mc.lookAtPoint(p[i].coordX(),p[i].coordY(),p[i].coordZ()) ) {
00304             if ( curReq->verbosity & MBVbadGazePoint )
00305               cout << "MapBuilder noting unreachable gaze point " << (Point)p[i] << endl;
00306             badGazePoints.push_back((Point)p[i]);
00307           }
00308           nextGazePoint = p[i];
00309           return true;
00310         }
00311       }
00312     }
00313     // look for blobs with incomplete bounding boxes
00314     if ( (*it)->isType(blobDataType) ) {
00315       const Shape<BlobData>& bd = ShapeRootTypeConst(*it,BlobData);
00316       if ( ! (bd->bottomValid && bd->topValid && bd->leftValid && bd->rightValid) ) {
00317         Point bcentroid = bd->getCentroid();
00318         if ( ! isBadGazePoint(bcentroid) &&
00319              badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), bcentroid)) {
00320           // blob may never be completely visible, so give it one try but don't keep trying
00321           badGazePoints.push_back(bcentroid);
00322           cout << "Next gazepoint for blob at " << bcentroid << endl;
00323           nextGazePoint = bcentroid;
00324           return true;
00325         }
00326       }
00327     }
00328     // look for shapes w/ <2 confidence
00329     /*
00330     if ((!(*it)->isType(agentDataType)) &&
00331         (*it)->getLastMatchId() != 0 &&
00332         (*it)->getConfidence() <= 1 &&
00333         ! isBadGazePoint((*it)->getCentroid()) &&
00334         badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (*it)->getCentroid()))  {
00335       const Point pt = (*it)->getCentroid();
00336       cout << "Next gaze point is shape " << (*it)
00337            << " (confidence level: " << (*it)->getConfidence() << ")" << endl;      
00338       cout << " at " << pt << endl;  
00339       if (! headpointer_mc.lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ()))
00340         badGazePoints.push_back(pt);
00341       nextGazePoint = pt;
00342       return true;
00343     }
00344     */
00345     
00346     if ( curReq->verbosity & MBVskipShape )
00347       cout << "Skipping shape " << (*it)
00348            << " (confidence level: " << (*it)->getConfidence() << ")" << endl;      
00349   }
00350   return false;
00351 }
00352 
00353 
00354 //! Find the next valid gazepoint in the queue, converting to egocentric if necessary.
00355 bool MapBuilder::determineNextGazePoint(vector<GazePoint>& gazePts) {
00356   if ( ! gazePts.empty() && (curReq->verbosity & MBVgazePointQueue) )
00357     std::cout << "Gazepoint queue has " << gazePts.size() << " entries." << std::endl;
00358   for ( vector<GazePoint>::iterator it = gazePts.begin();
00359         it != gazePts.end();  it = gazePts.erase(it) ) {
00360     if (it->point.getRefFrameType() == DualCoding::allocentric) {
00361       it->point.applyTransform(worldToLocalMatrix,egocentric);
00362       it->point.setRefFrameType(egocentric);
00363     }
00364     if ( ! isBadGazePoint(it->point) ) {
00365       nextGazePoint = it->point;
00366       gazePts.erase(it);
00367       badGazePoints.push_back(nextGazePoint);
00368       return true;
00369     }
00370   }
00371   return false;
00372 }
00373 
00374 void MapBuilder::moveToNextGazePoint(const bool manualOverride) {
00375   if ( curReq == NULL ) {
00376     cout << "curReq == NULL in moveToNextGazePoint!" << endl;
00377     return;
00378   }
00379   if ( (curReq->verbosity & MBVnextGazePoint) || (curReq->manualHeadMotion && manualOverride==false) )
00380     cout << "moveToNextGazePoint " << nextGazePoint << endl;
00381   if ( curReq->manualHeadMotion && manualOverride==false ) {
00382     cout << "To proceed to this gaze point:  !msg MoveHead" << endl;
00383     return;
00384   }
00385   else
00386     storeImage(true);
00387 }
00388 
00389 
00390 void MapBuilder::doNextSearch() {
00391   LookoutSearchRequest *curLSR = curReq->worldTargets.front();
00392   curReq->worldTargets.pop();
00393   pointAtID = VRmixin::lookout->executeRequest(*curLSR);
00394 }
00395 
00396 bool MapBuilder::isBadGazePoint(const Point& pt) const {
00397   const coordinate_t x = pt.coordX();
00398   const coordinate_t y = pt.coordY();
00399   if ( x*x + y*y > maxDistSq )
00400     return true;
00401   float const badGazePointRadiusSq = 100;
00402   for ( vector<Point>::const_iterator it = badGazePoints.begin();
00403         it != badGazePoints.end(); it++ )
00404     if ( ((x-it->coordX())*(x-it->coordX()) + (y-it->coordY())*(y-it->coordY())) < badGazePointRadiusSq )
00405       return true;
00406   return false;
00407 }
00408 
00409 void MapBuilder::storeImage(bool useNextGazePoint) {
00410   LookoutPointRequest lreq;
00411   lreq.motionSettleTime = curReq->motionSettleTime;
00412   lreq.numSamples = curReq->numSamples;
00413   lreq.sampleInterval = curReq->sampleInterval;
00414   if ( useNextGazePoint )
00415     lreq.setTarget(nextGazePoint);
00416   else
00417     lreq.setHeadMotionType(LookoutRequestBase::noMotion);
00418   pointAtID = VRmixin::lookout->executeRequest(lreq);
00419 }
00420 
00421 void MapBuilder::grabCameraImageAndGo() {
00422   // This is a performance hack to avoid calling the Lookout or event
00423   // router, so the MapBuilder can produce results very quickly when
00424   // we need real-time performance, e.g., for particle filtering where
00425   // we take multiple snapshots.
00426   pointAtID = 0;
00427   Sketch<uchar> camFrame(VRmixin::sketchFromSeg());
00428 #ifdef TGT_HAS_CAMERA
00429   const fmat::Transform camToBase = kine->linkToBase(CameraFrameOffset);
00430 #else
00431   const fmat::Transform &camToBase = fmat::Transform::identity();
00432 #endif
00433   LookoutSketchEvent dummy(true, camFrame, camToBase,
00434                            EventBase::lookoutEGID, pointAtID, EventBase::deactivateETID);
00435   processImage(dummy);
00436   requestComplete();
00437 }
00438 
00439 void MapBuilder::scanForGazePts() {
00440   LookoutScanRequest lreq;
00441   lreq.searchArea = curReq->searchArea;
00442   lreq.motionSettleTime = curReq->motionSettleTime;
00443   set<color_index> colors;  // colors of all the shape types we're looking for
00444   for (map<ShapeType_t,set<color_index> >::const_iterator it1 = curReq->objectColors.begin();
00445        it1 != curReq->objectColors.end(); it1++)
00446     for (set<color_index>::const_iterator it2 = it1->second.begin();
00447          it2 != it1->second.end(); it2++)
00448       colors.insert(*it2);
00449   lreq.addTask(LookoutScanRequest::VisionRegionTask(colors,curReq->dTheta));
00450   scanID = VRmixin::lookout->executeRequest(lreq);
00451 }
00452 
00453 void MapBuilder::extendLocal(const fmat::Transform& baseToCam) {
00454   vector<ShapeRoot> all = localShS.allShapes();
00455   removeNoise(localShS, baseToCam);
00456   matchSrcToDst(groundShS,localShS,curReq->objectColors[polygonDataType]);
00457   // Took out the call to removeGazePts because AprilTags can get cut off if their 
00458   // centroid is visible but near the edge of the camera image; need to foveate them.
00459   // 
00460   // removeGazePts(curReq->gazePts, baseToCam);
00461   curReq->baseToCamMats.push_back(baseToCam);
00462 }
00463 
00464 void MapBuilder::extendWorld(const fmat::Transform& baseToCam) {
00465   worldShS.applyTransform(worldToLocalMatrix,egocentric);
00466   removeNoise(worldShS, baseToCam);
00467   matchSrcToDst(localShS,worldShS,curReq->objectColors[polygonDataType]);
00468   worldShS.applyTransform(localToWorldMatrix,allocentric);
00469   removeGazePts(curReq->gazePts,baseToCam);
00470   curReq->baseToCamMats.push_back(baseToCam);
00471 }
00472 
00473 bool MapBuilder::requestExitTest() {
00474   if ( curReq->exitTest == NULL )
00475     return false;
00476   else
00477     return (*curReq->exitTest)();
00478 }
00479 
00480 void MapBuilder::requestComplete() {
00481   const size_t reqID = curReq->requestID;
00482   if ( curReq->verbosity & MBVcomplete )
00483     cout << "MapBuilderRequest " << reqID << " complete\n";
00484   BehaviorBase* reqbeh = curReq->requestingBehavior;
00485   delete curReq;
00486   curReq = NULL;
00487   requests.pop();
00488   if ( reqbeh )
00489     erouter->postEvent(EventBase::mapbuilderEGID, (size_t)reqbeh, EventBase::statusETID,0);
00490   else {
00491     erouter->postEvent(EventBase::mapbuilderEGID, reqID, EventBase::statusETID,0);
00492     erouter->postEvent(EventBase::mapbuilderEGID, reqID, EventBase::deactivateETID,0);
00493   }
00494   if ( requests.empty() )
00495     VRmixin::lookout->relax();
00496   else
00497     executeRequest(); // execute next request AFTER deactivate event has finished processing
00498 }
00499 
00500 void MapBuilder::setAgent(const Point &worldLocation, const AngTwoPi worldHeading, bool quiet) {
00501   if ( !quiet && ((curReq==NULL ? verbosity : curReq->verbosity) & MBVsetAgent) )
00502     cout << "Agent now at " << worldLocation << " hdg " << worldHeading 
00503          << " (= " << float(worldHeading)*180/M_PI << " deg.)" << endl;
00504   theAgent->setCentroidPt(worldLocation);
00505   theAgent->setOrientation(worldHeading);
00506   const coordinate_t dx = worldLocation.coordX();
00507   const coordinate_t dy = worldLocation.coordY();
00508   const coordinate_t dz = worldLocation.coordZ();
00509   float const c = cos(worldHeading);
00510   float const s = sin(worldHeading);
00511   float localToWorld[] =
00512     {c, -s, 0, dx,
00513      s,  c, 0, dy, 
00514      0,  0, 1, dz};
00515   localToWorldMatrix = fmat::Matrix<4,3>(localToWorld).transpose();;
00516   worldToLocalMatrix = localToWorldMatrix.inverse();
00517 }
00518 
00519 void MapBuilder::moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, coordinate_t const local_dz, AngSignPi dtheta) {
00520   Point const agentLoc = theAgent->getCentroid();
00521   AngTwoPi const heading = theAgent->getOrientation();
00522   float const c = cos(heading);
00523   float const s = sin(heading);
00524   float const dx = local_dx*c + local_dy*-s;
00525   float const dy = local_dx*s + local_dy*c;
00526   setAgent(agentLoc + Point(dx,dy,local_dz,allocentric), heading+dtheta);
00527 }
00528 
00529 void MapBuilder::importLocalToWorld() {
00530   worldShS.applyTransform(worldToLocalMatrix,egocentric);
00531   matchSrcToDst(localShS,worldShS);
00532   worldShS.applyTransform(localToWorldMatrix,allocentric);
00533 }
00534 
00535 ShapeRoot MapBuilder::importLocalShapeToWorld(const ShapeRoot &localShape) {
00536   ShapeRoot worldShape(worldShS.importShape(localShape));
00537   worldShape->applyTransform(localToWorldMatrix, allocentric);
00538   return worldShape;
00539 }
00540 
00541 ShapeRoot MapBuilder::importWorldToLocal(const ShapeRoot &worldShape) {
00542   ShapeRoot localShape(localShS.importShape(worldShape));
00543   localShape->applyTransform(worldToLocalMatrix,egocentric);
00544   return localShape;
00545 }
00546 
00547 bool MapBuilder::isPointVisible(const Point &pt, const fmat::Transform& baseToCam, float maxDistanceSq) {
00548   fmat::Column<3> camCoords = baseToCam*fmat::pack(pt.coordX(),pt.coordY(),pt.coordZ());
00549   //  if (camCoords(3) <=0 || distSq(camCoords) >= maxDistanceSq) return false;
00550   if ( fmat::SubVector<2>(camCoords).sumSq() >= maxDistanceSq )
00551     return false;
00552   float normX,normY; // normalized coordinates in cam frame
00553   config->vision.computePixel(camCoords[0],camCoords[1],camCoords[2],normX,normY);
00554   return (fabs(normX) < 0.9f && fabs(normY) < config->vision.aspectRatio*.9f); //normX and normY range from -1 to 1. Giving 10% offset here
00555 }
00556 
00557 bool MapBuilder::isLineVisible(const LineData& ln, const fmat::Transform& baseToCam) {
00558   float normX1,normX2,normY1,normY2;
00559   fmat::Column<3> camCoords;
00560   Point pt = ln.end1Pt();
00561   camCoords = baseToCam*fmat::pack(pt.coordX(),pt.coordY(),pt.coordZ());
00562   config->vision.computePixel(camCoords[0],camCoords[1],camCoords[2],normX1,normY1);
00563   pt = ln.end2Pt();
00564   camCoords = baseToCam*fmat::pack(pt.coordX(),pt.coordY(),pt.coordZ());
00565   config->vision.computePixel(camCoords[0],camCoords[1],camCoords[2],normX2,normY2);
00566   const float xRange = 0.9f;
00567   const float yRange = config->vision.aspectRatio*.9f; // y ranges ± aspect ratio, then 10% margin
00568   const bool end1Pt_visible = fabs(normX1) < xRange && fabs(normY1) < yRange;
00569   const bool end2Pt_visible = fabs(normX2) < xRange && fabs(normY2) < yRange;
00570   if (end1Pt_visible && end2Pt_visible)
00571     return true;
00572   LineData lnCam(VRmixin::groundShS, Point(normX1,normY1), Point(normX2,normY2));
00573   // define bounding box of camera frame in terms of normalized coordinates with 10% offset
00574   LineData camBounds[] = {LineData(VRmixin::groundShS, Point( xRange, yRange), Point( xRange,-yRange)),
00575                           LineData(VRmixin::groundShS, Point( xRange,-yRange), Point(-xRange,-yRange)),
00576                           LineData(VRmixin::groundShS, Point(-xRange,-yRange), Point(-xRange, yRange)),
00577                           LineData(VRmixin::groundShS, Point(-xRange, yRange), Point( xRange, yRange))};
00578   unsigned int ptCount = 0;
00579   Point p[2];
00580   // find if a portion of the line shows up in cam
00581   if (end1Pt_visible) p[ptCount++].setCoords(normX1,normY1,0); // end1Pt in frame
00582   else if (end2Pt_visible) p[ptCount++].setCoords(normX2,normY2,0); // end2Pt in frame
00583   for (int i = 0; i < 4; i++)
00584     if (camBounds[i].intersectsLine(lnCam)) {
00585       p[ptCount++].setCoords(lnCam.intersectionWithLine(camBounds[i]));
00586       // Let's say portion of line seen in cam should be longer than .1 normalized
00587       if (ptCount > 1)
00588         return p[0].distanceFrom(p[1]) > 0.1; 
00589     }
00590   return false;
00591 }
00592 
00593 bool MapBuilder::isShapeVisible(const ShapeRoot &ground_shape, const fmat::Transform& baseToCam,
00594                                 float maxDistanceSq) {
00595   if (ground_shape->isType(lineDataType))
00596     return isLineVisible(ShapeRootTypeConst(ground_shape,LineData).getData(), baseToCam);
00597   else if (ground_shape->isType(polygonDataType)) {
00598     const Shape<PolygonData>& polygon = ShapeRootTypeConst(ground_shape,PolygonData);
00599     for (vector<LineData>::const_iterator edge_it = polygon->getEdges().begin();
00600          edge_it != polygon->getEdges().end(); edge_it++)
00601       if (isLineVisible(*edge_it,baseToCam))
00602         return true;
00603     return false;
00604   }
00605   else 
00606     return isPointVisible(ground_shape->getCentroid(), baseToCam, maxDistanceSq);
00607 }
00608 
00609 
00610 // filter "bad" ground shapes before importing to dst shape space.
00611 // 1. ignore shapes too far from dog or projected to the other side of cam plane
00612 // 2. chop off line at max distance if it is extending beyond the distance and leave the endpoint invalid
00613 void MapBuilder::filterGroundShapes(const fmat::Transform& baseToCam) {
00614   //  cout << "MapBuilder::filterGroundShapes()" << endl;
00615   vector<ShapeRoot> ground_shapes = groundShS.allShapes();
00616 
00617   for (vector<ShapeRoot>::iterator ground_it = ground_shapes.begin();
00618        ground_it != ground_shapes.end(); ground_it++ ) {
00619     const coordinate_t cenX = (*ground_it)->getCentroid().coordX();
00620     const coordinate_t cenY = (*ground_it)->getCentroid().coordY();
00621     if (cenX*cenX + cenY*cenY > maxDistSq) { // too far
00622       if ( curReq->verbosity & MBVnotAdmissible )
00623         cout << "ground shape " << (*ground_it)->getId() << " (lastMatch " 
00624              << (*ground_it)->getLastMatchId() << ") too far, delete\n";
00625       ground_it->deleteShape();
00626     }
00627     fmat::Column<3> coords = Kinematics::pack(cenX,cenY,(*ground_it)->getCentroid().coordZ());
00628     coords = baseToCam*coords;
00629     if (coords[2] < 0) { // negative z-coordinate in camera frame indicates projection failed
00630       if ( curReq->verbosity & MBVprojectionFailed )
00631         cout << "Projection failed for ground shape " << (*ground_it)->getId()
00632              << ": " << coords
00633              << " (lastMatch " << (*ground_it)->getLastMatchId() << "): deleting\n";
00634       ground_it->deleteShape();
00635     }
00636     // if a line is extending to maxDistance, chop it off at maxdistance and mark the endpoint invalid
00637     else if ((*ground_it)->isType(lineDataType)) {
00638       Shape<LineData>& line = ShapeRootType(*ground_it,LineData);
00639       const coordinate_t e1x = line->end1Pt().coordX();
00640       const coordinate_t e1y = line->end1Pt().coordY();
00641       const coordinate_t e2x = line->end2Pt().coordX();
00642       const coordinate_t e2y = line->end2Pt().coordY();
00643       if (e1x*e1x + e1y*e1y > maxDistSq && e2x*e2x + e2y*e2y > maxDistSq)
00644         ground_it->deleteShape();
00645       else if (e1x*e1x + e1y*e1y > maxDistSq || e2x*e2x + e2y*e2y > maxDistSq) {
00646         //  cout << (*ground_it)->getId() << "(lastMatch " << (*ground_it)->getLastMatchId() 
00647         //       << ")  extends beyond maximum distance we want. Chop off the line" << endl;
00648         vector<float> line_abc = line->lineEquation_abc();
00649         Point pt;
00650         const EndPoint* far_ept = (e1x*e1x + e1y*e1y > maxDistSq) ? &line->end1Pt() : &line->end2Pt(); 
00651         if (line_abc[1] == 0.0) {
00652           const coordinate_t y_abs = sqrt(maxDistSq - line_abc[2]*line_abc[2]);
00653           if (fabs(far_ept->coordY()-y_abs) < fabs(far_ept->coordY()+y_abs))
00654             pt.setCoords(e1x, y_abs, far_ept->coordZ());
00655           else
00656             pt.setCoords(e1x, -y_abs, far_ept->coordZ());
00657         }
00658         else {
00659           const float a = - line_abc[0]/line_abc[1];
00660           const float b = line_abc[2]/line_abc[1];
00661           const coordinate_t x1 = (std::sqrt((a*a+1)*maxDistSq-b*b)-a*b)/(a*a+1);
00662           const coordinate_t x2 = (-std::sqrt((a*a+1)*maxDistSq-b*b)-a*b)/(a*a+1);
00663           if (std::abs(far_ept->coordX()-x1) < std::abs(far_ept->coordX()-x2))
00664             pt.setCoords(x1, a*x1+b, far_ept->coordZ());
00665           else
00666             pt.setCoords(x2, a*x2+b, far_ept->coordZ());
00667         }
00668         EndPoint temp_endPt(pt);
00669         temp_endPt.setValid(false);
00670         //  cout << " (" << far_ept->coordX() << "," << far_ept->coordY() << ") => ("
00671         //       << pt.coordX() << "," << pt.coordY() << ")" << endl;
00672         if (e1x*e1x + e1y*e1y > maxDistSq)
00673           line->setEndPts(temp_endPt, line->end2Pt());
00674         else
00675           line->setEndPts(line->end1Pt(), temp_endPt);
00676         badGazePoints.push_back(pt);
00677       }
00678     }
00679   }
00680 }
00681 
00682 void MapBuilder::calculateGroundPlane() {
00683   switch(curReq->groundPlaneAssumption) {
00684   case MapBuilderRequest::onLegs:
00685     ground_plane = kine->calculateGroundPlane(); 
00686     if ( curReq->verbosity & MBVgroundPlane ) 
00687       cout << "Calculated ground plane: " << ground_plane << endl;
00688     break;
00689   case MapBuilderRequest::onStand:
00690 #if defined(TGT_ERS210) || defined(TGT_ERS220) || defined(TGT_ERS2xx)
00691     ground_plane = PlaneEquation(0,0,-1,200);
00692 #else
00693     ground_plane = PlaneEquation(0,0,-1,170);
00694 #endif
00695     // cout << "Hard-coded ground plane: " << NEWMAT::printmat(ground_plane) << endl;    
00696     break;
00697   case MapBuilderRequest::onWheel:
00698 #ifdef TGT_REGIS1
00699     std::cout << "Target Regis 1 Mapping";
00700     ground_plane = fmat::pack<float>(0,0,(-1/85.0),1);
00701 #endif
00702     break;    
00703   case MapBuilderRequest::custom:
00704     ground_plane = curReq->customGroundPlane;
00705   }
00706 }
00707 
00708 void MapBuilder::projectToGround(const fmat::Transform& camToBase) {
00709   VRmixin::projectToGround(camToBase, ground_plane);
00710 }
00711 
00712 ShapeRoot MapBuilder::projectToLocal(ShapeRoot &shape) {
00713 #ifdef TGT_HAS_CAMERA
00714   fmat::Transform camToBase = kine->linkToBase(CameraFrameOffset);
00715 #else
00716   fmat::Transform camToBase = fmat::Transform::identity();
00717 #endif
00718   ground_plane = kine->calculateGroundPlane();
00719   
00720   groundShS.importShape(shape);
00721   ShapeRoot &newShape = groundShS.allShapes().back();
00722   newShape->projectToGround(camToBase, ground_plane);
00723   localShS.importShape(newShape);
00724   return localShS.allShapes().back();
00725 }
00726   
00727 void MapBuilder::matchSrcToDst(ShapeSpace &srcShS, ShapeSpace &dstShS,
00728                                set<color_index> polCols, bool mergeSrc, bool mergeDst) {
00729   vector<ShapeRoot> src_shapes = srcShS.allShapes();
00730   vector<ShapeRoot> dst_shapes = dstShS.allShapes();
00731   vector<LineData> polygon_edges;
00732   
00733   // clean up src_shapes before messing with dst space
00734   std::set<int> markedForDeletion;
00735   vector<ShapeRoot> cleaned_src;
00736   for (vector<ShapeRoot>::iterator src_it = src_shapes.begin();
00737        src_it != src_shapes.end(); src_it++ ) {
00738     // mark inadmissible shapes for deletion
00739     if (!(*src_it)->isAdmissible()) {
00740       if (curReq && curReq->verbosity & MBVnotAdmissible )
00741         cout << "shape " << (*src_it)->getId() << " (lastMatch " 
00742              << (*src_it)->getLastMatchId() << ") is not admissible." << endl;
00743       // (*src_it)->printParams();
00744       markedForDeletion.insert((*src_it)->getId());
00745     }
00746     else if ((*src_it)->isType(polygonDataType) && (*src_it)->getParentId() != 0) {
00747       const vector<LineData>& edges = ShapeRootTypeConst(*src_it, PolygonData)->getEdges();
00748       // for old polygons not built by the user (parentId not 0), save
00749       // their edges and mark them for deletion because we're going to
00750       // rebuild them
00751       polygon_edges.insert(polygon_edges.end(), edges.begin(), edges.end());
00752       markedForDeletion.insert((*src_it)->getId());
00753     }
00754     else if ((*src_it)->isType(lineDataType)) {
00755       // mark lines for deletion (but save their edges) if we will be
00756       // incorporating them into polygons
00757       const color_index colorIndex = ProjectInterface::getColorIndex((*src_it)->getColor());
00758       if ( polCols.end() != find(polCols.begin(), polCols.end(), colorIndex)) {
00759         polygon_edges.push_back(ShapeRootTypeConst(*src_it, LineData).getData());
00760         markedForDeletion.insert((*src_it)->getId());
00761       }
00762     }
00763   }
00764   // now copy the shapes we didn't mark for deletion
00765   for ( vector<ShapeRoot>::iterator it = src_shapes.begin();
00766         it != src_shapes.end(); it++ )
00767     if ( markedForDeletion.find((*it)->getId()) == markedForDeletion.end() )
00768       cleaned_src.push_back(*it);
00769   src_shapes = cleaned_src;
00770 
00771   // merge shapes inside srcShS (useful for lines that should have been merged)
00772   if (mergeSrc) {
00773     markedForDeletion.clear();
00774     cleaned_src.clear();
00775     for ( vector<ShapeRoot>::iterator it = src_shapes.begin();
00776           it != src_shapes.end(); it++ )
00777       for ( vector<ShapeRoot>::iterator it2 = it+1;
00778             it2 != src_shapes.end(); it2++)
00779         if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2)) {
00780           if (curReq && curReq->verbosity & MBVshapesMerge )
00781             cout << "merging shape " << (*it)->getId() << " (from " << (*it)->getLastMatchId()
00782                  << ") and shape " << (*it2)->getId() << " (from " << (*it2)->getLastMatchId() << ")" << endl;
00783           markedForDeletion.insert((*it2)->getId());
00784         }
00785     // now copy the shapes we didn't mark for deletion
00786     for ( vector<ShapeRoot>::iterator it = src_shapes.begin();
00787           it != src_shapes.end(); it++ )
00788       if ( markedForDeletion.find((*it)->getId()) == markedForDeletion.end() )
00789         cleaned_src.push_back(*it);
00790     src_shapes = cleaned_src;
00791   }
00792 
00793   // update dst shapes from src shapes, and note existing polygons
00794   markedForDeletion.clear();
00795   cleaned_src.clear();
00796   std::set<int> markedForDeletionDst;
00797   vector<Shape<PolygonData> > existingPolygons;
00798   for (vector<ShapeRoot>::iterator dst_it = dst_shapes.begin();
00799        dst_it != dst_shapes.end(); dst_it++ ) {
00800     // cout << "update dest shape " << *dst_it << endl;
00801     if ( (*dst_it)->isType(polygonDataType) && (*dst_it)->getParentId() != 0 ) {
00802       existingPolygons.push_back(ShapeRootType(*dst_it,PolygonData));
00803       continue;
00804     }
00805     if ( (*dst_it)->isType(localizationParticleDataType) )
00806       continue;
00807     for (vector<ShapeRoot>::iterator src_it = src_shapes.begin();
00808          src_it != src_shapes.end(); src_it++)
00809       if ((*dst_it)->isMatchFor(*src_it) && (*dst_it)->updateParams((*src_it))) {
00810         (*dst_it)->increaseConfidence(2);  // add 2 because we'll subtract 1 at the end
00811         (*dst_it)->setLastMatchId((*src_it)->getId());
00812         if (curReq && curReq->verbosity & MBVshapeMatch )
00813           cout << "Matched src shape " << *src_it << " (lastMatch " << (*src_it)->getLastMatchId()
00814                << ") to dst shape " << (*dst_it)->getId() << endl;
00815         markedForDeletion.insert((*src_it)->getId());
00816       } else if((*dst_it)->getType() == agentDataType && (*src_it)->getType() == agentDataType &&
00817                 (*dst_it)->getName() == (*src_it)->getName()) {
00818         //any agents that were just seen and don't match an existing agent's position
00819         //but have the same name should take precedence over the existing agent
00820         //i.e. the agent moved and we are seeing it in a new position
00821         //cout << "marking " << (*dst_it)->getId() << " for deletion" << endl;
00822         markedForDeletionDst.insert((*dst_it)->getId());
00823         break;
00824       }
00825   }
00826 
00827   // now copy the shapes we didn't mark for deletion
00828   for ( vector<ShapeRoot>::iterator it = src_shapes.begin();
00829         it != src_shapes.end(); it++ )
00830     if ( markedForDeletion.find((*it)->getId()) == markedForDeletion.end() ) {
00831       cleaned_src.push_back(*it);
00832       if((*it)->getType() == agentDataType) {
00833         const Shape<AgentData> &a = ShapeRootTypeConst(*it, AgentData);
00834         cout << a->getName() << ", " << a->getId() << ": " << a->getCentroid() << ", " << a->getOrientation() << endl;
00835       }
00836     }
00837   src_shapes = cleaned_src;
00838   for(vector<ShapeRoot>::iterator it = dst_shapes.begin();
00839             it != dst_shapes.end(); ++it) {
00840     if(markedForDeletionDst.find((*it)->getId()) != markedForDeletionDst.end()) {
00841       dstShS.deleteShape(*it);
00842     }
00843   }
00844   
00845   // form polygons from lines and import unmatched src shapes into dstShS
00846   vector<Shape<PolygonData> > deletedPolygons;
00847   vector<ShapeRoot> newPolygons = PolygonData::rebuildPolygons(polygon_edges,existingPolygons,deletedPolygons);
00848   for (vector<Shape<PolygonData> >::iterator delete_it = deletedPolygons.begin();
00849        delete_it != deletedPolygons.end(); delete_it++) {
00850     delete_it->deleteShape();
00851   }
00852   dstShS.importShapes(newPolygons);
00853   dstShS.importShapes(src_shapes);
00854   if (curReq && curReq->verbosity & MBVimportShapes ) {
00855     std::map<ShapeType_t,int> shapeCounts;
00856     if ( ! newPolygons.empty() )
00857       shapeCounts[polygonDataType] += newPolygons.size();
00858     for (std::vector<ShapeRoot>::iterator it = src_shapes.begin();
00859          it != src_shapes.end(); it++)
00860       ++shapeCounts[(*it)->getType()];
00861     cout << dstShS.name << "ShS imported";
00862     int comma = 0;
00863     if ( src_shapes.empty() && newPolygons.empty() )
00864       cout << " nothing";
00865     else
00866       for (std::map<ShapeType_t,int>::const_iterator it = shapeCounts.begin();
00867            it != shapeCounts.end(); it++)
00868         cout << (comma++ ? "," : "") << " " << it->second << " " << data_name(it->first);
00869     cout << endl;
00870   }
00871   // match shapes inside dstShS
00872   // ***NOTE: this code is wrong because deleting a shape should force a restart of the iteration
00873   if (mergeDst) {
00874     dst_shapes = dstShS.allShapes();
00875     for ( vector<ShapeRoot>::iterator it = dst_shapes.begin();
00876           it != dst_shapes.end(); it++ ) {
00877       if ( (*it)->isType(localizationParticleDataType) || (*it)->isType(agentDataType) )
00878         continue;
00879       for ( vector<ShapeRoot>::iterator it2 = it+1;
00880             it2 < dst_shapes.end(); it2++)
00881         if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2,true)) {
00882           cout << "Matched src shape " << *it << " (lastMatch " 
00883                << (*it)->getLastMatchId()<< ") is a match for " 
00884                << *it2 << " (lastMatch " << (*it2)->getLastMatchId()
00885                << "), delete " << (*it2)->getId() << endl;;
00886           (*it)->setLastMatchId((*it2)->getLastMatchId());
00887           it2->deleteShape();
00888         }
00889     }
00890   }
00891 }
00892 
00893 // decrease confidence of those shapes that should have been seen in the last snap,
00894 // remove shapes from ShapeSpace if confidence becomes < 0
00895 void MapBuilder::removeNoise(ShapeSpace& ShS, const fmat::Transform& baseToCam) {
00896   //  cout << "MapBuilder::removeNoise()\n";
00897   vector<ShapeRoot> shapes = ShS.allShapes();
00898   for (vector<ShapeRoot>::iterator it = shapes.begin();
00899        it != shapes.end(); it++ ) {
00900     // If we were not looking for this shape in the last snap, then
00901     // it's not fair to decrease this shape's confidence.
00902     if (curReq->objectColors[(*it)->getType()].find(ProjectInterface::getColorIndex((*it)->getColor())) ==
00903         curReq->objectColors[(*it)->getType()].end())
00904       continue; 
00905     // For polygons, verify that each edge that should be visible is seen.
00906     // If not, then delete the edge and rebuild the polygon.
00907     if ( (*it)->isType(polygonDataType) && (*it)->getParentId() != 0 ) {
00908       Shape<PolygonData>& polygon = ShapeRootType(*it,PolygonData);
00909       vector<LineData>& edges = polygon->getEdgesRW();
00910       unsigned int edge_index = 0;
00911       for (vector<LineData>::iterator edge_it = edges.begin();
00912            edge_it != edges.end(); edge_it++, edge_index++) {
00913         if (isLineVisible(*edge_it, baseToCam)) {
00914           // if ( curReq->verbosity & MBVshouldSee )
00915           //   cout << "edge " << edge_index << " of polygon " << (*it)->getId() << "(confidence: " 
00916           //        << edge_it->getConfidence() << ") should be visible in this frame" << endl;
00917           edge_it->decreaseConfidence();
00918         }
00919       }
00920       vector<ShapeRoot> brokenPolygons = polygon->updateState();
00921       ShS.importShapes(brokenPolygons);
00922       if (!polygon->isAdmissible()) {
00923         if ( curReq->verbosity & MBVdeleteShape )
00924           cout << "delete polygon " << (*it)->getId() << " from map" << endl;
00925         it->deleteShape();
00926       }
00927     }
00928     else if ((!(*it)->isType(agentDataType)) && isShapeVisible(*it, baseToCam, maxDistSq)) {
00929       if ((*it)->getConfidence() < 0 ) {
00930         if ( curReq->verbosity & MBVshouldSee )
00931           cout << "shape " << (*it)->getId() << "(confidence: " << (*it)->getConfidence() 
00932                << ") should be visible in this frame; delete it" << endl;
00933         it->deleteShape();
00934       }
00935       else  // confidence good enough; decrement here because we'll increment later
00936         (*it)->decreaseConfidence(); // decrease confidence by 1 for every visible shape
00937     }
00938   }
00939 }
00940 
00941 //================ Gaze points ================
00942 
00943 void MapBuilder::setInitialGazePts() {
00944   const ShapeRoot &sArea = curReq->searchArea;
00945   if ( ! sArea.isValid() )
00946     return;
00947   else if ( curReq->doScan == true )
00948     scanForGazePts();
00949   else
00950     switch ( sArea->getType() ) {
00951 
00952     case pointDataType:
00953       curReq->gazePts.push_back(GazePoint(GazePoint::centered,sArea->getCentroid()));
00954       break;
00955 
00956     case lineDataType: {
00957       static const float meshSize=50;
00958       const Shape<LineData>& line = ShapeRootTypeConst(sArea,LineData);
00959       if ( curReq->doScan == true )
00960         scanForGazePts();
00961       else {
00962         int numIntervals = (int) (line->getLength()/meshSize);
00963         const EndPoint& ep1 = line->end1Pt();
00964         const EndPoint& ep2 = line->end2Pt();
00965         curReq->gazePts.push_back(GazePoint(GazePoint::centered,ep1));
00966         for (int i = 1; i < numIntervals; i++)
00967           curReq->gazePts.push_back(GazePoint(GazePoint::centered,(ep1*i + ep2*(numIntervals-i))/numIntervals));
00968         curReq->gazePts.push_back(GazePoint(GazePoint::centered,ep2));
00969       }
00970     }
00971       break;
00972 
00973     case polygonDataType:
00974       if ( curReq->doScan == true )
00975         scanForGazePts();
00976       else {
00977         const Shape<PolygonData> &poly = ShapeRootTypeConst(sArea,PolygonData);
00978         const vector<Point> &verts = poly->getVertices();
00979         for (std::vector<Point>::const_iterator it = verts.begin(); it != verts.end(); it++)
00980           curReq->gazePts.push_back(GazePoint(GazePoint::centered,*it));
00981       }
00982       break;
00983 
00984     default:
00985       cerr << "ERROR MapBuilder::setInitialGazePts: Supported searchArea shapes are Point, Line, and Polygon\n";
00986       requestComplete();
00987       break;
00988     }
00989 }
00990 
00991 void MapBuilder::removeGazePts(vector<GazePoint> &gazePts, const fmat::Transform& baseToCam) {
00992   if (curReq->removePts) {
00993     int num_points_seen = 0;
00994     for ( vector<GazePoint>::iterator it = gazePts.begin();
00995           it != gazePts.end(); it++ ) {
00996       if ( it->point == nextGazePoint || 
00997            (it->type == GazePoint::visible && isPointVisible(it->point,baseToCam,maxDistSq)) ) {
00998         if ( it->point != nextGazePoint )
00999           cout << "Removing already-visible gaze point " << it->point << endl;
01000         num_points_seen++;
01001         gazePts.erase(it--);
01002       }
01003     }
01004     // cout << num_points_seen << " pre-defined gaze points seen in last image, "
01005     //      << gazePts.size() << " left\n";
01006   }
01007 }
01008 
01009 
01010 //================================================================
01011 
01012 //Prints shapespace in the format to be used for particle filter on simulator
01013 void MapBuilder::printShS(ShapeSpace &ShS) const {
01014   cout << "MapBuilder::printShS()" << endl;
01015   unsigned int line_count = 0;
01016   vector<ShapeRoot> shapes = ShS.allShapes();
01017   for (vector<ShapeRoot>::const_iterator it = shapes.begin();
01018        it != shapes.end(); it++) {
01019     if ((*it)->isType(lineDataType)) {
01020       const Shape<LineData>& ld = ShapeRootTypeConst(*it,LineData);
01021       cout << (*it)->getId() << " " << lineDataType << " " 
01022            << ProjectInterface::getColorIndex((*it)->getColor()) 
01023            << " " << ld->end1Pt().coordX()  << " " << ld->end1Pt().coordY()
01024            << " " << ++line_count << " " << ld->getLength() << " " << ld->end1Pt().isValid() << endl; 
01025       cout << (*it)->getId() << " " << lineDataType << " " 
01026            << ProjectInterface::getColorIndex((*it)->getColor()) 
01027            << " " << ld->end2Pt().coordX()  << " " << ld->end2Pt().coordY()
01028            << " " << line_count << " " << ld->getLength() << " " << ld->end2Pt().isValid() << endl;
01029     }
01030     else {
01031       cout << (*it)->getId() << " " << (*it)->getType() << " " 
01032            << ProjectInterface::getColorIndex((*it)->getColor()) 
01033            << " " << (*it)->getCentroid().coordX()  << " " << (*it)->getCentroid().coordY() << endl;
01034     }
01035   }
01036 }
01037 
01038 
01039 //================ Shape extraction ================
01040 
01041 void MapBuilder::getCameraShapes(const Sketch<uchar>& camFrame) { 
01042   getCamLines(camFrame, curReq->objectColors[lineDataType], curReq->occluderColors[lineDataType]);
01043   getCamEllipses(camFrame, curReq->objectColors[ellipseDataType], curReq->occluderColors[ellipseDataType]);
01044   getCamPolygons(camFrame, curReq->objectColors[polygonDataType], curReq->occluderColors[polygonDataType]);
01045   getCamSpheres(camFrame, curReq->objectColors[sphereDataType], curReq->occluderColors[sphereDataType]);
01046   getCamCylinders(camFrame, curReq->objectColors[cylinderDataType], curReq->assumedCylinderHeights, curReq->minBlobAreas);
01047   getCamWalls(camFrame, curReq->floorColor);
01048   if ( curReq->numSamples == 1 && !curReq->searchArea.isValid() && 
01049        !curReq->objectColors[blobDataType].empty() && curReq->userImageProcessing == NULL )  // use CMVision's blob extraction from current camera frame
01050     getCamBlobs(curReq->objectColors[blobDataType]);
01051   else
01052     getCamBlobs(camFrame, curReq->objectColors[blobDataType], curReq->minBlobAreas, curReq->blobOrientations, curReq->assumedBlobHeights);
01053   getCamTargets(camFrame, curReq->objectColors[targetDataType], curReq->occluderColors[targetDataType]);
01054   getCamMarkers(camFrame, curReq->objectColors[markerDataType], curReq->occluderColors[markerDataType],
01055                 curReq->markerTypes);
01056   getCamDominoes(camFrame, curReq->objectColors[dominoDataType], curReq->secondColors[dominoDataType]);
01057 
01058   getCamNaughts(camFrame, curReq->objectColors[naughtDataType], curReq->naughtDimensions);
01059   getCamCrosses(camFrame, curReq->objectColors[crossDataType]);
01060 
01061   // ***** HACK: SHOULD BE GETTING THIS FROM LOOKOUT *****
01062   if ( !curReq->siftDatabasePath.empty() || curReq->aprilTagFamily.first != 0) {
01063     NEW_SKETCH_N(rawY, uchar, VRmixin::sketchFromRawY());
01064     getCamSiftObjects(rawY, curReq->siftDatabasePath, curReq->siftObjectNames);
01065     getCamAprilTags(rawY);
01066   }
01067   if ( ! curReq->objectColors[agentDataType].empty() ) {
01068     NEW_SKETCH_N(camFrameYUV, yuv, VRmixin::sketchFromYUV());
01069     getCamAgents(camFrame, camFrameYUV, curReq->objectColors[agentDataType]);
01070   }
01071 }
01072 
01073 vector<Shape<LineData> > MapBuilder::getCamLines(const Sketch<uchar> &camFrame, const set<color_index>& objectColors, 
01074                                                  const set<color_index>& occluderColors) const {
01075   vector<Shape<LineData> > linesReturned;
01076   if ( objectColors.empty() ) 
01077     return linesReturned;
01078   if ( curReq->verbosity & MBVshapeSearch )
01079     cout << "*** Find the lines ***" << endl;
01080   NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame));
01081   for ( set<color_index>::const_iterator it = occluderColors.begin();
01082         it != occluderColors.end(); it++ )
01083     occluders |= visops::minArea(visops::colormask(camFrame,*it));
01084 
01085   for (set<color_index>::const_iterator it = objectColors.begin();
01086        it != objectColors.end(); it++) {
01087     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01088     NEW_SKETCH_N(cleancolor,bool,visops::minArea(colormask));
01089     NEW_SKETCH_N(fatmask,bool,visops::fillin(cleancolor,1,2,8));
01090     NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01091     vector<Shape<LineData> > line_shapes(LineData::extractLines(skel,cleancolor|occluders));
01092     linesReturned.insert(linesReturned.end(), line_shapes.begin(), line_shapes.end());
01093     if ( curReq->verbosity & MBVshapesFound )
01094       cout << "Found " << line_shapes.size() << " " 
01095            << ProjectInterface::getColorName(*it) << " lines." << endl;
01096   }
01097   return linesReturned;
01098 }
01099 
01100 vector<Shape<EllipseData> > 
01101 MapBuilder::getCamEllipses(const Sketch<uchar> &camFrame,
01102                            const set<color_index>& objectColors, const set<color_index>& ) const {
01103   vector<Shape<EllipseData> > ellipses;
01104   if (objectColors.empty())
01105     return ellipses;
01106   if ( curReq->verbosity & MBVshapeSearch )
01107     cout << "*** Find the ellipses ***" << endl;
01108   for (set<color_index>::const_iterator it = objectColors.begin();
01109        it !=objectColors.end(); it++) {
01110     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01111     vector<Shape<EllipseData> > ellipse_shapes = EllipseData::extractEllipses(colormask);
01112     ellipses.insert(ellipses.end(), ellipse_shapes.begin(),ellipse_shapes.end());
01113     if ( curReq->verbosity & MBVshapesFound )
01114       cout << "Found " << ellipse_shapes.size() << " "
01115            << ProjectInterface::getColorName(*it) << " ellipses." << endl;
01116   }
01117   return ellipses;
01118 }
01119 
01120 void MapBuilder::getCamPolygons(const Sketch<uchar> &camFrame,
01121                                 const set<color_index>& objectColors,
01122                                 const set<color_index>& occluderColors) const {
01123   if ( objectColors.empty() ) 
01124     return;
01125   if ( curReq->verbosity & MBVshapeSearch )
01126     cout << "*** Find the polygons ***" << endl;
01127   NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame));
01128   for ( set<color_index>::const_iterator it = occluderColors.begin();
01129         it !=occluderColors.end(); it++ )
01130     occluders |= visops::colormask(camFrame,*it);
01131   
01132   for (set<color_index>::const_iterator it = objectColors.begin();
01133        it !=objectColors.end(); it++) {
01134     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01135     NEW_SKETCH_N(fatmask,bool,visops::fillin(colormask,1,2,8));
01136     NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01137     NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8));
01138     
01139     vector<Shape<LineData> > pLines = LineData::extractLines(fatskel,fatmask|occluders);
01140     vector<LineData> polygonLines;
01141     for ( vector<Shape<LineData> >::iterator ln_it = pLines.begin();
01142           ln_it != pLines.end(); ln_it++ ) {
01143       polygonLines.push_back(ln_it->getData());
01144       // cout << "pLine id = " << (*ln_it)->getId() << endl;
01145       ln_it->deleteShape();
01146     }
01147     vector<ShapeRoot> polygons = PolygonData::formPolygons(polygonLines);
01148     if ( curReq->verbosity & MBVshapesFound )
01149       cout << "Found " << polygons.size() << " polygons." << endl;
01150   }
01151 }
01152 
01153 void MapBuilder::getCamSpheres(const Sketch<uchar> &camFrame,
01154                                const set<color_index>& objectColors, const set<color_index>& ) const {
01155   vector<Shape<SphereData> > spheres;
01156   if ( objectColors.empty() )
01157     return;
01158   if ( curReq->verbosity & MBVshapeSearch )
01159     cout << "*** Find the spheres ***" << endl;
01160   for (set<color_index>::const_iterator it = objectColors.begin();
01161        it !=objectColors.end(); it++) {
01162     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01163     vector<Shape<SphereData> > sphere_shapes = SphereData::extractSpheres(colormask);
01164     spheres.insert(spheres.end(), spheres.begin(), spheres.end());
01165     if ( curReq->verbosity & MBVshapesFound )
01166       cout << "Found " << sphere_shapes.size() << " spheres." << endl;
01167   }
01168 }
01169 
01170 void MapBuilder::getCamCylinders(const Sketch<uchar>& camFrame,
01171                                  const set<color_index>& colors,
01172                                  const map<color_index,coordinate_t>& assumedHeights,
01173                                  const map<color_index,int>& minCylinderAreas) {
01174   if ( colors.empty() )
01175     return;
01176   if ( curReq->verbosity & MBVshapeSearch )
01177     cout << "*** Find the cylinders ***" << endl;
01178   int const maxcylinders = 50;
01179   vector<GazePoint> addGazePts;
01180   vector<Shape<CylinderData> > result(CylinderData::extractCylinders(camFrame, colors, assumedHeights, 
01181                                                                      minCylinderAreas, maxcylinders, addGazePts));
01182   if ( curReq->verbosity & MBVshapesFound )
01183     if ( !result.empty() )
01184       cout << "Found " << result.size() << " cylinders." << endl;
01185   if ( ! addGazePts.empty() ) {
01186     const fmat::Transform camToBase = kine->linkToBase(CameraFrameOffset);
01187     for ( std::vector<GazePoint>::iterator it = addGazePts.begin();
01188           it != addGazePts.end(); it++ ) {
01189       it->point.projectToGround(camToBase,ground_plane);
01190       // curReq->gazePts.insert(curReq->gazePts.begin(),*it);
01191       std::cout << "Blob not valid; would have added gazepoint at " << it->point << std::endl;
01192     }
01193   }
01194 }
01195   
01196 vector<Shape<LineData> > 
01197 MapBuilder::getCamWalls(const Sketch<uchar> &camFrame, unsigned int floorColor) const {
01198   if (floorColor == 0)
01199     return vector<Shape<LineData> >();
01200   if ( curReq->verbosity & MBVshapeSearch )
01201     cout << "*** Find the walls ***" << endl;
01202   const int camFrame_offset = 8;
01203   
01204   NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,floorColor));
01205   NEW_SKETCH_N(fillinmask ,bool,visops::fillin(colormask, 1, 6, 8)); //remove pixels w/ connectivity<6 (noise)
01206   NEW_SKETCH_N(fillinmask2 ,bool,visops::fillin(fillinmask, 2, 3, 8)); //remove pixels w/ connectivity<3 and fill in the others
01207   NEW_SKETCH_N(edgemask ,bool,visops::fillin(fillinmask2, 1, 5, 7)); //remove pixels w/ connectivity=8 (non-edge pixels)
01208   NEW_SKETCH_N(edgemask2 ,bool,visops::non_bounds(edgemask, camFrame_offset)); //remove pixels close to cam_bound
01209 
01210   NEW_SKETCH_N(occluders_floor, bool, (camFrame != uchar(0)) & (camFrame != uchar(floorColor)));
01211   NEW_SKETCH_N(occ_mask ,bool,visops::fillin(occluders_floor, 1, 8, 8)); //remove pixels w/ connectivity<7 (noises)
01212   usint const clear_dist = 15;
01213   Sketch<bool> not_too_close = (visops::mdist(occ_mask) >= clear_dist); 
01214   edgemask2 &= not_too_close; //remove pixels around occuluders
01215   
01216   NEW_SKETCH_N(fatmask ,bool,visops::fillin(edgemask2,2,2,8)); //make the remaining pixels fat
01217   NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01218   NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8));
01219   
01220   vector<Shape<LineData> > wall_bounds = PolygonData::extractPolygonEdges(fatskel,fatmask|occluders_floor);
01221 
01222   // larger offset from the cam frame should be applied to these lines
01223   // since all pixels near cam frame bounds are removed before extracting these lines.
01224   for (vector<Shape<LineData> >::iterator it = wall_bounds.begin();
01225        it != wall_bounds.end(); it++) {
01226     if (((*it)->end1Pt().coordX() < camFrame_offset*2.0 || (*it)->end1Pt().coordX() > xres - camFrame_offset*2.0
01227          || (*it)->end1Pt().coordY() < camFrame_offset*2.0 || (*it)->end1Pt().coordY() > yres - camFrame_offset*2.0)
01228         && (*it)->end1Pt().isValid())
01229       (*it)->end1Pt().setValid(false);
01230     if (((*it)->end2Pt().coordX() < camFrame_offset*2.0 || (*it)->end2Pt().coordX() > xres - camFrame_offset*2.0
01231          || (*it)->end2Pt().coordY() < camFrame_offset*2.0 || (*it)->end2Pt().coordY() > yres - camFrame_offset*2.0)
01232         && (*it)->end2Pt().isValid())
01233       (*it)->end2Pt().setValid(false);
01234   }
01235   
01236   if ( curReq->verbosity & MBVshapesFound )
01237     cout << "Found " << wall_bounds.size() << " wall boundary lines" << endl;
01238   return wall_bounds;
01239 }
01240 
01241 void MapBuilder::getCamBlobs(const Sketch<uchar>& camFrame,
01242                              const set<color_index>& colors,
01243                              const map<color_index,int>& minBlobAreas,
01244                              const map<color_index, BlobData::BlobOrientation_t>& blobOrientations,
01245                              const map<color_index,coordinate_t>& assumedBlobHeights) {
01246   if ( colors.empty() )
01247     return;
01248   int const maxblobs = 50;
01249   vector<Shape<BlobData> > result(BlobData::extractBlobs(camFrame, colors, minBlobAreas, blobOrientations, assumedBlobHeights, maxblobs));
01250   if ( curReq->verbosity & MBVshapesFound )
01251     if ( !result.empty() )
01252       cout << "Found " << result.size() << " blobs." << endl;
01253 }
01254 
01255 // The code below grabs blobs directly from the region generator stream, instead of
01256 // calling BlobgData::extractBlobs to do region extraction on camFrame 
01257 void MapBuilder::getCamBlobs(const set<color_index>& colors, int defMinBlobArea) {
01258   // don't check objectColors here because this may also be called by extractors for various marker types
01259   if ( curReq->verbosity & MBVshapeSearch )
01260     cout << "*** Find the blobs ***" << endl;
01261   for (set<color_index>::const_iterator it = colors.begin();
01262        it != colors.end(); it++) {
01263     int const minarea = (curReq->minBlobAreas.find(*it)==curReq->minBlobAreas.end()) ? 
01264       defMinBlobArea : curReq->minBlobAreas[*it];
01265     BlobData::BlobOrientation_t const orient = (curReq->blobOrientations.find(*it)==curReq->blobOrientations.end()) ? 
01266       BlobData::groundplane : curReq->blobOrientations[*it];
01267     coordinate_t const height = (curReq->assumedBlobHeights.find(*it)==curReq->assumedBlobHeights.end()) ? 
01268       0 : curReq->assumedBlobHeights[*it];
01269     vector<Shape<BlobData> > blob_shapes(VRmixin::getBlobsFromRegionGenerator(*it,minarea,orient,height));
01270     if ( curReq->verbosity & MBVshapesFound )
01271       if ( !blob_shapes.empty() )
01272         cout << "Found " << blob_shapes.size() << " "
01273              << ProjectInterface::getColorName(*it) << " region generator blobs." << endl;
01274   }
01275 }
01276 
01277 void MapBuilder::getCamTargets(const Sketch<uchar> &camFrame, const set<color_index>& objectColors, const set<color_index>& occluderColors) const {
01278   vector<Shape<TargetData> > targets;
01279   if (objectColors.empty())
01280     return;
01281   if ( curReq->verbosity & MBVshapeSearch )
01282     cout << "*** Find the targets ***" << endl;
01283   
01284   NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame));
01285   for (set<color_index>::const_iterator it = occluderColors.begin();
01286        it != occluderColors.end(); it++)
01287     occluders |= visops::minArea(visops::colormask(camFrame,*it));
01288   
01289   // assumes multiples of 3 for objectColors (stays on the last color otherwise)
01290   for (set<color_index>::const_iterator it = objectColors.begin();
01291        it != objectColors.end(); it++) {
01292     NEW_SKETCH_N(front_colormask, bool, visops::colormask(camFrame,*it));
01293     it++;
01294     if (it == objectColors.end()) {
01295       it--;
01296     }
01297     NEW_SKETCH_N(back_colormask, bool, visops::colormask(camFrame,*it));
01298     it++;
01299     if (it == objectColors.end()) {
01300       it--;
01301     }
01302     NEW_SKETCH_N(right_colormask, bool, visops::colormask(camFrame,*it));
01303     Shape<TargetData> target = TargetData::extractLineTarget(front_colormask, back_colormask, right_colormask, occluders);
01304     if (target.isValid()) {
01305       targets.insert(targets.end(), target);
01306     }
01307     if ( curReq->verbosity & MBVshapesFound )
01308       cout << "Found " << (target.isValid() ? 1 : 0) << " targets." << endl;
01309   }
01310 }
01311 
01312 vector<Shape<MarkerData> > 
01313 MapBuilder::getCamMarkers(const Sketch<uchar> &camFrame, const set<color_index>& objectColors,
01314                           const set<color_index>&, const set<MarkerType_t>& markerTypes) const
01315 {
01316   vector<Shape<MarkerData> > markers;
01317   if (objectColors.empty() || markerTypes.empty())
01318     return markers;
01319 
01320   if ( curReq->verbosity & MBVshapeSearch )
01321     cout << "*** Find the markers ***" << endl;
01322 
01323   /* Skip this and let the individual marker extraction algorithms do masking if they need it
01324      NEW_SKETCH_N(colormask,bool,visops::zeros(camFrame));
01325      for (set<color_index>::const_iterator it = objectColors.begin();
01326      it != objectColors.end(); it++) {
01327      colormask |= visops::colormask(camFrame,*it);
01328      }
01329      NEW_SKETCH_N(masked,uchar,visops::mask(camFrame,colormask));
01330   */
01331 
01332   for (set<MarkerType_t>::const_iterator it = markerTypes.begin();
01333        it != markerTypes.end(); it++) {
01334     vector<Shape<MarkerData> > single_type = MarkerData::extractMarkers(camFrame, *it, *curReq);
01335     markers.insert(markers.end(), single_type.begin(), single_type.end());
01336   }
01337 
01338   if ( curReq->verbosity & MBVshapesFound )
01339     cout << "Found " << markers.size() << " markers." << endl;
01340             
01341   return markers;
01342 }
01343 
01344 void MapBuilder::getCamSiftObjects(const Sketch<uchar> &rawY, const std::string &siftDatabasePath, 
01345                                    const std::set<std::string> &siftObjectNames) {
01346   if ( siftDatabasePath.empty() ) return;
01347   SiftTekkotsu *matcher = siftMatchers[siftDatabasePath];
01348   if ( matcher == NULL ) {
01349     matcher = new SiftTekkotsu;
01350     matcher->loadFile(siftDatabasePath);
01351     matcher->setParameter("probOfMatch", 0.8);
01352     siftMatchers[siftDatabasePath] = matcher;
01353   }
01354   ImageBuffer buff = matcher->sketchToBuffer(rawY);
01355   vector<SiftMatch*> results;
01356   if ( siftObjectNames.empty() )
01357     matcher->findAllObjectsInImage(buff, results);
01358   else
01359     for ( std::set<std::string>::const_iterator it = siftObjectNames.begin();
01360           it != siftObjectNames.end(); it++ ) {
01361       int id = matcher->getObjectID(*it);
01362       matcher->findObjectInImage(id, buff, results);
01363     };
01364   if ( curReq->verbosity & MBVshapesFound )
01365     cout << "Found " << results.size() << " sift objects." << endl;
01366   for ( vector<SiftMatch*>::const_iterator it = results.begin();
01367         it != results.end(); it++ ) {
01368     NEW_SHAPE(siftobj, SiftData, new SiftData(VRmixin::camShS, *it));
01369   }
01370 }
01371     
01372 void MapBuilder::newSiftMatcher(const std::string &siftDatabasePath) {
01373   if ( siftDatabasePath.empty() ) return;
01374   SiftTekkotsu *matcher = siftMatchers[siftDatabasePath];
01375   if ( matcher == NULL ) {
01376     matcher = new SiftTekkotsu;
01377     // need to check if file exists matcher->loadFile(siftDatabasePath);
01378     matcher->setParameter("probOfMatch", 0.8);
01379     siftMatchers[siftDatabasePath] = matcher;
01380   }
01381 }
01382 
01383 void MapBuilder::getCamAprilTags(const Sketch<uchar> &rawY) {
01384   std::map<std::pair<int,int>,AprilTags::TagFamily*>::const_iterator registryEntry =
01385     AprilTags::TagFamily::tagFamilyRegistry.find(curReq->aprilTagFamily);
01386   if ( registryEntry == AprilTags::TagFamily::tagFamilyRegistry.end() )
01387     return;
01388   AprilTags::TagFamily *tagFamily = registryEntry->second;
01389   std::vector<Shape<AprilTagData> > results = AprilTagData::extractAprilTags(rawY, *tagFamily);
01390   // calculate distance from the camera based on assumed marker height
01391   coordinate_t markerHeight = MapBuilderRequest::defaultMarkerHeight;
01392   for ( std::vector<Shape<AprilTagData> >::iterator it = results.begin();
01393         it != results.end(); it++ ) {
01394     Point c = (*it)->getCentroid();
01395     MarkerData::calculateCameraDistance(c, markerHeight);
01396     (*it)->setCentroid(c);
01397   }
01398 }
01399 
01400 void MapBuilder::getCamDominoes(const Sketch<uchar> &camFrame,
01401                                 const std::set<color_index>& objectColors,
01402                                 const std::set<color_index>& secondColors) {
01403   if ( objectColors.empty() ) 
01404     return;
01405   if ( curReq->verbosity & MBVshapeSearch )
01406     cout << "*** Find the dominoes ***" << endl;
01407 
01408   float saveMinLineLength = curReq->minLineLength;
01409   curReq->minLineLength = 35; // hard-coded based on Mauricio Contreras' domino demo
01410   vector<Shape<LineData> > camLines;
01411   // If secondColors is empty, assume the lines are the same colors as the dots.
01412   const set<color_index> &lineColors = (! secondColors.empty()) ? secondColors : objectColors;
01413   for (set<color_index>::const_iterator it = lineColors.begin();
01414        it != lineColors.end(); it++) {
01415     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01416     NEW_SKETCH_N(fatmask,bool,visops::fillin(colormask,1,2,8));
01417     NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01418     NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8));
01419     fatskel->retain(); fatmask->retain();
01420     vector<Shape<LineData> > newLines = LineData::extractLines(fatskel,fatmask);
01421     camLines.insert(camLines.end(),newLines.begin(),newLines.end());
01422   }
01423   curReq->minLineLength = saveMinLineLength;
01424 
01425   float saveMinEllipseSemiMajor = curReq->minEllipseSemiMajor;
01426   curReq->minEllipseSemiMajor = 3; // hard-coded from Maurcio Contreras' domino demo
01427   vector<Shape<EllipseData> > camEllipses;
01428   for (set<color_index>::const_iterator it = objectColors.begin();
01429        it != objectColors.end(); it++) {
01430     NEW_SKETCH_N(dotcolormask,bool,visops::colormask(camFrame,*it));
01431     vector<Shape<EllipseData> > ellipse_shapes = EllipseData::extractEllipses(dotcolormask);
01432     camEllipses.insert(camEllipses.end(), ellipse_shapes.begin(),ellipse_shapes.end());
01433   }
01434   curReq->minEllipseSemiMajor = saveMinEllipseSemiMajor;
01435 
01436   if ( camLines.empty() || camEllipses.empty() ) {
01437     if ( curReq->verbosity & MBVshapesFound )
01438       cout << "Found " << camLines.size() << " lines and "
01439            << camEllipses.size() << " ellipses in camera space; no dominoes." << endl;
01440     SHAPEVEC_ITERATE(camLines, LineData, line) {
01441       line.deleteShape();
01442     } END_ITERATE;
01443     SHAPEVEC_ITERATE(camEllipses, EllipseData, ellipse) {
01444       ellipse.deleteShape();
01445     } END_ITERATE;
01446     return;
01447   }
01448 
01449 # ifdef TGT_HAS_CAMERA
01450     fmat::Transform camToBase = kine->linkToBase(CameraFrameOffset);
01451 # else
01452     fmat::Transform camToBase = fmat::Transform::identity();
01453 # endif
01454   PlaneEquation groundPlane(fmat::pack(0,0,1), curReq->dominoDimensions[2]); // domino top face
01455 
01456   std::vector<Shape<LineData> > localLines;
01457   float minlen = curReq->dominoDimensions[1] * 0.65; // because actual line is narrower than brick
01458   float maxlen = curReq->dominoDimensions[1] * 1.1;
01459   SHAPEVEC_ITERATE(camLines, LineData, line) {
01460     line->projectToGround(camToBase, groundPlane);
01461     float len = line->getLength();
01462     if ( len >= minlen && len <= maxlen )
01463       localLines.push_back(line);
01464   } END_ITERATE;
01465 
01466   std::vector<Shape<EllipseData> > localEllipses;
01467   SHAPEVEC_ITERATE(camEllipses, EllipseData, ellipse) {
01468     ellipse->projectToGround(camToBase, groundPlane);
01469     if ( ellipse->getSemimajor() < maxlen/6 ) // if not a giant (spurious) ellipse
01470       localEllipses.push_back(ellipse);
01471   } END_ITERATE;
01472 
01473   int dominoesFound = 0;
01474   if ( localLines.size() > 0 && localEllipses.size() > 0 ) {
01475     fmat::Column<3> halfdims = fmat::pack(curReq->dominoDimensions[0]/2,
01476                                           curReq->dominoDimensions[1]/2,
01477                                           curReq->dominoDimensions[2]/2);
01478     SHAPEVEC_ITERATE(localLines, LineData, line) {
01479       NEW_SHAPE(domino, DominoData,
01480                 new DominoData(localShS, 0, 0,
01481                                fmat::pack(line->getCentroid().coordX(),
01482                                           line->getCentroid().coordY(),
01483                                           halfdims[2]),
01484                                halfdims, // extents
01485                                fmat::rotationZ(AngPi(line->getOrientation() + (AngPi)(M_PI/2)))));
01486       domino->setColor(localEllipses[0]->getColor());
01487       domino->setLineColor(line->getColor());
01488       domino->increaseConfidence(5); // *** hack
01489       LineData line1(localShS, domino->getTFL(), domino->getTFR());
01490       LineData line2(localShS, domino->getTBL(), domino->getTBR());
01491       std::vector<Point> points1, points2;
01492       points1.push_back(line1.getCentroid());
01493       points1.push_back(domino->getTFL());
01494       points1.push_back(domino->getTBL());
01495       points1.push_back(line2.getCentroid());
01496       points2.push_back(line1.getCentroid());
01497       points2.push_back(domino->getTFR());
01498       points2.push_back(domino->getTBR());
01499       points2.push_back(line2.getCentroid());
01500       PolygonData poly1(localShS, points1, true);
01501       PolygonData poly2(localShS, points2, true);
01502       int count1 = 0, count2 = 0;
01503       SHAPEVEC_ITERATE(localEllipses, EllipseData, ellipse) {
01504         if ( poly1.isInside(ellipse->getCentroid()) ) count1++;
01505         else if ( poly2.isInside(ellipse->getCentroid()) ) count2++;
01506       } END_ITERATE; // ellipses
01507       if ( count1 > 0 && count2 > 0 ) {
01508         domino->setValues(min(count1,count2), max(count1,count2));
01509         if ( domino->getLowValue() != count1 )
01510           domino->flipLeftRight();
01511         ++dominoesFound;
01512       }
01513       else
01514         domino.deleteShape();
01515     } END_ITERATE; // lines
01516   }
01517   if ( curReq->verbosity & MBVshapesFound ) {
01518     cout << "Built " << dominoesFound << " local dominoes from " << localLines.size()
01519          << " local lines and " << localEllipses.size() << " local ellipses";
01520     if ( camLines.size() != localLines.size() || camEllipses.size() != localEllipses.size() )
01521       cout << " (" << camLines.size() << " camera lines, " << camEllipses.size() << " camera ellipses)";
01522     cout << endl;
01523   }
01524 
01525   // Cleanup: delete the lines and ellipses so they don't get imported
01526   SHAPEVEC_ITERATE(camLines, LineData, line) {
01527     line.deleteShape();
01528   } END_ITERATE;
01529   SHAPEVEC_ITERATE(camEllipses, EllipseData, ellipse) {
01530     ellipse.deleteShape();
01531   } END_ITERATE;
01532 }
01533 
01534 void MapBuilder::getCamNaughts(const Sketch<uchar> &camFrame, 
01535                                const set<color_index>& objectColors, 
01536                                const fmat::Column<3>& dimensions) const {
01537   vector<Shape<NaughtData> > naughts;
01538   if (objectColors.empty())
01539     return;
01540   if ( curReq->verbosity & MBVshapeSearch )
01541     cout << "*** Find the naughts ***" << endl;
01542   for (set<color_index>::const_iterator it = objectColors.begin();
01543        it !=objectColors.end(); it++) {
01544     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01545     vector<Shape<NaughtData> > naught_shapes = 
01546       NaughtData::extractNaughts(colormask, dimensions);
01547     naughts.insert(naughts.end(), naught_shapes.begin(),naught_shapes.end());
01548     if ( curReq->verbosity & MBVshapesFound )
01549       cout << "Found " << naught_shapes.size() << " "
01550            << ProjectInterface::getColorName(*it) << " naughts." << endl;
01551   }
01552 }
01553 
01554 void MapBuilder::getCamCrosses(const Sketch<uchar> &camFrame, const set<color_index>& objectColors) const {
01555   cout << "Entering getCamCrosses\n";
01556   if ( objectColors.empty() ) 
01557     return;
01558   if ( curReq->verbosity & MBVshapeSearch )
01559     cout << "*** Find the crosses ***" << endl;
01560 
01561   float saveMinLineLength = curReq->minLineLength;
01562   curReq->minLineLength = 10; // hard-coded based on Mauricio Contreras' domino demo
01563   vector<Shape<LineData> > camLines;
01564   for (set<color_index>::const_iterator it = objectColors.begin();
01565        it != objectColors.end(); it++) {
01566     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
01567     NEW_SKETCH_N(cleancolor,bool,visops::minArea(colormask));
01568     NEW_SKETCH_N(fatmask,bool,visops::fillin(cleancolor,1,2,8));
01569     NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01570     skel->retain(); fatmask->retain();
01571     vector<Shape<LineData> > newLines = LineData::extractLines(skel,fatmask);
01572     camLines.insert(camLines.end(),newLines.begin(),newLines.end());
01573   }
01574   curReq->minLineLength = saveMinLineLength;
01575 
01576 
01577 # ifdef TGT_HAS_CAMERA
01578     fmat::Transform camToBase = kine->linkToBase(CameraFrameOffset);
01579 # else
01580     fmat::Transform camToBase = fmat::Transform::identity();
01581 # endif
01582   PlaneEquation groundPlane(fmat::pack(0,0,1), curReq->crossDimensions[2]); // domino top face
01583 
01584   // Convert from camShS to localShS and makes sure they have a valid length for crosses
01585   std::vector<LineData> localLines;
01586   float minlen = curReq->crossDimensions[0] * 1.5; 
01587   float maxlen = curReq->crossDimensions[0] * 2.5;
01588   SHAPEVEC_ITERATE(camLines, LineData, camLine) {
01589     LineData localLine(camLine.getData());
01590     localLine.projectToGround(camToBase, groundPlane);
01591     float len = localLine.getLength();
01592     if ( len >= minlen && len <= maxlen ) {
01593       localLine.setSpace(&localShS);
01594       localLines.push_back(localLine);
01595     }
01596   } END_ITERATE;
01597 
01598   // Checks to makes sure there are a valid number of lines to check for crosses
01599   if (localLines.size() < 2) {
01600     cout << "Not enough lines found.\n";
01601   } else {
01602     vector<Shape<CrossData> > crosses;
01603 
01604     // Iterates through all possible combinations of pairs of lines
01605     for (unsigned int i = 0; i < localLines.size()-1; i++) {
01606       for (unsigned int j = i+1; j < localLines.size(); j++) {
01607       Point intersection = localLines[i].intersectionWithLine(localLines[j]);
01608 
01609       // Make sure intersection point is on both lines and that
01610       // said lines are of valid orientations with respect to one another
01611       if (localLines[i].pointOnLine(intersection) && localLines[j].pointOnLine(intersection))
01612         if (abs(localLines[i].getOrientation() - localLines[j].getOrientation()) > M_PI/9) { 
01613           Shape<CrossData> cross(new CrossData(localShS, localLines[i],
01614                                                localLines[j],
01615                                                curReq->crossDimensions));
01616           cross->setColor(localLines[i].getColor());
01617           cross->setViewable(true);
01618           crosses.push_back(cross);
01619         }
01620       }
01621     }
01622     cout << "Found " << localLines.size() << " lines and recognized "
01623          << crosses.size() << " cross" << (crosses.size()==1 ? "" : "es") << endl;
01624   }
01625   // Cleanup: delete the lines so they don't get imported
01626   SHAPEVEC_ITERATE(camLines, LineData, line) {
01627     line.deleteShape();
01628   } END_ITERATE;
01629 }
01630 
01631 void MapBuilder::getCamAgents(const Sketch<uchar> &camFrame, const Sketch<yuv> &camFrameYUV,
01632                               const std::set<color_index>& objectColors) const {
01633   if ( objectColors.empty() ) return;
01634   if ( curReq->verbosity & MBVshapeSearch )
01635     cout << "*** Find the agents ***" << endl;
01636   AgentData::extractAgents(camFrame, camFrameYUV, objectColors);
01637 }
01638 
01639 void MapBuilder::saveSiftDatabase(const std::string &siftDatabasePath) {
01640   if ( siftDatabasePath.empty() ) return;
01641   SiftTekkotsu *matcher = siftMatchers[siftDatabasePath];
01642   if ( matcher == NULL )
01643     cout << "Error in saveSiftDatabase: no database named '" << siftDatabasePath << "' has been loaded." << endl;
01644   else {
01645     matcher->saveToFile(siftDatabasePath, false);
01646     cout << "Wrote SIFT database " << siftDatabasePath << endl;
01647   }
01648 }
01649 
01650 void MapBuilder::trainSiftObject(const std::string &siftDatabasePath,
01651                                  const std::string &objectName, const std::string &modelName) {
01652   NEW_SKETCH_N(rawY, uchar, VRmixin::sketchFromRawY());
01653   trainSiftObject(siftDatabasePath, rawY, objectName, modelName);
01654 }
01655 
01656 void MapBuilder::trainSiftObject(const std::string &siftDatabasePath, const Sketch<uchar> &sketch,
01657                                  const std::string &objectName, const std::string &modelName) {
01658   if ( siftDatabasePath.empty() ) return;
01659   SiftTekkotsu *matcher = siftMatchers[siftDatabasePath];
01660   if ( matcher == NULL ) {
01661     cout << "Error in trainSiftObject:: no database named '" << siftDatabasePath << "' has been loaded." << endl;
01662     return;
01663   }
01664   ImageBuffer buffer = SiftTekkotsu::sketchToBuffer(sketch);
01665   int id = matcher->getObjectID(objectName);
01666   if ( id == -1 ) {
01667     int new_id = matcher->train_addNewObject(buffer);
01668     matcher->setObjectName(new_id, objectName);
01669   } else {
01670     matcher->train_addToObject(id, buffer);
01671   }
01672 }
01673 
01674 } // namespace
01675 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:44 2016 by Doxygen 1.6.3