/**
* 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();
}
