UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
ply.cpp
1// UFO
2#include <ufo/io/file_handler.hpp>
3#include <ufo/io/ply.hpp>
4
5// STL
6#include <filesystem>
7#include <format>
8#include <print>
9#include <stdexcept>
10
11// RPLY
12#include <rply/rply.h>
13
14namespace ufo
15{
16namespace detail
17{
18int pointCallback(p_ply_argument argument)
19{
20 std::vector<Vec3f>* points;
21 long index;
22 ply_get_argument_user_data(argument, reinterpret_cast<void**>(&points), &index);
23
24 auto value = static_cast<float>(ply_get_argument_value(argument));
25 if (0 == index) {
26 points->emplace_back(value, 0.0f, 0.0f);
27 } else {
28 points->back()[index] = value;
29 }
30
31 return 1;
32}
33
34template <class Color>
35int colorCallback(p_ply_argument argument)
36{
37 std::vector<Color>* colors;
38 long index;
39 ply_get_argument_user_data(argument, reinterpret_cast<void**>(&colors), &index);
40
41 auto value = static_cast<std::uint8_t>(ply_get_argument_value(argument));
42 if (0 == index) {
43 colors->emplace_back(value, 0u, 0u);
44 } else if (1 == index) {
45 colors->back().green = value;
46 } else if (2 == index) {
47 colors->back().blue = value;
48 } else if (3 == index) {
49 if constexpr (has_alpha_v<Color>) {
50 colors->back().alpha = value;
51 }
52 }
53
54 return 1;
55}
56
57int normalCallback(p_ply_argument argument)
58{
59 std::vector<Normal>* normals;
60 long index;
61 ply_get_argument_user_data(argument, reinterpret_cast<void**>(&normals), &index);
62
63 auto value = static_cast<float>(ply_get_argument_value(argument));
64 if (0 == index) {
65 normals->emplace_back(value, 0.0f, 0.0f);
66 } else if (1 == index) {
67 normals->back().y = value;
68 } else if (2 == index) {
69 normals->back().z = value;
70 }
71
72 return 1;
73}
74
75int intensityCallback(p_ply_argument argument)
76{
77 std::vector<Intensity>* intensities;
78 long index;
79 ply_get_argument_user_data(argument, reinterpret_cast<void**>(&intensities), &index);
80
81 auto value = static_cast<float>(ply_get_argument_value(argument));
82 intensities->emplace_back(value);
83
84 return 1;
85}
86
87bool readPLY(std::filesystem::path const& file, std::vector<Vec3f>& points,
88 std::vector<SmallRGB>* colors, std::vector<SmallRGBA>* colors_with_alpha,
89 std::vector<Normal>* normals, std::vector<Intensity>* intensities)
90{
91 p_ply fp = ply_open(file.c_str(), nullptr, 0, nullptr);
92
93 if (!fp) {
94 std::println(stderr, "[UFO | Read PLY] Failed to open file: {}", file.string());
95 return false;
96 }
97
98 if (!ply_read_header(fp)) {
99 std::println(stderr, "[UFO | Read PLY] Failed to read header: {}", file.string());
100 ply_close(fp);
101 return false;
102 }
103
104 long num_points = 0;
105 long num_colors = 0;
106 long num_normals = 0;
107 long num_intensities = 0;
108
109 num_points = ply_set_read_cb(fp, "vertex", "x", pointCallback, &points, 0);
110 ply_set_read_cb(fp, "vertex", "y", pointCallback, &points, 1);
111 ply_set_read_cb(fp, "vertex", "z", pointCallback, &points, 2);
112
113 if (nullptr != colors) {
114 num_colors = ply_set_read_cb(fp, "vertex", "red", colorCallback<SmallRGB>, colors, 0);
115 ply_set_read_cb(fp, "vertex", "green", colorCallback<SmallRGB>, colors, 1);
116 ply_set_read_cb(fp, "vertex", "blue", colorCallback<SmallRGB>, colors, 2);
117 } else if (nullptr != colors_with_alpha) {
118 num_colors = ply_set_read_cb(fp, "vertex", "red", colorCallback<SmallRGBA>,
119 colors_with_alpha, 0);
120 ply_set_read_cb(fp, "vertex", "green", colorCallback<SmallRGBA>, colors_with_alpha,
121 1);
122 ply_set_read_cb(fp, "vertex", "blue", colorCallback<SmallRGBA>, colors_with_alpha, 2);
123 ply_set_read_cb(fp, "vertex", "alpha", colorCallback<SmallRGBA>, colors_with_alpha,
124 2);
125 }
126
127 if (nullptr != normals) {
128 num_normals = ply_set_read_cb(fp, "vertex", "nx", normalCallback, normals, 0);
129 ply_set_read_cb(fp, "vertex", "ny", normalCallback, normals, 1);
130 ply_set_read_cb(fp, "vertex", "nz", normalCallback, normals, 2);
131 }
132
133 if (nullptr != intensities) {
134 num_intensities =
135 ply_set_read_cb(fp, "vertex", "intensity", intensityCallback, intensities, 0);
136 }
137
138 if (0 >= num_points) {
139 std::println(stderr, "[UFO | Read PLY] Number of vertices <= 0");
140 ply_close(fp);
141 return false;
142 }
143
144 long total = std::max({num_points, num_colors, num_normals, num_intensities});
145
146 points.reserve(total);
147 if (nullptr != colors) {
148 colors->reserve(total);
149 } else if (nullptr != colors_with_alpha) {
150 colors_with_alpha->reserve(total);
151 }
152 if (nullptr != normals) {
153 normals->reserve(total);
154 }
155 if (nullptr != intensities) {
156 intensities->reserve(total);
157 }
158
159 if (!ply_read(fp)) {
160 std::println(stderr, "[UFO | Read PLY] Failed to read file: {}", file.string());
161 ply_close(fp);
162 return false;
163 }
164
165 auto actual_size =
166 std::max({points.size(), nullptr != colors ? colors->size() : 0,
167 nullptr != colors_with_alpha ? colors_with_alpha->size() : 0,
168 nullptr != normals ? normals->size() : 0,
169 nullptr != intensities ? intensities->size() : 0});
170
171 if (actual_size != static_cast<std::size_t>(num_points)) {
172 points.clear();
173 if (nullptr != colors) {
174 colors->clear();
175 } else if (nullptr != colors_with_alpha) {
176 colors_with_alpha->clear();
177 }
178 if (nullptr != normals) {
179 normals->clear();
180 }
181 if (nullptr != intensities) {
182 intensities->clear();
183 }
184 std::println(stderr,
185 "[UFO | Read PLY] Mismatch in number of points: expected {}, got {}",
186 num_points, actual_size);
187 ply_close(fp);
188 return false;
189 }
190
191 points.resize(total);
192 if (nullptr != colors) {
193 colors->resize(total);
194 } else if (nullptr != colors_with_alpha) {
195 colors_with_alpha->resize(total);
196 }
197 if (nullptr != normals) {
198 normals->resize(total);
199 }
200 if (nullptr != intensities) {
201 intensities->resize(total);
202 }
203
204 ply_close(fp);
205
206 return true;
207}
208
209bool writePLY(std::filesystem::path const& file, std::span<Vec3f const> points,
210 std::span<SmallRGB const> colors,
211 std::span<SmallRGBA const> colors_with_alpha,
212 std::span<Normal const> normals, std::span<Intensity const> intensities,
213 bool ascii)
214{
215 p_ply fp = ply_create(file.c_str(), ascii ? PLY_ASCII : PLY_LITTLE_ENDIAN, nullptr, 0,
216 nullptr);
217
218 if (!fp) {
219 std::println(stderr, "[UFO | Write PLY] Failed to create file: {}", file.string());
220 return false;
221 }
222
223 ply_add_comment(fp, "Created by UFO");
224
225 ply_add_element(fp, "vertex", static_cast<long>(points.size()));
226 ply_add_property(fp, "x", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
227 ply_add_property(fp, "y", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
228 ply_add_property(fp, "z", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
229
230 if (!colors.empty()) {
231 ply_add_property(fp, "red", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
232 ply_add_property(fp, "green", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
233 ply_add_property(fp, "blue", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
234 } else if (!colors_with_alpha.empty()) {
235 ply_add_property(fp, "red", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
236 ply_add_property(fp, "green", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
237 ply_add_property(fp, "blue", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
238 ply_add_property(fp, "alpha", PLY_UCHAR, PLY_UCHAR, PLY_UCHAR);
239 }
240
241 if (!normals.empty()) {
242 ply_add_property(fp, "nx", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
243 ply_add_property(fp, "ny", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
244 ply_add_property(fp, "nz", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
245 }
246
247 if (!intensities.empty()) {
248 ply_add_property(fp, "intensity", PLY_FLOAT32, PLY_FLOAT32, PLY_FLOAT32);
249 }
250
251 if (!ply_write_header(fp)) {
252 std::println(stderr, "[UFO | Write PLY] Failed to write header");
253 ply_close(fp);
254 return false;
255 }
256
257 for (std::size_t i{}; points.size() > i; ++i) {
258 ply_write(fp, points[i].x);
259 ply_write(fp, points[i].y);
260 ply_write(fp, points[i].z);
261
262 if (!colors.empty()) {
263 ply_write(fp, static_cast<unsigned char>(colors[i].red));
264 ply_write(fp, static_cast<unsigned char>(colors[i].green));
265 ply_write(fp, static_cast<unsigned char>(colors[i].blue));
266 } else if (!colors_with_alpha.empty()) {
267 ply_write(fp, static_cast<unsigned char>(colors_with_alpha[i].red));
268 ply_write(fp, static_cast<unsigned char>(colors_with_alpha[i].green));
269 ply_write(fp, static_cast<unsigned char>(colors_with_alpha[i].blue));
270 ply_write(fp, static_cast<unsigned char>(colors_with_alpha[i].alpha));
271 }
272
273 if (!normals.empty()) {
274 ply_write(fp, static_cast<float>(normals[i].x));
275 ply_write(fp, static_cast<float>(normals[i].y));
276 ply_write(fp, static_cast<float>(normals[i].z));
277 }
278
279 if (!intensities.empty()) {
280 ply_write(fp, static_cast<float>(intensities[i].intensity));
281 }
282 }
283
284 ply_close(fp);
285
286 return true;
287}
288} // namespace detail
289
290CloudProperties cloudPropertiesPLY(std::filesystem::path const& file)
291{
292 p_ply fp = ply_open(file.c_str(), nullptr, 0, nullptr);
293
294 if (!fp) {
295 throw std::runtime_error(std::format(
296 "[UFO | Cloud Properties PLY] Failed to open file: {}", file.string()));
297 }
298
299 if (!ply_read_header(fp)) {
300 ply_close(fp);
301 throw std::runtime_error(
302 std::format("[UFO | Cloud Properties PLY] Failed to read header"));
303 }
304
305 CloudProperties prop;
306
307 prop.color = 0 < ply_set_read_cb(fp, "vertex", "red", nullptr, nullptr, 0);
308 prop.alpha = 0 < ply_set_read_cb(fp, "vertex", "alpha", nullptr, nullptr, 0);
309 prop.normal = 0 < ply_set_read_cb(fp, "vertex", "nx", nullptr, nullptr, 0);
310 prop.intensity = 0 < ply_set_read_cb(fp, "vertex", "intensity", nullptr, nullptr, 0);
311
312 ply_close(fp);
313
314 return prop;
315}
316} // namespace ufo
ufo
All vision-related classes and functions.
Definition cloud.hpp:49
ufo::green
constexpr T green(Lrgb< T, Flags > color) noexcept
Returns the un-weighted green channel value.
Definition lrgb.hpp:309
ufo::blue
constexpr T blue(Lrgb< T, Flags > color) noexcept
Returns the un-weighted blue channel value.
Definition lrgb.hpp:325
ufo::alpha
constexpr alpha_type_t< C > alpha(C color) noexcept
Returns the un-weighted alpha of color.
Definition alpha.hpp:105
ufo::red
constexpr T red(Lrgb< T, Flags > color) noexcept
Returns the un-weighted red channel value.
Definition lrgb.hpp:293