compareTrajectory.cc
Go to the documentation of this file.
00001 #include <Eigen/Eigen>
00002 #include <cstdio>
00003 #include <iostream>
00004 #include <vector>
00005 
00006 using namespace std;
00007 
00008 int main(int argc, char ** argv)
00009 {
00010 
00011     if(argc != 3)
00012     {
00013         cout<<"Usage:"<<argv[0]<<" pathToGTtrajectory pathToProposedTrajectory\n";
00014         return -1;
00015     }
00016 
00017     const char *fname1 = argv[1];
00018     const char *fname2 = argv[2];
00019 
00020     char foutname [500];
00021 
00022     snprintf(foutname,499,"%s_res.m",fname1);
00023 
00024     cout<<"saving to << "<<foutname<<endl;
00025 
00026     FILE *fin1 = fopen(fname1,"r");
00027     FILE *fin2 = fopen(fname2,"r");
00028     FILE *fout= fopen(foutname,"w");
00029 
00030     //per file
00031     char *line = NULL;
00032     size_t len;
00033     if(fin1 == NULL ||fin2 == NULL || fout==NULL)
00034     {
00035         cout<<"Error reading file "<<fname1<<endl;
00036         return -1;
00037     }
00038 
00039     bool first = true;
00040     double x,y,z, px,py,pz,pw, ts;
00041     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> refPose,Pose,dP;
00042     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> refPosePrev,PosePrev;
00043     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> refPoseRel,PoseRel;
00044 
00045     Eigen::Quaterniond id, errorQ;
00046     id.setIdentity();
00047 
00048     vector<double> eT, eR;
00049 
00050     while(getline(&line,&len,fin1) > 0)
00051     {
00052 
00053         int n = sscanf(line,"%lf %lf %lf %lf %lf %lf %lf %lf",
00054                        &ts,&x,&y,&z,&px,&py,&pz,&pw);
00055         if(n != 8)
00056         {
00057             cout<<"wrong format of pose at : "<<line<<endl;
00058             break;
00059         }
00060 
00061         refPose =Eigen::Translation<double,3>(x,y,z)*
00062                  Eigen::Quaternion<double>(pw,px,py,pz);
00063 
00064 
00065         if(getline(&line,&len,fin2) > 0)
00066         {
00067 
00068             int n2 = sscanf(line,"%lf %lf %lf %lf %lf %lf %lf %lf",
00069                             &ts,&x,&y,&z,&px,&py,&pz,&pw);
00070             if(n2 != 8)
00071             {
00072                 cout<<"wrong format of pose at : "<<line<<endl;
00073                 break;
00074             }
00075             Pose =Eigen::Translation<double,3>(x,y,z)*
00076                   Eigen::Quaternion<double>(pw,px,py,pz);
00077 
00078         }
00079         else
00080         {
00081             continue;
00082         }
00083 
00084         if(first)
00085         {
00086             refPosePrev = refPose;
00087             PosePrev = Pose;
00088             first = false;
00089             continue;
00090         }
00091 
00092 
00093         //find the two relative poses
00094         refPoseRel = refPosePrev.inverse()*refPose;
00095         PoseRel = PosePrev.inverse()*Pose;
00096 
00097         dP = PoseRel.inverse()*refPoseRel;
00098         errorQ = dP.rotation();
00099         double angle = acos(id.dot(errorQ))/2;
00100 
00101         eR.push_back(angle);
00102         eT.push_back(dP.translation().norm());
00103 
00104         refPosePrev = refPose;
00105         PosePrev = Pose;
00106 
00107     }
00108 
00109     fclose(fin1);
00110     fclose(fin2);
00111 
00112     fprintf(fout,"eT = [");
00113     for(int i=0; i<eT.size(); i++)
00114     {
00115         fprintf(fout,"%lf ",eT[i]);
00116     }
00117     fprintf(fout,"];\n");
00118 
00119     fprintf(fout,"eR = [");
00120     for(int i=0; i<eR.size(); i++)
00121     {
00122         fprintf(fout,"%lf ",eR[i]);
00123     }
00124     fprintf(fout,"];\n");
00125 
00126     fclose(fout);
00127 
00128 }


pointcloud_vrml
Author(s): Dev
autogenerated on Mon Jan 6 2014 11:31:55