51 #define LINESIZE 81920 57 bool TreePoseGraph3::load(
const char* filename,
bool overrideCovariances,
bool twoDimensions){
59 ifstream is(filename);
66 istringstream ls(buf);
73 Pose p(0.,0.,0.,0.,0.,0.);
74 ls >>
id >> p.
x() >> p.
y() >> p.
yaw();
75 TreePoseGraph3::Vertex* v=addVertex(
id,p);
84 ls >>
id >> p.
x() >> p.
y() >> p.
z() >> p.
roll() >> p.
pitch() >> p.
yaw();
85 TreePoseGraph3::Vertex* v=addVertex(
id,p);
93 is.seekg(0, ios::beg);
99 istringstream ls(buf);
106 Pose p(0.,0.,0.,0.,0.,0.);
108 ls >> id1 >> id2 >> p.
x() >> p.
y() >> p.
yaw();
110 if (! overrideCovariances){
111 ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
112 m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
115 TreePoseGraph3::Vertex* v1=vertex(id1);
116 TreePoseGraph3::Vertex* v2=vertex(id2);
118 if (!addEdge(v1, v2,t ,m)){
119 cerr <<
"Fatal, attempting to insert an edge between non existing nodes, skipping";
120 cerr <<
"edge=" << id1 <<
" -> " << id2 << endl;
129 ls >> id1 >> id2 >> p.
x() >> p.
y() >> p.
z() >> p.
roll() >> p.
pitch() >> p.
yaw();
131 if (! overrideCovariances){
132 for (
int i=0; i<6; i++)
133 for (
int j=i; j<6; j++)
136 TreePoseGraph3::Vertex* v1=vertex(id1);
137 TreePoseGraph3::Vertex* v2=vertex(id2);
139 if (!addEdge(v1, v2,t ,m)){
140 cerr <<
"Fatal, attempting to insert an edge between non existing nodes, skipping";
141 cerr <<
"edge=" << id1 <<
" -> " << id2 << endl;
151 bool TreePoseGraph3::loadEquivalences(
const char* filename){
152 ifstream is(filename);
160 istringstream ls(buf);
166 Edge*
e=edge(id1,id2);
170 suppressed.push_back(e);
175 for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
177 if (e->v1->id > e->v2->id)
184 bool TreePoseGraph3::saveGnuplot(
const char* filename){
185 ofstream os(filename);
188 for (TreePoseGraph3::VertexMap::iterator it=
vertices.begin(); it!=
vertices.end(); it++){
189 TreePoseGraph3::Vertex* v=it->second;
190 v->pose=v->transformation.toPoseType();
192 for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
193 const TreePoseGraph3::Edge *
e=it->second;
194 const Vertex* v1=e->v1;
195 const Vertex* v2=e->v2;
197 os << v1->pose.x() <<
" " << v1->pose.y() <<
" " << v1->pose.z() <<
" " 198 << v1->pose.roll() <<
" " << v1->pose.pitch() <<
" " << v1->pose.yaw() <<endl;
199 os << v2->pose.x() <<
" " << v2->pose.y() <<
" " << v2->pose.z() <<
" " 200 << v2->pose.roll() <<
" " << v2->pose.pitch() <<
" " << v2->pose.yaw() <<endl;
207 bool TreePoseGraph3::save(
const char* filename){
208 ofstream os(filename);
212 for (TreePoseGraph3::VertexMap::iterator it=
vertices.begin(); it!=
vertices.end(); it++){
213 TreePoseGraph3::Vertex* v=it->second;
214 v->pose=v->transformation.toPoseType();
218 << v->pose.x() <<
" " 219 << v->pose.y() <<
" " 220 << v->pose.z() <<
" " 221 << v->pose.roll() <<
" " 222 << v->pose.pitch() <<
" " 223 << v->pose.yaw() << endl;
225 for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
226 const TreePoseGraph3::Edge *
e=it->second;
227 os <<
"EDGE3 " << e->v1->id <<
" " << e->v2->id <<
" ";
228 Pose p=e->transformation.toPoseType();
229 os << p.
x() <<
" " << p.
y() <<
" " << p.
z() <<
" " << p.
roll() <<
" " << p.
pitch() <<
" " << p.
yaw() <<
" ";
230 for (
int i=0; i<6; i++)
231 for (
int j=i; j<6; j++)
232 os << e->informationMatrix[i][j] <<
" ";
244 std::cout <<
"(" << v->id <<
"," << v->level <<
")" << endl;
248 void TreePoseGraph3::printDepth( std::ostream& os ){
250 treeDepthVisit(ip,
root);
253 void TreePoseGraph3::printWidth( std::ostream& os ){
255 treeBreadthVisit(ip);
269 assert(v->parentEdge->v1==v->parent);
270 assert(v->parentEdge->v2==v);
271 v->transformation=tNode;
275 void TreePoseGraph3::initializeOnTree(){
277 treeDepthVisit(pp,
root);
281 void TreePoseGraph3::printEdgesStat(std::ostream& os){
282 for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
283 const TreePoseGraph3::Edge *
e=it->second;
284 os <<
"EDGE " << e->v1->id <<
" " << e->v2->id <<
" ";
285 Pose p=e->transformation.toPoseType();
286 os << p.
x() <<
" " << p.
y() <<
" " << p.
z() <<
" " << p.
roll() <<
" " << p.
pitch() <<
" " << p.
yaw() << endl;
287 os <<
" top=" << e->top->id <<
" length=" << e->length << endl;
291 void TreePoseGraph3::revertEdgeInfo(Edge*
e){
294 e->transformation=e->transformation.inv();
297 void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
299 v->transformation=tp;
301 v->parameters=v->parentEdge->transformation;
305 void TreePoseGraph3::collapseEdge(Edge*
e){
310 for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
316 for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
331 for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
350 e2->transformation=T1x_pred;
351 e2->informationMatrix=I1x_pred;
356 std::list<Transformation> tList;
357 std::list<InformationMatrix> iList;
358 std::list<Vertex*> vList;
361 for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
364 if ( ((*it2)->v1!=v1)){
366 for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
367 if ((*it1)->v2==(*it2)->v2)
381 tList.push_back(e2x->transformation);
382 iList.push_back(e2x->informationMatrix);
383 vList.push_back(e2x->v2);
386 removeVertex(v2->id);
388 std::list<Transformation>::iterator t=tList.begin();
389 std::list<InformationMatrix>::iterator i=iList.begin();
390 std::list<Vertex*>::iterator v=vList.begin();
391 while (i!=iList.end()){
392 addEdge(v1,*v,*t,*i);
399 void TreePoseGraph3::recomputeAllTransformations(){
401 treeDepthVisit(tp,
root);
Defines the graph of 3D poses, with specific functionalities such as loading, saving, merging constraints, and etc.
void perform(TreePoseGraph3::Vertex *v)
GLM_FUNC_DECL genType e()
static const float vertices[]
void perform(TreePoseGraph3::Vertex *v)
std::list< Edge * > EdgeList
IdPrinter(std::ostream &_os)
A class (struct) for realizing the pose update of the individual nodes. Assumes the correct order of ...
A class (struct) used to print vertex information to a stream. Needed for debugging.