void tree2(int index)
{
if(a[index][0]!=0)
{
tree2( a[index][0] );
}
printf("%c", index+64);
if(a[index][1]!=0)
{
tree2( a[index][1] );
}
}
TEST (CorrespondenceEstimation, CorrespondenceEstimationSetSearchMethod)
{
// Generating 3 random clouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
for ( size_t i = 0; i < 50; i++ )
{
cloud1->points.emplace_back(float (rand()), float (rand()), float (rand()));
cloud2->points.emplace_back(float (rand()), float (rand()), float (rand()));
}
// Build a KdTree for each
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ> ());
tree1->setInputCloud (cloud1);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
tree2->setInputCloud (cloud2);
// Compute correspondences
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ, double> ce;
ce.setInputSource (cloud1);
ce.setInputTarget (cloud2);
pcl::Correspondences corr_orig;
ce.determineCorrespondences(corr_orig);
// Now set the kd trees
ce.setSearchMethodSource (tree1, true);
ce.setSearchMethodTarget (tree2, true);
pcl::Correspondences corr_cached;
ce.determineCorrespondences (corr_cached);
// Ensure they're the same
EXPECT_EQ(corr_orig.size(), corr_cached.size());
for(size_t i = 0; i < corr_orig.size(); i++)
{
EXPECT_EQ(corr_orig[i].index_query, corr_cached[i].index_query);
EXPECT_EQ(corr_orig[i].index_match, corr_cached[i].index_match);
}
}
int main(int argc, char **argv)
{
srand((unsigned int)time(NULL));
cout << "BSTree" << endl;
tree::BSTree<int> tree;
tree.clear();
for (int i = 0; i < 20; ++i) {
tree.insert((rand() % 100) + 1);
}
tree.print();
cout << endl;
cout << "KDTree" << endl;
const int ndim = 3;
tree::KDTree<tree::Container<int> > tree2(ndim);
for (int i = 0; i < 20; ++i) {
tree::Container<int> dat(ndim);
for (int d = 0; d < ndim; ++d) {
dat[d] = (rand() % 100) + 1;
}
tree2.insert(dat);
}
tree2.print();
return 0;
}
int main()
{
LeftistTree<float> tree(5);
for(int i = 0; i != 12; i++)
{
tree.insert(i);
}
tree.print();
std::cout << "----------------------------------" << std::endl;
LeftistTree<float> tree2(5);
tree2.insert(2);
tree2.insert(8);
tree2.insert(12);
tree2.insert(4);
tree2.insert(9);
tree2.insert(10);
// tree2.insert(16);
// tree2.insert(17);
// tree2.insert(18);
// tree2.insert(19);
tree2.print();
std::cout << "----------------------------------" << std::endl;
tree.merge(tree2);
tree2.print();
std::cout << "min: " << tree.deleteMin() << std::endl;
std::cout << "min: " << tree.deleteMin() << std::endl;
tree.print();
return 0;
}
int main (int argc, char** argv)
{
// 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 ("12_inch_block_downsampled.pcd", cloud_blob);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
//* 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();
for(int i = 0; i < parts.size(); i++){
std::cout<<parts[i]<<", "<<states[i]<<std::endl;
}
// Finish
return (0);
}
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 subCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
if(msg->data.empty())
{
ROS_WARN("Received an empty cloud message. Skipping further processing");
return;
}
// Convert ROS message to PCL-compatible data structure
ROS_INFO_STREAM("Received a cloud message with " << msg->height * msg->width << " points");
ROS_INFO("Converting ROS cloud message to PCL compatible data structure");
pcl::PointCloud<pcl::PointNormal> pclCloud;
pcl::fromROSMsg(*msg, pclCloud);
pcl::PointCloud<pcl::PointNormal>::Ptr p_with_normals(new pcl::PointCloud<pcl::PointNormal>(pclCloud));
// Fast triangulation
// Source: http://pointclouds.org/documentation/tutorials/greedy_projection.php
// Create search tree
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(p_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius(searchRadius);
// Set typical values for the parameters
gp3.setMu(mu);
gp3.setMaximumNearestNeighbors(maxNN);
gp3.setMaximumSurfaceAngle(maxSurfaceAngle); // 45 degrees
gp3.setMinimumAngle(minAngle); // 10 degrees
gp3.setMaximumAngle(maxAngle); // 120 degrees
gp3.setNormalConsistency(normalConsistency);
// Get result
gp3.setInputCloud(p_with_normals);
gp3.setSearchMethod(tree2);
gp3.reconstruct(*mesh);
ROS_INFO_STREAM("Number of polygons: " << mesh->polygons.size());
// Additional vertex information
//std::vector<int> parts = gp3.getPartIDs();
//std::vector<int> states = gp3.getPointStates();
if(toggleWritingToFile)
{
std::string path = "/home/redbaron/catkin_ws/src/pmd_camboard_nano/samples/temp/";
ss << path << "cloud_mesh_generator_fast_triangulation_" << fileIdx << ".stl";
pcl::io::savePolygonFileSTL(ss.str(), *mesh);
ROS_INFO_STREAM("Writing to file \"" << ss.str() << "\"");
fileIdx++;
ss.str("");
}
// sensor_msgs::PointCloud2 output;
// pcl::toROSMsg(*p, output);
// pub.publish(output);
}
/**
* 指定した頂点配下を比べて、アンバランス度を計算する。
*/
int BFS2::computeUnbalanceness(RoadGraph* roads1, RoadVertexDesc node1, RoadGraph* roads2, RoadVertexDesc node2) {
int score = 0;
// 木構造を作成する
BFSTree tree1(roads1, node1);
BFSTree tree2(roads2, node2);
std::list<RoadVertexDesc> seeds1;
seeds1.push_back(node1);
std::list<RoadVertexDesc> seeds2;
seeds2.push_back(node2);
while (!seeds1.empty()) {
RoadVertexDesc parent1 = seeds1.front();
seeds1.pop_front();
RoadVertexDesc parent2 = seeds2.front();
seeds2.pop_front();
// 子リストを取得
std::vector<RoadVertexDesc> children1 = tree1.getChildren(parent1);
std::vector<RoadVertexDesc> children2 = tree2.getChildren(parent2);
// どちらのノードにも、子ノードがない場合は、スキップ
if (children1.size() == 0 && children2.size() == 0) continue;
QMap<RoadVertexDesc, bool> paired1;
QMap<RoadVertexDesc, bool> paired2;
while (true) {
RoadVertexDesc child1, child2;
if (!findBestPairByDirection(roads1, parent1, &tree1, paired1, roads2, parent2, &tree2, paired2, true, child1, child2)) break;
paired1[child1] = true;
paired2[child2] = true;
seeds1.push_back(child1);
seeds2.push_back(child2);
}
// ペアにならなかったノードについて、ペナルティをカウントする
for (int i = 0; i < children1.size(); i++) {
if (paired1.contains(children1[i])) continue;
if (!roads1->graph[children1[i]]->valid) continue;
score += tree1.getTreeSize(children1[i]);
}
for (int i = 0; i < children2.size(); i++) {
if (paired2.contains(children2[i])) continue;
if (!roads2->graph[children2[i]]->valid) continue;
score += tree2.getTreeSize(children2[i]);
}
}
return score;
}
void Controller::generate_polygon(){
//MP SEARCH FOR FAST MESH GENERATION AND PCL VOXEL GRID
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr vertices (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(this->_pcl_cloud_polygon, *vertices);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
ne.setRadiusSearch (this->_property_manager->_3d_normal_radius);
ne.setInputCloud (this->_pcl_cloud_polygon.makeShared());
ne.compute (*vertices);
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree2->setInputCloud (vertices);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
pcl::PolygonMesh triangles;
/*
setMaximumNearestNeighbors(unsigned) and setMu(double) control the size of the neighborhood.
The former defines how many neighbors are searched for, while the latter specifies the maximum acceptable distance
for a point to be considered, relative to the distance of the nearest point (in order to adjust to changing densities).
Typical values are 50-100 and 2.5-3 (or 1.5 for grids).
setSearchRadius(double) is practically the maximum edge length for every triangle.
This has to be set by the user such that to allow for the biggest triangles that should be possible.
setMinimumAngle(double) and setMaximumAngle(double) are the minimum and maximum angles in each triangle.
While the first is not guaranteed, the second is. Typical values are 10 and 120 degrees (in radians).
setMaximumSurfaceAgle(double) and setNormalConsistency(bool) are meant to deal with the cases where there are sharp
edges or corners and where two sides of a surface run very close to each other. To achieve this, points are not
connected to the current point if their normals deviate more than the specified angle
(note that most surface normal estimation methods produce smooth transitions between normal angles even at sharp edges).
This angle is computed as the angle between the lines defined by the normals (disregarding the normal’s direction)
if the normal-consistency-flag is not set, as not all normal estimation methods can guarantee consistently oriented normals.
Typically, 45 degrees (in radians) and false works on most datasets.
*/
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (10);
// Set typical values for the parameters
gp3.setMu (15);
gp3.setMaximumNearestNeighbors (200);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(true);
// Get result
gp3.setInputCloud (vertices);
gp3.setSearchMethod (tree2);
gp3.reconstruct (triangles);
this->_polygon = triangles;
}
void greedy_proj () {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::io::loadPCDFile ("input.pcd", *cloud_blob);
pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);
cloud_blob = cloud_filtered;
// 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);
// 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);
// 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 (1);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (10);
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::io::saveVTKFile("output.vtk", triangles);
pcl::io::savePolygonFileSTL("output.stl", triangles);
}
int second_tree(int inU[7], int inS[7], int inZ[7], struct node **head,
solution *US)
{
int late_index[7], outZ[7]; //zero case variables
int i[7]; //index variable
int u[7], v[7], s[7];
getLateIndices(late_index, inZ, 17);
int matrix[7][4];
solution *VS = malloc(sizeof(solution));
init_level(VS);
int branches = 0;
int vc = 0, l2c = 0, pc = 0;
for (i[0] = 0; i[0] < 5; i[0]++)
{//open loop 0
tree2(0, i[0], matrix, inZ, outZ, inU, inS, u, s, v);
for (i[1] = 0; i[1] < 5; i[1]++)
{//open loop 1
tree2(1, i[1], matrix, inZ, outZ, inU, inS, u, s, v);
for (i[2] = 0; i[2] < 5; i[2]++)
{//open loop 2
tree2(2, i[2], matrix, inZ, outZ, inU, inS, u, s, v);
for (i[3] = 0; i[3] < 5; i[3]++)
{//open loop 3
tree2(3, i[3], matrix, inZ, outZ, inU, inS, u, s, v);
for (i[4] = 0; i[4] < late_index[4]; i[4]++)
{//open loop 3
tree2(4, i[4], matrix, inZ, outZ, inU, inS, u, s, v);
for (i[5] = getLowerIndex(i[4], inZ, 5);
i[5] < late_index[5]; i[5]++)
{//open loop 3
tree2(5, i[5], matrix, inZ, outZ, inU, inS, u, s, v);
for (i[6] = getLowerIndex(i[5], inZ, 6);
i[6] < late_index[6]; i[6]++)
{//open loop 3
tree2(6, i[6], matrix, inZ, outZ, inU, inS, u, s, v);
vc = vCheck(v);
l2c = levelTWOcheck(matrix);
if (l2c) {
pc = prunecheck(US, VS, matrix, 1);
if (pc) {
branches++;
third_tree(u, v, s, outZ, VS, US, head);
}
}
}//close loop 6
}//close loop 5
}//close loop 4
}//close loop 3
}//close loop 2
}//close loop 1
}//close loop 0
free(VS);
return branches;
}
boost::shared_ptr<Lattice>
TwoFactorModel::tree(const TimeGrid& grid,
const boost::shared_ptr<AdditionalResultCalculator>& additionalResultCalculator) const {
boost::shared_ptr<ShortRateDynamics> dyn = dynamics();
boost::shared_ptr<TrinomialTree> tree1(
new TrinomialTree(dyn->xProcess(), grid));
boost::shared_ptr<TrinomialTree> tree2(
new TrinomialTree(dyn->yProcess(), grid));
return boost::shared_ptr<Lattice>(
new TwoFactorModel::ShortRateTree(tree1, tree2, dyn));
}
void recons(std::string infile, std::string outfile) {
sensor_msgs::PointCloud2 cloud_blob;
// run this from BodyScanner/build/surface_reconstructor/
pcl::io::loadPCDFile(infile, cloud_blob);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal> ());
pcl::fromROSMsg (cloud_blob, *cloud);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointNormal>());
int l = 0;
for(int j = 0; j < 480; j += 1) {
for(int i = j%2; i < 640; i += 2) {
int k = i + j*640;
pcl::PointNormal& p = (*cloud)[k];
if((i&1 && j&1) // keep 1/4 of the points
&& mag(p) < 3) { // keep only points within a close distance to the origin
filtered_cloud->push_back(p);
}
}
}
// Create search tree*
pcl::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointNormal>);
tree2->setInputCloud(filtered_cloud);
// 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);
setMaximumSurfaceAngle(gp3, 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(filtered_cloud);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);
//pcl::io::savePLYFile(outfile, triangles);
pcl::io::saveVTKFile(outfile, triangles);
}
pcl::PolygonMesh RScloud::triangulate_cloud() {
pcl::PointCloud<pointT>::Ptr cloudfiltered(new pcl::PointCloud<pointT>);
pcl::StatisticalOutlierRemoval<pointT> sor;
sor.setInputCloud (pointcloud);
sor.setMeanK (50);
sor.setStddevMulThresh (2.5);
sor.filter (*cloudfiltered);
// Normal estimation*
pcl::NormalEstimation<pointT, pointTnormal> n;
pcl::PointCloud<pointTnormal>::Ptr normals (new pcl::PointCloud<pointTnormal>);
pcl::search::KdTree<pointT>::Ptr tree (new pcl::search::KdTree<pointT>);
tree->setInputCloud (cloudfiltered);
n.setInputCloud (cloudfiltered);
n.setSearchMethod (tree);
n.setKSearch (50);
n.compute (*normals);
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields (*cloudfiltered, *normals, *cloud_with_normals);
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> 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);
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
gp3.setConsistentVertexOrdering(true);
// 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();
return triangles;
}
pcl::PolygonMesh::Ptr createMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
float gp3SearchRadius,
float gp3Mu,
int gp3MaximumNearestNeighbors,
float gp3MaximumSurfaceAngle,
float gp3MinimumAngle,
float gp3MaximumAngle,
bool gp3NormalConsistency)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
// Create search tree*
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree2->setInputCloud (cloudWithNormalsNoNaN);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (gp3SearchRadius);
// Set typical values for the parameters
gp3.setMu (gp3Mu);
gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle); // 45 degrees
gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
gp3.setNormalConsistency(gp3NormalConsistency);
gp3.setConsistentVertexOrdering(gp3NormalConsistency);
// Get result
gp3.setInputCloud (cloudWithNormalsNoNaN);
gp3.setSearchMethod (tree2);
gp3.reconstruct (*mesh);
//UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size());
//mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons);
return mesh;
}
pcl::PolygonMesh createMesh( pcl::PointCloud<PointType>::Ptr cloud )
{
pcl::console::print_info( "createMesh" );
// 法線を取得する
pcl::PointCloud<NormalType>::Ptr cloud_with_normals(
new pcl::PointCloud<NormalType> );
pcl::MovingLeastSquares<PointType, NormalType> mls;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(
new pcl::search::KdTree<PointType> );
mls.setComputeNormals( true );
mls.setInputCloud( cloud );
mls.setPolynomialFit( true );
mls.setSearchMethod( tree );
mls.setSearchRadius( 0.03 );
mls.process( *cloud_with_normals );
pcl::search::KdTree<NormalType>::Ptr tree2(
new pcl::search::KdTree<NormalType> );
// メッシュ化
pcl::GreedyProjectionTriangulation<NormalType> gp3;
gp3.setSearchRadius( 0.025 );
gp3.setMu( 2.5 );
gp3.setMaximumNearestNeighbors( 100 );
gp3.setMaximumSurfaceAngle( M_PI / 4 );
gp3.setMinimumAngle( M_PI / 18 );
gp3.setMaximumAngle( 2 * M_PI / 3 );
gp3.setNormalConsistency( false );
// 結果を取得
pcl::PolygonMesh triangles;
gp3.setInputCloud( cloud_with_normals );
gp3.setSearchMethod( tree2 );
gp3.reconstruct( triangles );
return triangles;
}
int main() {
// ================= test constructors and mutators
TBST tree1;
for(int i=0; i < 7; i++) {
tree1.insert(str(i));
}
assert(tree1.remove("0"));
TBST tree2(tree1);
TBST tree3 = tree2;
int oldHeight = tree1.getHeight();
tree1.selfBalance();
assert(oldHeight < tree1.getHeight());
// ================= test accessors
testAccessors(tree1, tree2);
// ================= test traversors
testTraversors(tree1, tree2, tree3);
}
void geonDetector::initialize()
{
totalCount = 0;
pcl::PointCloud<PointT>::Ptr cloud1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered21 (new pcl::PointCloud<PointT>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals21 (new pcl::PointCloud<pcl::Normal>());
pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr geonIndices1 (new pcl::PointIndices());
pcl::search::KdTree<PointT>::Ptr tree2 (new pcl::search::KdTree<PointT>());
this->cloud = cloud1;
this->cloud_filtered = cloud_filtered1;
this->cloud_normals = cloud_normals1;
this->cloud_filtered2 = cloud_filtered21;
this->cloud_normals2 = cloud_normals21;
this->coefficients = coefficients1;
this->geonIndices = geonIndices1;
this->tree = tree2;
}
void Scene::bench_construction()
{
if(m_pPolyhedron == NULL)
{
std::cout << "Load polyhedron first." << std::endl;
return;
}
std::cout << std::endl << "Benchmark construction" << std::endl;
std::cout << "#Facets alone (s) with KD-tree (s)" << std::endl;
while(m_pPolyhedron->size_of_facets() < 1000000)
{
// refines mesh at increasing speed
Refiner<Kernel,Polyhedron> refiner(m_pPolyhedron);
std::size_t digits = nb_digits(m_pPolyhedron->size_of_facets());
unsigned int nb_splits =
static_cast<unsigned int>(0.2 * std::pow(10.0,(double)digits - 1.0));
refiner.run_nb_splits(nb_splits);
// constructs tree
CGAL::Timer time1;
time1.start();
Facet_tree tree1(m_pPolyhedron->facets_begin(),m_pPolyhedron->facets_end());
double duration_construction_alone = time1.time();
CGAL::Timer time2;
time2.start();
Facet_tree tree2(m_pPolyhedron->facets_begin(),m_pPolyhedron->facets_end());
tree2.accelerate_distance_queries();
double duration_construction_and_kdtree = time2.time();
std::cout << m_pPolyhedron->size_of_facets() << "\t"
<< duration_construction_alone << "\t"
<< duration_construction_and_kdtree << std::endl;
}
}
int main (int argc, char** argv)
{
if(argc<2){
std::cout << "need ply/pcd file. " << std::endl;
return -1;
}
// 确定文件格式
char tmpStr[100];
strcpy(tmpStr,argv[1]);
char* pext = strrchr(tmpStr, '.');
std::string extply("ply");
std::string extpcd("pcd");
if(pext){
*pext='\0';
pext++;
}
std::string ext(pext);
//如果不支持文件格式，退出程序
if (!((ext == extply)||(ext == extpcd))){
std::cout << "文件格式不支持!" << std::endl;
std::cout << "支持文件格式：*.pcd和*.ply！" << std::endl;
return(-1);
}
//根据文件格式选择输入方式
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>) ; //创建点云对象指针，用于存储输入
if (ext == extply){
if (pcl::io::loadPLYFile(argv[1] , *cloud) == -1){
PCL_ERROR("Could not read ply file!\n") ;
return -1;
}
}
else{
if (pcl::io::loadPCDFile(argv[1] , *cloud) == -1){
PCL_ERROR("Could not read pcd file!\n") ;
return -1;
}
}
// 估计法向量 x,y,x + 法向量 + 曲率
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);//20个邻居
n.compute (*normals); //计算法线，结果存储在normals中
//* normals 不能同时包含点的法向量和表面的曲率
//将点云和法线放到一起
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
//创建搜索树
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals);
//初始化 移动立方体算法 MarchingCubes对象，并设置参数
pcl::MarchingCubes<pcl::PointNormal> *mc;
mc = new pcl::MarchingCubesHoppe<pcl::PointNormal> ();
/*
if (hoppe_or_rbf == 0)
mc = new pcl::MarchingCubesHoppe<pcl::PointNormal> ();
else
{
mc = new pcl::MarchingCubesRBF<pcl::PointNormal> ();
(reinterpret_cast<pcl::MarchingCubesRBF<pcl::PointNormal>*> (mc))->setOffSurfaceDisplacement (off_surface_displacement);
}
*/
//创建多变形网格，用于存储结果
pcl::PolygonMesh mesh;
//设置MarchingCubes对象的参数
mc->setIsoLevel (0.0f);
mc->setGridResolution (50, 50, 50);
mc->setPercentageExtendGrid (0.0f);
//设置搜索方法
mc->setInputCloud (cloud_with_normals);
//执行重构，结果保存在mesh中
mc->reconstruct (mesh);
//保存网格图
pcl::io::savePLYFile("result2.ply", mesh);
// 显示结果图
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0); //设置背景
viewer->addPolygonMesh(mesh,"my"); //设置显示的网格
//viewer->addCoordinateSystem (1.0); //设置坐标系
viewer->initCameraParameters ();
while (!viewer->wasStopped ()){
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
//! Compute candidate matches using the Euclidean distance.
vector<Match> AnnMatcher::compute_matches()
{
Timer t;
flann::KDTreeIndexParams params{ 8 };
flann::Matrix<float> data1, data2;
data1 = create_flann_matrix(_keys1.descriptors);
data2 = create_flann_matrix(_keys2.descriptors);
flann::Index<flann::L2<float> > tree1(data1, params);
flann::Index<flann::L2<float> > tree2(data2, params);
tree1.buildIndex();
tree2.buildIndex();
cout << "Built trees in " << t.elapsed() << " seconds." << endl;
vector<Match> matches;
matches.reserve(1e5);
t.restart();
for (auto i1 = 0u; i1 < _keys1.size(); ++i1)
append_nearest_neighbors(
i1, _keys1, _keys2, matches, data2, tree2,
_squared_ratio_thres, Match::Direction::SourceToTarget,
_self_matching, _is_too_close, _vec_indices, _vec_dists,
_max_neighbors);
for (auto i2 = 0u; i2 < _keys2.size(); ++i2)
append_nearest_neighbors(
i2, _keys2, _keys1, matches, data1, tree1,
_squared_ratio_thres, Match::Direction::TargetToSource,
_self_matching, _is_too_close, _vec_indices, _vec_dists,
_max_neighbors);
// Lexicographical comparison between matches.
auto compare_match = [](const Match& m1, const Match& m2)
{
if (m1.x_index() < m2.x_index())
return true;
if (m1.x_index() == m2.x_index() && m1.y_index() < m2.y_index())
return true;
if (m1.x_index() == m2.x_index() && m1.y_index() == m2.y_index() &&
m1.score() < m2.score())
return true;
return false;
};
sort(matches.begin(), matches.end(), compare_match);
// Remove redundant matches in each consecutive group of identical matches.
// We keep the one with the best Lowe score.
matches.resize(unique(matches.begin(), matches.end()) - matches.begin());
sort(matches.begin(), matches.end(),
[&](const Match& m1, const Match& m2) {
return m1.score() < m2.score();
}
);
cout << "Computed " << matches.size() << " matches in " << t.elapsed() << " seconds." << endl;
return matches;
}
int main(int argc, char *argv[]) {
if (argc != 2)
return 0;
Frame3D frames[8];
for (int i = 0; i < 8; ++i) {
frames[i].load(boost::str(boost::format("%s/%05d.3df") % argv[1] % i));
}
std::cout << "Merging point cloud" << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr model_point_cloud_norm = merge(frames);
std::cout << "Got: " << model_point_cloud_norm->size() << " points" << std::endl;
std::cout << "Generating mesh" << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr reduced_point_cloud(new pcl::PointCloud<pcl::PointNormal>());
pcl::PassThrough<pcl::PointNormal> filter;
filter.setInputCloud(model_point_cloud_norm);
filter.filter(*reduced_point_cloud);
std::cout << "Got: " << reduced_point_cloud->size() << " points" << std::endl;
pcl::Poisson<pcl::PointNormal> rec;
rec.setDepth(10);
rec.setInputCloud(reduced_point_cloud);
pcl::PolygonMesh triangles;
rec.reconstruct(triangles);
std::cout << "Finished" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
for (int i = 0; i < 8; ++i) {
float focal_length = frames[i].focal_length_;
// Camera width
double sizeX = frames[i].depth_image_.cols;
// Camera height
double sizeY = frames[i].depth_image_.rows;
// Centers
double cx = sizeX / 2.0;
double cy = sizeY / 2.0;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud = transformPointCloud(cloud, frames[i].getEigenTransform().inverse());
const cv::Mat& zbuffer = computeZbuffer(*transformed_cloud, frames[i]);
int point_found = 0;
for (const pcl::Vertices& polygon : triangles.polygons) {
const pcl::PointXYZRGB& point = transformed_cloud->at(polygon.vertices[0]);
int u_unscaled = std::round(focal_length * (point.x / point.z) + cx);
int v_unscaled = std::round(focal_length * (point.y / point.z) + cy);
const cv::Vec3f& zmap_point = zbuffer.at<cv::Vec3f>(v_unscaled, u_unscaled);
const float eps = 0.000000001;
// If not visible
if (std::fabs(zmap_point[0] - point.x) > eps
|| std::fabs(zmap_point[1] - point.y) > eps
|| std::fabs(zmap_point[2] - point.z) > eps)
continue;
float u = static_cast<float>(u_unscaled / sizeX);
float v = static_cast<float>(v_unscaled / sizeY);
if (u < 0. || u >= 1 || v < 0. || v >= 1)
continue;
int x = std::floor(frames[i].rgb_image_.cols * u);
int y = std::floor(frames[i].rgb_image_.rows * v);
for (int h = 0; h < 3; ++h) {
pcl::PointXYZRGB& original_point = cloud->at(polygon.vertices[h]);
const cv::Vec3b& rgb = frames[i].rgb_image_.at<cv::Vec3b>(y, x);
if (original_point.r != 0 && original_point.g != 0 && original_point.b != 0)
continue;
original_point.b = rgb[0];
original_point.g = rgb[1];
original_point.r = rgb[2];
}
++point_found;
}
std::cout << point_found << " points found" << std::endl;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr textured_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
textured_cloud->reserve(cloud->size());
for (const pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0) {
textured_cloud->push_back(point);
}
}
// Filling gaps
pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree2->setInputCloud(textured_cloud);
for (pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0)
continue;
std::vector<int> k_indices;
std::vector<float> k_dist;
tree2->nearestKSearch(point, 1, k_indices, k_dist);
const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[0]);
point.r = textured_point.r;
point.g = textured_point.g;
point.b = textured_point.b;
}
// Smoothing
tree2->setInputCloud(cloud);
for (pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0)
continue;
std::vector<int> k_indices;
std::vector<float> k_dist;
tree2->nearestKSearch(point, 5, k_indices, k_dist);
int r = 0;
int g = 0;
int b = 0;
for (int i = 0; i < 5; ++i) {
const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[i]);
r += textured_point.r;
g += textured_point.g;
b += textured_point.b;
}
r /= 5;
g /= 5;
b /= 5;
point.r = (uint8_t) r;
point.g = (uint8_t) g;
point.b = (uint8_t) b;
}
pcl::toPCLPointCloud2(*cloud, triangles.cloud);
std::cout << "Finished texturing" << std::endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(1, 1, 1);
viewer->addPolygonMesh(triangles, "meshes", 0);
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
while (!viewer->wasStopped()) {
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
void display(void)
{
/* number of recurses for tree */
int rec = 0;
GLfloat mat_red[] = {1.0, 0.0, 0.0, 0.0};
GLfloat mat_green[] = {0.0, 1.0, 0.0, 0.0};
GLfloat mat_blue[] = {0.0, 0.0, 1.0, 0.0};
GLfloat mat_black[] = {0.0, 0.0, 0.0, 0.0};
/* light placed at infinity in the direcion <10,10,10>*/
GLfloat light_position[] = {10.0, 10.0, 10.0, 0.0};
/* this will be your shadow matrix. You need to specify what this contains.
* OpenGL has a funky ordering for rows and columns
* use this ordering for rows and columns. The identity matrix with Mr,c = M3,3 = 0;
*/
GLfloat m[16];
m[0]= 1; m[4]= 0; m[8] = 0; m[12]= 0;
m[1]= 0; m[5]= 1; m[9] = 0; m[13]= 0;
m[2]= 0; m[6]= 0; m[10]= 0; m[14]= 0;
m[3]= 0; m[7]= 0; m[11]= 0; m[15]= 1;
/* replaces the matrix in the top of the stack with the identity matrix */
glLoadIdentity();
/* multiplies the current matrix by a tranformation matrix that puts the eyepoint
* at <10,0,5>, center of the image at <0,0,0>, and up vector of <0,0,1> (z-axis).
*/
gluLookAt(10,0,5, 0,0,0, 0,0,1);
/* move light0 to 'light_position' */
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
/* draw checkered floor */
/* enable textures */
glEnable(GL_TEXTURE_2D);
/* set texture mode */
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
/* set current texture to 'texName' */
glBindTexture(GL_TEXTURE_2D, texName);
/* draw polygon floor */
glBegin(GL_QUADS);
/* z=-0.01 so that the floor doesn't overlap your shadows
* glTexCoord fixes a point in the texture to the vertex that follows it.
*/
glTexCoord2f(0.0, 0.0); glVertex3f(-4.0, -4.0, -0.01);
glTexCoord2f(0.0, 1.0); glVertex3f(-4.0, 4.0, -0.01);
glTexCoord2f(1.0, 1.0); glVertex3f( 4.0, 4.0, -0.01);
glTexCoord2f(1.0, 0.0); glVertex3f( 4.0, -4.0, -0.01);
glEnd();
glFlush();
/* disable textures so that we can draw other polys w/o the texture */
glDisable(GL_TEXTURE_2D);
/* display tree */
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_green);
rec = 5;
glPushMatrix();
glTranslatef(0, -2.5, 0);
//tree(rec, 1.5);
glPopMatrix();
rec = 7;
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_red);
glPushMatrix();
//glTranslatef(0, 2.5, 0);
tree2(rec, 1.8);
glPopMatrix();
glFlush();
}
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;
}
void colorRecons(std::string infile, std::string outfile) {
sensor_msgs::PointCloud2 cloud_blob;
// run this from BodyScanner/build/surface_reconstructor/
pcl::io::loadPCDFile(infile, cloud_blob);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::fromROSMsg(cloud_blob, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
int l = 0;
for(int j = 0; j < 480; j += 1) {
for(int i = j%2; i < 640; i += 2) {
int k = i + j*640;
pcl::PointXYZRGB& p = (*cloud)[k];
pcl::PointXYZ pp;
pcl::PointXYZRGB pc;
pc.x = pp.x = p.x;
pc.y = pp.y = p.y;
pc.z = pp.z = p.z;
pc.r = p.r;
pc.g = p.g;
pc.b = p.b;
//pp.nx = 0; pp.ny = 0; pp.nz = 0;
//if((i&1 && j&1) // keep 1/4 of the points
if(mag(p) < 3) { // keep only points within a close distance to the origin
filtered_cloud->push_back(pp);
filtered_cloud_color->push_back(pc);
}
}
}
// Normal estimation*
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree->setInputCloud(filtered_cloud);
n.setInputCloud(filtered_cloud);
n.setSearchMethod(tree);
n.setKSearch(20);
n.compute(*normals);
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*filtered_cloud, *normals, *filtered_cloud_with_normals);
// Create search tree*
pcl::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointNormal>);
tree2->setInputCloud(filtered_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.1);
// Set typical values for the parameters
gp3.setMu(2.5);
gp3.setMaximumNearestNeighbors(50); // reducing this didn't fix flips
setMaximumSurfaceAngle(gp3, M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(true); // changing this didn't fix flips
// Get result
gp3.setInputCloud(filtered_cloud_with_normals);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);
saveObj(outfile, triangles, filtered_cloud_color);
}
void mbfit(){
gROOT->ProcessLine(".x /home/jholzbau/rootlogon.C");
TString var = "setfit";
TH1::SetDefaultSumw2();
//Plan is to plot SET for CAFE and PMCS, Then with SET-MBSET for PMCS vs CAFE, Then with scaled MBSET (event by event depending on SET or MBSET value or something)
//run3 files
// TString rootfile1 = "/prj_root/7055/wmass2/jenny/pythia_zee_run2b3_default_tree_20130703_152033/pythia_zee_run2b3_default_tree_20130703_152033_20130703_152344_23/ZRootTree3.root";
// TString rootfile2 = "/prj_root/7055/wmass2/jenny/mbtreecafe2/WMASS_MC_RUN2B3_zee_ovl_12p8M_1372882719_default_tree2_20130703151839-15902467.d0cabsrv1.fnal.gov/ZRootTree2.root";
//TString rootfile1 = "/prj_root/7055/wmass2/jenny/pythia_zee_run2b3_default_tree_20130703_152033/run3.root";
//TString rootfile1 = "/prj_root/7055/wmass2/jenny/pythia_zee_run2b3_default_tree_vtxcut_redo_20130716_142720/result.root";
// TString rootfile1 = "/prj_root/7055/wmass2/jenny/pythia_zee_run2b3_default_tree_vtxcut_redo_20130726_085527/tree.root";
TString rootfile1 = "/prj_root/7055/wmass2/jenny/pythia_zee_run2b3_mbtest_inclusiveweight_NEWmblib_pow04mbprob_TREE_20130726_085527/tree.root";
TString rootfile2 = "/prj_root/7055/wmass2/jenny/mbtreecafe2/run3.root";
Double_t xmin = 0;
Double_t xmax = 300;
Int_t nbin = 250;
TH1D *tcafe = new TH1D("topcafe", "topcafe", nbin, xmin, xmax);
TH1D *tpmcs = new TH1D("toppmcs", "toppmcs", nbin, xmin, xmax);
TH1D *tpmcsd = new TH1D("toppmcsd", "toppmcsd", nbin, xmin, xmax);
TH1D *tpmcsmb = new TH1D("toppmcsmb", "toppmcsmb", nbin, xmin, xmax);
TH1D *tpmcsother = new TH1D("toppmcsother", "toppmcsother", nbin, xmin, xmax);
TH1D *tcafesum = new TH1D("topcafesum", "topcafesum", nbin, xmin, xmax);
TH1D *tpmcssum = new TH1D("toppmcssum", "toppmcssum", nbin, xmin, xmax);
TH1D *tpmcsmbsum = new TH1D("toppmcsmbsum", "toppmcsmbsum", nbin, xmin, xmax);
TH1D *tpmcsothersum = new TH1D("toppmcsothersum", "toppmcsothersum", nbin, xmin, xmax);
TH1D *t = new TH1D("top", "top", nbin, xmin, xmax);
TH1D *b = new TH1D("bottom", "bottom", nbin, xmin, xmax);
TH2D *mbandset = new TH2D("mbandset", "mbandset", nbin, xmin, xmax, nbin, xmin, xmax);
TH2D *otherandset = new TH2D("otherandset", "otherandset", nbin, xmin, xmax, nbin, xmin, xmax);
TH1D *cafemb = new TH1D("cafemb", "cafemb", 500, 0, 125);
TH1D *pmcsmb = new TH1D("pmcsmb", "pmcsmb", 500, 0, 125);
TH1D *ratio = new TH1D("ratio", "ratio", 500, 0, 125);
//TH1D *t = new TH1D("top", "top", 100, 0, 150);
//TH1D *b = new TH1D("bottom", "bottom", 100, 0, 150);
TTree *tree1(0);
TFile *input1(0);
TTree *tree2(0);
TFile *input2(0);
if (!gSystem->AccessPathName( rootfile1 )) {
cout << "accessing " << rootfile1 << endl;
input1 = TFile::Open( rootfile1 );
}
else{
cout<< "problem accessing "<<rootfile1<< endl;
exit(0);
}
tree1 = (TTree*)input1->Get(TreeName1);
int nentries = tree1->GetEntries();
cout<<nentries<<" in tree"<<endl;
if (!gSystem->AccessPathName( rootfile2 )) {
cout << "accessing " << rootfile2 << endl;
input2 = TFile::Open( rootfile2 );
}
else{
cout<< "problem accessing "<<rootfile2<< endl;
exit(0);
}
tree2 = (TTree*)input2->Get(TreeName2);
int nentries2 = tree2->GetEntries();
cout<<nentries2<<" in tree"<<endl;
// if(nentries > 1000000) nentries = 1000000;
// if(nentries > 630000) nentries = 630000;
// if(nentries2 > 630000) nentries2 = 630000;
// if(nentries2 > 1000000) nentries2 = 1000000;
float em_e[10];
float em_pt[10];
float em_eta[10];
float em_phi[10];
float em_deteta[10];
tree2->SetBranchAddress("em_pt", em_pt);
tree2->SetBranchAddress("em_e", em_e);
tree2->SetBranchAddress("em_eta", em_eta);
tree2->SetBranchAddress("em_phi", em_phi);
tree2->SetBranchAddress("em_deteta", em_deteta);
// for (int j = 0; j < nentries2; j++){
// tree2->GetEntry(j);
// tree2->GetLeaf("nelec")->GetValue(0) > 0 &&
// if((fabs(em_deteta[0]) < 1.1 ) && (fabs(em_deteta[1]) < 1.1 ) && tree2->GetLeaf("Ptz")->GetValue(0) < 3 && tree2->GetLeaf("Luminosity")->GetValue(0) < 3){
// tcafe->Fill(tree2->GetLeaf("ScalarEt")->GetValue(0));
// tcafesum->Fill(tree2->GetLeaf("ScalarEt")->GetValue(0), tree2->GetLeaf("ScalarEt")->GetValue(0));
// }
// }
float em_e1[10];
float em_pt1[10];
float em_eta1[10];
float em_phi1[10];
float em_deteta1[10];
tree1->SetBranchAddress("em_pt", em_pt1);
tree1->SetBranchAddress("em_e", em_e1);
tree1->SetBranchAddress("em_eta", em_eta1);
tree1->SetBranchAddress("em_phi", em_phi1);
tree1->SetBranchAddress("em_deteta", em_deteta1);
// cout<<"here"<<endl;
// nentries = 100000;
double pmcsmbz = 0;
double pmcsset = 0;
double test = 0;
double test2 = 0;
double test3 = 0;
double wgt = 0;
for (int i = 0; i < nentries; i++){
//Need to do an iterative correction. Start with scale, adjust mb. then apply linear correction, refit. etc. This way approximation that cafe and pmcs set is the same during subtraction is better
//Should be able to do cafe-pmcs and use weight on mb to make mb account for difference in shape (neg weight issue? scale first?)
tree1->GetEntry(i);
if((fabs(em_deteta1[0]) < 1.1 ) && (fabs(em_deteta1[1]) < 1.1 ) && tree1->GetLeaf("Ptz")->GetValue(0) < 3 && tree1->GetLeaf("Luminosity")->GetValue(0) < 20){
// cout<<i<<" of "<<nentries<<endl;
// cout<<tree1->GetLeaf("ScalarEt")->GetValue(0)<<" "<<ratiotest->FindBin(tree1->GetLeaf("ScalarEt")->GetValue(0))<<endl;
// cout<<ratiotest->GetBinContent(ratiotest->FindBin(tree1->GetLeaf("ScalarEt")->GetValue(0)))<<endl;
// cout<<ratiotest->GetBinContent(20)<<endl;
pmcsmbz = tree1->GetLeaf("MBScalarEt")->GetValue(0);
wgt =1;
// if(tree1->GetLeaf("MBScalarEt")->GetValue(0) < 5 ){
// if (tree1->GetLeaf("MBScalarEt")->GetValue(0) < 2.5 && tree1->GetLeaf("MBScalarEt")->GetValue(0) > 0.5) wgt = 2.5/tree1->GetLeaf("MBS//calarEt")->GetValue(0);
// else wgt = tree1->GetLeaf("MBScalarEt")->GetValue(0)/5.0;
// }
if (tree1->GetLeaf("MBScalarEt")->GetValue(0) > 0.5 && tree1->GetLeaf("MBScalarEt")->GetValue(0) < 5 ){
wgt = 1.3;
}
/**
//fixes 30-50 GeV part of curve
if(tree1->GetLeaf("MBScalarEt")->GetValue(0) < 10 ){
wgt =0.5;
}
**/
// wgt = 0;
//}else if (tree1->GetLeaf("MBScalarEt")->GetValue(0) < 1 ){
// wgt = 1 + int(gRandom->Gaus(1,2));
// cout<<gRandom->Gaus(2.5,2)<<endl;
// }
// }else if (tree1->GetLeaf("MBScalarEt")->GetValue(0) < 10 ){
// wgt = 2.0;
// }
// wgt = TMath::Power(pmcsmbz, 0.5/(10/(TMath::Sqrt(pmcsmbz)))) ;
// wgt = TMath::Power(pmcsmbz, 0.5/(10/(TMath::Sqrt(pmcsmbz)))) + 0.2*pmcsmbz;// + pmcsmbz - 0.09*pmcsmbz*pmcsmbz;
// wgt = (TMath::Power(pmcsmbz, 0.5)* (0.5 * (1. + TMath::Erf((pmcsmbz-.5)/(TMath::Sqrt(2)*5))))) / ((1+TMath::Exp(-1*TMath::Power(pmcsmbz/2, 2.0))));
//wgt = int( (TMath::Power(pmcsmbz, 0.4)) );
// wgt = (TMath::Power(pmcsmbz, 0.70 - tree1->GetLeaf("Luminosity")->GetValue(0)/50));
// if(tree1->GetLeaf("MBScalarEt")->GetValue(0) < 15){
// wgt = wgt*2.0/tree1->GetLeaf("MBScalarEt")->GetValue(0);
//}
// / (0.5 * (1. + TMath::Erf((pmcsmbz-.5)/(TMath::Sqrt(2)*1.0))));
// + (0.5 * (1. + TMath::Erf((pmcsmbz-.5)/(TMath::Sqrt(2)*5)))); //0.5 is good for upper region, 0.75 for middle
//cout<<TMath::Power(pmcsmbz, 0.5/(10/(TMath::Sqrt(pmcsmbz))))<<" "<<(0.01 * (1. + TMath::Erf((pmcsmbz-5)/(TMath::Sqrt(2)))))<<endl;
//wgt = 0.582 * (1. + TMath::Erf((pmcsmbz-1.0935)/(TMath::Sqrt(2)*4.3111)));
// wgt = wgt*(0.53 * (1. + TMath::Erf((pmcsmbz-0.288)/(TMath::Sqrt(2)*4.0453))));
// wgt = wgt*(0.489 * (1. + TMath::Erf((pmcsmbz+0.305)/(TMath::Sqrt(2)*4.89144))));
/**
//works ok-ish but doesn't work as weight (obviously...)
pmcsmbz = tree1->GetLeaf("MBScalarEt")->GetValue(0)*1.5;
pmcsmbz = pmcsmbz*(0.544657 * (1. + TMath::Erf((pmcsmbz-0.6808)/(TMath::Sqrt(2)*3.18052))));
pmcsmbz = pmcsmbz*(0.530694 * (1. + TMath::Erf((pmcsmbz-0.726896)/(TMath::Sqrt(2)*1.0429))));
pmcsmbz = pmcsmbz*(0.480563 * (1. + TMath::Erf((pmcsmbz-0.321864)/(TMath::Sqrt(2)*0.536511))));
**/
// pmcsmbz = pmcsmbz*(0.342784 * (1. + TMath::Erf((pmcsmbz-0.167683)/(TMath::Sqrt(2)*0.540161))));
//pmcsmbz = pmcsmbz*(0.9587 + 0.00365*pmcsmbz);
// pmcsmbz = pmcsmbz*(0.903 + 0.007296*pmcsmbz + 0.00003*pmcsmbz*pmcsmbz - 0.00000119*pmcsmbz*pmcsmbz*pmcsmbz);//*ratiotest2->FindBin(tree1->GetLeaf("MBScalarEt")->GetValue(0));
// if (tree1->GetLeaf("ScalarEt")->GetValue(0) > 30) pmcsmbz = tree1->GetLeaf("MBScalarEt")->GetValue(0)*1.0*(0.42 + 0.0071*tree1->GetLeaf("ScalarEt")->GetValue(0));//*ratiotest->GetBinContent(ratiotest->FindBin(tree1->GetLeaf("MBScalarEt")->GetValue(0)));//1.4;
//cout<<pmcsmbz<<endl;
// pmcsmbz = pmcsmbz/(-0.00249 - (0.0003136*pmcsmbz) + (0.00121*pmcsmbz*pmcsmbz));
// if (pmcsmbz < 50) pmcsmbz = pmcsmbz/(-0.00005 - (0.00196*pmcsmbz) + (0.00132*pmcsmbz*pmcsmbz));
//cout<<pmcsmbz<<" new "<<endl;
//below way over corrects... obviously not compensating for "other" correctly...
// if (pmcsmbz > 0) pmcsmbz = pmcsmbz* ((0.5 * (1. + TMath::Erf((pmcsmbz-8.36)/(TMath::Sqrt(2)*4.9)))));
// if (pmcsmbz > 0) pmcsmbz = TMath::Sqrt(pmcsmbz);// * (1/(0.5 * (1. + TMath::Erf((pmcsmbz-8.36)/(sqrt(2)*4.9)))));
// else pmcsmbz = 0;
pmcsset = tree1->GetLeaf("ScalarEt")->GetValue(0) - tree1->GetLeaf("MBScalarEt")->GetValue(0) + pmcsmbz;
tpmcs->Fill(pmcsset, wgt);//minus mb plus mb
tpmcsd->Fill(pmcsset);
tpmcsother->Fill(pmcsset -pmcsmbz, wgt);
tpmcsmb->Fill(pmcsmbz, wgt);
pmcsmb->Fill(pmcsmbz, wgt);
tpmcssum->Fill(pmcsset, pmcsset);//minus mb plus mb
tpmcsothersum->Fill(pmcsset, pmcsset -pmcsmbz);
tpmcsmbsum->Fill(pmcsset, pmcsmbz);
test += pmcsset;
test2 += pmcsset -pmcsmbz;
test3 += pmcsmbz;
//cout<<pmcsset<<" "<<pmcsset -pmcsmbz<<" "<<pmcsmbz<<endl;
b->Fill(pmcsset, pmcsmbz);
t->Fill(pmcsset, pmcsmbz - pmcsset);
mbandset->Fill(pmcsset, pmcsmbz, wgt);
otherandset->Fill(pmcsset, pmcsset -pmcsmbz, wgt);
// tpmcs->Fill(pmcsmbz);
// tpmcs->Fill((pmcsset - pmcsmbz) + (pmcsmbz*(1.25 + 0.005*pmcsmbz- 0.0002*pmcsmbz*pmcsmbz)));
// tpmcs->Fill((pmcsset - pmcsmbz) + (pmcsmbz + (0.052 - 0.019*pmcsmbz+ 0.0018*pmcsmbz*pmcsmbz)));
// tpmcs->Fill((pmcsset - pmcsmbz) + (pmcsmbz * (0.052 - 0.019*pmcsmbz+ 0.0018*pmcsmbz*pmcsmbz)));
// tpmcs->Fill((pmcsset - pmcsmbz) + (1.6*pmcsmbz + (0.2 - 0.065*pmcsmbz+ 0.0023*pmcsmbz*pmcsmbz)));
//uncomment top before using
// tpmcs->Fill((pmcsset - pmcsmbz) + (pmcsmbz * (1.018 + 0.0319*pmcsmbz - 0.00228*pmcsmbz*pmcsmbz + 0.00008*pmcsmbz*pmcsmbz*pmcsmbz)));
}
}
TH1D *tmp;
int binz = 0;
for (int j = 0; j < nentries2; j++){
tree2->GetEntry(j);
// tree2->GetLeaf("nelec")->GetValue(0) > 0 &&
if((fabs(em_deteta[0]) < 1.1 ) && (fabs(em_deteta[1]) < 1.1 ) && tree2->GetLeaf("Ptz")->GetValue(0) < 3 && tree2->GetLeaf("Luminosity")->GetValue(0) < 20){
tcafe->Fill(tree2->GetLeaf("ScalarEt")->GetValue(0));
tcafesum->Fill(tree2->GetLeaf("ScalarEt")->GetValue(0), tree2->GetLeaf("ScalarEt")->GetValue(0));
binz = tpmcs->FindBin(tree2->GetLeaf("ScalarEt")->GetValue(0));
tmp = otherandset->ProjectionY("test"+j, binz, binz, "");
cafemb->Fill(tree2->GetLeaf("ScalarEt")->GetValue(0) - tmp->GetRandom());
}
}
// for(int i = 0; i < nbin+1; i++){
// tmp = otherandset->ProjectionY("test"+i, i, i, "");
// cafemb->Fill(tcafe->GetXaxis()->GetBinCenter(i) - tmp->GetRandom());
// }
TCanvas *c5 = new TCanvas("c5","c5",800,800);
cafemb->Draw();
pmcsmb->Scale(cafemb->Integral()/pmcsmb->Integral());
pmcsmb->SetLineColor(kBlue);
pmcsmb->Draw("same");
c5->Update();
TCanvas *c4 = new TCanvas("c4","c4",800,800);
tpmcsmbsum->Scale(tcafesum->Integral()/tpmcssum->Integral());//orget enertries?
tpmcsothersum->Scale(tcafesum->Integral()/tpmcssum->Integral());
tpmcssum->Scale(tcafesum->Integral()/tpmcssum->Integral());
tpmcssum->Draw();
tcafesum->SetMarkerColor(kRed);
tcafesum->SetLineColor(kRed);
tcafesum->Draw("same");
tpmcsmbsum->SetLineColor(kBlue);
tpmcsmbsum->Draw("same");
tpmcsothersum->SetLineColor(kGreen);
tpmcsothersum->Draw("same");
c4->Update();
c4->Print("PlotSETPTPMCS_RecoVarAfterFITSUM_"+var+".eps");
c4->Print("PlotSETPTPMCS_RecoVarAfterFITSUM_"+var+".gif");
TCanvas *c3 = new TCanvas("c3","c3",800,800);
tpmcsmb->Scale(tcafe->Integral()/tpmcs->Integral());
tpmcsother->Scale(tcafe->Integral()/tpmcs->Integral());
tpmcs->Scale(tcafe->Integral()/tpmcs->Integral());
tpmcsd->Scale(tcafe->Integral()/tpmcsd->Integral());
tpmcs->Draw();
tpmcsd->SetMarkerColor(kMagenta);
tpmcsd->SetLineColor(kMagenta);
tpmcsd->Draw("same");
tcafe->SetMarkerColor(kRed);
tcafe->SetLineColor(kRed);
tcafe->Draw("same");
tpmcsmb->SetLineColor(kBlue);
tpmcsmb->Draw("same");
tpmcsother->SetLineColor(kGreen);
tpmcsother->Draw("same");
cout<<test<<" "<<test2<<" "<<test3<<endl;
cout<<tpmcs->Integral()<<" "<<tpmcsmb->Integral()<<" "<<tpmcsother->Integral()<<endl;
c3->Update();
c3->Print("PlotSETPTPMCS_RecoVarAfterFIT_"+var+".eps");
c3->Print("PlotSETPTPMCS_RecoVarAfterFIT_"+var+".gif");
TCanvas *c1 = new TCanvas("c1","c1",800,800);
t->Scale(tcafe->Integral()/t->Integral());
//Would have to randomly get a SET value from CAFe distribution (treat it like pdf) and subtract the setother from PMCS to get cafemb, event by event.
// for(int i = 0; i < nbin+1; i++){
// t->SetBinContent(i, tcafe->GetXaxis()->GetBinCenter() - t->GetXaxis()->GetBinCenter()
t->Add(tcafe);
b->Scale(t->Integral()/b->Integral());
b->Draw();
t->SetMarkerColor(kRed);
t->SetLineColor(kRed);
t->Draw("same");
c1->Update();
c1->Print("PlotSETPTPMCS_MB_"+var+".eps");
c1->Print("PlotSETPTPMCS_MB_"+var+".gif");
TCanvas *c2 = new TCanvas("c2","c2",800,800);
//for ratio want (cafe(set+mbset) - pmcs(set))/pmcsmb
// ratio->Divide(t, b,1.0,1.0,"B");
// ratio->Divide(tpmcs, tcafe,1.0,1.0,"B");
ratio->Divide(cafemb, pmcsmb,1.0,1.0,"B");
ratio->Draw();
TF1 *func = new TF1("func",fitexp,0,125,3);
// func->SetParameter(0, 0.0);
// func->SetParameter(1, 30000);
// func->SetParameter(2, 300000);
// func->SetParameter(3, 0.1);
func->SetParameter(0, 10.0);
func->SetParameter(1, 10.0);
func->SetParameter(2, 0.6);
// func->SetParameter(2, 0.0015);
// func->SetParameter(3, 0.0015);
// func->FixParameter(0, 11);
// func->FixParameter(1, 40);
// func->FixParameter(2, 250000);
//func->FixParameter(3, 5);
// t->Fit("func", "+R");
// t->Draw();
ratio->Fit("func", "+R");
ratio->Draw();
c2->Update();
c2->Print("PlotSETPTPMCS_Ratio_"+var+".eps");
c2->Print("PlotSETPTPMCS_Ratio_"+var+".gif");
TCanvas *c6 = new TCanvas("c6","c6",800,800);
mbandset->Draw("colz");
c6->Update();
c6->Print("PlotSETPTPMCS_MBandSET_"+var+".eps");
c6->Print("PlotSETPTPMCS_MBandSET_"+var+".gif");
}
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);
}
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");
}
/* draw_scene takes in a n int to determine whether or not to draw objects
* or shadows. 0 will draw objects, 1 will draw shadows. */
void draw_scene(int shade)
{
/* variable for recursion level */
int rec = 0;
GLfloat mat_red[] = {1.0, 0.0, 0.0, 0.0};
GLfloat mat_green[] = {0.0, 1.0, 0.0, 0.0};
GLfloat mat_blue[] = {0.0, 0.0, 1.0, 0.0};
GLfloat mat_black[] = {0.0, 0.0, 0.0, 0.0};
GLfloat mat_brown[] = {0.43, 0.16, 0.08, 0.0};
GLfloat mat_dark_green[] = {0.23, 0.43, 0.21, 0.0};
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_blue);
}
else if (shade == 1) {
glPushMatrix();
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, mat_black);
glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, mat_black);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_black);
glMultMatrixf(m); /* apply shading matrix */
}
glPushMatrix();
glTranslatef(2.0, -2.0, 0);
glRotatef(20, 0, 0, 1);
draw_chair1();
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_red);
}
glPushMatrix();
glTranslatef(-1.75, 0.25, 0);
glRotatef(-20, 0, 0, 1);
draw_chair2();
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_green);
}
glPushMatrix();
glTranslatef(-1.0, -2.0, 0);
glRotatef(40, 0, 0, 1);
draw_chair2();
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_green);
}
glPushMatrix();
glTranslatef(-1.75, -2.0, 0);
glRotatef(-50, 0, 0, 1);
draw_chair1();
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_brown);
}
rec = 7;
glPushMatrix();
glTranslatef(0, -0.5, 0);
tree2(rec, 1.8);
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_dark_green);
}
rec = 6;
glPushMatrix();
glTranslatef(-3.0, 3.0, 0);
tree(rec, 1.1);
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_dark_green);
}
rec = 7;
glPushMatrix();
glTranslatef(-3.0, -3.5, 0);
tree(rec, 1.5);
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_brown);
}
rec = 6;
glPushMatrix();
glTranslatef(3.0, -3.0, 0);
glRotatef(-90, 0, 0, 1);
tree(rec, 1.3);
glPopMatrix();
if (shade == 0) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, mat_dark_green);
}
rec = 6;
glPushMatrix();
glTranslatef(3.0, 3.0, 0);
glRotatef(90, 0, 0, 1);
tree(rec, 1.5);
glPopMatrix();
if (shade == 1) {
glPopMatrix();
}
}
void FSModel::convertPointCloudToSurfaceMesh()
{
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
//sensor_msgs::PointCloud2 cloud_blob;
//pcl::io::loadPCDFile ("bearHigh.pcd", cloud_blob);
//pcl::fromROSMsg (cloud_blob, *cloud);
//* the data should be available in cloud
cloud->points.resize(pointCloud->size());
for (size_t i = 0; i < pointCloud->points.size(); i++) {
cloud->points[i].x = pointCloud->points[i].x;
cloud->points[i].y = pointCloud->points[i].y;
cloud->points[i].z = pointCloud->points[i].z;
}
// 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.setRadiusSearch(15.0);
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;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (15.00);
// 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::io::savePLYFile("mesh.ply", triangles);
}
