pointcloud_utils.hpp
Go to the documentation of this file.
00001 namespace lslgeneric
00002 {
00003 
00004 template< typename PointT>
00005 pcl::PointCloud<PointT> readVRML(const char* fname)
00006 {
00007     pcl::PointCloud<PointT> cloud;
00008     FILE *vrml = fopen(fname,"r");
00009 
00010     cloud = lslgeneric::readVRML<PointT>(vrml);
00011 
00012     if(vrml!= NULL) fclose(vrml);
00013     return cloud;
00014 }
00015 
00016 template< typename PointT>
00017 pcl::PointCloud<PointT> readVRML(FILE* vrml)
00018 {
00019 
00020     pcl::PointCloud<PointT> cloud;
00021     char *line = NULL;
00022     size_t len;
00023     bool first=true;
00024     size_t length = 0;
00025     if(vrml == NULL)
00026     {
00027         std::cout<<"couldn't process vrml file\n";
00028         return cloud;
00029     }
00030 
00031     while(getline(&line,&len,vrml) >0 )
00032     {
00033         if(first)
00034         {
00035             //look for 'point [' token
00036             char *token = strtok(line," ");
00037             while(token!=NULL)
00038             {
00039                 if(strncmp("point",token,5)==0)
00040                 {
00041                     first=false;
00042                     break;
00043                 }
00044                 else
00045                 {
00046                     token=strtok(NULL," ");
00047                 }
00048             }
00049         }
00050         else
00051         {
00052             //read everything until ]
00053             char *token = strtok(line," ");
00054             if(strncmp("]",token,1)==0)
00055             {
00056                 first=true;
00057                 continue;
00058             }
00059             PointT pt;
00060             if(token == NULL) continue;
00061             pt.x = atof(token);
00062             token = strtok(NULL," ");
00063             if(token == NULL) continue;
00064             pt.y = atof(token);
00065             token = strtok(NULL," ");
00066             if(token == NULL) continue;
00067             pt.z = atof(token);
00068             cloud.points.push_back(pt);
00069         }
00070     }
00071     length = cloud.points.size();
00072     cloud.width = length;
00073     cloud.height = 1;
00074     return cloud;
00075 }
00076 
00078 template< typename PointT>
00079 pcl::PointCloud<PointT> readVRMLIntensity(const char* fname)
00080 {
00081     pcl::PointCloud<PointT> cloud;
00082     FILE *vrml = fopen(fname,"r");
00083 
00084     cloud = lslgeneric::readVRMLIntensity<PointT>(vrml);
00085 
00086     fclose(vrml);
00087     return cloud;
00088 }
00089 
00090 template< typename PointT>
00091 pcl::PointCloud<PointT> readVRMLIntensity(FILE* vrml)
00092 {
00093 
00094     pcl::PointCloud<PointT> cloud;
00095     char *line = NULL;
00096     size_t len;
00097     bool first=true;
00098     bool second=false;
00099     int ctr=0;
00100 
00101     size_t length = 0;
00102     if(vrml == NULL) return cloud;
00103 
00104     while(getline(&line,&len,vrml) >0 )
00105     {
00106         if(first)
00107         {
00108             //look for 'point [' token
00109             char *token = strtok(line," ");
00110             while(token!=NULL)
00111             {
00112                 if(strncmp("point",token,5)==0)
00113                 {
00114                     first=false;
00115                     second=false;
00116                     break;
00117                 }
00118                 else if(strncmp("color",token,5)==0)
00119                 {
00120                     first=false;
00121                     second=true;
00122                     break;
00123                 }
00124                 else
00125                 {
00126                     token=strtok(NULL," ");
00127                 }
00128             }
00129         }
00130         else
00131         {
00132             if(!second)
00133             {
00134                 //read everything until ]
00135                 char *token = strtok(line," ");
00136                 if(strncmp("]",token,1)==0)
00137                 {
00138                     first=true;
00139                     continue;
00140                 }
00141                 PointT pt;
00142                 if(token == NULL) continue;
00143                 pt.x = atof(token);
00144                 token = strtok(NULL," ");
00145                 if(token == NULL) continue;
00146                 pt.y = atof(token);
00147                 token = strtok(NULL," ");
00148                 if(token == NULL) continue;
00149                 pt.z = atof(token);
00150                 cloud.points.push_back(pt);
00151             }
00152             else
00153             {
00154                 //we are at second pass, reading color info
00155                 char *token = strtok(line," ");
00156                 if(strncmp("]",token,1)==0)
00157                 {
00158                     first=true;
00159                     second=false;
00160                     continue;
00161                 }
00162                 if(strncmp("color",token,5)==0)
00163                 {
00164                     continue;
00165                 }
00166                 if(ctr<cloud.points.size())
00167                 {
00168                     if(token == NULL) continue;
00169                     //red channel, skip
00170                     token = strtok(NULL," ");
00171                     if(token == NULL) continue;
00172                     //green channel = intensity
00173                     cloud.points[ctr].intensity = atof(token);
00174                     token = strtok(NULL," ");
00175                     //blue channel, skip
00176                     ctr++;
00177                 }
00178                 else
00179                 {
00180                     //error occured, we are at the wrong place
00181                     first=true;
00182                     second=false;
00183                     continue;
00184 
00185                 }
00186             }
00187         }
00188     }
00189     length = cloud.points.size();
00190     cloud.width = length;
00191     cloud.height = 1;
00192     return cloud;
00193 }
00194 
00195 template< typename PointT>
00196 void writeToVRML(const char* fname, pcl::PointCloud<PointT> &pc, Eigen::Vector3d col)
00197 {
00198     FILE *out = fopen(fname,"w");
00199     fprintf(out,"#VRML V2.0 utf8\n");
00200     writeToVRML<PointT>(out,pc,col);
00201     fclose(out);
00202 }
00203 
00204 template< typename PointT>
00205 void writeToVRML(FILE* fout, pcl::PointCloud<PointT> &pc,
00206                  Eigen::Vector3d col)
00207 {
00208     fprintf(fout,"Shape {\n geometry PointSet {\n coord Coordinate {\n point [\n");
00209     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00210     {
00211         PointT thisPoint = pc.points[pit];
00212         if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00213         fprintf(fout,"%.5lf %.5lf %.5lf\n", thisPoint.x, thisPoint.y, thisPoint.z);
00214     }
00215 
00216     fprintf(fout,"]\n}\n color Color {\n color [\n");
00217     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00218     {
00219         PointT thisPoint = pc.points[pit];
00220         if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00221         fprintf(fout,"%.2f,%.2f,%.2f\n",col(0),col(1),col(2));
00222     }
00223     fprintf(fout,"]\n }\n }\n }\n");
00224 
00225 }
00226 
00227 template< typename PointT>
00228 void writeToVRMLIntensity(const char* fname, pcl::PointCloud<PointT> &pc, Eigen::Vector3d col)
00229 {
00230     FILE *out = fopen(fname,"w");
00231     fprintf(out,"#VRML V2.0 utf8\n");
00232     writeToVRMLIntensity<PointT>(out,pc,col);
00233     fclose(out);
00234 }
00235 
00236 template< typename PointT>
00237 void writeToVRMLIntensity(FILE* fout, pcl::PointCloud<PointT> &pc,
00238                           Eigen::Vector3d col)
00239 {
00240     fprintf(fout,"Shape {\n geometry PointSet {\n coord Coordinate {\n point [\n");
00241     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00242     {
00243         PointT thisPoint = pc.points[pit];
00244         if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00245         fprintf(fout,"%.5lf %.5lf %.5lf\n", thisPoint.x, thisPoint.y, thisPoint.z);
00246     }
00247 
00248     fprintf(fout,"]\n}\n color Color {\n color [\n");
00249     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00250     {
00251         PointT thisPoint = pc.points[pit];
00252         if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00253         if(col == Eigen::Vector3d(1,1,1))
00254         {
00255             fprintf(fout,"%.5f,%.5f,%.5f\n",thisPoint.intensity, thisPoint.intensity, thisPoint.intensity);
00256         }
00257         else
00258         {
00259             fprintf(fout,"%.2f,%.2f,%.2f\n",col(0),col(1),col(2));
00260         }
00261     }
00262     fprintf(fout,"]\n }\n }\n }\n");
00263 
00264 }
00265 
00266 template< typename PointT>
00267 void writeToVRMLColor(const char* fname, pcl::PointCloud<PointT> &pc)
00268 {
00269     FILE *out = fopen(fname,"w");
00270     fprintf(out,"#VRML V2.0 utf8\n");
00271     writeToVRMLColor<PointT>(out,pc);
00272     fclose(out);
00273 }
00274 
00275 template< typename PointT>
00276 void writeToVRMLColor(FILE* fout, pcl::PointCloud<PointT> &pc)
00277 {
00278     fprintf(fout,"Shape {\n geometry PointSet {\n coord Coordinate {\n point [\n");
00279     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00280     {
00281         PointT thisPoint = pc.points[pit];
00282         if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00283         fprintf(fout,"%.5lf %.5lf %.5lf\n", thisPoint.x, thisPoint.y, thisPoint.z);
00284     }
00285 
00286     fprintf(fout,"]\n}\n color Color {\n color [\n");
00287     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00288     {
00289         PointT thisPoint = pc.points[pit];
00290         if(std::isnan(thisPoint.x) || std::isnan(thisPoint.y) || std::isnan(thisPoint.z)) continue;
00291         uint32_t rgb = *reinterpret_cast<int*>(&thisPoint.rgb);
00292         uint8_t r = (rgb >> 16) & 0x0000ff;
00293         uint8_t g = (rgb >> 8)  & 0x0000ff;
00294         uint8_t b = (rgb)       & 0x0000ff;
00295         fprintf(fout,"%.5f,%.5f,%.5f\n",(double) r/255., (double) g/255., (double) b/255.);
00296     }
00297     fprintf(fout,"]\n }\n }\n }\n");
00298 
00299 }
00300 
00301 template< typename PointT>
00302 pcl::PointCloud<PointT> transformPointCloud(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tr, const pcl::PointCloud<PointT> &pc)
00303 {
00304     Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> T = Tr.cast<float>();
00305     pcl::PointCloud<PointT> cloud;
00306     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00307     {
00308         PointT thisPoint = pc.points[pit];
00309         Eigen::Map<Eigen::Vector3f> pt((float*)&thisPoint,3);
00310         pt = T*pt;
00311         cloud.points.push_back(thisPoint);
00312     }
00313     cloud.width = pc.width;
00314     cloud.height = pc.height;
00315     return cloud;
00316 }
00317 
00318 template< typename PointT>
00319 void transformPointCloudInPlace(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tr, pcl::PointCloud<PointT> &pc)
00320 {
00321     Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> T = Tr.cast<float>();
00322     for(unsigned int pit=0; pit<pc.points.size(); ++pit)
00323     {
00324         Eigen::Map<Eigen::Vector3f> pt((float*)&pc.points[pit],3);
00325         pt = T*pt;
00326     }
00327 }
00328 
00329 template< typename PointT>
00330 double geomDist(PointT p1, PointT p2)
00331 {
00332     Eigen::Vector3d v;
00333     v << p1.x-p2.x, p1.y-p2.y, p1.z-p2.z;
00334     return v.norm();
00335 }
00336 
00337 }


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