00001
00002
00003
00004
00005
00006
00007
00008 #include <pcl17/console/print.h>
00009 #include <iostream>
00010 #include <boost/filesystem.hpp>
00011 #include <boost/random.hpp>
00012 #include <pcl17/point_types.h>
00013 #include <pcl17/point_cloud.h>
00014 #include <pcl17/console/parse.h>
00015 #include <pcl17/io/vtk_lib_io.h>
00016 #include <pcl17/io/pcd_io.h>
00017 #include <pcl17/common/transforms.h>
00018 #include <pcl17/visualization/pcl_visualizer.h>
00019 #include <pcl17/filters/voxel_grid.h>
00020
00021 #include <pcl17/sample_consensus/ransac.h>
00022 #include <pcl17/search/kdtree.h>
00023 #include <pcl17/surface/mls.h>
00024
00025 #include <vtkTriangle.h>
00026 #include <vtkTriangleFilter.h>
00027 #include <vtkPolyDataMapper.h>
00028
00029 void addNoise(pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud, double noise_std)
00030 {
00031
00032 boost::mt19937 rng(static_cast<unsigned int> (std::time(0)));
00033 boost::normal_distribution<float> normal_distrib(0.0f, noise_std * noise_std);
00034 boost::variate_generator<boost::mt19937&, boost::normal_distribution<float> > gaussian_rng(rng, normal_distrib);
00035
00036 for (size_t cp = 0; cp < cloud->points.size(); cp++)
00037 {
00038 cloud->points[cp].z += gaussian_rng();
00039 }
00040
00041 }
00042
00043 void moveToNewCenterAndAlign(pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud,
00044 pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud_transformed, double new_center[3],
00045 double tilt_angle)
00046 {
00047
00048 Eigen::Affine3f view_transform;
00049 view_transform.setIdentity();
00050 Eigen::Translation<float, 3> translation(new_center[1], new_center[2], -new_center[0]);
00051 Eigen::AngleAxis<float> tilt_rotation(tilt_angle * M_PI / 180 + M_PI, Eigen::Vector3f(1, 0, 0));
00052 view_transform *= tilt_rotation;
00053 view_transform *= translation;
00054
00055
00056 Eigen::Affine3f change_coords;
00057 change_coords.matrix() << 0, 0, -1, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
00058
00059 view_transform.matrix() = change_coords.matrix() * view_transform.matrix();
00060
00061 pcl17::transformPointCloud(*cloud, *cloud_transformed, view_transform);
00062
00063 cloud_transformed->sensor_origin_ = view_transform * Eigen::Vector4f(0, 0, 0, 1);
00064 cloud_transformed->sensor_orientation_ = view_transform.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
00065
00066 }
00067
00068
00069 inline double uniform_deviate(int seed)
00070 {
00071 double ran = seed * (1.0 / (RAND_MAX + 1.0));
00072 return ran;
00073 }
00074
00075 inline void randomPointTriangle(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2,
00076 float c3, Eigen::Vector4f& p)
00077 {
00078 float r1 = static_cast<float> (uniform_deviate(rand()));
00079 float r2 = static_cast<float> (uniform_deviate(rand()));
00080 float r1sqr = sqrtf(r1);
00081 float OneMinR1Sqr = (1 - r1sqr);
00082 float OneMinR2 = (1 - r2);
00083 a1 *= OneMinR1Sqr;
00084 a2 *= OneMinR1Sqr;
00085 a3 *= OneMinR1Sqr;
00086 b1 *= OneMinR2;
00087 b2 *= OneMinR2;
00088 b3 *= OneMinR2;
00089 c1 = r1sqr * (r2 * c1 + b1) + a1;
00090 c2 = r1sqr * (r2 * c2 + b2) + a2;
00091 c3 = r1sqr * (r2 * c3 + b3) + a3;
00092 p[0] = c1;
00093 p[1] = c2;
00094 p[2] = c3;
00095 p[3] = 0;
00096 }
00097
00098 inline void randPSurface(vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea,
00099 Eigen::Vector4f& p)
00100 {
00101 float r = static_cast<float> (uniform_deviate(rand()) * totalArea);
00102
00103 std::vector<double>::iterator low = std::lower_bound(cumulativeAreas->begin(), cumulativeAreas->end(), r);
00104 vtkIdType el = vtkIdType(low - cumulativeAreas->begin());
00105
00106 double A[3], B[3], C[3];
00107 vtkIdType npts = 0;
00108 vtkIdType *ptIds = NULL;
00109 polydata->GetCellPoints(el, npts, ptIds);
00110 polydata->GetPoint(ptIds[0], A);
00111 polydata->GetPoint(ptIds[1], B);
00112 polydata->GetPoint(ptIds[2], C);
00113 randomPointTriangle(float(A[0]), float(A[1]), float(A[2]), float(B[0]), float(B[1]), float(B[2]), float(C[0]),
00114 float(C[1]), float(C[2]), p);
00115 }
00116
00117 void uniform_sampling(vtkSmartPointer<vtkPolyData> polydata, size_t n_samples,
00118 pcl17::PointCloud<pcl17::PointXYZ> & cloud_out)
00119 {
00120 polydata->BuildCells();
00121 vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys();
00122
00123 std::cerr << "Number of cells " << cells->GetNumberOfCells() << std::endl;
00124
00125 double p1[3], p2[3], p3[3], totalArea = 0;
00126 std::vector<double> cumulativeAreas(cells->GetNumberOfCells(), 0);
00127 size_t i = 0;
00128 vtkIdType npts = 0, *ptIds = NULL;
00129 for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds); i++)
00130 {
00131 polydata->GetPoint(ptIds[0], p1);
00132 polydata->GetPoint(ptIds[1], p2);
00133 polydata->GetPoint(ptIds[2], p3);
00134 totalArea += vtkTriangle::TriangleArea(p1, p2, p3);
00135 cumulativeAreas[i] = totalArea;
00136 }
00137
00138 cloud_out.points.resize(n_samples);
00139 cloud_out.width = static_cast<uint32_t> (n_samples);
00140 cloud_out.height = 1;
00141
00142 for (i = 0; i < n_samples; i++)
00143 {
00144 Eigen::Vector4f p;
00145 randPSurface(polydata, &cumulativeAreas, totalArea, p);
00146 cloud_out.points[i].x = p[0];
00147 cloud_out.points[i].y = p[1];
00148 cloud_out.points[i].z = p[2];
00149 }
00150 }
00151
00152
00153 void createFullModelPointcloud(vtkSmartPointer<vtkPolyData> polydata, size_t n_samples,
00154 pcl17::PointCloud<pcl17::PointXYZ> & cloud_out)
00155 {
00156
00157 vtkSmartPointer<vtkTriangleFilter> triangleFilter = vtkSmartPointer<vtkTriangleFilter>::New();
00158 triangleFilter->SetInput(polydata);
00159 triangleFilter->Update();
00160
00161 vtkSmartPointer<vtkPolyDataMapper> triangleMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
00162 triangleMapper->SetInputConnection(triangleFilter->GetOutputPort());
00163 triangleMapper->Update();
00164 polydata = triangleMapper->GetInput();
00165 polydata->Update();
00166
00167 uniform_sampling(polydata, n_samples, cloud_out);
00168
00169 }
00170
00171 int main(int argc, char** argv)
00172 {
00173
00174 if (argc < 5)
00175 {
00176 PCL17_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
00177 PCL17_INFO (" * where options are:\n"
00178 " -height <X> : simulate scans with sensor mounted on height X\n"
00179 " -noise_std <X> : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
00180 " -distance <X> : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
00181 " -tilt <X> : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
00182 " -shift <X> : shift object from the straight line. Can be used multiple times. Default value 0.\n"
00183 " -num_views <X> : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"
00184
00185 "");
00186 return -1;
00187 }
00188
00189 std::string input_dir, output_dir;
00190 double height = 1.5;
00191 double num_views = 6;
00192 double noise_std = 0.0001;
00193 std::vector<double> distances;
00194 std::vector<double> tilt;
00195 std::vector<double> shift;
00196 int full_model_n_points = 30000;
00197
00198 pcl17::console::parse_argument(argc, argv, "-input_dir", input_dir);
00199 pcl17::console::parse_argument(argc, argv, "-output_dir", output_dir);
00200 pcl17::console::parse_argument(argc, argv, "-num_views", num_views);
00201 pcl17::console::parse_argument(argc, argv, "-height", height);
00202 pcl17::console::parse_argument(argc, argv, "-noise_std", noise_std);
00203 pcl17::console::parse_multiple_arguments(argc, argv, "-distance", distances);
00204 pcl17::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
00205 pcl17::console::parse_multiple_arguments(argc, argv, "-shift", shift);
00206
00207 PCL17_INFO("distances size: %d\n", distances.size());
00208 for (size_t i = 0; i < distances.size(); i++)
00209 {
00210 PCL17_INFO("distance: %f\n", distances[i]);
00211 }
00212
00213
00214 if (distances.size() == 0)
00215 distances.push_back(4);
00216 if (tilt.size() == 0)
00217 tilt.push_back(-15);
00218 if (shift.size() == 0)
00219 shift.push_back(0);
00220
00221
00222 boost::filesystem::path input_path(input_dir);
00223 if (!boost::filesystem::exists(input_path))
00224 {
00225 PCL17_ERROR("Input directory doesnt exists.");
00226 return -1;
00227 }
00228
00229
00230 if (!boost::filesystem::is_directory(input_path))
00231 {
00232 PCL17_ERROR("%s is not directory.", input_path.c_str());
00233 return -1;
00234 }
00235
00236
00237 boost::filesystem::path output_path(output_dir);
00238 if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
00239 {
00240 if (!boost::filesystem::create_directories(output_path))
00241 {
00242 PCL17_ERROR ("Error creating directory %s.\n", output_path.c_str ());
00243 return -1;
00244 }
00245 }
00246
00247
00248 std::vector<std::string> files_to_process;
00249 PCL17_INFO("Processing following files:\n");
00250 boost::filesystem::directory_iterator end_iter;
00251 for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
00252 {
00253 boost::filesystem::path class_dir_path(*iter);
00254 for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
00255 {
00256 boost::filesystem::path file(*iter2);
00257 if (file.extension() == ".vtk")
00258 {
00259 files_to_process.push_back(file.c_str());
00260 PCL17_INFO("\t%s\n", file.c_str());
00261 }
00262 }
00263
00264 }
00265
00266
00267 if (files_to_process.size() == 0)
00268 {
00269 PCL17_ERROR("Directory %s has no .vtk files.", input_path.c_str());
00270 return -1;
00271 }
00272
00273
00274 for (size_t i = 0; i < files_to_process.size(); i++)
00275 {
00276 vtkSmartPointer<vtkPolyData> model;
00277 vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New();
00278 vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
00279 pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud(new pcl17::PointCloud<pcl17::PointXYZ>());
00280
00281
00282 std::vector<std::string> st;
00283 boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on);
00284 std::string class_dirname = st.at(st.size() - 2);
00285 std::string dirname = st.at(st.size() - 1);
00286 dirname = dirname.substr(0, dirname.size() - 4);
00287 dirname = output_dir + class_dirname + "/" + dirname;
00288
00289
00290 boost::filesystem::path dirpath(dirname);
00291 if (!boost::filesystem::exists(dirpath))
00292 {
00293 if (!boost::filesystem::create_directories(dirpath))
00294 {
00295 PCL17_ERROR ("Error creating directory %s.\n", dirpath.c_str ());
00296 return -1;
00297 }
00298 }
00299
00300
00301 reader->SetFileName(files_to_process.at(i).c_str());
00302 reader->Update();
00303 model = reader->GetOutput();
00304 PCL17_INFO("Number of points %d\n",model->GetNumberOfPoints());
00305
00306
00307 double bounds[6];
00308 model->GetBounds(bounds);
00309 double min_z_value = bounds[4];
00310
00311 double center[3];
00312 model->GetCenter(center);
00313
00314 createFullModelPointcloud(model, full_model_n_points, *cloud);
00315 pcl17::io::savePCDFile(dirname + "/full.pcd", *cloud);
00316
00317
00318 pcl17::visualization::PCLVisualizer viz;
00319 viz.initCameraParameters();
00320 viz.updateCamera();
00321 viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1);
00322 viz.addModelFromPolyData(model, transform);
00323 viz.setRepresentationToSurfaceForAllActors();
00324
00325
00326 for (size_t shift_index = 0; shift_index < shift.size(); shift_index++)
00327 {
00328
00329
00330 for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++)
00331 {
00332
00333
00334 for (size_t distance_index = 0; distance_index < distances.size(); distance_index++)
00335 {
00336
00337
00338 double angle = 0;
00339 double angle_step = 360.0 / num_views;
00340 for (int angle_index = 0; angle_index < num_views; angle_index++)
00341 {
00342
00343
00344 transform->Identity();
00345 transform->RotateY(tilt[tilt_index]);
00346 transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value));
00347 transform->RotateZ(angle);
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375 angle += angle_step;
00376
00377
00378
00379 }
00380
00381 }
00382 }
00383 }
00384
00385 }
00386
00387 return 0;
00388 }