Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

MapBuilder.cc Go to the documentation of this file. //-*-c++-*- #include "Events/EventRouter.h" #include "Events/LookoutEvents.h" #include "Events/TextMsgEvent.h" #include "Motion/HeadPointerMC.h" #include "Shared/mathutils.h" #include "Shared/newmat/newmat.h" #include "Measures.h" #include "EllipseData.h" // for extractEllipses #include "SphereData.h" // for extractSpheres #include "BlobData.h" // for extractSpheres #include "PolygonData.h" // for extractPolygons #include "ShapeRoot.h" #include "ShapeLine.h" #include "ShapeEllipse.h" #include "ShapeBlob.h" #include "ShapePolygon.h" #include "Sketch.h" // for NEW_SKETCH #include "visops.h" #include "VRmixin.h" #include "MapBuilder.h" #include "Lookout.h" namespace DualCoding { typedef map<ShapeType_t,set<int> > colorMap; inline float distSq(const NEWMAT::ColumnVector& vec) { return vec(1)*vec(1) + vec(2)*vec(2) + vec(3)*vec(3); } MapBuilder::MapBuilder() : BehaviorBase("MapBuilder"), camSkS(VRmixin::getCamSkS()), camShS(VRmixin::getCamSkS().getDualSpace()), groundShS(VRmixin::groundShS), local(maps::local,VRmixin::getLocalSkS()), world(maps::world,VRmixin::getWorldSkS()), cur(&local), xres(camSkS.getWidth()), yres(camSkS.getHeight()), ground_plane(4), theAgent(VRmixin::theAgent), localToWorldMatrix(NEWMAT::IdentityMatrix(4)), worldToLocalTranslateMatrix(NEWMAT::IdentityMatrix(4)), worldToLocalRotateMatrix(NEWMAT::IdentityMatrix(4)), badGazePoints(), agentAtOrigin(true), requests(), curReq(NULL), idCounter(0), maxDistSq(0), pointAtID(Lookout::Invalid_LO_ID), scanID(Lookout::Invalid_LO_ID), nextGazePoint() {} void MapBuilder::DoStart() { cout << "MapBuilder::DoStart()\n"; BehaviorBase::DoStart(); erouter->addListener(this,EventBase::textmsgEGID); } /* since MapBuilder is constructed as static from VRmixin, destructor doesn't get called until dog shuts down. And we have to do everything assumed to be done in destructor in DoStop, such as clearing request queue and setting parameters to the initial values as set in constructor */ void MapBuilder::DoStop() { cout << "MapBuilder::DoStop()\n"; while(!requests.empty()) { delete requests.front(); requests.pop(); } curReq = NULL; camSkS.clear(); camShS.clear(); groundShS.clear(); local.SkS.clear(); local.ShS.clear(); world.SkS.clear(); world.ShS.clear(); badGazePoints.clear(); local.gazePts.clear(); world.gazePts.clear(); setAgent(Point(0,0,0),0); BehaviorBase::DoStop(); } unsigned int MapBuilder::executeRequest(const MapBuilderRequest& req) { switch (req.getRequestType()) { case MapBuilderRequest::takeSnap: if (const TakeSnapAtRequest* snapAtReq = dynamic_cast<const TakeSnapAtRequest*>(&req)) requests.push(new TakeSnapAtRequest(*snapAtReq)); else requests.push(new TakeSnapRequest(*dynamic_cast<const TakeSnapRequest*>(&req))); break; case MapBuilderRequest::localMap: if (const LocalMapTestRequest* testReq = dynamic_cast<const LocalMapTestRequest*>(&req)) requests.push(new LocalMapTestRequest(*testReq)); else requests.push(new LocalMapRequest(*dynamic_cast<const LocalMapRequest*>(&req))); break; case MapBuilderRequest::worldMap: if (const WorldMapTestRequest* testReq = dynamic_cast<const WorldMapTestRequest*>(&req)) requests.push(new WorldMapTestRequest(*testReq)); else requests.push(new WorldMapRequest(*dynamic_cast<const WorldMapRequest*>(&req))); break; default: return -1U; }; requests.back()->requestID = idCounter++; // cout << "MapBuilder::executeRequest: new request " << requests.back()->requestID // << " added to the queue\n"; if (requests.size() == 1) { erouter->addListener(this,EventBase::lookoutEGID); executeRequest(); } return requests.back()->requestID; } void MapBuilder::executeRequest() { if ( requests.empty() || curReq != NULL ) return; curReq = requests.front(); cout << "MapBuilder::executeRequest: execute " << curReq->requestID << endl; erouter->postEvent(EventBase::mapbuilderEGID, requests.front()->requestID, EventBase::activateETID,0); if (curReq->groundPlaneAssumption == MapBuilderRequest::onStand) calculateGroundPlane(MapBuilderRequest::onStand); if (curReq->getRequestType() == MapBuilderRequest::takeSnap) if (const TakeSnapAtRequest* snapAtReq = dynamic_cast<const TakeSnapAtRequest*>(curReq)) storeImageAt(snapAtReq->gazePt); else storeImage(); else { const LocalMapRequest& curMapReq = *dynamic_cast<const LocalMapRequest*> (&(*curReq)); maxDistSq = curMapReq.maxDist*curMapReq.maxDist; cur = (&curMapReq.shs==&local.ShS) ? &local : &world; if (curMapReq.clearShapes) { // erase shapes and gaze points, start from scratch cur->ShS.clear(); cur->SkS.clear(); cur->gazePts.clear(); cur->baseToCamMats.clear(); } defineGazePts(cur->gazePts, curMapReq.area, curMapReq.doScan); if ( determineNextGazePoint() ) moveToNextGazePoint(); else requestComplete(); } } void MapBuilder::processEvent(const EventBase &e) { cout << "MapBuilder::processEvent got " << e.getDescription() << endl; switch ( e.getGeneratorID() ) { case EventBase::textmsgEGID: { const TextMsgEvent &txtev = dynamic_cast<const TextMsgEvent&>(e); if ( strcmp(txtev.getText().c_str(),"MoveHead") == 0 ) moveToNextGazePoint(true); break; } case EventBase::lookoutEGID: // image stored, extract shapes and extend local/world shape space cout << "MapBuilder got event " << e.getName() << " pointAtID=" << pointAtID << endl; if (e.getSourceID() == pointAtID && e.getTypeID() == EventBase::deactivateETID) { const Sketch<uchar>& camImg = dynamic_cast<const LookoutSketchEvent*>(&e)->getSketch(); NEW_SKETCH(camFrame, uchar, camImg); const NEWMAT::Matrix& camToBase = dynamic_cast<const LookoutSketchEvent*>(&e)->toBaseMatrix; const NEWMAT::Matrix baseToCam = camToBase.i(); processImage(camFrame, camToBase, baseToCam); if (curReq->getRequestType() == MapBuilderRequest::takeSnap) { requestComplete(); return; } if (cur->space == maps::local) extendLocal(baseToCam); else extendWorld(baseToCam); } // gaze points defined by scan, start constructing map else if (e.getSourceID() == scanID && e.getTypeID() != EventBase::activateETID) { const vector<Point>& pts = dynamic_cast<const LookoutScanEvent*>(&e)->getTasks().front()->data; cur->gazePts.insert(cur->gazePts.end(),pts.begin(), pts.end()); } else cout << "MapBuilder::processEvent(): unknown event " << e.getDescription() << " pointAtID=" << pointAtID << endl; if ( requestExitTest() || !determineNextGazePoint() ) requestComplete(); else moveToNextGazePoint(); break; default: cout << "Unexpected event " << e.getDescription() << endl; } } void MapBuilder::processImage(const Sketch<uchar>& camFrame, const NEWMAT::Matrix& camToBase, const NEWMAT::Matrix& baseToCam) { if (curReq->groundPlaneAssumption == MapBuilderRequest::onLegs) calculateGroundPlane(MapBuilderRequest::onLegs); getCameraShapes(camFrame); projectToGround(camToBase); filterGroundShapes(baseToCam); } bool MapBuilder::determineNextGazePoint() { if (&cur->ShS == &world.ShS && !agentAtOrigin) { cur->ShS.applyTransform(worldToLocalTranslateMatrix); cur->ShS.applyTransform(worldToLocalRotateMatrix); bool b = determineNextGazePoint(cur->ShS.allShapes()) || determineNextGazePoint(cur->gazePts); cur->ShS.applyTransform(localToWorldMatrix); // transform back return b; } else return determineNextGazePoint(cur->ShS.allShapes()) || determineNextGazePoint(cur->gazePts); } bool MapBuilder::determineNextGazePoint(const vector<ShapeRoot>& shapes) { const LocalMapRequest *locReq = dynamic_cast<const LocalMapRequest*>(curReq); if ( locReq == NULL || ! locReq->pursueShapes ) return false; HeadPointerMC headpointer_mc; for (vector<ShapeRoot>::const_iterator it = shapes.begin(); it != shapes.end(); it++) { // look for invalid endpoints of lines / polygons if ((*it)->isType(lineDataType) || (*it)->isType(polygonDataType)) { const Shape<LineData>& ld = ShapeRootTypeConst(*it,LineData); const Shape<PolygonData>& pd = ShapeRootTypeConst(*it,PolygonData); bool isLine = (*it)->isType(lineDataType); EndPoint p[2] = { isLine ? ld->end1Pt(): pd->end1Pt(), isLine ? ld->end2Pt() : pd->end2Pt()}; for (int i = 0; i < 2; i++) { if (!p[i].isValid() && !isBadGazePoint(p[i]) && badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (Point) p[i])) { cout << "take snap at endpoint" << (i+1) << " of shape id " << (*it)->getId() << " at " << p[i] << endl; if (!headpointer_mc.lookAtPoint(p[i].coordX(),p[i].coordY(),p[i].coordZ())) badGazePoints.push_back((Point)p[i]); nextGazePoint = p[i]; return true; } } } // look for shapes w/ <2 confidence if ((!(*it)->isType(agentDataType)) && (*it)->getConfidence() <= 1 && ! isBadGazePoint((*it)->getCentroid()) && badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (*it)->getCentroid())) { const Point pt = (*it)->getCentroid(); cout << "take snap at shape " << (*it)->getId() << " (confidence level: " << (*it)->getConfidence() << ")" << endl; cout << " at " << pt << endl; if (! headpointer_mc.lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ())) badGazePoints.push_back(pt); nextGazePoint = pt; return true; } cout << "Skipping shape " << (*it)->getId() << " (confidence level: " << (*it)->getConfidence() << ")" << endl; } return false; } bool MapBuilder::determineNextGazePoint(vector<Point>& gazePts) { HeadPointerMC headpointer_mc; for (vector<Point>::iterator it = gazePts.begin(); it != gazePts.end(); it++) { cout << "look at pre-defined gazePt: " << *it << endl; nextGazePoint = *it; gazePts.erase(it); return true; } /* cout << "Gaze points exhausted; " << badGazePoints.size() << " bad gaze points were eliminated: " << endl; for (vector<Point>::const_iterator bad_it = badGazePoints.begin(); bad_it != badGazePoints.end(); bad_it++) cout << *bad_it << " "; cout << endl; */ return false; } void MapBuilder::moveToNextGazePoint(const bool manualOverride) { if ( curReq == NULL ) { cout << "curReq == NULL in moveToNextGazePoint!" << endl; return; } cout << "curReq == " << (unsigned int)curReq << endl; const LocalMapRequest* locReq = dynamic_cast<LocalMapRequest*>(curReq); if ( locReq != NULL && locReq->manualHeadMotion && manualOverride==false ) { cout << "To proceed to this gaze point: !msg MoveHead" << endl; return; } storeImageAt(nextGazePoint); } bool MapBuilder::isBadGazePoint(const Point& Pt) const { const coordinate_t x = Pt.coordX(); const coordinate_t y = Pt.coordY(); return ( x*x + y*y > maxDistSq || x < -30.0); } void MapBuilder::storeImageAt(const Point& pt) { pointAtID = VRmixin::lookout.executeRequest(StoreImageAtRequest(pt)); cout << "MapBuilder sent new storeImageAt request; pointAtID=" << pointAtID << endl; } void MapBuilder::storeImage() { pointAtID = VRmixin::lookout.executeRequest(StoreImageRequest()); cout << "MapBuilder sent new storeImage request; pointAtID=" << pointAtID << endl; } void MapBuilder::scan(ScanRequest& req) { set<int> colors; for (colorMap::const_iterator it1 = curReq->objectColors.begin(); it1 != curReq->objectColors.end(); it1++) for (set<int>::const_iterator it2 = it1->second.begin(); it2 != it1->second.end(); it2++) colors.insert(*it2); req.addTask(ScanRequest::VisionRegionTask(colors,M_PI/18)); scanID = VRmixin::lookout.executeRequest(req); } void MapBuilder::extendLocal(const NEWMAT::Matrix& baseToCam) { vector<ShapeRoot> all = local.ShS.allShapes(); removeNoise(local.ShS, baseToCam); matchSrcToDst(groundShS,local.ShS,curReq->objectColors[polygonDataType]); removeGazePts(local.gazePts, baseToCam); local.baseToCamMats.push_back(baseToCam); } void MapBuilder::extendWorld(const NEWMAT::Matrix& baseToCam) { if (!agentAtOrigin) { world.ShS.applyTransform(worldToLocalTranslateMatrix); world.ShS.applyTransform(worldToLocalRotateMatrix); } removeNoise(world.ShS, baseToCam); matchSrcToDst(groundShS,world.ShS,curReq->objectColors[polygonDataType]); if (!agentAtOrigin) world.ShS.applyTransform(localToWorldMatrix); removeGazePts(world.gazePts,baseToCam); world.baseToCamMats.push_back(baseToCam); } bool MapBuilder::requestExitTest() { if (const LocalMapTestRequest* localTestReq = dynamic_cast<const LocalMapTestRequest*>(curReq)) return ((*localTestReq->exitTest)()); else if (const WorldMapTestRequest* worldTestReq = dynamic_cast<const WorldMapTestRequest*>(curReq)) return ((*worldTestReq->exitTest)()); else return false; } void MapBuilder::requestComplete() { const unsigned int reqID = curReq->requestID; cout << "MapBuilderRequest " << reqID << " complete\n"; delete curReq; curReq = NULL; requests.pop(); if ( requests.empty() ) erouter->removeListener(this,EventBase::lookoutEGID); // don't need to listen to Lookout events when there is no request pending erouter->postEvent(EventBase::mapbuilderEGID, reqID, EventBase::statusETID,0); executeRequest(); // execute next request AFTER status event has finished processing } void MapBuilder::setAgent(const Point &location, const AngTwoPi heading) { theAgent->setCentroidPt(location); theAgent->setOrientation(heading); const coordinate_t dx = location.coordX(); const coordinate_t dy = location.coordY(); const coordinate_t dz = location.coordZ(); float localToWorld[] = {cos(heading), -sin(heading), 0, dx, sin(heading),cos(heading), 0, dy, 0, 0, 1, dz, 0 ,0, 0, 1}; float worldToLocalTrans[] = {1, 0, 0, -dx, 0, 1, 0, -dy, 0, 0, 1, -dz, 0 ,0, 0, 1}; float worldToLocalRotate[] = {cos(heading), sin(heading), 0, 0, -sin(heading),cos(heading), 0, 0, 0, 0, 1, 0, 0 ,0, 0, 1}; localToWorldMatrix << localToWorld; worldToLocalTranslateMatrix << worldToLocalTrans; worldToLocalRotateMatrix << worldToLocalRotate; agentAtOrigin = (heading == 0 || heading == 2*M_PI) && dx == 0 && dy == 0; } void MapBuilder::moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, AngTwoPi dtheta) { AngTwoPi const heading = theAgent->getOrientation(); float const c = cos(heading); float const s = sin(heading); float const dx = local_dx*c + local_dy*-s; float const dy = local_dx*s + local_dy*c; setAgent(Point(dx+theAgent->getCentroid().coordX(), dy+theAgent->getCentroid().coordY(), theAgent->getCentroid().coordZ()), dtheta+heading); } void MapBuilder::importLocalToWorld() { if (!agentAtOrigin) { world.ShS.applyTransform(worldToLocalTranslateMatrix); world.ShS.applyTransform(worldToLocalRotateMatrix); } matchSrcToDst(local.ShS, world.ShS); if (!agentAtOrigin) world.ShS.applyTransform(localToWorldMatrix); } void MapBuilder::importWorldToLocal(const ShapeRoot &worldShape) { ShapeRoot localShape(local.ShS.importShape(worldShape)); localShape->applyTransform(worldToLocalTranslateMatrix); localShape->applyTransform(worldToLocalRotateMatrix); } bool MapBuilder::isPointVisible(const Point &pt, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) { NEWMAT::ColumnVector camCoords = baseToCam*pt.coords; if (camCoords(3) <=0 || distSq(camCoords) >= maxDistanceSq) return false; float normX,normY; // normalized coordinates in cam frame config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX,normY); return (fabs(normX) < 0.9 && fabs(normY) < 0.9); //normX and normY range from -1 to 1. Giving 10% offset here } bool MapBuilder::isLineVisible(const LineData& ln, const NEWMAT::Matrix& baseToCam) { float normX1,normX2,normY1,normY2; NEWMAT::ColumnVector camCoords(4); camCoords = baseToCam*ln.end1Pt().coords; config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX1,normY1); camCoords = baseToCam*ln.end2Pt().coords; config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX2,normY2); const bool end1Pt_visible = fabs(normX1) < 0.9 && fabs(normY1) < 0.9; const bool end2Pt_visible = fabs(normX2) < 0.9 && fabs(normY2) < 0.9; if (end1Pt_visible && end2Pt_visible) return true; LineData lnCam(VRmixin::groundShS, Point(normX1,normY1), Point(normX2,normY2)); // define bounding box of camera frame in terms of normalized coordinates with 10% offset LineData camBounds[] = {LineData(VRmixin::groundShS, Point(0.9,0.9),Point(0.9,-0.9)), LineData(VRmixin::groundShS, Point(0.9,-0.9),Point(-0.9,-0.9)), LineData(VRmixin::groundShS, Point(-0.9,-0.9),Point(-0.9,0.9)), LineData(VRmixin::groundShS, Point(-0.9,0.9),Point(0.9,0.9))}; unsigned int ptCount = 0; Point p[2]; // find if a portion of the line shows up in cam if (end1Pt_visible) p[ptCount++].setCoords(normX1,normY1,0); // end1Pt in frame else if (end2Pt_visible) p[ptCount++].setCoords(normX2,normY2,0); // end2Pt in frame for (int i = 0; i < 4; i++) if (camBounds[i].intersectsLine(lnCam)) { p[ptCount++].setCoords(lnCam.intersectionWithLine(camBounds[i])); // Let's say portion of line seen in cam should be longer than .1 normalized if (ptCount > 1) return p[0].distanceFrom(p[1]) > 0.1; } return false; } bool MapBuilder::isShapeVisible(const ShapeRoot &ground_shape, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) { if (ground_shape->isType(lineDataType)) return isLineVisible(ShapeRootTypeConst(ground_shape,LineData).getData(), baseToCam); else if (ground_shape->isType(polygonDataType)) { const Shape<PolygonData>& polygon = ShapeRootTypeConst(ground_shape,PolygonData); for (vector<LineData>::const_iterator edge_it = polygon->getEdges().begin(); edge_it != polygon->getEdges().end(); edge_it++) if (isLineVisible(*edge_it,baseToCam)) return true; return false; } else return isPointVisible(ground_shape->getCentroid(), baseToCam, maxDistanceSq); } // filter "bad" ground shapes before importing to dst shape space. // 1. ignore shapes too far from dog or projected to the other side of cam plane // 2. chop off line at max distance if it is extending beyond the distance and leave the endpoint invalid void MapBuilder::filterGroundShapes(const NEWMAT::Matrix& baseToCam) { // cout << "MapBuilder::filterGroundShapes()" << endl; vector<ShapeRoot> ground_shapes = groundShS.allShapes(); for (vector<ShapeRoot>::iterator ground_it = ground_shapes.begin(); ground_it != ground_shapes.end(); ground_it++ ) { const coordinate_t cenX = (*ground_it)->getCentroid().coordX(); const coordinate_t cenY = (*ground_it)->getCentroid().coordY(); if (cenX*cenX + cenY*cenY > maxDistSq) { // too far cout << "ground shape " << (*ground_it)->getId() << "(lastMatch " << (*ground_it)->getLastMatchId() << ") too far, delete\n"; ground_it->deleteShape(); } NEWMAT::ColumnVector coords = Kinematics::pack(cenX,cenY,(*ground_it)->getCentroid().coordZ()); coords = baseToCam*coords; if (coords(3) < 0) { // negative z-coordinate in camera frame indicates projection failed cout << "Projection failed for ground shape " << (*ground_it)->getId() << "(lastMatch " << (*ground_it)->getLastMatchId() << "), delete\n"; ground_it->deleteShape(); } // if a line is extending to maxDistance, chop it off at maxdistance and mark the endpoint invalid else if ((*ground_it)->isType(lineDataType)) { const Shape<LineData>& line = ShapeRootTypeConst(*ground_it,LineData); const coordinate_t e1x = line->end1Pt().coordX(); const coordinate_t e1y = line->end1Pt().coordY(); const coordinate_t e2x = line->end2Pt().coordX(); const coordinate_t e2y = line->end2Pt().coordY(); if (e1x*e1x + e1y*e1y > maxDistSq && e2x*e2x + e2y*e2y > maxDistSq) ground_it->deleteShape(); else if (e1x*e1x + e1y*e1y > maxDistSq || e2x*e2x + e2y*e2y > maxDistSq) { // cout << (*ground_it)->getId() << "(lastMatch " << (*ground_it)->getLastMatchId() // << ") extends beyond maximum distance we want. Chop off the line" << endl; vector<float> line_abc = line->lineEquation_abc(); Point pt; const EndPoint* far_ept = (e1x*e1x + e1y*e1y > maxDistSq) ? &line->end1Pt() : &line->end2Pt(); if (line_abc[1] == 0.0) { const coordinate_t y_abs = sqrt(maxDistSq - line_abc[2]*line_abc[2]); if (fabs(far_ept->coordY()-y_abs) < fabs(far_ept->coordY()+y_abs)) pt.setCoords(e1x, y_abs, far_ept->coordZ()); else pt.setCoords(e1x, -y_abs, far_ept->coordZ()); } else { const float a = - line_abc[0]/line_abc[1]; const float b = line_abc[2]/line_abc[1]; const coordinate_t x1 = (sqrt((a*a+1.0)*maxDistSq-b*b)-a*b)/(a*a+1.0); const coordinate_t x2 = (-sqrt((a*a+1.0)*maxDistSq-b*b)-a*b)/(a*a+1.0); if (fabs(far_ept->coordX()-x1) < fabs(far_ept->coordX()-x2)) pt.setCoords(x1, a*x1+b, far_ept->coordZ()); else pt.setCoords(x2, a*x2+b, far_ept->coordZ()); } EndPoint temp_endPt(pt); temp_endPt.setValid(false); // cout << " (" << far_ept->coordX() << "," << far_ept->coordY() << ") => (" // << pt.coordX() << "," << pt.coordY() << ")" << endl; if (e1x*e1x + e1y*e1y > maxDistSq) line->setEndPts(temp_endPt, line->end2Pt()); else line->setEndPts(line->end1Pt(), temp_endPt); badGazePoints.push_back(pt); } } } } void MapBuilder::calculateGroundPlane(const MapBuilderRequest::GroundPlaneAssumption_t& gpa) { switch(gpa) { case MapBuilderRequest::onLegs: ground_plane << kine->calculateGroundPlane(); cout << "Neck pan = " << state->outputs[HeadOffset+PanOffset] << endl; cout << "Calculated ground plane: " << NEWMAT::printmat(ground_plane) << endl; break; case MapBuilderRequest::onStand: #ifdef TGT_ERS210 ground_plane << 0 << 0 << (float)(-1/200.0) << 1; #else ground_plane << 0 << 0 << (float)(-1/170.0) << 1; //for the order-made stands for ERS7 in the lab #endif // cout << "Hard-coded ground plane: " << NEWMAT::printmat(ground_plane) << endl; break; }; } void MapBuilder::projectToGround(const NEWMAT::Matrix& camToBase) { VRmixin::projectToGround(camToBase, ground_plane); } void MapBuilder::matchSrcToDst(ShapeSpace &srcShS, ShapeSpace &dstShS, set<int> polCols, bool mergeSrc, bool mergeDst) { vector<ShapeRoot> src_shapes = srcShS.allShapes(); vector<ShapeRoot> dst_shapes = dstShS.allShapes(); vector<LineData> polygon_edges; // clean up src_shapes before messing with dst space for (vector<ShapeRoot>::iterator src_it = src_shapes.begin(); src_it != src_shapes.end(); src_it++ ) { if (!(*src_it)->isAdmissible()) { cout << "shape " << (*src_it)->getId() << "(lastMatch " << (*src_it)->getLastMatchId() << ") is not admissible:" << endl; (*src_it)->printParams(); src_shapes.erase(src_it--); } else if ((*src_it)->isType(polygonDataType)) { const vector<LineData>& lines = ShapeRootTypeConst(*src_it, PolygonData)->getEdges(); polygon_edges.insert(polygon_edges.end(), lines.begin(), lines.end()); src_shapes.erase(src_it--); } else if ((*src_it)->isType(lineDataType)) { const int colorIndex = ProjectInterface::getColorIndex((*src_it)->getColor()); if ( polCols.end() != find(polCols.begin(), polCols.end(), colorIndex)) { polygon_edges.push_back(ShapeRootTypeConst(*src_it, LineData).getData()); src_shapes.erase(src_it--); } } } // merge shapes inside srcShS if (mergeSrc) for ( vector<ShapeRoot>::iterator it = src_shapes.begin(); it != src_shapes.end(); it++ ) for ( vector<ShapeRoot>::iterator it2 = it+1; it2 != src_shapes.end(); it2++) if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2)) { cout << "merging shape " << (*it)->getId() << " and shape " << (*it2)->getId() << endl; src_shapes.erase(it2--); } vector<Shape<PolygonData> > existingPolygons; // update dst shapes from src shapes for (vector<ShapeRoot>::iterator dst_it = dst_shapes.begin(); dst_it != dst_shapes.end(); dst_it++ ) if ((*dst_it)->isType(polygonDataType)) existingPolygons.push_back(ShapeRootType(*dst_it,PolygonData)); else { for (vector<ShapeRoot>::iterator src_it = src_shapes.begin(); src_it != src_shapes.end(); src_it++) if ((*dst_it)->isMatchFor(*src_it) && (*dst_it)->updateParams((*src_it))) { (*dst_it)->increaseConfidence(*src_it); // cout << "Matched src shape " << (*src_it)->getId() << " (lastMatch " << (*src_it)->getLastMatchId() // << ") to dst shape " << (*dst_it)->getId() << endl; src_shapes.erase(src_it--); } } // form polygons from lines and import unmatched src shapes into dstShS vector<ShapeRoot> deletedPolygons; // cout << existingPolygons.size() << " polygons\n"; vector<ShapeRoot> newPolygons = PolygonData::formPolygons(polygon_edges,existingPolygons,deletedPolygons); // cout << existingPolygons.size()+deletedPolygons.size() << " polygons\n"; for (vector<ShapeRoot>::iterator delete_it = deletedPolygons.begin(); delete_it != deletedPolygons.end(); delete_it++) delete_it->deleteShape(); dstShS.importShapes(newPolygons); dstShS.importShapes(src_shapes); cout << "Imported " << (src_shapes.size()+newPolygons.size()) << " new shape(s) from " << srcShS.name << "ShS to " << dstShS.name << "ShS\n"; // match shapes inside dstShS if (mergeDst) { dst_shapes = dstShS.allShapes(); for ( vector<ShapeRoot>::iterator it = dst_shapes.begin(); it < dst_shapes.end(); it++ ) for ( vector<ShapeRoot>::iterator it2 = it+1; it2 < dst_shapes.end(); it2++) if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2,true)) { // cout << "Matched src shape " << (*it)->getId() << " (lastMatch " // << (*it)->getLastMatchId()<< ") is a match for " // << (*it2)->getId() << " (lastMatch " // << (*it2)->getLastMatchId() << "), delete\n"; it2->deleteShape(); dst_shapes.erase(it2--); } } } // decrease confidence of those shapes should have been seen in the last snap, // remove shapes from shapespace if confidence becomes <0 void MapBuilder::removeNoise(ShapeSpace& ShS, const NEWMAT::Matrix& baseToCam) { // cout << "MapBuilder::removeNoise()\n"; vector<ShapeRoot> shapes = ShS.allShapes(); for (vector<ShapeRoot>::iterator it = shapes.begin(); it != shapes.end(); it++ ) { // was not looking for this object in the last snap, // and it's not fair to decrease this guy's confidence if (curReq->objectColors[(*it)->getType()].find(ProjectInterface::getColorIndex((*it)->getColor())) == curReq->objectColors[(*it)->getType()].end()) continue; if ((*it)->isType(polygonDataType)) { Shape<PolygonData>& polygon = ShapeRootType(*it,PolygonData); vector<LineData>& edges = polygon->getEdgesRW(); unsigned int edge_index = 0; for (vector<LineData>::iterator edge_it = edges.begin(); edge_it != edges.end(); edge_it++, edge_index++) { if (isLineVisible(*edge_it, baseToCam)) { cout << "edge " << edge_index << " of polygon " << (*it)->getId() << "(confidence: " << edge_it->getConfidence() << ") should be visible in this frame" << endl; edge_it->decreaseConfidence(); } } vector<ShapeRoot> brokenPolygons = polygon->updateState(); ShS.importShapes(brokenPolygons); if (!polygon->isAdmissible()) { cout << "delete polygon " << (*it)->getId() << " from map" << endl; it->deleteShape(); } } else if ((!(*it)->isType(agentDataType)) && isShapeVisible(*it, baseToCam, maxDistSq)){ (*it)->decreaseConfidence(); // decrease confidence by 1 for every visible shape cout << "shape " << (*it)->getId() << "(confidence: " << (*it)->getConfidence() << ") should be visible in this frame" << endl; if ((*it)->getConfidence() < 0 ) { cout << "confidence < 0, shape " << (*it)->getId() << " deleted from map" << endl; // it->deleteShape(); } } } } //================ Gaze points ================ void MapBuilder::defineGazePts(vector<Point> &gazePts, const ShapeRoot& area, bool doScan) { cout << "MapBuilder::defineGazePts\n"; static const float meshSize=50; switch (area->getType()) { case lineDataType: { const Shape<LineData>& line = ShapeRootTypeConst(area,LineData); if (doScan) { ScanAlongLineRequest req(line->end1Pt(),line->end2Pt()); scan(req); } else { int numIntervals = (int) (line->getLength()/meshSize); const EndPoint& ep1 = line->end1Pt(); const EndPoint& ep2 = line->end2Pt(); gazePts.push_back(ep1); for (int i = 1; i < numIntervals; i++) gazePts.push_back((ep1*i + ep2*(numIntervals-i))/numIntervals); gazePts.push_back(ep2); } } break; case blobDataType: { const Shape<BlobData>& blob = ShapeRootTypeConst(area,BlobData); if (doScan) { ScanAreaRequest req(blob->topLeft.coordY(),blob->bottomRight.coordY(), blob->topLeft.coordX(),blob->bottomRight.coordX()); scan(req); } else { vector<Point> corners; corners.push_back(blob->topLeft); corners.push_back(blob->topRight); corners.push_back(blob->bottomRight); corners.push_back(blob->bottomLeft); defineGazePts(gazePts, corners, meshSize); } } break; case pointDataType: gazePts.push_back(area->getCentroid()); break; case polygonDataType: { const Shape<PolygonData> &poly = ShapeRootTypeConst(area,PolygonData); const vector<Point> &verts = poly->getVertices(); gazePts.insert(gazePts.end(),verts.begin(),verts.end()); } break; default: cerr << "ERRORR MapBuilder::defineGazePts: Only types of area supported are Blob, Line, Point, and Polygon\n"; requestComplete(); break; }; } void MapBuilder::defineGazePts(vector<Point>& gazePts, const vector<Point>& corners, float meshSize) { if (corners.size() != 4) { cout << "Region of interest has to have four corners" << endl; return; } for (vector<Point>::const_iterator it = corners.begin(); it != corners.end()-1; it++) { if (find(it+1, corners.end(), *it) != corners.end()) { cout << "At least two corners are identical" << endl; return; } } LineData ln1(groundShS, corners.at(0),corners.at(1)); LineData ln2(groundShS, corners.at(0),corners.at(2)); const bool edge0_next_to_edge1 = !ln1.intersectsLine(LineData(groundShS, corners.at(2),corners.at(3))); const bool edge0_next_to_edge2 = !ln2.intersectsLine(LineData(groundShS, corners.at(1),corners.at(3))); vector<LineData> horizontal, vertical; // passed corners may not be in the right order, sort them in clockwise or counterclockwise direction const Point p0(corners.at(0)); const Point p1(edge0_next_to_edge1 ? corners.at(1) : corners.at(3)); const Point p2(edge0_next_to_edge1 ? (edge0_next_to_edge2 ? corners.at(3) : corners.at(2)) : corners.at(1)); const Point p3(edge0_next_to_edge2 ? corners.at(2) : corners.at(3)); // define horizontal lines Point p1_minus_p0 = p1-p0; Point p2_minus_p3 = p2-p3; const int num_steps_x = (int) (sqrt(max(p1_minus_p0.coordX()*p1_minus_p0.coordX() + p1_minus_p0.coordY()*p1_minus_p0.coordY(), p2_minus_p3.coordX()*p2_minus_p3.coordX() + p2_minus_p3.coordY()*p2_minus_p3.coordY())) / meshSize) + 1; p1_minus_p0 /= (float) num_steps_x; p2_minus_p3 /= (float) num_steps_x; for (int i = 0; i <= num_steps_x; i++) horizontal.push_back(LineData(groundShS, p0+p1_minus_p0*i, p3+p2_minus_p3*i)); // define vertical lines Point p3_minus_p0 = p3-p0; Point p2_minus_p1 = p2-p1; const int num_steps_y = (int) (sqrt(max(p3_minus_p0.coordX()*p3_minus_p0.coordX() + p3_minus_p0.coordY()*p3_minus_p0.coordY(), p2_minus_p1.coordX()*p2_minus_p1.coordX() + p2_minus_p1.coordY()*p2_minus_p1.coordY())) / meshSize) + 1; p3_minus_p0 /= (float) num_steps_y; p2_minus_p1 /= (float) num_steps_y; for (int i = 0; i <= num_steps_y; i++) vertical.push_back(LineData(groundShS, p0+p3_minus_p0*i, p1+p2_minus_p1*i)); // define check points = intersection of horizontal and vertical lines // cout << "checkList: " << endl; for (vector<LineData>::const_iterator H_it = horizontal.begin(); H_it != horizontal.end(); H_it++) for (vector<LineData>::const_iterator V_it = vertical.begin(); V_it != vertical.end(); V_it++) gazePts.push_back(H_it->intersectionWithLine(*V_it)); } void MapBuilder::removeGazePts(vector<Point> &gazePts, const NEWMAT::Matrix& baseToCam) { if ( ! dynamic_cast<const LocalMapRequest*>(curReq)->removePts ) return; int num_points_seen = 0; for ( vector<Point>::iterator it = gazePts.begin(); it != gazePts.end(); it++ ) { if (isPointVisible(*it, baseToCam, maxDistSq)) { num_points_seen++; gazePts.erase(it--); } } // cout << num_points_seen << " pre-defined gaze points seen in last snap, " // << gazePts.size() << " left\n"; } //================================================================ //Prints shapespace in the format to be used for particle filter on simulator void MapBuilder::printShS(ShapeSpace &ShS) const { cout << "MapBuilder::printShS()" << endl; unsigned int line_count = 0; vector<ShapeRoot> shapes = ShS.allShapes(); for (vector<ShapeRoot>::const_iterator it = shapes.begin(); it != shapes.end(); it++) { if ((*it)->isType(lineDataType)) { const Shape<LineData>& ld = ShapeRootTypeConst(*it,LineData); cout << (*it)->getId() << " " << lineDataType << " " << ProjectInterface::getColorIndex((*it)->getColor()) << " " << ld->end1Pt().coordX() << " " << ld->end1Pt().coordY() << " " << ++line_count << " " << ld->getLength() << " " << ld->end1Pt().isValid() << endl; cout << (*it)->getId() << " " << lineDataType << " " << ProjectInterface::getColorIndex((*it)->getColor()) << " " << ld->end2Pt().coordX() << " " << ld->end2Pt().coordY() << " " << line_count << " " << ld->getLength() << " " << ld->end2Pt().isValid() << endl; } else { cout << (*it)->getId() << " " << (*it)->getType() << " " << ProjectInterface::getColorIndex((*it)->getColor()) << " " << (*it)->getCentroid().coordX() << " " << (*it)->getCentroid().coordY() << endl; } } } //================ Shape extraction ================ void MapBuilder::getCameraShapes(const Sketch<uchar>& camFrame) { camShS.clear(); getCamLines(camFrame, curReq->objectColors[lineDataType], curReq->occluderColors[lineDataType]); getCamEllipses(camFrame, curReq->objectColors[ellipseDataType], curReq->occluderColors[ellipseDataType]); getCamPolygons(camFrame, curReq->objectColors[polygonDataType], curReq->occluderColors[polygonDataType]); getCamSpheres(camFrame, curReq->objectColors[sphereDataType], curReq->occluderColors[sphereDataType]); getCamWalls(camFrame, curReq->floorColor); BlobData::extractBlobs(camFrame, curReq->objectColors[blobDataType]); } vector<Shape<LineData> > MapBuilder::getCamLines(const Sketch<uchar> &camFrame, const set<int>& objectColors, const set<int>& occluderColors) const { vector<Shape<LineData> > lines; if (objectColors.empty() ) return lines; cout << "*** Find the lines ***" << endl; NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame)); for ( set<int>::const_iterator it = occluderColors.begin(); it != occluderColors.end(); it++ ) occluders |= visops::minArea(visops::colormask(camFrame,*it)); for (set<int>::const_iterator it = objectColors.begin(); it != objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); NEW_SKETCH_N(cleancolor,bool,visops::minArea(colormask)); NEW_SKETCH_N(fatmask,bool,visops::fillin(cleancolor,1,2,8)); NEW_SKETCH_N(skel,bool,visops::skel(fatmask)); vector<Shape<LineData> > line_shapes = LineData::extractLines(skel,cleancolor|occluders); lines.insert(lines.end(), line_shapes.begin(), line_shapes.end()); cout << "Found " << line_shapes.size() << " lines." << endl; } return lines; } vector<Shape<EllipseData> > MapBuilder::getCamEllipses(const Sketch<uchar> &camFrame, const set<int>& objectColors, const set<int>& ) const { vector<Shape<EllipseData> > ellipses; if (objectColors.empty()) return ellipses; cout << "*** Find the ellipses ***" << endl; for (set<int>::const_iterator it = objectColors.begin(); it !=objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); vector<Shape<EllipseData> > ellipse_shapes = EllipseData::extractEllipses(colormask); ellipses.insert(ellipses.end(), ellipse_shapes.begin(),ellipse_shapes.begin()); cout << "Found " << ellipse_shapes.size() << " ellipses." << endl; } return ellipses; } vector<Shape<PolygonData> > MapBuilder::getCamPolygons(const Sketch<uchar> &camFrame, const set<int>& objectColors, const set<int>& occluderColors) const { vector<Shape<PolygonData> > polygons; if ( objectColors.empty() ) return polygons; cout << "*** Find the polygons ***" << endl; NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame)); for ( set<int>::const_iterator it = occluderColors.begin(); it !=occluderColors.end(); it++ ) occluders |= visops::colormask(camFrame,*it); for (set<int>::const_iterator it = objectColors.begin(); it !=objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); NEW_SKETCH_N(fatmask,bool,visops::fillin(colormask,1,2,8)); NEW_SKETCH_N(skel,bool,visops::skel(fatmask)); NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8)); vector<Shape<LineData> > polygon_lines = PolygonData::extractPolygonEdges(fatskel,fatmask|occluders); polygons.insert(polygons.end(), polygon_lines.begin(), polygon_lines.end()); cout << "Found " << polygon_lines.size() << " lines." << endl; } return polygons; } vector<Shape<SphereData> > MapBuilder::getCamSpheres(const Sketch<uchar> &camFrame, const set<int>& objectColors, const set<int>& ) const { vector<Shape<SphereData> > spheres; if ( objectColors.empty() ) return spheres; cout << "*** Find the spheres ***" << endl; for (set<int>::const_iterator it = objectColors.begin(); it !=objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); vector<ShapeRoot> sphere_shapes = SphereData::extractSpheres(colormask); spheres.insert(spheres.end(), spheres.begin(), spheres.end()); cout << "Found " << sphere_shapes.size() << " spheres." << endl; } return spheres; } vector<Shape<LineData> > MapBuilder::getCamWalls(const Sketch<uchar> &camFrame, unsigned int floorColor) const { if (floorColor == 0) return vector<Shape<LineData> >(); cout << "*** Find the walls ***" << endl; const int camFrame_offset = 8; NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,floorColor)); NEW_SKETCH_N(fillinmask ,bool,visops::fillin(colormask, 1, 6, 8)); //remove pixels w/ connectivity<6 (noise) NEW_SKETCH_N(fillinmask2 ,bool,visops::fillin(fillinmask, 2, 3, 8)); //remove pixels w/ connectivity<3 and fill in the others NEW_SKETCH_N(edgemask ,bool,visops::fillin(fillinmask2, 1, 5, 7)); //remove pixels w/ connectivity=8 (non-edge pixels) NEW_SKETCH_N(edgemask2 ,bool,visops::non_bounds(edgemask, camFrame_offset)); //remove pixels close to cam_bound NEW_SKETCH_N(occluders_floor, bool, camFrame != uchar(0) & camFrame != uchar(floorColor)); NEW_SKETCH_N(occ_mask ,bool,visops::fillin(occluders_floor, 1, 8, 8)); //remove pixels w/ connectivity<7 (noises) usint const clear_dist = 15; Sketch<bool> not_too_close = (visops::edist(occ_mask) >= clear_dist); edgemask2 &= not_too_close; //remove pixels around occuluders NEW_SKETCH_N(fatmask ,bool,visops::fillin(edgemask2,2,2,8)); //make the remaining pixels fat NEW_SKETCH_N(skel,bool,visops::skel(fatmask)); NEW_SKETCH_N(fatskel,bool,visops::fillin(skel,1,2,8)); vector<Shape<LineData> > wall_bounds = PolygonData::extractPolygonEdges(fatskel,fatmask|occluders_floor); // larger offset from the cam frame should be applied to these lines // since all pixels near cam frame bounds are removed before extracting these lines. for (vector<Shape<LineData> >::iterator it = wall_bounds.begin(); it != wall_bounds.end(); it++) { if (((*it)->end1Pt().coordX() < camFrame_offset*2.0 || (*it)->end1Pt().coordX() > xres - camFrame_offset*2.0 || (*it)->end1Pt().coordY() < camFrame_offset*2.0 || (*it)->end1Pt().coordY() > yres - camFrame_offset*2.0) && (*it)->end1Pt().isValid()) (*it)->end1Pt().setValid(false); if (((*it)->end2Pt().coordX() < camFrame_offset*2.0 || (*it)->end2Pt().coordX() > xres - camFrame_offset*2.0 || (*it)->end2Pt().coordY() < camFrame_offset*2.0 || (*it)->end2Pt().coordY() > yres - camFrame_offset*2.0) && (*it)->end2Pt().isValid()) (*it)->end2Pt().setValid(false); } cout << "Found " << wall_bounds.size() << " wall boundary lines" << endl; return wall_bounds; } void MapBuilder::getCamBlobs() { if ( curReq->objectColors[blobDataType].empty() ) return; cout << "*** Find the blobs ***" << endl; int minarea; BlobData::BlobOrientation_t orient; set<int>& blobColors = curReq->objectColors[blobDataType]; // unsigned int const numBlobColors = blobColors.size(); // for ( unsigned int i = 0; i<numBlobColors; i++ ) { for (set<int>::const_iterator it = blobColors.begin(); it != blobColors.end(); it++) { // minarea = ( i < (curReq->minBlobAreas).size() ) ? (curReq->minBlobAreas)[i] : 0; // minarea = ( i < (curReq->minBlobAreas).size() ) ? 1 : 0; // orient = ( i < curReq->blobOrientations.size() ) ? curReq->blobOrientations[i] : BlobData::groundplane; // vector<Shape<BlobData> > blob_shapes(VRmixin::getBlobsFromRegionGenerator(blobColors[i],minarea,orient)); minarea = (curReq->minBlobAreas.find(*it)==curReq->minBlobAreas.end()) ? 0 : curReq->minBlobAreas[*it]; orient = (curReq->blobOrientations.find(*it)==curReq->blobOrientations.end()) ? BlobData::groundplane : curReq->blobOrientations[*it]; vector<Shape<BlobData> > blob_shapes(VRmixin::getBlobsFromRegionGenerator(*it,minarea,orient)); cout << "Found " << blob_shapes.size() << " blobs." << endl; } } } // namespace

DualCoding 3.0beta

Generated Wed Oct 4 00:01:53 2006 by Doxygen 1.4.7