55 #define LINESIZE 81920
61 bool TreePoseGraph2::load(
const char* filename,
bool overrideCovariances){
70 istringstream ls(buf);
74 if (tag==
"VERTEX" || tag==
"VERTEX2"){
77 ls >>
id >>
p.x() >>
p.y() >>
p.theta();
83 if (tag==
"EDGE" || tag==
"EDGE2"){
87 ls >> id1 >> id2 >>
p.x() >>
p.y() >>
p.theta();
88 if (overrideCovariances){
89 m.values[0][0]=1;
m.values[1][1]=1;
m.values[2][2]=1;
90 m.values[0][1]=0;
m.values[0][2]=0;
m.values[1][2]=0;
92 ls >>
m.values[0][0] >>
m.values[0][1] >>
m.values [1][1]
93 >>
m.values[2][2] >>
m.values[0][2] >>
m.values [1][2];
95 m.values[1][0]=
m.values[0][1];
96 m.values[2][0]=
m.values[0][2];
97 m.values[2][1]=
m.values[1][2];
98 TreePoseGraph2::Vertex*
v1=vertex(id1);
99 TreePoseGraph2::Vertex*
v2=vertex(id2);
108 bool TreePoseGraph2::loadEquivalences(
const char* filename){
117 istringstream ls(buf);
123 Edge*
e=edge(id1,id2);
127 suppressed.push_back(
e);
132 for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
134 if (
e->v1->id >
e->v2->id)
138 for (TreePoseGraph2::VertexMap::iterator it=
vertices.begin(); it!=
vertices.end(); it++){
139 Vertex*
v=it->second;
142 for (TreePoseGraph2::EdgeMap::iterator it=
edges.begin(); it!=
edges.end(); it++){
143 TreePoseGraph2::Edge *
e=it->second;
144 e->v1->edges.push_back(
e);
145 e->v2->edges.push_back(
e);
150 bool TreePoseGraph2::saveGnuplot(
const char* filename){
155 for (TreePoseGraph2::EdgeMap::const_iterator it=
edges.begin(); it!=
edges.end(); it++){
156 const TreePoseGraph2::Edge *
e=it->second;
157 const Vertex*
v1=
e->v1;
158 const Vertex*
v2=
e->v2;
160 os <<
v1->pose.x() <<
" " <<
v1->pose.y() <<
" " <<
v1->pose.theta() << endl;
161 os <<
v2->pose.x() <<
" " <<
v2->pose.y() <<
" " <<
v2->pose.theta() << endl;
168 bool TreePoseGraph2::save(
const char* filename){
173 for (TreePoseGraph2::VertexMap::const_iterator it=
vertices.begin(); it!=
vertices.end(); it++){
174 const TreePoseGraph2::Vertex*
v=it->second;
177 <<
v->pose.x() <<
" "
178 <<
v->pose.y() <<
" "
179 <<
v->pose.theta()<< endl;
181 for (TreePoseGraph2::EdgeMap::const_iterator it=
edges.begin(); it!=
edges.end(); it++){
182 const TreePoseGraph2::Edge *
e=it->second;
183 os <<
"EDGE " <<
e->v1->id <<
" " <<
e->v2->id <<
" ";
184 Pose p=
e->transformation.toPoseType();
185 os <<
p.x() <<
" " <<
p.y() <<
" " <<
p.theta() <<
" ";
186 os <<
e->informationMatrix.values[0][0] <<
" "
187 <<
e->informationMatrix.values[0][1] <<
" "
188 <<
e->informationMatrix.values[1][1] <<
" "
189 <<
e->informationMatrix.values[2][2] <<
" "
190 <<
e->informationMatrix.values[0][2] <<
" "
191 <<
e->informationMatrix.values[1][2] << endl;
202 std::cout <<
"(" <<
v->id <<
"," <<
v->level <<
")" << endl;
206 void TreePoseGraph2::printDepth( std::ostream& os ){
208 treeDepthVisit(ip,
root);
211 void TreePoseGraph2::printWidth( std::ostream& os ){
213 treeBreadthVisit(ip);
234 assert(
v->parentEdge->v1==
v->parent);
235 assert(
v->parentEdge->v2==
v);
240 void TreePoseGraph2::initializeOnTree(){
242 treeDepthVisit(pp,
root);
246 void TreePoseGraph2::printEdgesStat(std::ostream& os){
247 for (TreePoseGraph2::EdgeMap::const_iterator it=
edges.begin(); it!=
edges.end(); it++){
248 const TreePoseGraph2::Edge *
e=it->second;
249 os <<
"EDGE " <<
e->v1->id <<
" " <<
e->v2->id <<
" ";
250 Pose p=
e->transformation.toPoseType();
251 os <<
p.x() <<
" " <<
p.y() <<
" " <<
p.theta() <<
" ";
252 os <<
e->informationMatrix.values[0][0] <<
" "
253 <<
e->informationMatrix.values[0][1] <<
" "
254 <<
e->informationMatrix.values[1][1] <<
" "
255 <<
e->informationMatrix.values[2][2] <<
" "
256 <<
e->informationMatrix.values[0][2] <<
" "
257 <<
e->informationMatrix.values[1][2] << endl;
258 os <<
" top=" <<
e->top->id <<
" length=" <<
e->length << endl;
262 void TreePoseGraph2::revertEdgeInfo(Edge*
e){
265 R.values[0][0]=
e->transformation.rotationMatrix[0][0];
266 R.values[0][1]=
e->transformation.rotationMatrix[0][1];
269 R.values[1][0]=
e->transformation.rotationMatrix[1][0];
270 R.values[1][1]=
e->transformation.rotationMatrix[1][1];
287 e->transformation=it;
288 e->informationMatrix=IM;
291 void TreePoseGraph2::initializeFromParentEdge(Vertex* v){
293 v->transformation=tp;
295 v->parameters=
v->pose;
296 v->parameters.x()-=
v->parent->pose.x();
297 v->parameters.y()-=
v->parent->pose.y();
298 v->parameters.theta()-=
v->parent->pose.theta();
299 v->parameters.theta()=
atan2(
sin(
v->parameters.theta()),
cos(
v->parameters.theta()));
302 void TreePoseGraph2::collapseEdge(Edge*
e){
303 EdgeMap::iterator ie_it=
edges.find(
e);
304 if (ie_it==
edges.end())
316 for (EdgeList::iterator it=
v2->edges.begin(); it!=
v2->edges.end(); it++){
322 for (EdgeList::iterator it=
v1->edges.begin(); it!=
v1->edges.end(); it++){
337 for (EdgeList::iterator it2=
v2->edges.begin(); it2!=
v2->edges.end(); it2++){
369 e2->transformation=T1x_pred;
370 e2->informationMatrix=I1x_pred;
375 std::list<Transformation> tList;
376 std::list<InformationMatrix> iList;
377 std::list<Vertex*> vList;
380 for (EdgeList::iterator it2=
v2->edges.begin(); it2!=
v2->edges.end(); it2++){
383 if ( ((*it2)->v1!=
v1)){
385 for (EdgeList::iterator it1=
v1->edges.begin(); it1!=
v1->edges.end(); it1++){
386 if ((*it1)->v2==(*it2)->v2)
421 e1x->informationMatrix=IM;
424 tList.push_back(e2x->transformation);
425 iList.push_back(e2x->informationMatrix);
426 vList.push_back(e2x->v2);
429 removeVertex(
v2->id);
431 std::list<Transformation>::iterator
t=tList.begin();
432 std::list<InformationMatrix>::iterator
i=iList.begin();
433 std::list<Vertex*>::iterator
v=vList.begin();
434 while (
i!=iList.end()){
435 addEdge(
v1,*
v,*
t,*
i);