void MapG2OReflector::copyEstimatesFromG2O(){
for (std::map<MapNode*, g2o::VertexSE3*>::iterator it=_nm2g.begin();
it!=_nm2g.end(); it++){
MapNode* n = it->first;
g2o::VertexSE3* v = it->second;
n->setTransform(v->estimate());
}
}
void GlobalMapViewer::updateCallback(const MapUpdateMsgConstPtr& msg) {
for (size_t i = 0; i<msg->updates.size(); i++){
const MapNodeUpdateMsg& update = msg->updates[i];
boss::Identifiable* o = _context->getById(update.node_id);
if (! o)
continue;
MapNode* n = dynamic_cast<MapNode*>(o);
if (!n) {
throw std::runtime_error("invalid type");
}
n->setTransform(pose2eigen(update.transform));
}
}