processLogDir.cc
Go to the documentation of this file.
00001 #include <pointcloud_utils.h>
00002 #include <cstdio>
00003 #include <iostream>
00004 
00005 using namespace std;
00006 
00007 int main(int argc, char ** argv)
00008 {
00009 
00010     if(argc != 5)
00011     {
00012         cout<<"Usage: ./logProcessor [b|h|m] numberOfFiles logPrefix outputPrefix\n\t b -- bremen log format \n\t h -- hanover log format\n";
00013         return -1;
00014     }
00015 
00016     bool isBremen = (argv[1][0] == 'b');
00017     bool isHanover = (argv[1][0] == 'h');
00018     bool isMartin = (argv[1][0] == 'm');
00019 
00020     if(!(isBremen || isHanover || isMartin))
00021     {
00022         cout<<"error, must be bremen or hanover format! \n";
00023         cout<<"Usage: ./logProcessor [b|h] numberOfFiles logPrefix outputPrefix\n\t b -- bremen log format \n\t h -- hanover log format\n";
00024         return -1;
00025     }
00026 
00027     const char *inDirName = argv[3];
00028     const char *outName = argv[4];
00029     //assume 3 digit format...
00030 
00031     int nFiles = atoi(argv[2]);
00032 
00033     int off = 0;
00034     for(int fileno=0; fileno<nFiles; fileno++)
00035     {
00036 
00037         if(fileno == 600)
00038         {
00039             fileno++;
00040             off=1;
00041         }
00042 
00043         char fname[300];
00044         pcl::PointCloud<pcl::PointXYZ> cloud;
00045         if(isBremen)
00046         {
00047             snprintf(fname,300,"%s%03d.txt",inDirName,fileno);
00048         }
00049         if(isHanover)
00050         {
00051             snprintf(fname,300,"%s%d.3d",inDirName,fileno+1);
00052         }
00053         if(isMartin)
00054         {
00055             snprintf(fname,300,"%s%03d.wrl",inDirName,fileno);
00056             pcl::PointCloud<pcl::PointXYZ> cl = lslgeneric::readVRML<pcl::PointXYZ>(fname);
00057             pcl::PointXYZ pt;
00058             for(int i=0; i<cl.points.size(); i++)
00059             {
00060                 pt = cl.points[i];
00061                 double d = sqrt(pt.x*pt.x+pt.y*pt.y+pt.z*pt.z);
00062                 if(d>1)
00063                 {
00064                     cloud.points.push_back(pt);
00065                 }
00066             }
00067         }
00068         else
00069         {
00070 
00071             FILE *vrml = fopen(fname,"r");
00072             //per file
00073             char *line = NULL;
00074             size_t len;
00075             bool first=true;
00076             size_t length = 0;
00077             if(vrml == NULL)
00078             {
00079                 cout<<"Error reading file "<<fname<<endl;
00080                 continue;
00081             }
00082 
00083             double MAX_RANGE = 25.0;
00084             while(getline(&line,&len,vrml) >0 )
00085             {
00086                 if(first && isBremen)
00087                 {
00088                     first=false;
00089                     continue;
00090                 }
00091                 else
00092                 {
00093                     //read everything until ]
00094                     char *token = strtok(line," ");
00095                     pcl::PointXYZ pt;
00096                     if(token == NULL) continue;
00097                     pt.x = atof(token)/1000.;
00098                     token = strtok(NULL," ");
00099                     if(token == NULL) continue;
00100                     pt.y = atof(token)/1000.;
00101                     token = strtok(NULL," ");
00102                     if(token == NULL) continue;
00103                     pt.z = 1 - atof(token)/1000.;
00104                     double len = sqrt(pt.x*pt.x+pt.y*pt.y+pt.z*pt.z);
00105                     if(len < MAX_RANGE)
00106                     {
00107                         cloud.points.push_back(pt);
00108                     }
00109                 }
00110             }
00111             length = cloud.points.size();
00112             cloud.width = length;
00113             cloud.height = 1;
00114         }
00115 
00116         snprintf(fname,300,"%s%03d.wrl",outName,fileno-off);
00117         FILE *fout = fopen(fname,"w");
00118 
00119         fprintf(fout,"#VRML V2.0 utf8\n");
00120         fprintf(fout,"Shape {\n geometry PointSet {\n coord Coordinate {\n point [\n");
00121         for(unsigned int pit=0; pit<cloud.points.size(); ++pit)
00122         {
00123             pcl::PointXYZ thisPoint = cloud.points[pit];
00124             if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00125             fprintf(fout,"%.5lf %.5lf %.5lf\n", thisPoint.x, thisPoint.y, thisPoint.z);
00126         }
00127         fprintf(fout,"]\n }\n }\n }\n");
00128         fclose(fout);
00129     }
00130 }


pointcloud_vrml
Author(s): Todor Stoyanov
autogenerated on Mon Oct 6 2014 03:18:38