int main()
{
int n,i;
char x,y,z;
scanf("%d",&n);
for(i=0; i<n; i++)
{
scanf("\n%c %c %c", &x,&y,&z);
if(y!='.')
{
a[x-64][0]=y-64;
}
else
{
a[x-64][0]=0;
}
if(z!='.')
{
a[x-64][1]=z-64;
}
else
{
a[x-64][1]=0;
}
}
tree1(1);
printf("\n");
tree2(1);
printf("\n");
tree3(1);
printf("\n");
}
void tree3(int index)
{
if(a[index][0]!=0)
{
tree3( a[index][0] );
}
if(a[index][1]!=0)
{
tree3( a[index][1] );
}
printf("%c", index+64);
}
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::Normal> extract_normals;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
// The cloud normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Voxel grid Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// // Create VoxelGrid filtering
// voxel_grid.setInputCloud (cloud);
// voxel_grid.setLeafSize (0.01, 0.01, 0.01);
// voxel_grid.filter (*voxelgrid_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Passthrough Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pass through filter
// pass.setInputCloud (cloud);
// pass.setFilterFieldName ("z");
// pass.setFilterLimits (0, 1.5);
// pass.filter (*cloud_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Estimate point normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time estimate_start = ros::Time::now();
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud, *transformed_cloud);
//
// // Estimate point normals
// normal_estimation.setSearchMethod (tree);
// normal_estimation.setInputCloud (transformed_cloud);
// normal_estimation.setKSearch (50);
// normal_estimation.compute (*cloud_normals);
//
// ros::Time estimate_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create and processing the plane extraction
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time plane_start = ros::Time::now();
//
// // Create the segmentation object for the planar model and set all the parameters
// segmentation_from_normals.setOptimizeCoefficients (true);
// segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// segmentation_from_normals.setNormalDistanceWeight (0.1);
// segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
// segmentation_from_normals.setMaxIterations (100);
// segmentation_from_normals.setDistanceThreshold (0.03);
// segmentation_from_normals.setInputCloud (transformed_cloud);
// segmentation_from_normals.setInputNormals (cloud_normals);
//
// // Obtain the plane inliers and coefficients
// segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
// //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
// // Extract the planar inliers from the input cloud
// extract_indices.setInputCloud (transformed_cloud);
// extract_indices.setIndices (inliers_plane);
// extract_indices.setNegative (false);
// extract_indices.filter (*cloud_plane);
//
// pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
// plane_pub.publish(plane_output_cloud);
//
// ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_start = ros::Time::now();
pcl::fromROSMsg (*cloud, *transformed_cloud);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (transformed_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(transformed_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*cloud_plane);
pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
plane_pub.publish(plane_output_cloud);
ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_transformed_cloud);
transformed_cloud.swap (remove_transformed_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*transformed_cloud, *rest_output_cloud);
rest_pub.publish(rest_output_cloud);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud);
// pass through filter
pass.setInputCloud (rest_output_cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*rest_cloud_filtered);
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for cylinder features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree2);
normal_estimation.setInputCloud (cylinder_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals2);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.5);
segmentation_from_normals.setInputCloud (cylinder_cloud);
segmentation_from_normals.setInputNormals (cloud_normals2);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
//std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (cylinder_cloud);
extract_indices.setIndices (inliers_cylinder);
extract_indices.setNegative (false);
extract_indices.filter (*cylinder_output);
if (cylinder_output->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud);
cylinder_pub.publish(cylinder_output_cloud);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree3);
normal_estimation.setInputCloud (sphere_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals3);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
//segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.2);
segmentation_from_normals.setInputCloud (sphere_cloud);
segmentation_from_normals.setInputNormals (cloud_normals3);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (sphere_cloud);
extract_indices.setIndices (inliers_sphere);
extract_indices.setNegative (false);
extract_indices.filter (*sphere_output);
if (sphere_output->points.empty ())
std::cerr << "Can't find the sphere component." << std::endl;
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
ros::Time sphere_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl;
//std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl;
//std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl;
std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl;
ros::Time whole_now = ros::Time::now();
printf("\n");
std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
//std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl;
std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n");
}
int main (int argc, char** argv)
{
/* DOWNSAMPLING ********************************************************************************************************************/
std::ofstream output_file("properties.txt");
std::ofstream curvature("curvature.txt");
std::ofstream normals_text("normals.txt");
/*
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
output_file << "Number of filtered points" << std::endl;
output_file << cloud_filtered->width * cloud_filtered->height<<std::endl;
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr descriptor (new pcl::PointCloud<pcl::PointXYZ>);
descriptor->width = 5000 ;
descriptor->height = 1 ;
descriptor->points.resize (descriptor->width * descriptor->height) ;
std::cerr << descriptor->points.size() << std::endl ;
pcl::PCDWriter writer;
writer.write ("out.pcd", *cloud_filtered, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
*/
/***********************************************************************************************************************************/
/* CALCULATING VOLUME AND SURFACE AREA ********************************************************************************************************************/
/*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud(test);
chull.reconstruct(*cloud_hull);*/
/*for (size_t i=0; i < test->points.size (); i++)
{
std::cout<< test->points[i].x <<std::endl;
std::cout<< test->points[i].y <<std::endl;
std::cout<< test->points[i].z <<std::endl;
}*/
//std::cout<<data.size()<<std::endl;
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud_blob;
pcl::io::loadPCDFile ("mini_soccer_ball_downsampled.pcd", cloud_blob);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud1);
//* the data should be available in cloud
// Normal estimation*
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (20);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals,*cloud_with_normals);
//* cloud_with_normals = cloud + normals
// Create search tree*
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (0.025);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
// Get result
gp3.setInputCloud (cloud_with_normals);
gp3.setSearchMethod (tree2);
gp3.reconstruct (triangles);
// Additional vertex information
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
pcl::PolygonMesh::Ptr mesh(&triangles);
pcl::PointCloud<pcl::PointXYZ>::Ptr triangle_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(mesh->cloud, *triangle_cloud);
/* for(int i = 0; i < 2; i++){
std::cout<<triangles.polygons[i]<<std::endl;
}
*/ std::cout<<"first vertice "<<triangles.polygons[0].vertices[0] << std::endl;
//std::cout<<"Prolly not gonna work "<<triangle_cloud->points[triangles.polygons[0].vertices[0]] << std::endl;
//pcl::fromPCLPointCloud2(triangles.cloud, triangle_cloud);
std::cout << "size of points " << triangle_cloud->points.size() << std::endl ;
std::cout<<triangle_cloud->points[0]<<std::endl;
for(unsigned i = 0; i < triangle_cloud->points.size(); i++){
std::cout << triangles.polgyons[i].getVector3fMap() <<"test"<< std::endl;
}
//std::cout<<"surface: "<<calculateAreaPolygon(triangles, triangle_cloud)<<std::endl;
/******************************************************************************************************************************************/
/* CALCULATING CURVATURE AND NORMALS ********************************************************************************************************************/
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree3);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
output_file << "size of points " << cloud->points.size() << std::endl ;
output_file << "size of the normals " << cloud_normals->points.size() << std::endl ;
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
/************************************************************************************************************************************/
/* PARSING DATA ********************************************************************************************************************/
int k=0 ;
float dist ;
float square_of_dist ;
float x1,y1,z1,x2,y2,z2 ;
float nu[3], nv[3], pv_pu[3], pu_pv[3] ;
float highest = triangle_cloud->points[0].z;
float lowest = triangle_cloud->points[0].z;
for ( int i = 0; i < cloud_normals->points.size() ; i++)
{
output_file<<i<<": triangulated "<<triangle_cloud->points[i].x<<", "<<triangle_cloud->points[i].y<<", "<<triangle_cloud->points[i].z<<std::endl;
output_file<<i<<": normal"<<cloud1->points[i].x<<", "<<cloud1->points[i].y<<", "<<cloud1->points[i].z<<std::endl;
if(triangle_cloud->points[i].z > highest)
{
highest = triangle_cloud->points[i].z;
}
if(triangle_cloud->points[i].z < lowest)
{
lowest = triangle_cloud->points[i].z;
}
normals_text <<i+1 <<": "<<" x-normal-> "<<cloud_normals->points[i].normal_x<<" y-normal-> "<<cloud_normals->points[i].normal_y<<" z-normal-> "<<cloud_normals->points[i].normal_z<<std::endl;
curvature <<i+1 <<": curvature: "<<cloud_normals->points[i].curvature<<std::endl;
float x, y, z, dist, nx, ny, nz, ndist;
/*
if(i != cloud_normals->points.size()-1){
x = cloud->points[i+1].x - cloud->points[i].x;
y = cloud->points[i+1].y - cloud->points[i].y;
z = cloud->points[i+1].z - cloud->points[i].z;
dist = sqrt(pow(x, 2)+pow(y, 2) + pow(z, 2));
output_file << i+1 <<" -> "<< i+2 << " distance normal: " << dist <<std::endl;
nx = triangle_cloud[i+1].indices[0] - triangle_cloud.points[i].x;
ny = triangle_cloud.points[i+1].y - triangle_cloud.points[i].y;
nz = triangle_cloud.points[i+1].z - triangle_cloud.points[i].z;
ndist = sqrt(pow(nx, 2)+pow(ny, 2) + pow(nz, 2));
output_file << i+1 <<" -> "<< i+2 << " distance triangulated: " << ndist <<std::endl;
}
*/
/*
pcl::PointXYZRGB point;
point.x = cloud_filtered_converted->points[i].x;
point.y = cloud_filtered_converted->points[i].y;
point.z = cloud_filtered_converted->points[i].z;
point.r = 0;
point.g = 100;
point.b = 200;
point_cloud_ptr->points.push_back (point);
*/
}
output_file << "highest point: "<< highest<<std::endl;
output_file << "lowest point: "<< lowest<<std::endl;
//pcl::PointCloud<pcl::PointXYZ>::Ptr test (new pcl::PointCloud<pcl::PointXYZ>);
//float surface_area = calculateAreaPolygon(test);
//std::cout<< surface_area<<std::endl;
/*
descriptor->width = k ;
descriptor->height = 1 ;
descriptor->points.resize (descriptor->width * descriptor->height) ;
std::cerr << descriptor->points.size() << std::endl ;
float voxelSize = 0.01f ; // how to find appropriate voxel resolution
pcl::octree::OctreePointCloud<pcl::PointXYZ> octree (voxelSize);
octree.setInputCloud(descriptor) ;
ctree.defineBoundingBox(0.0,0.0,0.0,3.14,3.14,3.14) ; //octree.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ)
octree.addPointsFromInputCloud (); // octree created for block
int k_ball=0 ;
float dist_ball ;
float square_of_dist_ball ;
double X,Y,Z ;
bool occupied ;
highest = cloud_ball->points[0].z;
for ( int i = 0; i < cloud_normals_ball->points.size() ; i++)
{
if(cloud->points[i].z > highest){
highest = cloud_ball->points[i].z;
}
for (int j = i+1 ; (j < cloud_normals_ball->points.size()) ; j++)
{
x1 = cloud_ball->points[i].x ;
y1 = cloud_ball->points[i].y ;
z1 = cloud_ball->points[i].z ;
nu[0] = cloud_normals_ball->points[i].normal_x ;
nu[1] = cloud_normals_ball->points[i].normal_y ;
nu[2] = cloud_normals_ball->points[i].normal_z ;
x2 = cloud_ball->points[j].x ;
y2 = cloud_ball->points[j].y ;
z2 = cloud_ball->points[j].z ;
nv[0] = cloud_normals_ball->points[j].normal_x ;
nv[1] = cloud_normals_ball->points[j].normal_y ;
nv[2] = cloud_normals_ball->points[j].normal_z ;
square_of_dist = ((x2-x1)*(x2-x1)) + ((y2-y1)*(y2-y1)) + ((z2-z1)*(z2-z1)) ;
dist = sqrt(square_of_dist) ;
//std::cerr << dist ;
pv_pu[0] = x2-x1 ;
pv_pu[1] = y2-y1 ;
pv_pu[2] = z2-z1 ;
pu_pv[0] = x1-x2 ;
pu_pv[1] = y1-y2 ;
pu_pv[2] = z1-z2 ;
if ((dist > 0.0099) && (dist < 0.0101))
{
X = angle_between_vectors (nu, nv) ;
Y = angle_between_vectors (nu, pv_pu) ;
Z = angle_between_vectors (nv, pu_pv) ;
// output_file << descriptor->points[k].x << "\t" << descriptor->points[k].y << "\t" << descriptor->points[k].z ;
// output_file << "\n";
//k_ball = k_ball + 1 ;
occupied = octree.isVoxelOccupiedAtPoint (X,Y,Z) ;
if (occupied == 1)
{
//k_ball = k_ball + 1 ;
std::cerr << "Objects Matched" << "\t" << k_ball << std::endl ;
return(0);
}
}
}
}
*/
/***********************************************************************************************************************************/
/*
points.open("secondItemPoints.txt");
myfile<<"Second point \n";
myfile<<"second highest "<<highest;
for(int i = 0; i < cloud_normals->points.size(); i++){
points<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
if(cloud->points[i].z >= highest - (highest/100)){
myfile<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
}
}
points.close();
myfile.close();
std::cerr << "Objects Not Matched" << "\t" << k_ball << std::endl ;
*/
//output_file <<"Volume: "<<volume <<std::endl;
//output_file <<"Surface Area: "<<surface_area <<std::endl;
return (0);
}
tree*
tree2(int type, tree *c0, tree *c1)
{
return tree3(type, c0, c1, (tree *)0);
}
tree*
tree1(int type, tree *c0)
{
return tree3(type, c0, (tree *)0, (tree *)0);
}
int main(int argc, char** argv) {
MPI_Init(&argc, &argv);
MPI_Comm comm = MPI_COMM_WORLD;
int np;
MPI_Comm_size(comm, &np);
int myrank;
MPI_Comm_rank(comm, &myrank);
parse_command_line_options(argc, argv);
int test =
strtoul(commandline_option(
argc, argv, "-test", "1", false,
"-test <int> = (1) : 1) Gaussian profile 2) Zalesak disk"),
NULL, 10);
{
tbslas::SimConfig* sim_config = tbslas::SimConfigSingleton::Instance();
tbslas::new_nodes<Tree_t::Real_t>(sim_config->tree_chebyshev_order, 3);
pvfmm::Profile::Enable(sim_config->profile);
// =========================================================================
// PRINT METADATA
// =========================================================================
if (!myrank) {
MetaData_t::Print();
}
// =========================================================================
// TEST CASE
// =========================================================================
double data_dof = 0;
pvfmm::BoundaryType bc;
switch (test) {
case 1:
// fn_2 = tbslas::get_linear_field_y<double,3>;
// data_dof = 1;
// fn_2 = get_gaussian_field_atT<double,3>;
// data_dof = 1;
// fn_2 = get_vorticity_field_tv_wrapper<double>;
// data_dof = 3;
fn_2 = get_taylor_green_field_tv_ns_wrapper<double>;
data_dof = 3;
// fn_2 = tbslas::get_vorticity_field<double,3>;
// data_dof = 3;
// fn_2 = get_hopf_field_wrapper<double>;
// data_dof = 3;
// fn_2 = get_taylor_green_field_tv_wrapper<double>;
// data_dof = 3;
bc = pvfmm::Periodic;
// bc = pvfmm::FreeSpace;
break;
}
// =========================================================================
// SIMULATION PARAMETERS
// =========================================================================
sim_config->vtk_filename_variable = "conc";
sim_config->bc = bc;
double DT = sim_config->dt;
// double DT = 0.3925;
// double DT = 1;
double cheb_deg = sim_config->tree_chebyshev_order;
std::vector<double> tree_set_times;
std::vector<Tree_t*> tree_set_elems;
// =========================================================================
// INIT THE TREE 1
// =========================================================================
tcurr = 0;
Tree_t tree1(comm);
tbslas::ConstructTree<Tree_t>(
sim_config->tree_num_point_sources,
sim_config->tree_num_points_per_octanct,
sim_config->tree_chebyshev_order, sim_config->tree_max_depth,
sim_config->tree_adap, sim_config->tree_tolerance, comm, fn_2, data_dof,
tree1);
if (sim_config->vtk_save_rate) {
tree1.Write2File(tbslas::GetVTKFileName(1, "tree").c_str(),
sim_config->vtk_order);
}
tree_set_times.push_back(tcurr);
tree_set_elems.push_back(&tree1);
// =========================================================================
// INIT THE TREE 2
// =========================================================================
tcurr += DT;
Tree_t tree2(comm);
tbslas::ConstructTree<Tree_t>(
sim_config->tree_num_point_sources,
sim_config->tree_num_points_per_octanct,
sim_config->tree_chebyshev_order, sim_config->tree_max_depth,
sim_config->tree_adap, sim_config->tree_tolerance, comm, fn_2, data_dof,
tree2);
if (sim_config->vtk_save_rate) {
tree2.Write2File(tbslas::GetVTKFileName(2, "tree").c_str(),
sim_config->vtk_order);
}
tree_set_times.push_back(tcurr);
tree_set_elems.push_back(&tree2);
// =========================================================================
// INIT THE TREE 3
// =========================================================================
tcurr += DT;
Tree_t tree3(comm);
tbslas::ConstructTree<Tree_t>(
sim_config->tree_num_point_sources,
sim_config->tree_num_points_per_octanct,
sim_config->tree_chebyshev_order, sim_config->tree_max_depth,
sim_config->tree_adap, sim_config->tree_tolerance, comm, fn_2, data_dof,
tree3);
if (sim_config->vtk_save_rate) {
tree3.Write2File(tbslas::GetVTKFileName(3, "tree").c_str(),
sim_config->vtk_order);
}
tree_set_times.push_back(tcurr);
tree_set_elems.push_back(&tree3);
// =========================================================================
// INIT THE TREE 4
// =========================================================================
tcurr += DT;
Tree_t tree4(comm);
tbslas::ConstructTree<Tree_t>(
sim_config->tree_num_point_sources,
sim_config->tree_num_points_per_octanct,
sim_config->tree_chebyshev_order, sim_config->tree_max_depth,
sim_config->tree_adap, sim_config->tree_tolerance, comm, fn_2, data_dof,
tree4);
if (sim_config->vtk_save_rate) {
tree4.Write2File(tbslas::GetVTKFileName(4, "tree").c_str(),
sim_config->vtk_order);
}
tree_set_times.push_back(tcurr);
tree_set_elems.push_back(&tree4);
// =========================================================================
// MERGE
// =========================================================================
tcurr = 1.5 * DT;
Tree_t merged_tree(comm);
tbslas::ConstructTree<Tree_t>(
sim_config->tree_num_point_sources,
sim_config->tree_num_points_per_octanct,
sim_config->tree_chebyshev_order, sim_config->tree_max_depth,
sim_config->tree_adap, sim_config->tree_tolerance, comm, fn_2, data_dof,
merged_tree);
double in_al2, in_rl2, in_ali, in_rli;
CheckChebOutput<Tree_t>(&merged_tree, fn_2, data_dof, in_al2, in_rl2,
in_ali, in_rli, std::string("Input"));
// =========================================================================
// COLLECT THE MERGED TREE POINTS
// =========================================================================
std::vector<double> merged_tree_points_pos;
int num_leaf =
tbslas::CollectChebTreeGridPoints(merged_tree, merged_tree_points_pos);
// =========================================================================
// CONSTRUCT THE FUNCTOR
// =========================================================================
tbslas::FieldSetFunctor<double, Tree_t> tree_set_functor(tree_set_elems,
tree_set_times);
// tbslas::NodeFieldFunctor<double,Tree_t> tree_functor(&merged_tree);
// int num_points = 1;
// std::vector<double> xtmp(num_points*3);
// std::vector<double> vtmp(num_points*data_dof);
// xtmp[0] = 0.8;
// xtmp[1] = 1.0;
// xtmp[2] = 0.3;
// tree_functor(xtmp.data(),
// num_points,
// 1.5*DT,
// vtmp.data());
// std::cout << "MYRANK: " << myrank << " vals: " << vtmp[0] << " " <<
// vtmp[1] << " " << vtmp[2] << std::endl;
// =========================================================================
// INTERPOLATE IN TIME
// =========================================================================
int merged_tree_num_points = merged_tree_points_pos.size() / 3;
std::vector<double> merged_tree_points_val(merged_tree_num_points *
data_dof);
pvfmm::Profile::Tic("EvalSet", &sim_config->comm, false, 5);
tree_set_functor(merged_tree_points_pos.data(), merged_tree_num_points,
1.5 * DT, merged_tree_points_val.data());
// tree_functor(merged_tree_points_pos.data(),
// merged_tree_num_points,
// merged_tree_points_val.data());
pvfmm::Profile::Toc();
// =========================================================================
// FIX THE VALUES MEMORY LAYOUT
// =========================================================================
int d = cheb_deg + 1;
int num_pnts_per_node = d * d * d;
std::vector<double> mt_pnts_val_ml(merged_tree_num_points * data_dof);
for (int nindx = 0; nindx < num_leaf; nindx++) {
int input_shift = nindx * num_pnts_per_node * data_dof;
for (int j = 0; j < num_pnts_per_node; j++) {
for (int i = 0; i < data_dof; i++) {
mt_pnts_val_ml[input_shift + j + i * num_pnts_per_node] =
merged_tree_points_val[input_shift + j * data_dof + i];
}
}
}
// =========================================================================
// SET INTERPOLATED VALUES
// =========================================================================
pvfmm::Profile::Tic("SetValues", &sim_config->comm, false, 5);
tbslas::SetTreeGridValues(merged_tree, cheb_deg, data_dof, mt_pnts_val_ml);
pvfmm::Profile::Toc();
if (sim_config->vtk_save_rate) {
merged_tree.Write2File(tbslas::GetVTKFileName(0, "tree_interp").c_str(),
sim_config->vtk_order);
}
double al2, rl2, ali, rli;
CheckChebOutput<Tree_t>(&merged_tree, fn_2, data_dof, al2, rl2, ali, rli,
std::string("Output"));
// =========================================================================
// TEST
// =========================================================================
// tree_set_functor(xtmp.data(),
// num_points,
// 1.5*DT,
// vtmp.data());
// std::cout << "vals: " << vtmp[0] << " " << vtmp[1] << " " << vtmp[2] <<
// std::endl;
// tbslas::NodeFieldFunctor<double,Tree_t> tf(&merged_tree);
// tf(xtmp.data(),
// num_points,
// 1.5*DT,
// vtmp.data());
// std::cout << "vals: " << vtmp[0] << " " << vtmp[1] << " " << vtmp[2] <<
// std::endl;
// =========================================================================
// REPORT RESULTS
// =========================================================================
int tcon_max_depth = 0;
int tvel_max_depth = 0;
tbslas::GetTreeMaxDepth(merged_tree, tcon_max_depth);
// tbslas::GetTreeMaxDepth(tvel, tvel_max_depth);
typedef tbslas::Reporter<double> Rep;
if (!myrank) {
Rep::AddData("NP", np, tbslas::REP_INT);
Rep::AddData("OMP", sim_config->num_omp_threads, tbslas::REP_INT);
Rep::AddData("TOL", sim_config->tree_tolerance);
Rep::AddData("Q", sim_config->tree_chebyshev_order, tbslas::REP_INT);
Rep::AddData("MaxD", sim_config->tree_max_depth, tbslas::REP_INT);
Rep::AddData("CMaxD", tcon_max_depth, tbslas::REP_INT);
// Rep::AddData("VMaxD", tvel_max_depth, tbslas::REP_INT);
// Rep::AddData("CBC", sim_config->use_cubic?1:0, tbslas::REP_INT);
// Rep::AddData("CUF", sim_config->cubic_upsampling_factor,
// tbslas::REP_INT);
Rep::AddData("DT", sim_config->dt);
// Rep::AddData("TN", sim_config->total_num_timestep, tbslas::REP_INT);
// Rep::AddData("TEST", test, tbslas::REP_INT);
// Rep::AddData("MERGE", merge, tbslas::REP_INT);
Rep::AddData("InAL2", in_al2);
Rep::AddData("OutAL2", al2);
Rep::AddData("InRL2", in_rl2);
Rep::AddData("OutRL2", rl2);
Rep::AddData("InALINF", in_ali);
Rep::AddData("OutALINF", ali);
Rep::AddData("InRLINF", in_rli);
Rep::AddData("OutRLINF", rli);
Rep::Report();
}
// Output Profiling results.
// pvfmm::Profile::print(&comm);
}
// Shut down MPI
MPI_Finalize();
return 0;
}
