int main (int argc, char** argv) { if (argc != 5) { std::cout << "usage: organized2unorganized <input dir> <prefix> <first index> <last index>" << std::endl; return (-1); } std::string input_dir(argv[1]); std::string prefix(argv[2]); int first_index = atoi(argv[3]); int last_index = atoi(argv[4]); pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>); for (int index = first_index; index <= last_index; index++) { char buf[4]; sprintf(buf,"%03d", index); std::string input_pcd = input_dir + prefix + std::string (buf) + ".pcd"; std::string output_pcd = "./" + prefix + std::string(buf) + ".pcd"; if (pcl::io::loadPCDFile<pcl::PointXYZ> (input_pcd, *input) == -1) //* load the file { PCL_ERROR ("Couldn't read file %s!\n", input_pcd.c_str()); return (-1); } PCL_INFO ("Loaded %d points from %s.\n", input->points.size(), input_pcd.c_str ()); output->resize(input->size()); int cnt = 0; for (int i = 0; i < input->size(); i++) { if ((std::isnan(input->points[i].x) || std::isnan(input->points[i].y) || std::isnan(input->points[i].z))) continue; output->points[cnt] = input->points[i]; cnt++; } PCL_INFO("There are %d valid points in %s.\n", cnt, input_pcd.c_str()); output->points.erase(output->points.begin() + cnt, output->points.end()); output->width = cnt; output->height = 1; pcl::io::savePCDFileBinary (output_pcd, *output); } return (0); }
// Given a filename name, return its absolute path. If name is already // absolute, there is no more to be done. Otherwise, append name to // the path of the current input. char *make_path(char *name) { if (isabsolute(name)) { return strdup(name); } // input_dir() includes the final '/' if len is nonzero size_t len; char *current = input_dir(&len); if (len == 0) { printf("Warning - empty base path\n"); return strdup(current); } char *new = malloc(len + strlen(name) + 1); strncpy(new, current, len); strcpy(new+len, name); return new; }