00001
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"
00011 #include "SphereData.h"
00012 #include "BlobData.h"
00013 #include "PolygonData.h"
00014 #include "SphereData.h"
00015 #include "TargetData.h"
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"
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);
00069 erouter->addListener(this,EventBase::lookoutEGID);
00070 }
00071
00072 bool MapBuilder::retain = true;
00073 MapBuilder::MapBuilderVerbosity_t MapBuilder::verbosity = -1U;
00074
00075
00076
00077
00078
00079
00080
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;
00142 else if ( curReq->worldTargets.size() > 0 )
00143 doNextSearch();
00144 else if ( determineNextGazePoint() )
00145 moveToNextGazePoint();
00146 else
00147 requestComplete();
00148 }
00149 }
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
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
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);
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
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
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
00291
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
00350
00351
00352
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;
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();
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
00477 if ( camCoords(1)*camCoords(1)+camCoords(2)*camCoords(2) >= maxDistanceSq )
00478 return false;
00479 float normX,normY;
00480 config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX,normY);
00481 return (fabs(normX) < 0.9 && fabs(normY) < 0.9);
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
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
00504 if (end1Pt_visible) p[ptCount++].setCoords(normX1,normY1,0);
00505 else if (end2Pt_visible) p[ptCount++].setCoords(normX2,normY2,0);
00506 for (int i = 0; i < 4; i++)
00507 if (camBounds[i].intersectsLine(lnCam)) {
00508 p[ptCount++].setCoords(lnCam.intersectionWithLine(camBounds[i]));
00509
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
00534
00535
00536 void MapBuilder::filterGroundShapes(const NEWMAT::Matrix& baseToCam) {
00537
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) {
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) {
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
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
00569
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
00593
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;
00616 #endif
00617
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
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
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
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
00690 vector<ShapeRoot> deletedPolygons;
00691
00692 vector<ShapeRoot> newPolygons = PolygonData::formPolygons(polygon_edges,existingPolygons,deletedPolygons);
00693
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
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
00712
00713
00714
00715 it2->deleteShape();
00716 dst_shapes.erase(it2--);
00717 }
00718 }
00719 }
00720
00721
00722
00723 void MapBuilder::removeNoise(ShapeSpace& ShS, const NEWMAT::Matrix& baseToCam) {
00724
00725 vector<ShapeRoot> shapes = ShS.allShapes();
00726 for (vector<ShapeRoot>::iterator it = shapes.begin();
00727 it != shapes.end(); it++ ) {
00728
00729
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();
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
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
00828
00829 }
00830 }
00831
00832
00833
00834
00835
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
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() )
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));
00982 NEW_SKETCH_N(fillinmask2 ,bool,visops::fillin(fillinmask, 2, 3, 8));
00983 NEW_SKETCH_N(edgemask ,bool,visops::fillin(fillinmask2, 1, 5, 7));
00984 NEW_SKETCH_N(edgemask2 ,bool,visops::non_bounds(edgemask, camFrame_offset));
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));
00988 usint const clear_dist = 15;
00989 Sketch<bool> not_too_close = (visops::edist(occ_mask) >= clear_dist);
00990 edgemask2 &= not_too_close;
00991
00992 NEW_SKETCH_N(fatmask ,bool,visops::fillin(edgemask2,2,2,8));
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
00999
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
01031
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
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 }