bool CVirtualWorld::LoadVirtualWorld(const std::string file)
{
CSimpleDatabaseTablePtr table;
vw.loadFromXML(file);
table=vw.getTable("Objects");
size_t numobj=table->getRecordCount();
for (size_t j=0;j<numobj;j++)
{
Object o;
o.id=table->get(j,"id");
// o.type=table->get(j,"type");
o.texture=table->get(j,"texture");
o.size=table->get(j,"size");
CPose3DPtr pose3D=CPose3D::Create();
printf("%s\n",table->get(j,"pose").c_str());
pose3D->fromString(table->get(j,"pose"));
o.pose=pose3D;
list_objects.push_back(o);
}
return true;
}
void CHierarchicalMHMap::loadFromXMLfile(std::string fileName)
{
CSimpleDatabase db;
CSimpleDatabaseTablePtr table;
size_t j,numnodes,numarcs;
std::map<size_t,CHMHMapNodePtr> nodemap;
std::map<size_t,CHMHMapNodePtr>::iterator nodemapit;
typedef std::pair<size_t,CHMHMapNodePtr> IDPair;
std::map<size_t,CHMHMapNode::TNodeID> nodeanotmap;
std::map<size_t,CHMHMapNode::TNodeID>::iterator nodeanotmapit;
typedef std::pair<size_t,CHMHMapNode::TNodeID> IDnodeanotPair;
db.loadFromXML(fileName);
table=db.getTable("nodes");
numnodes=table->getRecordCount();
//printf("Loading nodes\n");
std::vector<std::string> node_anots;
for (j=0;j<numnodes;j++)
{
CHMHMapNodePtr node;
node=CHMHMapNode::Create(this);
node->m_label=table->get(j,"nodename");
nodemap.insert(IDPair( atoi(table->get(j,"id").c_str()),node) );
node->m_nodeType.setType(table->get(j,"nodetype"));
node->m_hypotheses.insert( COMMON_TOPOLOG_HYP);
printf("Loaded node %s\n",node->m_label.c_str());
std::deque<std::string> lista;
mrpt::utils::tokenize(table->get(j,"annotation-list")," ",lista);
for (size_t r=0;r<lista.size();r++)
nodeanotmap.insert(IDnodeanotPair((size_t)atoi(lista[r].c_str()),node->getID()));
//A map with key the id of annotations and value the id of nodes;
}
table=db.getTable("arcs");
numarcs=table->getRecordCount();
printf("Loading arcs\n");
for (j=0;j<numarcs;j++)
{
CHMHMapArcPtr arc,arcrev;
size_t from,to;
from=atoi(table->get(j,"from").c_str());
to=atoi(table->get(j,"to").c_str());
CHMHMapNodePtr nodefrom,nodeto;
nodemapit=nodemap.find(from);
nodefrom=nodemapit->second;
std::cout<<"finding nodes"<<std::endl;
nodemapit=nodemap.find(to);
nodeto=nodemapit->second;
std::cout<<"added arc from "<< nodefrom->m_label << " to " <<nodeto->m_label<<std::endl;
arc=CHMHMapArc::Create(nodefrom,nodeto,0,this);
arc->m_arcType.setType(table->get(j,"arctype"));
arc->m_hypotheses.insert( COMMON_TOPOLOG_HYP);
if (atoi(table->get(j,"bidirectional").c_str())==1)
{
printf("Creating bidirectional arc\n");
arcrev=CHMHMapArc::Create(nodeto,nodefrom,0,this);
arcrev->m_arcType.setType(table->get(j,"arctype"));
arcrev->m_hypotheses.insert( COMMON_TOPOLOG_HYP);
}
}
std::cout<<"Graph with ["<<numnodes<<"] nodes and ["<<numarcs<<"] arcs loaded succesfully."<<std::endl;
table=db.getTable("annotations");
size_t numannot=table->getRecordCount();
printf("Loading annotations\n");
for (size_t j=0;j<numannot;j++)
{
string type=table->get(j,"annotation-type");
string value=table->get(j,"annotation-value");
nodeanotmapit =nodeanotmap.find(atoi(table->get(j,"id").c_str()));
if (nodeanotmapit!=nodeanotmap.end())
{
if (type=="placePose")
{
CPoint2DPtr o=CPoint2D::Create();
o->fromString(value);
CHMHMapNodePtr node=getNodeByID(nodeanotmapit->second);
node->m_annotations.set(NODE_ANNOTATION_PLACE_POSE,o,COMMON_TOPOLOG_HYP);
}
}
}
}
