8 #ifndef CHISEL_CONVERSIONS_H_
9 #define CHISEL_CONVERSIONS_H_
12 #include <open_chisel/Chisel.h>
13 #include <pcl/PolygonMesh.h>
14 #include <pcl/common/transforms.h>
21 UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC4);
22 std::shared_ptr<chisel::ColorImage<unsigned char> > imageChisel(
new chisel::ColorImage<unsigned char>(image.cols, image.rows, image.channels()));
23 memcpy(imageChisel->GetMutableData(), image.data, image.total()*
sizeof(
unsigned char)*image.channels());
29 UASSERT(image.type() == CV_32FC1);
30 std::shared_ptr<chisel::DepthImage<float> > imageChisel(
new chisel::DepthImage<float>(image.cols, image.rows));
31 memcpy(imageChisel->GetMutableData(), (
float*)image.data, image.total()*
sizeof(
float));
37 chisel::PinholeCamera cameraToReturn;
38 chisel::Intrinsics intrinsics;
39 intrinsics.SetFx(
camera.fx());
40 intrinsics.SetFy(
camera.fy());
41 intrinsics.SetCx(
camera.cx());
42 intrinsics.SetCy(
camera.cy());
43 cameraToReturn.SetIntrinsics(intrinsics);
44 cameraToReturn.SetWidth(
camera.imageWidth());
45 cameraToReturn.SetHeight(
camera.imageHeight());
46 return cameraToReturn;
49 template<
typename Po
intRGBT>
52 chisel::PointCloudPtr chiselCloud(
new chisel::PointCloud());
53 chiselCloud->GetMutablePoints().resize(cloud.size());
54 chiselCloud->GetMutableColors().resize(cloud.size());
55 float byteToFloat = 1.0f / 255.0f;
58 for(
unsigned int i=0;
i<cloud.size(); ++
i)
60 const PointRGBT & pt = cloud.at(
i);
65 chisel::Vec3& xyz = chiselCloud->GetMutablePoints().at(oi);
70 chisel::Vec3& rgb = chiselCloud->GetMutableColors().at(oi);
71 rgb(0) = ptt.r * byteToFloat;
72 rgb(1) = ptt.g * byteToFloat;
73 rgb(2) = ptt.b * byteToFloat;
78 chiselCloud->GetMutablePoints().resize(oi);
79 chiselCloud->GetMutableColors().resize(oi);
83 template<
typename Po
intT>
86 chisel::PointCloudPtr chiselCloud(
new chisel::PointCloud());
87 chiselCloud->GetMutablePoints().resize(cloud.size());
90 for(
unsigned int i=0;
i<cloud.size(); ++
i)
92 const PointT & pt = cloud.at(
i);
97 chisel::Vec3& xyz = chiselCloud->GetMutablePoints().at(oi);
105 chiselCloud->GetMutablePoints().resize(oi);
109 pcl::PolygonMesh::Ptr
chiselToPolygonMesh(
const chisel::MeshMap& meshMap,
unsigned char r=100,
unsigned char g=100,
unsigned char b=100)
111 pcl::PolygonMesh::Ptr mesh (
new pcl::PolygonMesh);
115 bool hasColor = meshMap.begin()->second->colors.size();
116 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
118 for (
const std::pair<chisel::ChunkID, chisel::MeshPtr>& it : meshMap)
120 UASSERT((!hasColor || (it.second->vertices.size() == it.second->colors.size())) &&
121 it.second->vertices.size() == it.second->normals.size());
122 cloud->resize(cloud->size() + it.second->vertices.size());
124 mesh->polygons.resize(mesh->polygons.size()+it.second->vertices.size()/3);
126 for (
unsigned int i=0;
i<it.second->vertices.size(); ++
i)
128 pcl::PointXYZRGBNormal & pt = cloud->at(
v);
129 pt.x = it.second->vertices[
i][0];
130 pt.y = it.second->vertices[
i][1];
131 pt.z = it.second->vertices[
i][2];
134 pt.r = it.second->colors[
i][0] * 255.0f;
135 pt.g = it.second->colors[
i][1] * 255.0f;
136 pt.b = it.second->colors[
i][2] * 255.0f;
144 pt.normal_x = it.second->normals[
i][0];
145 pt.normal_y = it.second->normals[
i][1];
146 pt.normal_z = it.second->normals[
i][2];
147 pcl::Vertices & polygon = mesh->polygons.at(
v/3);
148 polygon.vertices.push_back(
v++);
151 pcl::toPCLPointCloud2(*cloud, mesh->cloud);