00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00045 #include <iostream>
00046 #include <fstream>
00047 using namespace std;
00048
00049 #include "pcl/common/file_io.h"
00050 #include "pcl/io/pcd_io.h"
00051 #include "pcl/point_types.h"
00052
00053
00054 void
00055 printUsage (const char *progName)
00056 {
00057 cout << "\n\nUsage: " << progName <<
00058 " [options] <point_cloud_file_1.pointCloud> [point_cloud_file_2.pointCloud] ... [point_cloud_file_n.pointCloud]\n\n"
00059 << "Options:\n" << "-------------------------------------------\n" <<
00060 "-f Store far ranges in an extra file\n" << "-h this help\n" << "\n\n";
00061 }
00062
00063 bool
00064 checkFileHeader (std::istream & file, const std::string headerKeyWord)
00065 {
00066 unsigned int posInFile = file.tellg ();
00067 unsigned int headerKeyWordLength = headerKeyWord.length ();
00068
00069 char *headerKeyWordFromFileC = new char[headerKeyWordLength + 1];
00070 headerKeyWordFromFileC[headerKeyWordLength] = 0;
00071 file.read (headerKeyWordFromFileC, headerKeyWordLength);
00072 string headerKeyWordFromFile = headerKeyWordFromFileC;
00073
00074 file.seekg (posInFile);
00075
00076 if (headerKeyWord == headerKeyWordFromFile)
00077 {
00078
00079 delete[]headerKeyWordFromFileC;
00080 return (true);
00081 }
00082
00083
00084 delete[]headerKeyWordFromFileC;
00085 return (false);
00086 }
00087
00088 struct GeneralPoint
00089 {
00090 float x, y, z, distance, vp_x, vp_y, vp_z;
00091 union
00092 {
00093 uint32_t color_rgba;
00094 struct
00095 {
00096 unsigned char color_r, color_g, color_b, color_a;
00097 };
00098 };
00099 };
00100
00101 void
00102 getAverageViewpoint (const std::vector < GeneralPoint > &points, float &vp_mean_x, float &vp_mean_y, float &vp_mean_z)
00103 {
00104 vp_mean_x = vp_mean_y = vp_mean_z = 0.0f;
00105 for (unsigned int i = 0; i < points.size (); ++i)
00106 {
00107 vp_mean_x += points[i].vp_x;
00108 vp_mean_y += points[i].vp_y;
00109 vp_mean_z += points[i].vp_z;
00110 }
00111 vp_mean_x /= points.size ();
00112 vp_mean_y /= points.size ();
00113 vp_mean_z /= points.size ();
00114 }
00115
00116
00117 int
00118 main (int argc, char **argv)
00119 {
00120
00121
00122
00123 bool store_far_ranges_in_extra_file = false;
00124
00125
00126 #ifndef _WIN32
00127 for (char c; (c = getopt (argc, argv, "fh")) != -1;)
00128 {
00129 switch (c)
00130 {
00131 case 'f':
00132 {
00133 store_far_ranges_in_extra_file = true;
00134 cout << "Will store far ranges in an extra file.\n";
00135 break;
00136 }
00137 case 'h':
00138 {
00139 printUsage (argv[0]);
00140 exit (0);
00141 }
00142 }
00143 }
00144 if (optind >= argc)
00145 {
00146 std::cerr << "No file name(s) given as command line argument(s).\n\n";
00147 printUsage (argv[0]);
00148 exit (1);
00149 }
00150 #endif
00151
00152
00153
00154 #ifndef _WIN32
00155 for (int argument_index = optind; argument_index < argc; ++argument_index)
00156 #else
00157 for (int argument_index = 0; argument_index < argc; ++argument_index)
00158 #endif
00159 {
00160 string fileName = argv[argument_index];
00161 cout << "\nTrying to open \"" << fileName << "\".\n";
00162 std::ifstream file (fileName.c_str ());
00163 if (!file)
00164 {
00165 std::cerr << "Was not able to open file.\n";
00166 continue;
00167 }
00168 string output_file_name = pcl::getFilenameWithoutExtension (fileName) + ".pcd",
00169 output_file_name_far_ranges = pcl::getFilenameWithoutExtension (fileName) + "_far_ranges.pcd";
00170
00171 std::vector < GeneralPoint > points, far_ranges;
00172
00173 std::string headerLine;
00174 std::string headerKeyWord = "PointCloudColoredWithSensorPosesT<float>";
00175 if (checkFileHeader (file, headerKeyWord))
00176 {
00177 std::cout << "File is of type \"" << headerKeyWord.c_str () << "\".\n";
00178 getline (file, headerLine);
00179
00180 uint32_t num_points;
00181 file.read ((char *) &num_points, sizeof (num_points));
00182 std::cout << "Point cloud has " << num_points << " points.\n";
00183
00184 for (unsigned int i = 0; i < num_points; ++i)
00185 {
00186 GeneralPoint point;
00187
00188 uint32_t vectorSize;
00189 file.read ((char *) &vectorSize, sizeof (vectorSize));
00190 if (vectorSize != 3)
00191 {
00192 ROS_ERROR ("Vector size is not 3!");
00193 continue;
00194 }
00195
00196 float freiburg_x, freiburg_y, freiburg_z;
00197
00198 file.read ((char *) &freiburg_x, sizeof (freiburg_x));
00199 file.read ((char *) &freiburg_y, sizeof (freiburg_y));
00200 file.read ((char *) &freiburg_z, sizeof (freiburg_z));
00201 point.x = -freiburg_y;
00202 point.y = -freiburg_z;
00203 point.z = freiburg_x;
00204
00205 file.read ((char *) &vectorSize, sizeof (vectorSize));
00206 if (vectorSize != 3)
00207 {
00208 ROS_ERROR ("Vector size is not 3!");
00209 continue;
00210 }
00211
00212
00213 file.read ((char *) &freiburg_x, sizeof (freiburg_x));
00214 file.read ((char *) &freiburg_y, sizeof (freiburg_y));
00215 file.read ((char *) &freiburg_z, sizeof (freiburg_z));
00216 point.vp_x = -freiburg_y;
00217 point.vp_y = -freiburg_z;
00218 point.vp_z = freiburg_x;
00219
00220 point.distance =
00221 sqrtf (powf (point.x - point.vp_x, 2) + powf (point.y - point.vp_y, 2) + powf (point.y - point.vp_y, 2));
00222
00223 bool has_color;
00224 file.read ((char *) &has_color, sizeof (has_color));
00225 point.color_r = point.color_g = point.color_b = point.color_a = 0;
00226 if (has_color)
00227 {
00228 file.read ((char *) &point.color_r, sizeof (point.color_r));
00229 file.read ((char *) &point.color_g, sizeof (point.color_g));
00230 file.read ((char *) &point.color_b, sizeof (point.color_b));
00231 point.color_a = 255;
00232 }
00233
00234 points.push_back (point);
00235 }
00236
00237 float vp_mean_x, vp_mean_y, vp_mean_z;
00238 getAverageViewpoint (points, vp_mean_x, vp_mean_y, vp_mean_z);
00239
00240 std::ofstream file_out (output_file_name.c_str ());
00241 file_out << "# .PCD v.7 - Point Cloud Data file format\n"
00242 << "FIELDS x y z distance vp_x vp_y vp_z rgba\n"
00243 << "SIZE 4 4 4 4 4 4 4 4\n"
00244 << "TYPE F F F F F F F U\n"
00245 << "COUNT 1 1 1 1 1 1 1 1\n"
00246 << "WIDTH " << points.size () << "\n"
00247 << "HEIGHT 1\n"
00248 << "VIEWPOINT " << vp_mean_x << " " << vp_mean_y << " " << vp_mean_z << " 1 0 0 0\n"
00249 << "POINTS " << points.size () << "\n" << "DATA ascii\n";
00250 for (unsigned int i = 0; i < points.size (); ++i)
00251 {
00252 const GeneralPoint & point = points[i];
00253 file_out << point.x << " " << point.y << " " << point.z << " " << point.distance << " "
00254 << point.vp_x << " " << point.vp_y << " " << point.vp_z << " " << point.color_rgba << "\n";
00255 }
00256 std::cout << "Done.\n\n";
00257 file.close ();
00258 file_out.close ();
00259 continue;
00260 }
00261
00262 headerKeyWord = "PointCloudWithSensorPosesT<float>";
00263 if (checkFileHeader (file, headerKeyWord))
00264 {
00265 std::cout << "File is of type \"" << headerKeyWord.c_str () << "\".\n";
00266 getline (file, headerLine);
00267
00268 uint32_t num_points;
00269 file.read ((char *) &num_points, sizeof (num_points));
00270 std::cout << "Point cloud has " << num_points << " normal points.\n";
00271
00272 for (unsigned int i = 0; i < num_points; ++i)
00273 {
00274 GeneralPoint point;
00275
00276 float freiburg_x, freiburg_y, freiburg_z;
00277
00278 file.read ((char *) &freiburg_x, sizeof (freiburg_x));
00279 file.read ((char *) &freiburg_y, sizeof (freiburg_y));
00280 file.read ((char *) &freiburg_z, sizeof (freiburg_z));
00281 point.x = -freiburg_y;
00282 point.y = -freiburg_z;
00283 point.z = freiburg_x;
00284
00285
00286 file.read ((char *) &freiburg_x, sizeof (freiburg_x));
00287 file.read ((char *) &freiburg_y, sizeof (freiburg_y));
00288 file.read ((char *) &freiburg_z, sizeof (freiburg_z));
00289 point.vp_x = -freiburg_y;
00290 point.vp_y = -freiburg_z;
00291 point.vp_z = freiburg_x;
00292
00293 point.distance =
00294 sqrtf (powf (point.x - point.vp_x, 2) + powf (point.y - point.vp_y, 2) + powf (point.y - point.vp_y, 2));
00295
00296 points.push_back (point);
00297 }
00298
00299 uint32_t num_far_ranges;
00300 file.read ((char *) &num_far_ranges, sizeof (num_far_ranges));
00301 std::cout << "Point cloud has " << num_far_ranges << " far range points.\n";
00302
00303 for (unsigned int i = 0; i < num_far_ranges; ++i)
00304 {
00305 GeneralPoint point;
00306
00307 float freiburg_x, freiburg_y, freiburg_z;
00308
00309 file.read ((char *) &freiburg_x, sizeof (freiburg_x));
00310 file.read ((char *) &freiburg_y, sizeof (freiburg_y));
00311 file.read ((char *) &freiburg_z, sizeof (freiburg_z));
00312 point.x = -freiburg_y;
00313 point.y = -freiburg_z;
00314 point.z = freiburg_x;
00315
00316
00317 file.read ((char *) &freiburg_x, sizeof (freiburg_x));
00318 file.read ((char *) &freiburg_y, sizeof (freiburg_y));
00319 file.read ((char *) &freiburg_z, sizeof (freiburg_z));
00320 point.vp_x = -freiburg_y;
00321 point.vp_y = -freiburg_z;
00322 point.vp_z = freiburg_x;
00323
00324 if (store_far_ranges_in_extra_file)
00325 {
00326 far_ranges.push_back (point);
00327 }
00328 else
00329 {
00330
00331 point.distance = point.x;
00332 point.x = std::numeric_limits<float>::quiet_NaN ();
00333 points.push_back (point);
00334 }
00335 }
00336 file.close ();
00337
00338 float vp_mean_x, vp_mean_y, vp_mean_z;
00339 getAverageViewpoint (points, vp_mean_x, vp_mean_y, vp_mean_z);
00340
00341 std::ofstream file_out (output_file_name.c_str ());
00342 file_out << "# .PCD v.7 - Point Cloud Data file format\n"
00343 << "FIELDS x y z distance vp_x vp_y vp_z\n"
00344 << "SIZE 4 4 4 4 4 4 4\n"
00345 << "TYPE F F F F F F F\n"
00346 << "COUNT 1 1 1 1 1 1 1\n"
00347 << "WIDTH " << points.size () << "\n"
00348 << "HEIGHT 1\n"
00349 << "VIEWPOINT " << vp_mean_x << " " << vp_mean_y << " " << vp_mean_z << " 1 0 0 0\n"
00350 << "POINTS " << points.size () << "\n" << "DATA ascii\n";
00351 for (unsigned int i = 0; i < points.size (); ++i)
00352 {
00353 const GeneralPoint & point = points[i];
00354 file_out << point.x << " " << point.y << " " << point.z << " " << point.distance << " "
00355 << point.vp_x << " " << point.vp_y << " " << point.vp_z << "\n";
00356 }
00357 file_out.close ();
00358
00359 if (!far_ranges.empty ())
00360 {
00361 file_out.open (output_file_name_far_ranges.c_str ());
00362 file_out << "# .PCD v.7 - Point Cloud Data file format\n"
00363 << "FIELDS x y z vp_x vp_y vp_z\n"
00364 << "SIZE 4 4 4 4 4 4\n"
00365 << "TYPE F F F F F F\n"
00366 << "COUNT 1 1 1 1 1 1\n"
00367 << "WIDTH " << far_ranges.size () << "\n"
00368 << "HEIGHT 1\n"
00369 << "VIEWPOINT " << vp_mean_x << " " << vp_mean_y << " " << vp_mean_z << " 1 0 0 0\n"
00370 << "POINTS " << far_ranges.size () << "\n" << "DATA ascii\n";
00371 for (unsigned int i = 0; i < far_ranges.size (); ++i)
00372 {
00373 const GeneralPoint & point = far_ranges[i];
00374 file_out << point.x << " " << point.y << " " << point.z << " " << " "
00375 << point.vp_x << " " << point.vp_y << " " << point.vp_z << "\n";
00376 }
00377 file_out.close ();
00378 }
00379
00380 std::cout << "Done.\n";
00381 continue;
00382 }
00383
00384 headerKeyWord = "PointCloudT<float>";
00385 if (checkFileHeader (file, headerKeyWord))
00386 {
00387 ROS_INFO ("File is of type \"%s\".", headerKeyWord.c_str ());
00388 ROS_ERROR ("Sorry, not implemented yet.");
00389
00390 continue;
00391 }
00392
00393 ROS_ERROR ("Could not determine file type. Doing nothing.\n");
00394
00395 continue;
00396 }
00397 }
00398
00399