// // 計算 pointcloud 的 VFH, 存成 vfh.pcd // void vfh_demo() { string filename; cout << "Input filename: "; cin >> filename; // Create new point clouds to hold data pcl::PointCloud<PointType>::Ptr points (new pcl::PointCloud<PointType>); pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs(new pcl::PointCloud<pcl::VFHSignature308>); // Load point data pcl::io::loadPCDFile(filename, *points); // Compute normals compute_surface_normals(points, normals); // Create VFHEstimation pcl::VFHEstimation<PointType, pcl::Normal, pcl::VFHSignature308> vfh; vfh.setInputCloud(points); vfh.setInputNormals(normals); pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>()); vfh.setSearchMethod(tree); vfh.compute(*vfhs); // Save VFH pcl::io::savePCDFileASCII("vfh.pcd", *vfhs); }
// // normals 的 demo 程式 // void normals_demo() { string filename; cout << "Input filename: "; cin >> filename; // Create new point clouds to hold data pcl::PointCloud<PointType>::Ptr points (new pcl::PointCloud<PointType>); pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); // Load point cloud pcl::io::loadPCDFile(filename, *points); // Compute surface normals compute_surface_normals(points, normals); // Visualize the points and normals visualize_normals(points, normals); }
static void read_curves_and_surfaces(int argc, char *argv[]) { xscale=yscale=zscale=1.0; int ref=DEFAULT_REF; // Number of new knots between old ones. int maxref=DEFAULT_MAX_REF; // Maximal number of coeffs in any given direction. // (n, n) makes sure new knots are inserted close to max limit = n... int i; // // This must be reset every time we change control vertices, since // the discretization is done in the drawing routine. // for (i=0; i<curves; i++) { if (discr_curve[i]!=NULL) { delete discr_curve[i]; discr_curve[i]=NULL; } } // @HH@ // @HH@ Use the following optional command line options: // @HH@ surfaces=0; curves=0; for (i=1; i<argc; i++) { switch (argv[i][0]) { case 's': { // Next string is filename for surface. (One surface.) puts("Reading surfaces"); // surface[surfaces-1]=read_nurbs_sf(argv[i+1]); // old format std::vector<SISLSurf*> tmp; std::ifstream is(argv[i+1]); if (!is) { CRIT_ERR(printf("Could not open file: '%s'.\n", argv[i+1])); } try { eatwhite(is); while (!is.eof()) { //surface[surfaces-1] = readGoSurface(is); tmp.push_back(readGoSurface(is)); eatwhite(is); } } catch (std::exception& e) { CRIT_ERR(printf("Error occured while reading surface: %s\n", e.what())); } is.close(); int num_surfaces = tmp.size(); if (surfaces + num_surfaces > MAX_SURFACES) { CRIT_ERR(puts("Increase MAX_SURFACES.")); } for (int k = 0; k < num_surfaces; ++k) { // // 010116: This should be a quick fix for periodic // surfaces... // if (tmp[k] == NULL) { CRIT_ERR(printf("Couldn't read SISLSurf '%s'.\n", argv[i+1])); } if ((tmp[k]->cuopen_1 == SISL_CRV_PERIODIC || tmp[k]->cuopen_2 == SISL_CRV_PERIODIC)) { int kstat; SISLSurf *tmp_surf; make_sf_kreg(tmp[k], &tmp_surf, &kstat); if (kstat < 0) { CRIT_ERR(printf("make_sf_kreg failed!\n")); } freeSurf(tmp[k]); tmp[k] = tmp_surf; } if (tmp[k]->idim != 3) { CRIT_ERR(printf("Dimension of surface is %d and not 3!\n", tmp[k]->idim)); } if (surface[surfaces]) { // deleting old surface freeSurf(surface[surfaces]); } surface[surfaces] = tmp[k]; surface_name[surfaces] = argv[i+1]; surf_enabled[surfaces] = 1; // Generating an approximating polygon. lower_degree_and_subdivide(surface + surfaces, ref, maxref); // evaluating normals (normalized) delete normal[surfaces]; compute_surface_normals(surface[surfaces], normal + surfaces); ++surfaces; } } i++; break; case 'c': { // Next string is filename for file containing 1 curve. printf("Reading a single curve into slot %d.\n", curves); std::vector<SISLCurve*> tmp; //n=get_curve_set(argv[i+1], &tmp, &stat); //get_single_curve(argv[i+1], &tmp, &stat); // old format std::ifstream is(argv[i+1]); if (!is) { CRIT_ERR(printf("Could not open file: '%s'.\n", argv[i+1])); } try { eatwhite(is); while (!is.eof()) { tmp.push_back(readGoCurve(is)); eatwhite(is); } } catch (std::exception& e) { CRIT_ERR(printf("Error occured while reading curve: %s\n", e.what())); } is.close(); int num_curves = tmp.size(); if (curves + num_curves > MAX_CURVES) { CRIT_ERR(puts("Increase MAX_CURVES.")); } for(int k = 0; k < num_curves; ++k) { if (curve[curves + k] != NULL) { freeCurve(curve[curves + k]); } curve[curves + k] = tmp[k]; // // 001206: If the dimension is 2, we set up a // new curve, filling in zeros. // if (curve[curves + k]->idim==2) { double *new_coeffs= new double[3*curve[curves + k]->in]; if (new_coeffs==NULL) CRIT_ERR(puts("Couldn't allocate memory.")); int j; for (j=0; j<curve[curves + k]->in; j++) { new_coeffs[3*j+0]= curve[curves + k]->ecoef[2*j+0]; new_coeffs[3*j+1]= curve[curves + k]->ecoef[2*j+1]; new_coeffs[3*j+2]=0.0; } SISLCurve *tmp2=curve[curves + k]; curve[curves + k]= newCurve(tmp2->in, tmp2->ik, tmp2->et, new_coeffs, tmp2->ikind, 3, 1); freeCurve(tmp2); } if (curve[curves + k]->idim!=3) { CRIT_ERR(printf("Dimension of curve is %d and not 3?!\n", curve[curves + k]->idim)); } curve_name[curves + k]=argv[i+1]; curve_enabled[curves + k]=1; } curves+=num_curves; } i++; break; case 'p': { // Next string is filename for file containing a point cloud. printf("Reading a point cloud.\n"); std::ifstream is(argv[i+1]); if (!is) { CRIT_ERR(printf("Could not open file: '%s'.\n", argv[i+1])); } try { vector<double> coords; readGoPoints(coords, is); int num_points = int(coords.size()) / 3; printf("Number of vertices: %d\n", num_points); for (int i = 0; i < num_points; ++i) { pcloud.push_back(vector3t<float>(coords[3 * i], coords[3 * i + 1], coords[3 * i + 2])); } // eatwhite(is); // int tmp; // is >> tmp; // is >> tmp; // is >> tmp; // is >> tmp; // is >> tmp; // is >> tmp; // is >> tmp; // is >> tmp; // eatwhite(is); // is >> tmp; // printf("Number of vertices: %d\n", tmp); // while (!is.eof()) { // double x, y, z; // is >> x; // is >> y; // is >> z; // //printf("point: %f %f %f\n", x, y, z); // pcloud.push_back(vector3t<float>(x, y, z)); // eatwhite(is); // } } catch (std::exception& e) { CRIT_ERR(printf("Error occured while reading curve: %s\n", e.what())); } is.close(); printf("pcloud size is now %d\n", pcloud.size()); } i++; break; // case 'r': // // Set refinement factor. Default value is ???. // puts("Reading surface refinement factor"); // ref=atoi(argv[i+1]); // i++; // break; // case 'R': case 'r': // Set max refinement factor. Default value is ???. puts("Reading upper bound for surface refinement."); maxref=atoi(argv[i+1]); i++; break; case 'e': // String with keypresses to execute follows. Not // everything will work! printf("Executing '%s'.\n", argv[i+1]); strncpy(init_key_string, argv[i+1], 1000); i++; break; default: puts("Huh?! Unknown option."); exit(0); } } }
// // 對 pointcloude 做特定處理, 測試用 // void pointcloud_process() { string filename, savename; cout << "Input process filename: "; cin >> filename; //filename = "data_smooth.pcd"; ///* pcl::PointCloud<pcl::PointXYZ>::Ptr points (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr points_out (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_rgb (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::FPFHSignature33>::Ptr features (new pcl::PointCloud<pcl::FPFHSignature33>); pcl::io::loadPCDFile(filename, *points); compute_surface_normals(points, normals); pcl::io::savePCDFileASCII(filename + "_normals.pcd", *normals); // test /* pcl::copyPointCloud(*points, *points_rgb); for (int i = 0; i < points->size(); ++i) { points_rgb->points[i].r = 255; points_rgb->points[i].g = 255; points_rgb->points[i].b = 255; } vector<int> pointIdx; vector<float> pointSquDisIdx; pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree; kdtree.setInputCloud(points_rgb); if (kdtree.radiusSearch(12656, 0.005, pointIdx, pointSquDisIdx) <= 0) { exit(-1); } cout << pointIdx.size() << endl; for (int i = 0; i < pointIdx.size(); ++i) { points_rgb->points[pointIdx[i]].g = 0; points_rgb->points[pointIdx[i]].b = 0; } pcl::io::savePCDFileASCII("output.pcd", *points_rgb); */ // end test ///* cout << "before filter: " << points->size() << endl; //filter(points); //cout << "after filter: " << points->size() << endl; pcl::copyPointCloud(*points, *points_rgb); compute_surface_normals(points, normals); vector<int> pointIdx; vector<float> pointSquDisIdx; pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; kdtree.setInputCloud(points); FILE *pFile; pFile = fopen("RGB.txt", "r"); int r, g, b; vector<int> R; vector<int> G; vector<int> B; while (fscanf(pFile, "%d %d %d", &r, &g, &b) != EOF) { R.push_back(r); G.push_back(g); B.push_back(b); } fclose(pFile); //pcl::io::savePCDFileASCII("features.pcd", *features); Eigen::Vector4f pi, pj, ps, pt; Eigen::Vector4f ni, nj, ns, nt; pi[3] = pj[3] = ni[3] = nj[3] = ps[3] = pt[3] = ns[3] = nt[3] = 0.0f; vector<double> stDev; double stdev_max = 0.0f; for (size_t i = 0; i < points->size(); ++i) { double sum = 0.0f, mid_square_sum = 0.0f; float freq = 0.0f; float f11, f22, f33, f44; double f3_sum = 0.0f; double var, stdev, mean, mid_square, f3_mean; int cnt = 0; if (kdtree.radiusSearch(i, 0.005, pointIdx, pointSquDisIdx) <= 0) { //if (kdtree.nearestKSearch(i, 50, pointIdx, pointSquDisIdx) <= 0) { exit(-1); } for (size_t j = 1; j < pointIdx.size(); ++j) { pi[0] = points->points[pointIdx[j]].x; pi[1] = points->points[pointIdx[j]].y; pi[2] = points->points[pointIdx[j]].z; ni[0] = normals->points[pointIdx[j]].normal_x; ni[1] = normals->points[pointIdx[j]].normal_y; ni[2] = normals->points[pointIdx[j]].normal_z; for (size_t k = j + 1; k < pointIdx.size(); ++k) { pj[0] = points->points[pointIdx[k]].x; pj[1] = points->points[pointIdx[k]].y; pj[2] = points->points[pointIdx[k]].z; nj[0] = normals->points[pointIdx[k]].normal_x; nj[1] = normals->points[pointIdx[k]].normal_y; nj[2] = normals->points[pointIdx[k]].normal_z; pcl::computePairFeatures(pi, ni, pj, nj, f11, f22, f33, f44); f3_sum += f33; ++cnt; } //freq = freq + features->points[i].histogram[j]; //sum = sum + (features->points[i].histogram[j] * (j + 1)); //mid_square_sum = mid_square_sum + (features->points[i].histogram[j] * (j + 1) * (j + 1)); } f3_mean = (f3_sum + cnt ) / cnt / 2; points_rgb->points[i].r = R[f3_mean * 63]; points_rgb->points[i].g = G[f3_mean * 63]; points_rgb->points[i].b = B[f3_mean * 63]; } pcl::io::savePCDFileASCII("output.pcd", *points_rgb); //*/ /* shift point cloud */ /* pcl::PointCloud<pcl::PointXYZ>::Ptr points1 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr points2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile("foreground_10_transformed.pcd", *points1); pcl::io::loadPCDFile("model.pcd", *points2); double x = points2->points[144385].x - points1->points[9101].x; double y = points2->points[144385].y - points1->points[9101].y; double z = points2->points[144385].z - points1->points[9101].z; for (int i = 0; i < points1->size(); ++i) { points1->points[i].x += x; points1->points[i].y += y; points1->points[i].z += z; } pcl::io::savePCDFileASCII("shift.pcd", *points1); */ /* //pcl::visualization::PCLVisualizer viewer; //viewer.setBackgroundColor(0.2, 0.3, 0.4); //viewer.addPolygonMesh(*model_mesh, "polygon"); //pcl::PointCloud<pcl::PointXYZ>::Ptr model (new pcl::PointCloud<pcl::PointXYZ>); //viewer.addPointCloud(model, "cloud"); //viewer.spin(); //*/ //pcl::visualization::PCLVisualizer viewer; //viewer.addPointCloud(points_out, "points"); //viewer.spin(); //cout << "Input save filename: "; //cin >> savename; //pcl::io::savePCDFile(savename, *points_out); }