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 "Shared/mathutils.h"
00007 #include "Shared/newmat/newmat.h"
00008 
00009 #include "Shared/Measures.h"
00010 #include "EllipseData.h"    // for extractEllipses
00011 #include "SphereData.h"     // for extractSpheres
00012 #include "BlobData.h"       // for extractSpheres
00013 #include "PolygonData.h"   // for extractPolygons
00014 #include "SphereData.h"
00015 #include "TargetData.h"     // for extractTarget
00016 #include "ShapeRoot.h"
00017 #include "ShapeLine.h"
00018 #include "ShapeEllipse.h"
00019 #include "ShapeBlob.h"
00020 #include "ShapePolygon.h"
00021 #include "ShapeSphere.h"
00022 #include "ShapeTarget.h"
00023 #include "Sketch.h"    // for NEW_SKETCH
00024 #include "visops.h"
00025 #include "VRmixin.h"
00026 
00027 #include "LookoutRequests.h"
00028 #include "Lookout.h"
00029 #include "MapBuilder.h"
00030 
00031 using namespace std;
00032 
00033 namespace DualCoding {
00034 
00035 inline float distSq(const NEWMAT::ColumnVector& vec) {
00036   return vec(1)*vec(1) + vec(2)*vec(2) + vec(3)*vec(3);
00037 }
00038 
00039 MapBuilder::MapBuilder() : 
00040   BehaviorBase("MapBuilder"),
00041   camSkS(VRmixin::getCamSkS()), camShS(camSkS.getDualSpace()),
00042   groundShS(VRmixin::getGroundShS()),
00043   localSkS(VRmixin::getLocalSkS()), localShS(localSkS.getDualSpace()),
00044   worldSkS(VRmixin::getWorldSkS()), worldShS(worldSkS.getDualSpace()),
00045   xres(camSkS.getWidth()), yres(camSkS.getHeight()), 
00046   ground_plane(4),
00047   theAgent(VRmixin::theAgent),
00048   localToWorldMatrix(NEWMAT::IdentityMatrix(4)),
00049   worldToLocalTranslateMatrix(NEWMAT::IdentityMatrix(4)), 
00050   worldToLocalRotateMatrix(NEWMAT::IdentityMatrix(4)),
00051   badGazePoints(), 
00052   requests(), curReq(NULL), idCounter(0), maxDistSq(0), 
00053   pointAtID(Lookout::invalid_LO_ID), scanID(Lookout::invalid_LO_ID),
00054   nextGazePoint() {}
00055 
00056 void MapBuilder::DoStart() {
00057   if ( verbosity & MBVstart )
00058     cout << "MapBuilder::DoStart()\n";
00059   BehaviorBase::DoStart();
00060 
00061   camSkS.clear(); camShS.clear();
00062   groundShS.clear();
00063   localSkS.clear(); localShS.clear();
00064   worldSkS.clear(); worldShS.clear();
00065   badGazePoints.clear();
00066   setAgent(Point(0,0,0),0);
00067 
00068   erouter->addListener(this,EventBase::textmsgEGID);  // listen for commands to move to next gaze point
00069   erouter->addListener(this,EventBase::lookoutEGID);  // listen for Lookout completion events
00070 }
00071 
00072 bool MapBuilder::retain = true;
00073 MapBuilder::MapBuilderVerbosity_t MapBuilder::verbosity = -1U;
00074 
00075 /*
00076   Since MapBuilder is constructed as static from VRmixin, the
00077   destructor doesn't get called until the robot shuts down.  We have
00078   to do everything assumed to be done in destructor in DoStop, such as
00079   clearing the request queue and setting parameters to the initial
00080   values as set in the constructor.
00081 */
00082 void MapBuilder::DoStop() {
00083   cout << "MapBuilder::DoStop()\n";
00084   while(!requests.empty()) {
00085     delete requests.front();
00086     requests.pop();
00087   }
00088   curReq = NULL;
00089   BehaviorBase::DoStop();
00090 }
00091 
00092 unsigned int MapBuilder::executeRequest(const MapBuilderRequest& req, unsigned int *req_id) {
00093   MapBuilderRequest *newreq = new MapBuilderRequest(req);
00094   const unsigned int this_request  = ++idCounter;
00095   newreq->requestID = this_request;
00096   if ( req_id != NULL ) *req_id = this_request;
00097   requests.push(newreq);
00098   executeRequest();
00099   return this_request;
00100 }
00101 
00102 void MapBuilder::executeRequest() {
00103   if ( curReq != NULL || requests.empty() ) return;
00104   curReq = requests.front();
00105   if ( verbosity & MBVexecute )
00106     cout << "MapBuilder::executeRequest: execute " << curReq->requestID << endl;
00107   erouter->postEvent(EventBase::mapbuilderEGID, curReq->requestID, EventBase::activateETID,0);  
00108   if ( (curReq->pursueShapes || curReq->doScan || curReq->worldTargets.size()>0) &&
00109        curReq->getRequestType() == MapBuilderRequest::cameraMap ) {
00110     cout << "Warning: switching MapBuilderRequest type from cameraMap to localMap because request parameters require head movement." << endl;
00111     curReq->requestType = MapBuilderRequest::localMap;
00112   }
00113 
00114   calculateGroundPlane();
00115   maxDistSq = curReq->maxDist*curReq->maxDist;
00116  
00117  if ( curReq->clearShapes ) {
00118    curReq->gazePts.clear();
00119    curReq->baseToCamMats.clear();
00120    switch ( curReq->getRequestType() ) {
00121    case MapBuilderRequest::localMap:
00122      localShS.clear();
00123      localSkS.clear();
00124      break;
00125    case MapBuilderRequest::worldMap:
00126      worldShS.clear();
00127      worldSkS.clear();
00128      break;
00129    default:
00130      break;
00131    }
00132  }
00133 
00134  if ( curReq->immediateRequest() )
00135    grabCameraImageAndGo();
00136  else if ( !curReq->searchArea.isValid() & curReq->worldTargets.size() == 0 )
00137    storeImage();
00138  else {
00139    defineGazePts();
00140    if ( curReq->doScan == true )
00141      return; // wait for Lookout to finish scanning
00142    else if ( curReq->worldTargets.size() > 0 )
00143      doNextSearch();
00144    else if ( determineNextGazePoint() )
00145      moveToNextGazePoint();
00146    else
00147      requestComplete();
00148  }
00149 }
00150   
00151 /*
00152 There are three kinds of events for MapBuilder::processEvent to handle:
00153 
00154 1. TextMsg event saying to move to the next gaze point; only needed
00155 when we've asserted manual control of gaze point sequencing for
00156 debugging.
00157 
00158 2. Lookout event announcing that a "point at" request is complete and
00159 an image has been stored.  We can now go ahead and process that image.
00160 
00161 3. Lookout event announcing that a "scan" request is complete and gaze
00162 points have been stored.  We can now start examining those gaze points.
00163 */
00164 
00165 
00166 void MapBuilder::processEvent(const EventBase &e) {
00167   if ( curReq == NULL) return;
00168   if ( verbosity & MBVevents )
00169     cout << "MapBuilder got event " << e.getName() << ";   current pointAtID=" << pointAtID
00170    << " scanID=" << scanID << endl;
00171 
00172   switch ( e.getGeneratorID() ) {
00173   case EventBase::textmsgEGID:
00174     if ( strcmp(dynamic_cast<const TextMsgEvent&>(e).getText().c_str(),"MoveHead") == 0 )
00175       moveToNextGazePoint(true);
00176     break;
00177 
00178   case EventBase::lookoutEGID:
00179     if ( e.getSourceID() == pointAtID )
00180       processImage(dynamic_cast<const LookoutSketchEvent&>(e));
00181     else if ( e.getSourceID() == scanID ) {
00182       const vector<Point>& pts = dynamic_cast<const LookoutScanEvent*>(&e)->getTasks().front()->data;
00183       cout << " doScan found " << pts.size() << " interest points." << endl;
00184       curReq->gazePts.insert(curReq->gazePts.end(),pts.begin(), pts.end());
00185     }
00186     else {
00187       cout << "MapBuilder: unexpected Lookout event " << e.getDescription()
00188      << ",   current pointAtID=" << pointAtID << ", scanID=" << scanID << endl;
00189       return;
00190     }
00191     // we've dealt with the event (processed an image or did a scan); now see what we should do next
00192     if ( requestExitTest() )
00193       requestComplete();
00194     else if ( curReq->worldTargets.size() > 0 )
00195       doNextSearch();
00196     else if ( determineNextGazePoint() )
00197       moveToNextGazePoint();
00198     else
00199       requestComplete();
00200     break;
00201 
00202   default:
00203     cout << "MapBuilder received unexpected event: " << e.getDescription() << endl;
00204   }
00205 }
00206 
00207 void MapBuilder::processImage(const LookoutSketchEvent &e) {
00208   camSkS.clear();
00209   if ( curReq->rawY ) {
00210     NEW_SKETCH(rawY, uchar, VRmixin::sketchFromRawY());
00211   }
00212   NEW_SKETCH(camFrame, uchar, e.getSketch());
00213   const NEWMAT::Matrix& camToBase = e.toBaseMatrix;
00214   const NEWMAT::Matrix baseToCam = camToBase.i();
00215   getCameraShapes(camFrame);
00216   if (curReq->userCamProcessing != NULL) (*curReq->userCamProcessing)();
00217   if (curReq->getRequestType() > MapBuilderRequest::cameraMap) {
00218     projectToGround(camToBase);
00219     if (curReq->userGroundProcessing != NULL) (*curReq->userGroundProcessing)();
00220     filterGroundShapes(baseToCam);
00221   }
00222   switch ( curReq->getRequestType() ) {
00223   case MapBuilderRequest::cameraMap:
00224   case MapBuilderRequest::groundMap:
00225     break;
00226   case MapBuilderRequest::localMap:
00227     extendLocal(baseToCam);
00228     if (curReq->userLocalProcessing != NULL) (*curReq->userLocalProcessing)();
00229     break;
00230   case MapBuilderRequest::worldMap:
00231     extendWorld(baseToCam);
00232     if (curReq->userWorldProcessing != NULL) (*curReq->userWorldProcessing)();
00233   }
00234 }
00235 
00236 bool MapBuilder::determineNextGazePoint() {
00237   if (curReq->getRequestType() == MapBuilderRequest::worldMap) {
00238     worldShS.applyTransform(worldToLocalTranslateMatrix,egocentric);
00239     worldShS.applyTransform(worldToLocalRotateMatrix,egocentric);
00240     bool b = determineNextGazePoint(worldShS.allShapes()) || determineNextGazePoint(curReq->gazePts);
00241     worldShS.applyTransform(localToWorldMatrix,allocentric); // transform back
00242     return b;
00243   }
00244   else
00245     return determineNextGazePoint(localShS.allShapes()) || determineNextGazePoint(curReq->gazePts);
00246 }
00247 
00248   
00249 bool MapBuilder::determineNextGazePoint(const vector<ShapeRoot>& shapes) {
00250   if ( ! curReq->pursueShapes ) return false;
00251   HeadPointerMC headpointer_mc;
00252   for (vector<ShapeRoot>::const_iterator it = shapes.begin();
00253        it != shapes.end(); it++) {
00254     // look for invalid endpoints of lines / polygons
00255     if ((*it)->isType(lineDataType) || (*it)->isType(polygonDataType)) {
00256       const Shape<LineData>& ld = ShapeRootTypeConst(*it,LineData);
00257       const Shape<PolygonData>& pd = ShapeRootTypeConst(*it,PolygonData);
00258       bool isLine = (*it)->isType(lineDataType);
00259       EndPoint p[2] = { isLine ? ld->end1Pt(): pd->end1Pt(), isLine ? ld->end2Pt() : pd->end2Pt()};
00260       for (int i = 0; i < 2; i++) {
00261   if ( !p[i].isValid() && !isBadGazePoint(p[i] )
00262       && badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (Point) p[i])) {
00263     cout << "Take image at endpoint" << (i+1) << " of shape id " 
00264          << (*it)->getId() << " at " << p[i] << endl;
00265     if ( !headpointer_mc.lookAtPoint(p[i].coordX(),p[i].coordY(),p[i].coordZ()) ) {
00266       if ( verbosity & MBVbadGazePoint )
00267         cout << "MapBuilder noting unreachable gaze point " << (Point)p[i] << endl;
00268       badGazePoints.push_back((Point)p[i]);
00269     }
00270     nextGazePoint = p[i];
00271     return true;
00272   }
00273       }
00274     }
00275     // look for shapes w/ <2 confidence
00276     if ((!(*it)->isType(agentDataType)) &&
00277   (*it)->getLastMatchId() != 0 &&
00278   (*it)->getConfidence() <= 1 &&
00279   ! isBadGazePoint((*it)->getCentroid()) &&
00280   badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (*it)->getCentroid()))  {
00281       const Point pt = (*it)->getCentroid();
00282       cout << "take image at shape " << (*it)
00283      << " (confidence level: " << (*it)->getConfidence() << ")" << endl;      
00284       cout << " at " << pt << endl;  
00285       if (! headpointer_mc.lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ()))
00286   badGazePoints.push_back(pt);
00287       nextGazePoint = pt;
00288       return true;
00289     }
00290     // cout << "Skipping shape " << (*it)->getId()
00291     //      << " (confidence level: " << (*it)->getConfidence() << ")" << endl;      
00292   }
00293   return false;
00294 }
00295 
00296 
00297 bool MapBuilder::determineNextGazePoint(vector<Point>& gazePts) {
00298   for (vector<Point>::iterator it = gazePts.begin();
00299        it != gazePts.end(); it++) {
00300     nextGazePoint = *it;
00301     gazePts.erase(it);
00302     if ( ! isBadGazePoint(nextGazePoint) )
00303    return true;
00304   }
00305   return false;
00306 }
00307 
00308 void MapBuilder::moveToNextGazePoint(const bool manualOverride) {
00309   if ( curReq == NULL ) {
00310     cout << "curReq == NULL in moveToNextGazePoint!" << endl;
00311     return;
00312   }
00313   if ( (verbosity & MBVnextGazePoint) || (curReq->manualHeadMotion && manualOverride==false) )
00314     cout << "moveToNextGazePoint " << nextGazePoint << endl;
00315   if ( curReq->manualHeadMotion && manualOverride==false ) {
00316     cout << "To proceed to this gaze point:  !msg MoveHead" << endl;
00317     return;
00318   }
00319   else
00320     storeImage(nextGazePoint,true);
00321 }
00322 
00323 
00324 void MapBuilder::doNextSearch() {
00325   LookoutSearchRequest *curLSR = curReq->worldTargets.front();
00326   curReq->worldTargets.pop();
00327   pointAtID = VRmixin::lookout.executeRequest(*curLSR);
00328 }
00329 
00330 bool MapBuilder::isBadGazePoint(const Point& Pt) const {
00331   const coordinate_t x = Pt.coordX();
00332   const coordinate_t y = Pt.coordY();
00333   return ( x*x + y*y > maxDistSq || x < -30.0);
00334 }
00335 
00336 void MapBuilder::storeImage(const Point& pt, bool havepoint) { 
00337   LookoutPointRequest lreq;
00338   if ( havepoint )
00339     lreq.setTarget(pt);
00340   else
00341     lreq.setHeadMotionType(LookoutRequest::noMotion);
00342   lreq.motionSettleTime = curReq->motionSettleTime;
00343   lreq.numSamples = curReq->numSamples;
00344   lreq.sampleInterval = curReq->sampleInterval;
00345   pointAtID = VRmixin::lookout.executeRequest(lreq);
00346 }
00347 
00348 void MapBuilder::grabCameraImageAndGo() {
00349   // This is a performance hack to avoid calling the Lookout or event
00350   // router, so the MapBuilder can produce results very quickly when
00351   // we need real-time performance, e.g., for particle filtering where
00352   // we take multiple snapshots.
00353   pointAtID = 0;
00354   Sketch<uchar> camFrame(VRmixin::sketchFromSeg());
00355 #ifdef TGT_HAS_CAMERA
00356   const NEWMAT::Matrix camToBase = kine->jointToBase(CameraFrameOffset);
00357 #else
00358   NEWMAT::Matrix camToBase(4,4);
00359   camToBase << ROBOOP::fourbyfourident;
00360 #endif
00361   LookoutSketchEvent dummy(true, camFrame, camToBase,
00362          EventBase::lookoutEGID, pointAtID, EventBase::deactivateETID);
00363   processImage(dummy);
00364   requestComplete();
00365 }
00366 
00367 void MapBuilder::scanForGazePts() {
00368   LookoutScanRequest lreq;
00369   lreq.searchArea = curReq->searchArea;
00370   lreq.motionSettleTime = curReq->motionSettleTime;
00371   set<color_index> colors;  // colors of all the shape types we're looking for
00372   for (map<ShapeType_t,set<color_index> >::const_iterator it1 = curReq->objectColors.begin();
00373        it1 != curReq->objectColors.end(); it1++)
00374     for (set<color_index>::const_iterator it2 = it1->second.begin();
00375    it2 != it1->second.end(); it2++)
00376       colors.insert(*it2);
00377   lreq.addTask(LookoutScanRequest::VisionRegionTask(colors,curReq->dTheta));
00378   scanID = VRmixin::lookout.executeRequest(lreq);
00379 }
00380 
00381 void MapBuilder::extendLocal(const NEWMAT::Matrix& baseToCam) {
00382   vector<ShapeRoot> all = localShS.allShapes();
00383   removeNoise(localShS, baseToCam);
00384   matchSrcToDst(groundShS,localShS,curReq->objectColors[polygonDataType]);
00385   removeGazePts(curReq->gazePts, baseToCam);
00386   curReq->baseToCamMats.push_back(baseToCam);
00387 }
00388 
00389 void MapBuilder::extendWorld(const NEWMAT::Matrix& baseToCam) {
00390   worldShS.applyTransform(worldToLocalTranslateMatrix,egocentric);
00391   worldShS.applyTransform(worldToLocalRotateMatrix,egocentric);
00392   removeNoise(worldShS, baseToCam);
00393   matchSrcToDst(groundShS,worldShS,curReq->objectColors[polygonDataType]);
00394   worldShS.applyTransform(localToWorldMatrix,allocentric);
00395   removeGazePts(curReq->gazePts,baseToCam);
00396   curReq->baseToCamMats.push_back(baseToCam);
00397 }
00398 
00399 bool MapBuilder::requestExitTest() {
00400   if ( curReq->exitTest == NULL )
00401     return false;
00402   else
00403     return (*curReq->exitTest)();
00404 }
00405 
00406 void MapBuilder::requestComplete() {
00407   const unsigned int reqID = curReq->requestID;
00408   if ( verbosity & MBVcomplete )
00409     cout << "MapBuilderRequest " << reqID << " complete\n";
00410   delete curReq;
00411   curReq = NULL;
00412   requests.pop();
00413   erouter->postEvent(EventBase::mapbuilderEGID, reqID, EventBase::deactivateETID,0);
00414   if ( requests.empty() )
00415     VRmixin::lookout.relax();
00416   else
00417     executeRequest(); // execute next request AFTER deactivate event has finished processing
00418 }
00419 
00420 void MapBuilder::setAgent(const Point &worldLocation, const AngTwoPi worldHeading) {
00421   if ( verbosity & MBVsetAgent )
00422     cout << "Agent now at " << worldLocation << " hdg " << worldHeading 
00423    << " (= " << float(worldHeading)*180/M_PI << " deg.)" << endl;
00424   theAgent->setCentroidPt(worldLocation);
00425   theAgent->setOrientation(worldHeading);
00426   const coordinate_t dx = worldLocation.coordX();
00427   const coordinate_t dy = worldLocation.coordY();
00428   const coordinate_t dz = worldLocation.coordZ();
00429   float const c = cos(worldHeading);
00430   float const s = sin(worldHeading);
00431   float localToWorld[] = {c, -s, 0, dx,
00432         s,  c, 0, dy, 
00433         0,  0, 1, dz,
00434         0 , 0, 0, 1};
00435   float worldToLocalTrans[] = {1, 0, 0, -dx,
00436              0, 1, 0, -dy, 
00437              0, 0, 1, -dz,
00438              0 ,0, 0,  1};
00439   float worldToLocalRotate[] = { c, s, 0, 0,
00440         -s, c, 0, 0, 
00441          0, 0, 1, 0,
00442          0, 0, 0, 1};
00443   localToWorldMatrix << localToWorld;
00444   worldToLocalTranslateMatrix << worldToLocalTrans;
00445   worldToLocalRotateMatrix << worldToLocalRotate;
00446 }
00447 
00448 void MapBuilder::moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, AngTwoPi dtheta) {
00449   Point const agentLoc = theAgent->getCentroid();
00450   AngTwoPi const heading = theAgent->getOrientation();
00451   float const c = cos(heading);
00452   float const s = sin(heading);
00453   float const dx = local_dx*c + local_dy*-s;
00454   float const dy = local_dx*s + local_dy*c;
00455   setAgent(agentLoc + Point(dx,dy,agentLoc.coordZ(),allocentric),
00456      heading+dtheta);
00457 }
00458 
00459 void MapBuilder::importLocalToWorld() {
00460   worldShS.applyTransform(worldToLocalTranslateMatrix,egocentric);
00461   worldShS.applyTransform(worldToLocalRotateMatrix,egocentric);
00462   matchSrcToDst(localShS,worldShS);
00463   worldShS.applyTransform(localToWorldMatrix,allocentric);
00464 }
00465 
00466 
00467 ShapeRoot MapBuilder::importWorldToLocal(const ShapeRoot &worldShape) {
00468   ShapeRoot localShape(localShS.importShape(worldShape));
00469   localShape->applyTransform(worldToLocalTranslateMatrix,egocentric);
00470   localShape->applyTransform(worldToLocalRotateMatrix,egocentric);
00471   return localShape;
00472 }
00473 
00474 bool MapBuilder::isPointVisible(const Point &pt, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) {
00475   NEWMAT::ColumnVector camCoords = baseToCam*pt.coords;
00476   //  if (camCoords(3) <=0 || distSq(camCoords) >= maxDistanceSq) return false;
00477   if ( camCoords(1)*camCoords(1)+camCoords(2)*camCoords(2) >= maxDistanceSq )
00478     return false;
00479   float normX,normY; // normalized coordinates in cam frame
00480   config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX,normY);
00481   return (fabs(normX) < 0.9 && fabs(normY) < 0.9); //normX and normY range from -1 to 1. Giving 10% offset here
00482 }
00483 
00484 bool MapBuilder::isLineVisible(const LineData& ln, const NEWMAT::Matrix& baseToCam) {
00485   float normX1,normX2,normY1,normY2;
00486   NEWMAT::ColumnVector camCoords(4);
00487   camCoords = baseToCam*ln.end1Pt().coords;
00488   config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX1,normY1);
00489   camCoords = baseToCam*ln.end2Pt().coords;
00490   config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX2,normY2);
00491   const bool end1Pt_visible = fabs(normX1) < 0.9 && fabs(normY1) < 0.9;
00492   const bool end2Pt_visible = fabs(normX2) < 0.9 && fabs(normY2) < 0.9;
00493   if (end1Pt_visible && end2Pt_visible)
00494     return true;
00495   LineData lnCam(VRmixin::groundShS, Point(normX1,normY1), Point(normX2,normY2));
00496   // define bounding box of camera frame in terms of normalized coordinates with 10% offset
00497   LineData camBounds[] = {LineData(VRmixin::groundShS, Point(0.9,0.9),Point(0.9,-0.9)),
00498         LineData(VRmixin::groundShS, Point(0.9,-0.9),Point(-0.9,-0.9)),
00499         LineData(VRmixin::groundShS, Point(-0.9,-0.9),Point(-0.9,0.9)),
00500         LineData(VRmixin::groundShS, Point(-0.9,0.9),Point(0.9,0.9))};
00501   unsigned int ptCount = 0;
00502   Point p[2];
00503   // find if a portion of the line shows up in cam
00504   if (end1Pt_visible) p[ptCount++].setCoords(normX1,normY1,0); // end1Pt in frame
00505   else if (end2Pt_visible) p[ptCount++].setCoords(normX2,normY2,0); // end2Pt in frame
00506   for (int i = 0; i < 4; i++)
00507     if (camBounds[i].intersectsLine(lnCam)) {
00508       p[ptCount++].setCoords(lnCam.intersectionWithLine(camBounds[i]));
00509       // Let's say portion of line seen in cam should be longer than .1 normalized
00510       if (ptCount > 1)
00511   return p[0].distanceFrom(p[1]) > 0.1; 
00512     }
00513   return false;
00514 }
00515 
00516 bool MapBuilder::isShapeVisible(const ShapeRoot &ground_shape, const NEWMAT::Matrix& baseToCam,
00517            float maxDistanceSq) {
00518   if (ground_shape->isType(lineDataType))
00519     return isLineVisible(ShapeRootTypeConst(ground_shape,LineData).getData(), baseToCam);
00520   else if (ground_shape->isType(polygonDataType)) {
00521     const Shape<PolygonData>& polygon = ShapeRootTypeConst(ground_shape,PolygonData);
00522     for (vector<LineData>::const_iterator edge_it = polygon->getEdges().begin();
00523    edge_it != polygon->getEdges().end(); edge_it++)
00524       if (isLineVisible(*edge_it,baseToCam))
00525   return true;
00526     return false;
00527   }
00528   else 
00529     return isPointVisible(ground_shape->getCentroid(), baseToCam, maxDistanceSq);
00530 }
00531 
00532 
00533 // filter "bad" ground shapes before importing to dst shape space.
00534 // 1. ignore shapes too far from dog or projected to the other side of cam plane
00535 // 2. chop off line at max distance if it is extending beyond the distance and leave the endpoint invalid
00536 void MapBuilder::filterGroundShapes(const NEWMAT::Matrix& baseToCam) {
00537   //  cout << "MapBuilder::filterGroundShapes()" << endl;
00538   vector<ShapeRoot> ground_shapes = groundShS.allShapes();
00539 
00540   for (vector<ShapeRoot>::iterator ground_it = ground_shapes.begin();
00541        ground_it != ground_shapes.end(); ground_it++ ) {
00542     const coordinate_t cenX = (*ground_it)->getCentroid().coordX();
00543     const coordinate_t cenY = (*ground_it)->getCentroid().coordY();
00544     if (cenX*cenX + cenY*cenY > maxDistSq) { // too far
00545       if ( verbosity & MBVnotAdmissible )
00546   cout << "ground shape " << (*ground_it)->getId() << " (lastMatch " 
00547        << (*ground_it)->getLastMatchId() << ") too far, delete\n";
00548       ground_it->deleteShape();
00549     }
00550     NEWMAT::ColumnVector coords = Kinematics::pack(cenX,cenY,(*ground_it)->getCentroid().coordZ());
00551     coords = baseToCam*coords;
00552     if (coords(3) < 0) { // negative z-coordinate in camera frame indicates projection failed
00553       if ( verbosity & MBVprojectionFailed )
00554   cout << "Projection failed for ground shape " << (*ground_it)->getId() 
00555        << " (lastMatch " << (*ground_it)->getLastMatchId() << "): deleting\n";
00556       ground_it->deleteShape();
00557     }
00558     // if a line is extending to maxDistance, chop it off at maxdistance and mark the endpoint invalid
00559     else if ((*ground_it)->isType(lineDataType)) {
00560       const Shape<LineData>& line = ShapeRootTypeConst(*ground_it,LineData);
00561       const coordinate_t e1x = line->end1Pt().coordX();
00562       const coordinate_t e1y = line->end1Pt().coordY();
00563       const coordinate_t e2x = line->end2Pt().coordX();
00564       const coordinate_t e2y = line->end2Pt().coordY();
00565       if (e1x*e1x + e1y*e1y > maxDistSq && e2x*e2x + e2y*e2y > maxDistSq)
00566   ground_it->deleteShape();
00567       else if (e1x*e1x + e1y*e1y > maxDistSq || e2x*e2x + e2y*e2y > maxDistSq) {
00568   //  cout << (*ground_it)->getId() << "(lastMatch " << (*ground_it)->getLastMatchId() 
00569   //       << ")  extends beyond maximum distance we want. Chop off the line" << endl;
00570   vector<float> line_abc = line->lineEquation_abc();
00571   Point pt;
00572   const EndPoint* far_ept = (e1x*e1x + e1y*e1y > maxDistSq) ? &line->end1Pt() : &line->end2Pt(); 
00573         if (line_abc[1] == 0.0) {
00574     const coordinate_t y_abs = sqrt(maxDistSq - line_abc[2]*line_abc[2]);
00575     if (fabs(far_ept->coordY()-y_abs) < fabs(far_ept->coordY()+y_abs))
00576       pt.setCoords(e1x, y_abs, far_ept->coordZ());
00577     else
00578       pt.setCoords(e1x, -y_abs, far_ept->coordZ());
00579   }
00580   else {
00581     const float a = - line_abc[0]/line_abc[1];
00582     const float b = line_abc[2]/line_abc[1];
00583     const coordinate_t x1 = (sqrt((a*a+1.0)*maxDistSq-b*b)-a*b)/(a*a+1.0);
00584     const coordinate_t x2 = (-sqrt((a*a+1.0)*maxDistSq-b*b)-a*b)/(a*a+1.0);
00585     if (fabs(far_ept->coordX()-x1) < fabs(far_ept->coordX()-x2))
00586       pt.setCoords(x1, a*x1+b, far_ept->coordZ());
00587     else
00588       pt.setCoords(x2, a*x2+b, far_ept->coordZ());
00589   }
00590   EndPoint temp_endPt(pt);
00591   temp_endPt.setValid(false);
00592   //  cout << " (" << far_ept->coordX() << "," << far_ept->coordY() << ") => ("
00593   //       << pt.coordX() << "," << pt.coordY() << ")" << endl;
00594   if (e1x*e1x + e1y*e1y > maxDistSq)
00595     line->setEndPts(temp_endPt, line->end2Pt());
00596   else
00597     line->setEndPts(line->end1Pt(), temp_endPt);
00598   badGazePoints.push_back(pt);
00599       }
00600     }
00601   }
00602 }
00603 
00604 void MapBuilder::calculateGroundPlane() {
00605   switch(curReq->groundPlaneAssumption) {
00606   case MapBuilderRequest::onLegs:
00607     ground_plane << kine->calculateGroundPlane(); 
00608     if ( verbosity & MBVgroundPlane ) 
00609       cout << "Calculated ground plane: " << NEWMAT::printmat(ground_plane) << endl;
00610     break;
00611   case MapBuilderRequest::onStand:
00612 #ifdef TGT_ERS210
00613     ground_plane << 0 << 0 << (float)(-1/200.0) << 1;
00614 #else
00615     ground_plane << 0 << 0 << (float)(-1/170.0) << 1; //for the order-made stands for ERS7 in the lab
00616 #endif
00617     // cout << "Hard-coded ground plane: " << NEWMAT::printmat(ground_plane) << endl;
00618     break;
00619   case MapBuilderRequest::custom:
00620     ground_plane = curReq->customGroundPlane;
00621   }
00622 }
00623 
00624 void MapBuilder::projectToGround(const NEWMAT::Matrix& camToBase) {
00625   VRmixin::projectToGround(camToBase, ground_plane);
00626 }
00627     
00628 
00629 void MapBuilder::matchSrcToDst(ShapeSpace &srcShS, ShapeSpace &dstShS,
00630           set<color_index> polCols, bool mergeSrc, bool mergeDst) {
00631   vector<ShapeRoot> src_shapes = srcShS.allShapes();
00632   vector<ShapeRoot> dst_shapes = dstShS.allShapes();
00633   vector<LineData> polygon_edges;
00634   
00635   // clean up src_shapes before messing with dst space
00636   for (vector<ShapeRoot>::iterator src_it = src_shapes.begin();
00637        src_it != src_shapes.end(); src_it++ ) {
00638     if (!(*src_it)->isAdmissible()) {
00639       if ( verbosity & MBVnotAdmissible )
00640   cout << "shape " << (*src_it)->getId() << "(lastMatch " 
00641        << (*src_it)->getLastMatchId() << ") is not admissible:" << endl;
00642       (*src_it)->printParams();
00643       src_shapes.erase(src_it--);
00644     }
00645     else if ((*src_it)->isType(polygonDataType)) { 
00646       const vector<LineData>& lines = ShapeRootTypeConst(*src_it, PolygonData)->getEdges();
00647       polygon_edges.insert(polygon_edges.end(), lines.begin(), lines.end());
00648       src_shapes.erase(src_it--);
00649     }
00650     else if ((*src_it)->isType(lineDataType)) {
00651       const color_index colorIndex = ProjectInterface::getColorIndex((*src_it)->getColor());
00652       if ( polCols.end() != find(polCols.begin(), polCols.end(), colorIndex)) {
00653   polygon_edges.push_back(ShapeRootTypeConst(*src_it, LineData).getData());
00654   src_shapes.erase(src_it--);
00655       }
00656     }
00657   }
00658 
00659   // merge shapes inside srcShS
00660   if (mergeSrc)
00661     for ( vector<ShapeRoot>::iterator it = src_shapes.begin();
00662     it != src_shapes.end(); it++ )
00663       for ( vector<ShapeRoot>::iterator it2 = it+1;
00664     it2 != src_shapes.end(); it2++)
00665   if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2)) {
00666     if ( verbosity & MBVshapesMerge )
00667       cout << "merging shape " << (*it)->getId() << " and shape " << (*it2)->getId() << endl;
00668     src_shapes.erase(it2--);
00669   }
00670 
00671   vector<Shape<PolygonData> > existingPolygons;
00672   // update dst shapes from src shapes
00673   for (vector<ShapeRoot>::iterator dst_it = dst_shapes.begin();
00674        dst_it != dst_shapes.end(); dst_it++ )
00675     if ((*dst_it)->isType(polygonDataType))
00676       existingPolygons.push_back(ShapeRootType(*dst_it,PolygonData));
00677     else {
00678       for (vector<ShapeRoot>::iterator src_it = src_shapes.begin();
00679      src_it != src_shapes.end(); src_it++)
00680   if ((*dst_it)->isMatchFor(*src_it) && (*dst_it)->updateParams((*src_it))) {
00681     (*dst_it)->increaseConfidence(*src_it);
00682     if ( verbosity & MBVshapeMatch )
00683       cout << "Matched src shape " << (*src_it)->getId() << " (lastMatch " << (*src_it)->getLastMatchId()
00684      << ") to dst shape " << (*dst_it)->getId() << endl;
00685     src_shapes.erase(src_it--);
00686   }
00687     }
00688   
00689   // form polygons from lines and import unmatched src shapes into dstShS
00690   vector<ShapeRoot> deletedPolygons;
00691   //  cout << existingPolygons.size() << " polygons\n";
00692   vector<ShapeRoot> newPolygons = PolygonData::formPolygons(polygon_edges,existingPolygons,deletedPolygons);
00693   //  cout << existingPolygons.size()+deletedPolygons.size() << " polygons\n";
00694   for (vector<ShapeRoot>::iterator delete_it = deletedPolygons.begin();
00695        delete_it != deletedPolygons.end(); delete_it++)
00696     delete_it->deleteShape();
00697   dstShS.importShapes(newPolygons);
00698   dstShS.importShapes(src_shapes);
00699   if ( verbosity & MBVimportShapes )
00700     cout << "Imported " << (src_shapes.size()+newPolygons.size()) << " new shape(s) from "
00701    << srcShS.name << "ShS to " << dstShS.name << "ShS\n";
00702   
00703   // match shapes inside dstShS
00704   if (mergeDst) {
00705     dst_shapes = dstShS.allShapes();
00706     for ( vector<ShapeRoot>::iterator it = dst_shapes.begin();
00707     it < dst_shapes.end(); it++ )
00708       for ( vector<ShapeRoot>::iterator it2 = it+1;
00709       it2 < dst_shapes.end(); it2++)
00710   if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2,true)) {
00711     // cout << "Matched src shape " << (*it)->getId() << " (lastMatch " 
00712     //  << (*it)->getLastMatchId()<< ") is a match for " 
00713     //   << (*it2)->getId() << " (lastMatch " 
00714     //   << (*it2)->getLastMatchId() << "), delete\n";
00715     it2->deleteShape();
00716     dst_shapes.erase(it2--);
00717   }
00718   }
00719 }
00720 
00721 // decrease confidence of those shapes that should have been seen in the last snap,
00722 // remove shapes from ShapeSpace if confidence becomes < 0
00723 void MapBuilder::removeNoise(ShapeSpace& ShS, const NEWMAT::Matrix& baseToCam) {
00724   //  cout << "MapBuilder::removeNoise()\n";
00725   vector<ShapeRoot> shapes = ShS.allShapes();
00726   for (vector<ShapeRoot>::iterator it = shapes.begin();
00727        it != shapes.end(); it++ ) {
00728     // was not looking for this object in the last snap, 
00729     // and it's not fair to decrease this guy's confidence
00730     if (curReq->objectColors[(*it)->getType()].find(ProjectInterface::getColorIndex((*it)->getColor())) ==
00731   curReq->objectColors[(*it)->getType()].end())
00732       continue; 
00733     if ((*it)->isType(polygonDataType)) {
00734       Shape<PolygonData>& polygon = ShapeRootType(*it,PolygonData);
00735       vector<LineData>& edges = polygon->getEdgesRW();
00736       unsigned int edge_index = 0;
00737       for (vector<LineData>::iterator edge_it = edges.begin();
00738      edge_it != edges.end(); edge_it++, edge_index++) {
00739   if (isLineVisible(*edge_it, baseToCam)) {
00740     if ( verbosity & MBVshouldSee )
00741       cout << "edge " << edge_index << " of polygon " << (*it)->getId() << "(confidence: " 
00742      << edge_it->getConfidence() << ") should be visible in this frame" << endl;
00743     edge_it->decreaseConfidence();
00744   }
00745       }
00746       vector<ShapeRoot> brokenPolygons = polygon->updateState();
00747       ShS.importShapes(brokenPolygons);
00748       if (!polygon->isAdmissible()) {
00749   if ( verbosity & MBVdeleteShape )
00750     cout << "delete polygon " << (*it)->getId() << " from map" << endl;
00751   it->deleteShape();
00752       }
00753     }
00754     else if ((!(*it)->isType(agentDataType)) && isShapeVisible(*it, baseToCam, maxDistSq)){
00755       (*it)->decreaseConfidence(); // decrease confidence by 1 for every visible shape
00756       if ((*it)->getConfidence() < 0 ) {
00757   if ( verbosity & MBVshouldSee )
00758     cout << "shape " << (*it)->getId() << "(confidence: " << (*it)->getConfidence() 
00759          << ") should be visible in this frame; deleted" << endl;
00760   it->deleteShape();
00761       }
00762     }
00763   }
00764 }
00765 
00766 //================ Gaze points ================
00767 
00768 void MapBuilder::defineGazePts() {
00769   const ShapeRoot &sArea = curReq->searchArea;
00770   if ( !sArea.isValid() )
00771     return;
00772   else if ( curReq->doScan == true )
00773     scanForGazePts();
00774   else
00775     switch ( sArea->getType() ) {
00776 
00777     case pointDataType:
00778       curReq->gazePts.push_back(sArea->getCentroid());
00779       break;
00780 
00781     case lineDataType: {
00782       static const float meshSize=50;
00783       const Shape<LineData>& line = ShapeRootTypeConst(sArea,LineData);
00784       if ( curReq->doScan == true )
00785   scanForGazePts();
00786       else {
00787   int numIntervals = (int) (line->getLength()/meshSize);
00788   const EndPoint& ep1 = line->end1Pt();
00789   const EndPoint& ep2 = line->end2Pt();
00790   curReq->gazePts.push_back(ep1);
00791   for (int i = 1; i < numIntervals; i++)
00792     curReq->gazePts.push_back((ep1*i + ep2*(numIntervals-i))/numIntervals);
00793   curReq->gazePts.push_back(ep2);
00794       }
00795     }
00796       break;
00797 
00798     case polygonDataType:
00799       if ( curReq->doScan == true )
00800   scanForGazePts();
00801       else {
00802   const Shape<PolygonData> &poly = ShapeRootTypeConst(sArea,PolygonData);
00803   const vector<Point> &verts = poly->getVertices();
00804   curReq->gazePts.insert(curReq->gazePts.end(),verts.begin(),verts.end());
00805       }
00806       break;
00807 
00808       default:
00809   cerr << "ERROR MapBuilder::defineGazePts: Supported searchArea shapes are Point, Line, and Polygon\n";
00810   requestComplete();
00811   break;
00812     }
00813 }
00814 
00815 
00816 void MapBuilder::removeGazePts(vector<Point> &gazePts, const NEWMAT::Matrix& baseToCam) {
00817   if (curReq->removePts) {
00818     int num_points_seen = 0;
00819     for ( vector<Point>::iterator it = gazePts.begin();
00820     it != gazePts.end(); it++ ) {
00821       if ( isPointVisible(*it,baseToCam,maxDistSq) ) {
00822   cout << "Removing already-visible gaze point " << *it << endl;
00823   num_points_seen++;
00824   gazePts.erase(it--);
00825       }
00826     }
00827     // cout << num_points_seen << " pre-defined gaze points seen in last image, "
00828     //      << gazePts.size() << " left\n";
00829   }
00830 }
00831 
00832 
00833 //================================================================
00834 
00835 //Prints shapespace in the format to be used for particle filter on simulator
00836 void MapBuilder::printShS(ShapeSpace &ShS) const {
00837   cout << "MapBuilder::printShS()" << endl;
00838   unsigned int line_count = 0;
00839   vector<ShapeRoot> shapes = ShS.allShapes();
00840   for (vector<ShapeRoot>::const_iterator it = shapes.begin();
00841        it != shapes.end(); it++) {
00842     if ((*it)->isType(lineDataType)) {
00843       const Shape<LineData>& ld = ShapeRootTypeConst(*it,LineData);
00844       cout << (*it)->getId() << " " << lineDataType << " " 
00845      << ProjectInterface::getColorIndex((*it)->getColor()) 
00846      << " " << ld->end1Pt().coordX()  << " " << ld->end1Pt().coordY()
00847      << " " << ++line_count << " " << ld->getLength() << " " << ld->end1Pt().isValid() << endl; 
00848       cout << (*it)->getId() << " " << lineDataType << " " 
00849      << ProjectInterface::getColorIndex((*it)->getColor()) 
00850      << " " << ld->end2Pt().coordX()  << " " << ld->end2Pt().coordY()
00851      << " " << line_count << " " << ld->getLength() << " " << ld->end2Pt().isValid() << endl;
00852     }
00853     else {
00854       cout << (*it)->getId() << " " << (*it)->getType() << " " 
00855      << ProjectInterface::getColorIndex((*it)->getColor()) 
00856      << " " << (*it)->getCentroid().coordX()  << " " << (*it)->getCentroid().coordY() << endl;
00857     }
00858   }
00859 }
00860 
00861 
00862 //================ Shape extraction ================
00863 
00864 void MapBuilder::getCameraShapes(const Sketch<uchar>& camFrame) { 
00865   camShS.clear();
00866   getCamLines(camFrame, curReq->objectColors[lineDataType], curReq->occluderColors[lineDataType]);
00867   getCamEllipses(camFrame, curReq->objectColors[ellipseDataType], curReq->occluderColors[ellipseDataType]);
00868   getCamPolygons(camFrame, curReq->objectColors[polygonDataType], curReq->occluderColors[polygonDataType]);
00869   getCamSpheres(camFrame, curReq->objectColors[sphereDataType], curReq->occluderColors[sphereDataType]);
00870   getCamWalls(camFrame, curReq->floorColor);
00871   if ( curReq->numSamples == 1 && !curReq->searchArea.isValid() )  // for fast blob extraction from current camera frame
00872     getCamBlobs();
00873   else
00874     getCamBlobs(camFrame, curReq->objectColors[blobDataType], curReq->minBlobAreas, curReq->blobOrientations, curReq->assumedBlobHeights);
00875   getCamTargets(camFrame, curReq->objectColors[targetDataType], curReq->occluderColors[targetDataType]);
00876 }
00877 
00878 vector<Shape<LineData> > MapBuilder::getCamLines(const Sketch<uchar> &camFrame, const set<color_index>& objectColors, 
00879              const set<color_index>& occluderColors) const {
00880   vector<Shape<LineData> > linesReturned;
00881   if ( objectColors.empty() ) 
00882     return linesReturned;
00883   if ( verbosity & MBVshapeSearch )
00884     cout << "*** Find the lines ***" << endl;
00885   NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame));
00886   for ( set<color_index>::const_iterator it = occluderColors.begin();
00887   it != occluderColors.end(); it++ )
00888     occluders |= visops::minArea(visops::colormask(camFrame,*it));
00889 
00890   for (set<color_index>::const_iterator it = objectColors.begin();
00891        it != objectColors.end(); it++) {
00892     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
00893     NEW_SKETCH_N(cleancolor,bool,visops::minArea(colormask));
00894     NEW_SKETCH_N(fatmask,bool,visops::fillin(cleancolor,1,2,8));
00895     NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
00896     vector<Shape<LineData> > line_shapes(LineData::extractLines(skel,cleancolor|occluders));
00897     linesReturned.insert(linesReturned.end(), line_shapes.begin(), line_shapes.end());
00898     if ( verbosity & MBVshapesFound )
00899       cout << "Found " << line_shapes.size() << " " 
00900      << ProjectInterface::getColorName(*it) << " lines." << endl;
00901   }
00902   return linesReturned;
00903 }
00904 
00905 vector<Shape<EllipseData> > 
00906 MapBuilder::getCamEllipses(const Sketch<uchar> &camFrame,
00907          const set<color_index>& objectColors, const set<color_index>& ) const {
00908   vector<Shape<EllipseData> > ellipses;
00909   if (objectColors.empty())
00910     return ellipses;
00911   if ( verbosity & MBVshapeSearch )
00912     cout << "*** Find the ellipses ***" << endl;
00913   for (set<color_index>::const_iterator it = objectColors.begin();
00914        it !=objectColors.end(); it++) {
00915     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
00916     vector<Shape<EllipseData> > ellipse_shapes = EllipseData::extractEllipses(colormask);
00917     ellipses.insert(ellipses.end(), ellipse_shapes.begin(),ellipse_shapes.begin());
00918     if ( verbosity & MBVshapesFound )
00919       cout << "Found " << ellipse_shapes.size() << " "
00920      << ProjectInterface::getColorName(*it) << " ellipses." << endl;
00921   }
00922   return ellipses;
00923 }
00924 
00925 vector<Shape<PolygonData> > 
00926 MapBuilder::getCamPolygons(const Sketch<uchar> &camFrame,
00927             const set<color_index>& objectColors, const set<color_index>& occluderColors) const {
00928   vector<Shape<PolygonData> > polygons;
00929   if ( objectColors.empty() ) 
00930     return polygons;
00931   if ( verbosity & MBVshapeSearch )
00932     cout << "*** Find the polygons ***" << endl;
00933   NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame));
00934   for ( set<color_index>::const_iterator it = occluderColors.begin();
00935   it !=occluderColors.end(); it++ )
00936     occluders |= visops::colormask(camFrame,*it);
00937   
00938   for (set<color_index>::const_iterator it = objectColors.begin();
00939        it !=objectColors.end(); it++) {
00940     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
00941     NEW_SKETCH_N(fatmask,bool,visops::fillin(colormask,1,2,8));
00942     NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
00943     NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8));
00944     
00945     vector<Shape<LineData> > polygon_lines = PolygonData::extractPolygonEdges(fatskel,fatmask|occluders);
00946     polygons.insert(polygons.end(), polygon_lines.begin(), polygon_lines.end());
00947     if ( verbosity & MBVshapesFound )
00948       cout << "Found " << polygon_lines.size() << " lines." << endl;
00949   }
00950   return polygons;
00951 }
00952 
00953 vector<Shape<SphereData> > 
00954 MapBuilder::getCamSpheres(const Sketch<uchar> &camFrame,
00955         const set<color_index>& objectColors, const set<color_index>& ) const {
00956   vector<Shape<SphereData> > spheres;
00957   if ( objectColors.empty() )
00958     return spheres;
00959   if ( verbosity & MBVshapeSearch )
00960     cout << "*** Find the spheres ***" << endl;
00961   for (set<color_index>::const_iterator it = objectColors.begin();
00962        it !=objectColors.end(); it++) {
00963     NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it));
00964     vector<Shape<SphereData> > sphere_shapes = SphereData::extractSpheres(colormask);
00965     spheres.insert(spheres.end(), spheres.begin(), spheres.end());
00966     if ( verbosity & MBVshapesFound )
00967       cout << "Found " << sphere_shapes.size() << " spheres." << endl;
00968   }
00969   return spheres;
00970 }
00971 
00972 vector<Shape<LineData> > 
00973 MapBuilder::getCamWalls(const Sketch<uchar> &camFrame, unsigned int floorColor) const {
00974   if (floorColor == 0)
00975     return vector<Shape<LineData> >();
00976   if ( verbosity & MBVshapeSearch )
00977     cout << "*** Find the walls ***" << endl;
00978   const int camFrame_offset = 8;
00979 
00980   NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,floorColor));
00981   NEW_SKETCH_N(fillinmask ,bool,visops::fillin(colormask, 1, 6, 8)); //remove pixels w/ connectivity<6 (noise)
00982   NEW_SKETCH_N(fillinmask2 ,bool,visops::fillin(fillinmask, 2, 3, 8)); //remove pixels w/ connectivity<3 and fill in the others
00983   NEW_SKETCH_N(edgemask ,bool,visops::fillin(fillinmask2, 1, 5, 7)); //remove pixels w/ connectivity=8 (non-edge pixels)
00984   NEW_SKETCH_N(edgemask2 ,bool,visops::non_bounds(edgemask, camFrame_offset)); //remove pixels close to cam_bound
00985 
00986   NEW_SKETCH_N(occluders_floor, bool, camFrame != uchar(0) & camFrame != uchar(floorColor));
00987   NEW_SKETCH_N(occ_mask ,bool,visops::fillin(occluders_floor, 1, 8, 8)); //remove pixels w/ connectivity<7 (noises)
00988   usint const clear_dist = 15;
00989   Sketch<bool> not_too_close = (visops::edist(occ_mask) >= clear_dist); 
00990   edgemask2 &= not_too_close; //remove pixels around occuluders
00991   
00992   NEW_SKETCH_N(fatmask ,bool,visops::fillin(edgemask2,2,2,8)); //make the remaining pixels fat
00993   NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
00994   NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8));
00995   
00996   vector<Shape<LineData> > wall_bounds = PolygonData::extractPolygonEdges(fatskel,fatmask|occluders_floor);
00997 
00998   // larger offset from the cam frame should be applied to these lines
00999   // since all pixels near cam frame bounds are removed before extracting these lines.
01000   for (vector<Shape<LineData> >::iterator it = wall_bounds.begin();
01001        it != wall_bounds.end(); it++) {
01002     if (((*it)->end1Pt().coordX() < camFrame_offset*2.0 || (*it)->end1Pt().coordX() > xres - camFrame_offset*2.0
01003    || (*it)->end1Pt().coordY() < camFrame_offset*2.0 || (*it)->end1Pt().coordY() > yres - camFrame_offset*2.0)
01004   && (*it)->end1Pt().isValid())
01005       (*it)->end1Pt().setValid(false);
01006     if (((*it)->end2Pt().coordX() < camFrame_offset*2.0 || (*it)->end2Pt().coordX() > xres - camFrame_offset*2.0
01007    || (*it)->end2Pt().coordY() < camFrame_offset*2.0 || (*it)->end2Pt().coordY() > yres - camFrame_offset*2.0)
01008   && (*it)->end2Pt().isValid())
01009       (*it)->end2Pt().setValid(false);
01010   }
01011   
01012   if ( verbosity & MBVshapesFound )
01013     cout << "Found " << wall_bounds.size() << " wall boundary lines" << endl;
01014   return wall_bounds;
01015 }
01016 
01017 void MapBuilder::getCamBlobs(const Sketch<uchar>& camFrame,
01018            const set<color_index>& colors,
01019            const map<color_index,int>& minBlobAreas,
01020            const map<color_index, BlobData::BlobOrientation_t>& blobOrientations,
01021            const map<color_index,coordinate_t>& assumedBlobHeights) {
01022   if ( colors.empty() )
01023     return;
01024   int const maxblobs = 50;
01025   vector<Shape<BlobData> > result(BlobData::extractBlobs(camFrame, colors, minBlobAreas, blobOrientations, assumedBlobHeights, maxblobs));
01026   if ( verbosity & MBVshapesFound )
01027     cout << "Found " << result.size() << " blobs." << endl;
01028 }
01029 
01030 // The code below grabs blobs directly from the region generator stream, instead of
01031 // calling CMVision to do region extraction on camFrame 
01032 void MapBuilder::getCamBlobs() {
01033   if (  curReq->objectColors[blobDataType].empty() )
01034     return;
01035   if ( verbosity & MBVshapeSearch )
01036     cout << "*** Find the blobs ***" << endl;
01037   const set<color_index>& blobColors = curReq->objectColors[blobDataType];
01038   for (set<color_index>::const_iterator it = blobColors.begin();
01039        it != blobColors.end(); it++) {
01040     int const minarea = (curReq->minBlobAreas.find(*it)==curReq->minBlobAreas.end()) ? 
01041       0 : curReq->minBlobAreas[*it];
01042     BlobData::BlobOrientation_t const orient = (curReq->blobOrientations.find(*it)==curReq->blobOrientations.end()) ? 
01043       BlobData::groundplane : curReq->blobOrientations[*it];
01044     coordinate_t const height = (curReq->assumedBlobHeights.find(*it)==curReq->assumedBlobHeights.end()) ? 
01045       0 : curReq->assumedBlobHeights[*it];
01046     vector<Shape<BlobData> > blob_shapes(VRmixin::getBlobsFromRegionGenerator(*it,minarea,orient,height));
01047     if ( verbosity & MBVshapesFound )
01048       cout << "Found " << blob_shapes.size() << " " << ProjectInterface::getColorName(*it) << " blobs." << endl;
01049   }
01050 }
01051 
01052 vector<Shape<TargetData> > 
01053 MapBuilder::getCamTargets(const Sketch<uchar> &camFrame, const set<color_index>& objectColors, const set<color_index>& occluderColors) const {
01054   vector<Shape<TargetData> > targets;
01055   if (objectColors.empty())
01056     return targets;
01057   if ( verbosity & MBVshapeSearch )
01058     cout << "*** Find the targets ***" << endl;
01059   
01060   NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame));
01061   for (set<color_index>::const_iterator it = occluderColors.begin();
01062        it != occluderColors.end(); it++)
01063     occluders |= visops::minArea(visops::colormask(camFrame,*it));
01064   
01065   // assumes multiples of 3 for objectColors (stays on the last color otherwise)
01066   for (set<color_index>::const_iterator it = objectColors.begin();
01067        it != objectColors.end(); it++) {
01068     NEW_SKETCH_N(front_colormask, bool, visops::colormask(camFrame,*it));
01069     it++;
01070     if (it == objectColors.end()) {
01071       it--;
01072     }
01073     NEW_SKETCH_N(back_colormask, bool, visops::colormask(camFrame,*it));
01074     it++;
01075     if (it == objectColors.end()) {
01076       it--;
01077     }
01078     NEW_SKETCH_N(right_colormask, bool, visops::colormask(camFrame,*it));
01079     Shape<TargetData> target = TargetData::extractLineTarget(front_colormask, back_colormask, right_colormask, occluders);
01080     if (target.isValid()) {
01081       targets.insert(targets.end(), target);
01082     }
01083     if ( verbosity & MBVshapesFound )
01084       cout << "Found " << (target.isValid() ? 1 : 0) << " targets." << endl;
01085   }
01086   
01087   return targets;
01088 }
01089 
01090 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4