Select Git revision
tetrahedronGenerator.hpp
tetrahedronGenerator.hpp 2.70 KiB
#ifndef TETRAHEDRONGENERATOR_HPP
#define TETRAHEDRONGENERATOR_HPP
#include "pcl/common/distances.h"
#include <fstream>
#include <pcl/impl/point_types.hpp>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
inline std::vector<pcl::PointXYZ>
calculateTetrahedronVertices(float trisize, float x, float y, float z) {
// float trisize = 0.06;
float z_trisize = trisize + 0.04;
float pi = 3.14;
pcl::PointXYZ A(x, y, z + 2 * trisize / 3);
pcl::PointXYZ B(x + trisize * cos(0.0 / 3 * 2 * pi),
y + trisize * sin(0.0 / 3 * 2 * pi), z - z_trisize / 3);
pcl::PointXYZ C(x + trisize * cos(1.0 / 3 * 2 * pi),
y + trisize * sin(1.0 / 3 * 2 * pi), z - z_trisize / 3);
pcl::PointXYZ D(x + trisize * cos(2.0 / 3 * 2 * pi),
y + trisize * sin(2.0 / 3 * 2 * pi), z - z_trisize / 3);
std::vector<pcl::PointXYZ> ret;
ret.emplace_back(A);
ret.emplace_back(B);
ret.emplace_back(C);
ret.emplace_back(D);
return ret;
}
inline void createTetrahedronModel(std::string plyFile) {
// load pcl cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::io::loadPLYFile("../pointClouds/" + plyFile, *cloud);
// create and open output obj file
std::fstream fout;
fout.open("../pointClouds/tetrahedron_" +
plyFile.substr(0, plyFile.length() - 4) + ".obj",
std::ios::out);
if (!fout) {
std::cout << "file cannot be opened" << std::endl;
return;
}
// set trisize according to distance of the points
float dist = pcl::euclideanDistance(cloud->points[0], cloud->points[1]);
dist *= 0.2f;
int i = 1;
int vn_i = 1;
for (pcl::PointXYZRGB &pt : cloud->points) {
std::vector<pcl::PointXYZ> vertex =
calculateTetrahedronVertices(dist, pt.x, pt.y, pt.z);
// write the vertices into the obj file
for (pcl::PointXYZ &vpt : vertex) {
fout << "v " << vpt.x << " " << vpt.y << " " << vpt.z << " "
<< pt.r / 255.0 << " " << pt.g / 255.0 << " " << pt.b / 255.0
<< std::endl;
}
// write the original point coordinate as the vertex normal
fout << "vn " << pt.x << " " << pt.y << " " << pt.z << std::endl;
// write the faces
std::string vn = "//" + std::to_string(vn_i);
fout << "f " << i << vn << " " << i + 1 << vn << " " << i + 2 << vn
<< std::endl;
fout << "f " << i << vn << " " << i + 2 << vn << " " << i + 3 << vn
<< std::endl;
fout << "f " << i << vn << " " << i + 3 << vn << " " << i + 1 << vn
<< std::endl;
fout << "f " << i + 1 << vn << " " << i + 3 << vn << " " << i + 2 << vn
<< std::endl;
i += 4;
vn_i++;
}
fout.close();
}
#endif // !TETRAHEDRONGENERATOR_HPP