//
// 計算 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);
}
