i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed some unused variable warnings in toro3d files

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-12 17:17:20 -04:00
parent aa8fe2e55c
commit b9628b0447
2 changed files with 28 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/toro3d/posegraph2.cpp
+24 -24
View File
@@ -276,12 +276,12 @@ void TreePoseGraph2::revertEdgeInfo(Edge* e){
InformationMatrix IM=R.transpose()*e->informationMatrix*R; InformationMatrix IM=R.transpose()*e->informationMatrix*R;
Pose np=e->transformation.toPoseType(); //Pose np=e->transformation.toPoseType();
Pose ip=it.toPoseType(); //Pose ip=it.toPoseType();
Transformation tc=it*e->transformation; //Transformation tc=it*e->transformation;
Pose pc=tc.toPoseType(); //Pose pc=tc.toPoseType();
e->transformation=it; e->transformation=it;
e->informationMatrix=IM; e->informationMatrix=IM;
@@ -302,10 +302,10 @@ void TreePoseGraph2::collapseEdge(Edge* e){
EdgeMap::iterator ie_it=edges.find(e); EdgeMap::iterator ie_it=edges.find(e);
if (ie_it==edges.end()) if (ie_it==edges.end())
return; return;
VertexMap::iterator it1=vertices.find(e->v1->id); //VertexMap::iterator it1=vertices.find(e->v1->id);
VertexMap::iterator it2=vertices.find(e->v2->id); //VertexMap::iterator it2=vertices.find(e->v2->id);
assert(it1!=vertices.end()); assert(vertices.find(e->v1->id)!=vertices.end());
assert(it2!=vertices.end()); assert(vertices.find(e->v2->id)!=vertices.end());
Vertex* v1=e->v1; Vertex* v1=e->v1;
Vertex* v2=e->v2; Vertex* v2=e->v2;
@@ -328,37 +328,37 @@ void TreePoseGraph2::collapseEdge(Edge* e){
InformationMatrix I12=e->informationMatrix; InformationMatrix I12=e->informationMatrix;
CovarianceMatrix C12=I12.inv(); CovarianceMatrix C12=I12.inv();
Transformation T12=e->transformation; Transformation T12=e->transformation;
Pose p12=T12.toPoseType(); //Pose p12=T12.toPoseType();
Transformation iT12=T12.inv(); //Transformation iT12=T12.inv();
//compute the marginal information of the nodes in the path v1-v2-v* //compute the marginal information of the nodes in the path v1-v2-v*
for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){ for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
Edge* e2=*it2; Edge* e2=*it2;
if (e2->v1==v2){ //edge leaving v2 if (e2->v1==v2){ //edge leaving v2
Transformation T2x=e2->transformation; //Transformation T2x=e2->transformation;
Pose p2x=T2x.toPoseType(); //Pose p2x=T2x.toPoseType();
InformationMatrix I2x=e2->informationMatrix; InformationMatrix I2x=e2->informationMatrix;
CovarianceMatrix C2x=I2x.inv(); CovarianceMatrix C2x=I2x.inv();
//compute the estimate of the vertex based on the path v1-v2-vx //compute the estimate of the vertex based on the path v1-v2-vx
Transformation tr=iT12*T2x; //Transformation tr=iT12*T2x;
InformationMatrix R; //InformationMatrix R;
R.values[0][0]=tr.rotationMatrix[0][0]; //R.values[0][0]=tr.rotationMatrix[0][0];
R.values[0][1]=tr.rotationMatrix[0][1]; //R.values[0][1]=tr.rotationMatrix[0][1];
R.values[0][2]=0; //R.values[0][2]=0;
R.values[1][0]=tr.rotationMatrix[1][0]; //R.values[1][0]=tr.rotationMatrix[1][0];
R.values[1][1]=tr.rotationMatrix[1][1]; //R.values[1][1]=tr.rotationMatrix[1][1];
R.values[1][2]=0; //R.values[1][2]=0;
R.values[2][0]=0; //R.values[2][0]=0;
R.values[2][1]=0; //R.values[2][1]=0;
R.values[2][2]=1; //R.values[2][2]=1;
CovarianceMatrix CM=R.transpose()*C2x*R; //CovarianceMatrix CM=R.transpose()*C2x*R;
Transformation T1x_pred=T12*e2->transformation; Transformation T1x_pred=T12*e2->transformation;
corelib/src/toro3d/posegraph3.cpp
+3 -3
View File
@@ -93,7 +93,7 @@ bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool t
is.clear(); /* clears the end-of-file and error flags */ is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg); is.seekg(0, ios::beg);
bool edgesOk=true; //bool edgesOk=true;
while(is){ while(is){
char buf[LINESIZE]; char buf[LINESIZE];
is.getline(buf,LINESIZE); is.getline(buf,LINESIZE);
@@ -119,7 +119,7 @@ bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool t
if (!addEdge(v1, v2,t ,m)){ if (!addEdge(v1, v2,t ,m)){
cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping"; cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
cerr << "edge=" << id1 <<" -> " << id2 << endl; cerr << "edge=" << id1 <<" -> " << id2 << endl;
edgesOk=false; //edgesOk=false;
} }
} }
} else { } else {
@@ -140,7 +140,7 @@ bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool t
if (!addEdge(v1, v2,t ,m)){ if (!addEdge(v1, v2,t ,m)){
cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping"; cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
cerr << "edge=" << id1 <<" -> " << id2 << endl; cerr << "edge=" << id1 <<" -> " << id2 << endl;
edgesOk=false; //edgesOk=false;
} }
} }
} }