18 #include <Eigen/Dense>
24 template <
class POINTMAP>
26 const int H,
const int W,
const float* kxs,
const float* kys,
29 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
32 template <
class POINTMAP>
34 const int H,
const int W,
const float* kxs,
const float* kys,
37 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
40 template <
typename POINTMAP>
46 const int DECIM,
const bool use_rotated_LUT)
48 const size_t WH = W * H;
52 const auto& Kxs = use_rotated_LUT ? lut.
Kxs_rot : lut.Kxs;
53 const auto& Kys = use_rotated_LUT ? lut.Kys_rot : lut.Kys;
54 const auto& Kzs = use_rotated_LUT ? lut.Kzs_rot : lut.Kzs;
59 const float* kxs = &Kxs[0];
60 const float* kys = &Kys[0];
61 const float* kzs = &Kzs[0];
80 if ((W & 0x07) == 0 && pp.
USE_SSE2 && DECIM == 1 &&
83 H, W, kxs, kys, kzs, *ri, src_obs.
rangeUnits, pca,
89 H, W, kxs, kys, kzs, *ri, src_obs.
rangeUnits, pca,
96 const size_t nPts = pca.size();
100 for (
size_t i = 0; i < nPts; i++)
103 pca.getPointXYZ(i, pt.
x, pt.
y, pt.
z);
105 pca.setPointXYZ(i, pt.
x, pt.
y, pt.
z);
110 template <
class POINTMAP>
125 if (!pp.
layer.empty())
136 const bool use_rotated_LUT =
150 const size_t WH = W * H;
168 (W % DECIM) == 0 && (H % DECIM == 0),
169 "Width/Height are not an exact multiple of decimation");
170 const int Wd = W / DECIM;
171 const int Hd = H / DECIM;
173 const size_t WHd = Wd * Hd;
179 range2XYZ_LUT<POINTMAP>(pca, src_obs, pp, fp, H, W, DECIM, use_rotated_LUT);
184 if constexpr (pca.HAS_RGB)
199 const bool isDirectCorresp =
209 if (!isDirectCorresp)
228 const size_t nPts = pca.size();
230 const uint8_t* img_data = iimg.
ptrLine<uint8_t>(0);
231 const auto img_stride = iimg.getRowStride();
232 for (
size_t i = 0; i < nPts; i++)
237 bool pointWithinImage =
false;
240 pointWithinImage =
true;
250 i, pt_wrt_depth[0], pt_wrt_depth[1], pt_wrt_depth[2]);
251 pt_wrt_color = T_inv * pt_wrt_depth;
257 cx + fx * pt_wrt_color[0] / pt_wrt_color[2]);
259 cy + fy * pt_wrt_color[1] / pt_wrt_color[2]);
260 pointWithinImage = img_idx_x >= 0 && img_idx_x < imgW &&
261 img_idx_y >= 0 && img_idx_y < imgH;
265 if (pointWithinImage)
267 if (hasColorIntensityImg)
270 img_stride * img_idx_y + 3 * img_idx_x;
271 pCol.
R = img_data[px_idx + 2];
272 pCol.
G = img_data[px_idx + 1];
273 pCol.
B = img_data[px_idx + 0];
277 const auto px_idx = img_stride * img_idx_y + img_idx_x;
278 pCol.
R = pCol.
G = pCol.
B = img_data[px_idx];
283 pCol.
R = pCol.
G = pCol.
B = 255;
286 pca.setPointRGBu8(i, pCol.
R, pCol.
G, pCol.
B);
295 if (!use_rotated_LUT &&
314 const size_t nPts = pca.size();
315 for (
size_t i = 0; i < nPts; i++)
317 pca.getPointXYZ(i, pt[0], pt[1], pt[2]);
319 pca.setPointXYZ(i, pt_transf[0], pt_transf[1], pt_transf[2]);
325 template <
class POINTMAP>
327 const int H,
const int W,
const float* kxs,
const float* kys,
330 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
339 for (
int r = 0; r < H; r++)
340 for (
int c = 0; c < W; c++)
342 const float D = rangeImage.
coeff(r, c) * rangeUnits;
344 const auto kx = *kxs++, ky = *kys++, kz = *kzs++;
347 if (MAKE_ORGANIZED) pca.setInvalidPoint(idx++);
351 pca.setPointXYZ(idx, kx * D, ky * D , kz * D );
359 const int Hd = H / DECIM, Wd = W / DECIM;
361 for (
int rd = 0; rd < Hd; rd++)
362 for (
int cd = 0; cd < Wd; cd++)
364 bool valid_pt =
false;
365 float min_d = std::numeric_limits<float>::max();
366 for (
int rb = 0; rb < DECIM; rb++)
367 for (
int cb = 0; cb < DECIM; cb++)
369 const auto r = rd * DECIM + rb, c = cd * DECIM + cb;
370 const float D = rangeImage.
coeff(r, c) * rangeUnits;
374 if (D < min_d) min_d = D;
384 if (MAKE_ORGANIZED) pca.setInvalidPoint(idx++);
387 const auto eq_r = rd * DECIM + DECIM / 2,
388 eq_c = cd * DECIM + DECIM / 2;
389 const auto eq_idx = eq_c + eq_r * W;
390 const auto kx = kxs[eq_idx], ky = kys[eq_idx], kz = kzs[eq_idx];
392 idx, kx * min_d , ky * min_d , kz * min_d );
406 template <
class POINTMAP>
408 const int H,
const int W,
const float* kxs,
const float* kys,
411 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
417 const int W_4 = W >> 2;
419 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
float xs[4], ys[4], zs[4];
420 const __m128 D_zeros = _mm_set_ps(.0f, .0f, .0f, .0f);
421 const __m128 xormask =
427 for (
int r = 0; r < H; r++)
429 const uint16_t* Du16_ptr = &rangeImage(r, 0);
430 const float* Dgt_ptr =
432 const float* Dlt_ptr =
435 for (
int c = 0; c < W_4; c++)
439 alignas(16)
float tmp[4];
440 for (
int b = 0; b < 4; b++) tmp[b] = Du16_ptr[b] * rangeUnits;
442 const __m128 D = _mm_load_ps(&tmp[0]);
443 const __m128 nz_mask = _mm_cmpgt_ps(D, D_zeros);
444 __m128 valid_range_mask;
447 valid_range_mask = nz_mask;
455 const __m128 Dmin = _mm_load_ps(Dgt_ptr);
456 valid_range_mask = _mm_or_ps(
457 _mm_cmpgt_ps(D, Dmin), _mm_cmpeq_ps(Dmin, D_zeros));
461 const __m128 Dmax = _mm_load_ps(Dlt_ptr);
462 valid_range_mask = _mm_or_ps(
463 _mm_cmplt_ps(D, Dmax), _mm_cmpeq_ps(Dmax, D_zeros));
465 valid_range_mask = _mm_and_ps(
466 valid_range_mask, nz_mask);
472 const __m128 Dmin = _mm_load_ps(Dgt_ptr);
473 const __m128 Dmax = _mm_load_ps(Dlt_ptr);
475 const __m128 gt_mask = _mm_or_ps(
476 _mm_cmpgt_ps(D, Dmin), _mm_cmpeq_ps(Dmin, D_zeros));
477 const __m128 lt_mask = _mm_or_ps(
478 _mm_cmplt_ps(D, Dmax), _mm_cmpeq_ps(Dmax, D_zeros));
481 _mm_and_ps(nz_mask, _mm_and_ps(gt_mask, lt_mask));
482 valid_range_mask = _mm_xor_ps(valid_range_mask, xormask);
484 valid_range_mask = _mm_or_ps(
485 valid_range_mask, _mm_and_ps(
486 _mm_cmpeq_ps(Dmin, D_zeros),
487 _mm_cmpeq_ps(Dmax, D_zeros)));
489 valid_range_mask = _mm_and_ps(valid_range_mask, nz_mask);
492 const int valid_range_maski = _mm_movemask_epi8(
493 _mm_castps_si128(valid_range_mask));
494 if (valid_range_maski != 0)
496 const __m128 KX = _mm_load_ps(kxs);
497 const __m128 KY = _mm_load_ps(kys);
498 const __m128 KZ = _mm_load_ps(kzs);
500 _mm_storeu_ps(xs, _mm_mul_ps(KX, D));
501 _mm_storeu_ps(ys, _mm_mul_ps(KY, D));
502 _mm_storeu_ps(zs, _mm_mul_ps(KZ, D));
504 for (
int q = 0; q < 4; q++)
506 const int actual_c = (c << 2) + q;
507 if ((valid_range_maski & (1 << (q * 4))) != 0)
509 pca.setPointXYZ(idx, xs[q], ys[q], zs[q]);
510 idxs_x[idx] = actual_c;
518 pca.setInvalidPoint(idx);
522 rangeImage.
coeffRef(r, actual_c) = 0;
526 else if (MAKE_ORGANIZED)
528 for (
int q = 0; q < 4; q++)
530 pca.setInvalidPoint(idx);
532 const int actual_c = (c << 2) + q;
534 rangeImage.
coeffRef(r, actual_c) = 0;
538 if (Dgt_ptr) Dgt_ptr += 4;
539 if (Dlt_ptr) Dlt_ptr += 4;