#include <stdio.h>
#include <boost/thread/thread.hpp>
// pcl
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
// PCLVisualizer のテスト
void TestCloudViewer003(const char* pFilePath)
{
// テキストファイルから頂点を取得
FILE* fp = fopen(pFilePath, "rt");
int n;
char buf[256];
fscanf(fp, "%s %d", buf, &n);
int num = n;
// 点群の作成
pcl::PointCloud::Ptr cloud( new pcl::PointCloud );
cloud->width = num;
cloud->height = 1;
cloud->points.resize( cloud->width * cloud->height );
int txtFmt = 1;
float x, y, z;
for(int i = 0; i < num; i++)
{
switch(txtFmt) {
case 0:
fscanf(fp, "%f %f %f", &x, &y, &z);
break;
case 1:
fscanf(fp, "%f, %f, %f", &x, &y, &z);
break;
}
cloud->points[i] = pcl::PointXYZ(x, y, z);
}
fclose(fp);
// 点群の削減
// フィルタリング結果の格納用
pcl::PointCloud::Ptr filtered_cloud( new pcl::PointCloud );
// ボクセルグリッドで点群のフィルタリングを行う
pcl::VoxelGrid sor;
sor.setInputCloud (cloud);
sor.setLeafSize(1.0f, 1.0f, 1.0f);
sor.filter(*filtered_cloud);
// 点群の数を出力
printf("source cloud size = %d\n", cloud->size());
printf("filtered cloud size = %d\n", filtered_cloud->size());
// 元データ
pcl::PointCloud::Ptr src_cloud = filtered_cloud;
pcl::PolygonMesh polyMesh;
// KdTreeを作成
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
float epsilon = tree->getEpsilon();
tree->setEpsilon(0.01f);
tree->setInputCloud(src_cloud);
// 法線ベクトルの推定
pcl::NormalEstimation ne;
ne.setInputCloud(src_cloud);
ne.setSearchMethod(tree);
ne.setKSearch(8);
// 法線ベクトル格納用
pcl::PointCloud::Ptr norm(new pcl::PointCloud);
ne.compute(*norm);
// 座標と法線ベクトルの点群
pcl::PointCloud::Ptr clPosNormal(new pcl::PointCloud);
pcl::concatenateFields(*src_cloud, *norm, *clPosNormal);
// ↓今回のテスト対象
pcl::GreedyProjectionTriangulation gpt;
pcl::search::KdTree::Ptr tree2 (new pcl::search::KdTree);
tree2->setInputCloud(clPosNormal);
// パラメータをいろいろ設定
gpt.setSearchRadius(20.0);
gpt.setMu(2.5);
gpt.setMaximumNearestNeighbors (150);
// gpt.setMaximumSurfaceAngle(M_PI/4);
// gpt.setMinimumAngle(M_PI/18);
// gpt.setMaximumAngle(2*M_PI/3);
// gpt.setNormalConsistency(false);
// 入力データ等をセット
gpt.setInputCloud (clPosNormal);
gpt.setSearchMethod (tree2);
// 再構築
gpt.reconstruct (polyMesh);
// 点群のビューア
pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
viewer.setBackgroundColor (0.0, 0.2, 0.6);
// 原点に座標軸を追加
viewer.addCoordinateSystem(100.0, 0.0, 0.0, 0.0);
// カメラ座標のセット
double camPos[] = { 0.0, 60.0, 350.0 };
double camLookAt[] = { 0.0, 60.0, 0.0 };
double camUp[] = { 0.0, 1.0, 0.0 };
viewer.setCameraPose(camPos[0], camPos[1], camPos[2],
camLookAt[0], camLookAt[1], camLookAt[2], camUp[0], camUp[1], camUp[2]);
// ビューアに点群を追加
// viewer.addPointCloud(filtered_cloud);
viewer.addPointCloud(cloud);
// ビューアにポリゴンメッシュを追加
// viewer.addPolygonMesh(polyMesh);
while (!viewer.wasStopped ())
{
viewer.spinOnce(10);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
int main(int argc, char** argv)
{
TestCloudViewer003(argv[1]);
return 0;
}
|
