/** * Create a new environment by copying an existing one and replacing some definitions * @param xenv existing environment we will copy * @param ldefs list of pairs (symbol id x definition) that will replace old definitions * @param visited set of visited symbols (used for recursive definition detection) * @param lenv the current environment to evaluate the definitions * @return the new environment */ Tree copyEnvReplaceDefs(Tree anEnv, Tree ldefs, Tree visited, Tree curEnv) { vector<Tree> ids, clos; Tree copyEnv; anEnv->exportProperties(ids, clos); // get the definitions of the environment copyEnv = pushNewLayer(anEnv->branch(0)); // create new environment with same stack updateClosures(clos, anEnv, copyEnv); // update the closures replacing oldEnv with newEnv for(unsigned int i = 0; i < clos.size(); i++) // transfers the updated definitions to the new environment { setProperty(copyEnv, ids[i], clos[i]); } while(!isNil(ldefs)) // replace the old definitions with the new ones { Tree def = hd(ldefs); Tree id = hd(def); Tree rhs = tl(def); Tree cl = closure(rhs, nil, visited, curEnv); stringstream s; s << boxpp(id); if(!isBoxCase(rhs)) setDefNameProperty(cl, s.str()); setProperty(copyEnv, id, cl); ldefs = tl(ldefs); } return copyEnv; }
void GraphSLAM::findConstraints(){ boost::mutex::scoped_lock lockg(graphMutex); //graph is quickly optimized first so last added edge is satisfied _graph->initializeOptimization(); _graph->optimize(1); OptimizableGraph::VertexSet vset; _vf.findVerticesScanMatching( _lastVertex, vset); checkCovariance(vset); addNeighboringVertices(vset, 8); checkHaveLaser(vset); std::set<OptimizableGraph::VertexSet> setOfVSet; _vf.findSetsOfVertices(vset, setOfVSet); OptimizableGraph::EdgeSet loopClosingEdges; for (std::set<OptimizableGraph::VertexSet>::iterator it = setOfVSet.begin(); it != setOfVSet.end(); it++) { OptimizableGraph::VertexSet myvset = *it; OptimizableGraph::Vertex* closestV = _vf.findClosestVertex(myvset, _lastVertex); if (closestV->id() == _lastVertex->id() - 1) //Already have this edge continue; SE2 transf; if (!isMyVertex(closestV) || (isMyVertex(closestV) && abs(_lastVertex->id() - closestV->id()) > 10)){ /*VertexEllipse* ellipse = findEllipseData(_lastVertex); if (ellipse){ for (OptimizableGraph::VertexSet::iterator itv = myvset.begin(); itv != myvset.end(); itv++){ VertexSE2 *vertex = (VertexSE2*) *itv; SE2 relativetransf = _lastVertex->estimate().inverse() * vertex->estimate(); ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y()); ellipse->addMatchingVertexID(vertex->id()); } }*/ std::vector<SE2> results; /*OptimizableGraph::VertexSet referenceVset; referenceVset.insert(_lastVertex); int j = 1; int comm_gap = 5; while (j <= comm_gap){ VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j)); if (vj) referenceVset.insert(vj); else break; j++; }*/ //Loop Closing Edge bool shouldIAdd = _LCMatcher.scanMatchingLC(myvset, closestV, _lastVertex, results, maxScore); //bool shouldIAdd = _mf.scanMatchingLC(myvset, closestV, referenceVset, _lastVertex, results, maxScore); if (shouldIAdd){ for (unsigned int i =0; i< results.size(); i++){ EdgeSE2 *ne = new EdgeSE2; ne->setId(++_runningEdgeId + _baseId); ne->vertices()[0] = closestV; ne->vertices()[1] = _lastVertex; ne->setMeasurement(results[i]); ne->setInformation(_SMinf); loopClosingEdges.insert(ne); _SMEdges.insert(ne); } }else { std::cout << "Rejecting LC edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl; } }else{ //Edge between close vertices bool shouldIAdd = _closeMatcher.closeScanMatching(myvset, closestV, _lastVertex, &transf, maxScore); if (shouldIAdd){ EdgeSE2 *ne = new EdgeSE2; ne->setId(++_runningEdgeId + _baseId); ne->vertices()[0] = closestV; ne->vertices()[1] = _lastVertex; ne->setMeasurement(transf); ne->setInformation(_SMinf); _graph->addEdge(ne); _SMEdges.insert(ne); }else { std::cout << "Rejecting edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl; } } } if (loopClosingEdges.size()) addClosures(loopClosingEdges); checkClosures(); updateClosures(); }