rpy2q.cc
Go to the documentation of this file.
00001 #include <Eigen/Eigen>
00002 #include <cstdio>
00003 #include <iostream>
00004 
00005 using namespace std;
00006 
00007 int main(int argc, char ** argv)
00008 {
00009 
00010     if(argc != 2)
00011     {
00012         cout<<"Usage:"<<argv[0]<<" pathToLogFile\n";
00013         return -1;
00014     }
00015 
00016     const char *fname = argv[1];
00017     char foutname [500];
00018 
00019     snprintf(foutname,499,"%s.quat",fname);
00020 
00021     cout<<"saving to << "<<foutname<<endl;
00022 
00023     FILE *fin = fopen(fname,"r");
00024     FILE *fout= fopen(foutname,"w");
00025 
00026     //per file
00027     char *line = NULL;
00028     size_t len;
00029     if(fin == NULL || fout==NULL)
00030     {
00031         cout<<"Error reading file "<<fname<<endl;
00032         return -1;
00033     }
00034 
00035     bool first = true;
00036     double xd,yd,zd, r,p,ya, x,y,z;
00037     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> currPose;
00038 
00039     while(getline(&line,&len,fin) > 0)
00040     {
00041 
00042         double ts;
00043         int n = sscanf(line,"%lf %lf %lf %lf %lf %lf %lf",
00044                        &ts,&x,&y,&z,&r,&p,&ya);
00045         if(n != 7)
00046         {
00047             cout<<"wrong format of pose at : "<<line<<endl;
00048             break;
00049         }
00050 
00051         x = x/1000;
00052         y = y/1000;
00053         z = z/1000;
00054 
00055         currPose =Eigen::Translation<double,3>(x,y,z)*
00056                   Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitX()) *
00057                   Eigen::AngleAxis<double>(p,Eigen::Vector3d::UnitY()) *
00058                   Eigen::AngleAxis<double>(ya,Eigen::Vector3d::UnitZ()) ;
00059 
00060 
00061         Eigen::Vector3d v = currPose.translation();
00062         Eigen::Quaterniond q;
00063         q = currPose.rotation();
00064 
00065         fprintf(fout,"0 %lf %lf %lf %lf %lf %lf %lf\n",
00066                 v(0),v(1),v(2),q.x(),q.y(),q.z(),q.w());
00067 
00068     }
00069 
00070     fclose(fin);
00071     fclose(fout);
00072 
00073 }


pointcloud_vrml
Author(s): Todor Stoyanov
autogenerated on Wed Oct 8 2014 23:30:29