libvisiontransfer  10.8.0
reconstruct3d.cpp
1 /*******************************************************************************
2  * Copyright (c) 2024 Allied Vision Technologies GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #include "reconstruct3d.h"
16 #include "visiontransfer/internal/alignedallocator.h"
17 #include <vector>
18 #include <cstring>
19 #include <algorithm>
20 #include <fstream>
21 #include <stdexcept>
22 #include <cmath>
23 #include <limits>
24 
25 // SIMD Headers
26 #ifdef __AVX2__
27 #include <immintrin.h>
28 #elif __SSE2__
29 #include <emmintrin.h>
30 #elif __aarch64__
31 #include <arm_neon.h>
32 #endif
33 
34 #include <iostream>
35 
36 using namespace std;
37 using namespace visiontransfer;
38 using namespace visiontransfer::internal;
39 
40 namespace visiontransfer {
41 
42 /*************** Pimpl class containing all private members ***********/
43 
44 class Reconstruct3D::Pimpl {
45 public:
46  Pimpl();
47 
48  float* createPointMap(const unsigned short* dispMap, int width, int height,
49  int rowStride, const float* q, unsigned short minDisparity, int subpixelFactor,
50  unsigned short maxDisparity);
51 
52  float* createPointMap(const ImageSet& imageSet, unsigned short minDisparity, unsigned short maxDisparity = 0xFFF);
53 
54  float* createZMap(const ImageSet& imageSet, unsigned short minDisparity, unsigned short maxDisparity);
55 
56  void projectSinglePoint(int imageX, int imageY, unsigned short disparity, const float* q,
57  float& pointX, float& pointY, float& pointZ, int subpixelFactor);
58 
59  void writePlyFile(const char* file, const unsigned short* dispMap,
60  const unsigned char* image, int width, int height, ImageSet::ImageFormat format,
61  int dispRowStride, int imageRowStride, const float* q,
62  double maxZ, bool binary, int subpixelFactor, unsigned short maxDisparity);
63 
64  void writePlyFile(const char* file, const ImageSet& imageSet,
65  double maxZ, bool binary, ColorSource colSource, unsigned short maxDisparity);
66 
67 private:
68  std::vector<float, AlignedAllocator<float> > pointMap;
69 
70  float* createPointMapFallback(const unsigned short* dispMap, int width, int height,
71  int rowStride, const float* q, unsigned short minDisparity, int subpixelFactor,
72  unsigned short maxDisparity);
73 
74  float* createPointMapSSE2(const unsigned short* dispMap, int width, int height,
75  int rowStride, const float* q, unsigned short minDisparity, int subpixelFactor,
76  unsigned short maxDisparity);
77 
78  float* createPointMapAVX2(const unsigned short* dispMap, int width, int height,
79  int rowStride, const float* q, unsigned short minDisparity, int subpixelFactor,
80  unsigned short maxDisparity);
81 
82  float* createPointMapNEON(const unsigned short* dispMap, int width, int height,
83  int rowStride, const float* q, unsigned short minDisparity, int subpixelFactor,
84  unsigned short maxDisparity);
85 };
86 
87 /******************** Stubs for all public members ********************/
88 
89 Reconstruct3D::Reconstruct3D()
90  :pimpl(new Pimpl) {
91 }
92 
93 Reconstruct3D::~Reconstruct3D() {
94  delete pimpl;
95 }
96 
97 float* Reconstruct3D::createPointMap(const unsigned short* dispMap, int width, int height,
98  int rowStride, const float* q, unsigned short minDisparity, int subpixelFactor,
99  unsigned short maxDisparity) {
100  return pimpl->createPointMap(dispMap, width, height, rowStride, q, minDisparity,
101  subpixelFactor, maxDisparity);
102 }
103 
104 float* Reconstruct3D::createPointMap(const ImageSet& imageSet, unsigned short minDisparity, unsigned short maxDisparity) {
105  return pimpl->createPointMap(imageSet, minDisparity, maxDisparity);
106 }
107 
108 float* Reconstruct3D::createZMap(const ImageSet& imageSet, unsigned short minDisparity,
109  unsigned short maxDisparity) {
110  return pimpl->createZMap(imageSet, minDisparity, maxDisparity);
111 }
112 
113 void Reconstruct3D::projectSinglePoint(int imageX, int imageY, unsigned short disparity,
114  const float* q, float& pointX, float& pointY, float& pointZ, int subpixelFactor) {
115  pimpl->projectSinglePoint(imageX, imageY, disparity, q, pointX, pointY, pointZ,
116  subpixelFactor);
117 }
118 
119 void Reconstruct3D::writePlyFile(const char* file, const unsigned short* dispMap,
120  const unsigned char* image, int width, int height, ImageSet::ImageFormat format, int dispRowStride,
121  int imageRowStride, const float* q, double maxZ, bool binary, int subpixelFactor,
122  unsigned short maxDisparity) {
123  pimpl->writePlyFile(file, dispMap, image, width, height, format, dispRowStride,
124  imageRowStride, q, maxZ, binary, subpixelFactor, maxDisparity);
125 }
126 
127 void Reconstruct3D::writePlyFile(const char* file, const ImageSet& imageSet,
128  double maxZ, bool binary, ColorSource colSource, unsigned short maxDisparity) {
129  pimpl->writePlyFile(file, imageSet, maxZ, binary, colSource, maxDisparity);
130 }
131 
132 /******************** Implementation in pimpl class *******************/
133 
134 Reconstruct3D::Pimpl::Pimpl() {
135 }
136 
137 float* Reconstruct3D::Pimpl::createPointMap(const unsigned short* dispMap, int width,
138  int height, int rowStride, const float* q, unsigned short minDisparity,
139  int subpixelFactor, unsigned short maxDisparity) {
140 
141  // Allocate the buffer
142  if(pointMap.size() < static_cast<unsigned int>(4*width*height)) {
143  pointMap.resize(4*width*height);
144  }
145 
146  // +inf mapping of invalid points only works for fallback implementation
147  // in case of angled cameras.
148  bool angledCameraFallback = (q[15] != 0.0 && minDisparity == 0);
149  (void)angledCameraFallback; // Suppresses unused variable warning
150 
151 # ifdef __AVX2__
152  if(!angledCameraFallback && maxDisparity <= 0x1000 && width % 16 == 0 && (uintptr_t)dispMap % 32 == 0) {
153  return createPointMapAVX2(dispMap, width, height, rowStride, q,
154  minDisparity, subpixelFactor, maxDisparity);
155  } else
156 # endif
157 # ifdef __SSE2__
158  if(!angledCameraFallback && maxDisparity <= 0x1000 && width % 8 == 0 && (uintptr_t)dispMap % 16 == 0) {
159  return createPointMapSSE2(dispMap, width, height, rowStride, q,
160  minDisparity, subpixelFactor, maxDisparity);
161  } else
162 # endif
163 # ifdef __aarch64__
164  if(!angledCameraFallback && maxDisparity <= 0x1000 && width % 8 == 0) {
165  return createPointMapNEON(dispMap, width, height, rowStride, q,
166  minDisparity, subpixelFactor, maxDisparity);
167  } else
168 # endif
169  {
170  return createPointMapFallback(dispMap, width, height, rowStride, q,
171  minDisparity, subpixelFactor, maxDisparity);
172  }
173 }
174 
175 float* Reconstruct3D::Pimpl::createPointMap(const ImageSet& imageSet, unsigned short minDisparity, unsigned short maxDisparity) {
176  if(!imageSet.hasImageType(ImageSet::IMAGE_DISPARITY)) {
177  throw std::runtime_error("ImageSet does not contain a disparity map!");
178  }
179 
180  if(imageSet.getPixelFormat(ImageSet::IMAGE_DISPARITY) != ImageSet::FORMAT_12_BIT_MONO) {
181  throw std::runtime_error("Disparity map must have 12-bit pixel format!");
182  }
183 
184  return createPointMap(reinterpret_cast<unsigned short*>(imageSet.getPixelData(ImageSet::IMAGE_DISPARITY)), imageSet.getWidth(),
185  imageSet.getHeight(), imageSet.getRowStride(ImageSet::IMAGE_DISPARITY), imageSet.getQMatrix(), minDisparity,
186  imageSet.getSubpixelFactor(), maxDisparity);
187 }
188 
189 float* Reconstruct3D::Pimpl::createPointMapFallback(const unsigned short* dispMap, int width,
190  int height, int rowStride, const float* q, unsigned short minDisparity,
191  int subpixelFactor, unsigned short maxDisparity) {
192  // Code without SSE or AVX optimization
193  float* outputPtr = &pointMap[0];
194  int stride = rowStride / 2;
195 
196  double dInvalid;
197  if(minDisparity == 0) {
198  // Manually force invalid points to +inf, so that this works even
199  // for for angled cameras
200  dInvalid = std::numeric_limits<double>::infinity();
201  } else {
202  // Set invalid disparity to minimum disparity
203  dInvalid = double(minDisparity) / double(subpixelFactor);
204  }
205 
206  for(int y = 0; y < height; y++) {
207  double qx = q[1]*y + q[3];
208  double qy = q[5]*y + q[7];
209  double qz = q[9]*y + q[11];
210  double qw = q[13]*y + q[15];
211 
212  const unsigned short* dispRow = &dispMap[y*stride];
213  for(int x = 0; x < width; x++) {
214  unsigned short intDisp = std::max(minDisparity, dispRow[x]);
215  double d;
216 
217  if(intDisp >= maxDisparity) {
218  d = dInvalid;
219  } else {
220  d = double(intDisp) / double(subpixelFactor);
221  }
222 
223  double w = qw + q[14]*d;
224 
225  *outputPtr = static_cast<float>((qx + q[2]*d)/w); // x
226  outputPtr++;
227 
228  *outputPtr = static_cast<float>((qy + q[6]*d)/w); // y
229  outputPtr++;
230 
231  *outputPtr = static_cast<float>((qz + q[10]*d)/w); // z
232  outputPtr+=2; // Consider padding
233 
234  qx += q[0];
235  qy += q[4];
236  qz += q[8];
237  qw += q[12];
238  }
239  }
240  return &pointMap[0];
241 }
242 
243 float* Reconstruct3D::Pimpl::createZMap(const ImageSet& imageSet, unsigned short minDisparity,
244  unsigned short maxDisparity) {
245  // Allocate the buffer
246  if(pointMap.size() < static_cast<unsigned int>(imageSet.getWidth()*imageSet.getHeight())) {
247  pointMap.resize(imageSet.getWidth()*imageSet.getHeight());
248  }
249 
250  float* outputPtr = &pointMap[0];
251  int stride = imageSet.getRowStride(ImageSet::IMAGE_DISPARITY) / 2;
252  const unsigned short* dispMap = reinterpret_cast<const unsigned short*>(imageSet.getPixelData(ImageSet::IMAGE_DISPARITY));
253  int subpixelFactor = imageSet.getSubpixelFactor();
254  const float* q = imageSet.getQMatrix();
255 
256  double dInvalid;
257  if(minDisparity == 0) {
258  // Manually force invalid points to +inf, so that this works even
259  // for for angled cameras
260  dInvalid = std::numeric_limits<double>::infinity();
261  } else {
262  // Set invalid disparity to minimum disparity
263  dInvalid = double(minDisparity) / double(subpixelFactor);
264  }
265 
266  for(int y = 0; y < imageSet.getHeight(); y++) {
267  double qz = q[9]*y + q[11];
268  double qw = q[13]*y + q[15];
269 
270  const unsigned short* dispRow = &dispMap[y*stride];
271  for(int x = 0; x < imageSet.getWidth(); x++) {
272  unsigned short intDisp = std::max(minDisparity, dispRow[x]);
273  double d;
274 
275  if(intDisp >= maxDisparity) {
276  d = dInvalid;
277  } else {
278  d = double(intDisp) / double(subpixelFactor);
279  }
280 
281  double w = qw + q[14]*d;
282 
283  *outputPtr = static_cast<float>((qz + q[10]*d)/w); // z
284  outputPtr++;
285 
286  qz += q[8];
287  }
288  }
289  return &pointMap[0];
290 }
291 
292 void Reconstruct3D::Pimpl::projectSinglePoint(int imageX, int imageY, unsigned short disparity,
293  const float* q, float& pointX, float& pointY, float& pointZ, int subpixelFactor) {
294 
295  double d = disparity / double(subpixelFactor);
296  double w = q[15] + q[14]*d;
297  pointX = static_cast<float>((imageX*q[0] + q[3])/w);
298  pointY = static_cast<float>((imageY*q[5] + q[7])/w);
299  pointZ = static_cast<float>(q[11]/w);
300 }
301 
302 # ifdef __AVX2__
303 float* Reconstruct3D::Pimpl::createPointMapAVX2(const unsigned short* dispMap, int width,
304  int height, int rowStride, const float* q, unsigned short minDisparity,
305  int subpixelFactor, unsigned short maxDisparity) {
306 
307  // Create column vectors of q
308  const __m256 qCol0 = _mm256_setr_ps(q[0], q[4], q[8], q[12], q[0], q[4], q[8], q[12]);
309  const __m256 qCol1 = _mm256_setr_ps(q[1], q[5], q[9], q[13], q[1], q[5], q[9], q[13]);
310  const __m256 qCol2 = _mm256_setr_ps(q[2], q[6], q[10], q[14], q[2], q[6], q[10], q[14]);
311  const __m256 qCol3 = _mm256_setr_ps(q[3], q[7], q[11], q[15], q[3], q[7], q[11], q[15]);
312 
313  // More constants that we need
314  const __m256i minDispVector = _mm256_set1_epi16(minDisparity);
315  const __m256i maxDispVector = _mm256_set1_epi16(maxDisparity);
316  const __m256 scaleVector = _mm256_set1_ps(1.0/double(subpixelFactor));
317  const __m256i zeroVector = _mm256_set1_epi16(0);
318 
319  float* outputPtr = &pointMap[0];
320 
321  for(int y = 0; y < height; y++) {
322  const unsigned char* rowStart = &reinterpret_cast<const unsigned char*>(dispMap)[y*rowStride];
323  const unsigned char* rowEnd = &reinterpret_cast<const unsigned char*>(dispMap)[y*rowStride + 2*width];
324 
325  int x = 0;
326  for(const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 32) {
327  __m256i disparities = _mm256_load_si256(reinterpret_cast<const __m256i*>(ptr));
328 
329  // Find invalid disparities and set them to 0
330  __m256i validMask = _mm256_cmpgt_epi16(maxDispVector, disparities);
331  disparities = _mm256_and_si256(validMask, disparities);
332 
333  // Clamp to minimum disparity
334  disparities = _mm256_max_epi16(disparities, minDispVector);
335 
336  // Stupid AVX2 unpack mixes everything up! Lets swap the register beforehand.
337  __m256i disparitiesMixup = _mm256_permute4x64_epi64(disparities, 0xd8);
338 
339  // Convert to floats and scale with 1/subpixelFactor
340  __m256 floatDisp = _mm256_cvtepi32_ps(_mm256_unpacklo_epi16(disparitiesMixup, zeroVector));
341  __m256 dispScaled = _mm256_mul_ps(floatDisp, scaleVector);
342 
343  // Copy to array
344 #ifdef _MSC_VER
345  __declspec(align(32)) float dispArray[16];
346 #else
347  float dispArray[16]__attribute__((aligned(32)));
348 #endif
349  _mm256_store_ps(&dispArray[0], dispScaled);
350 
351  // Same for other half
352  floatDisp = _mm256_cvtepi32_ps(_mm256_unpackhi_epi16(disparitiesMixup, zeroVector));
353  dispScaled = _mm256_mul_ps(floatDisp, scaleVector);
354  _mm256_store_ps(&dispArray[8], dispScaled);
355 
356  // Iterate over disparities and perform matrix multiplication for each
357  for(int i=0; i<16; i+=2) {
358  // Create two vectors
359  __m256 vec = _mm256_setr_ps(x, y, dispArray[i], 1.0,
360  x+1, y, dispArray[i+1], 1.0);
361 
362  // Multiply with matrix
363  __m256 u1 = _mm256_shuffle_ps(vec, vec, _MM_SHUFFLE(0,0,0,0));
364  __m256 u2 = _mm256_shuffle_ps(vec, vec, _MM_SHUFFLE(1,1,1,1));
365  __m256 u3 = _mm256_shuffle_ps(vec, vec, _MM_SHUFFLE(2,2,2,2));
366  __m256 u4 = _mm256_shuffle_ps(vec, vec, _MM_SHUFFLE(3,3,3,3));
367 
368  __m256 prod1 = _mm256_mul_ps(u1, qCol0);
369  __m256 prod2 = _mm256_mul_ps(u2, qCol1);
370  __m256 prod3 = _mm256_mul_ps(u3, qCol2);
371  __m256 prod4 = _mm256_mul_ps(u4, qCol3);
372 
373  __m256 multResult = _mm256_add_ps(_mm256_add_ps(prod1, prod2), _mm256_add_ps(prod3, prod4));
374 
375  // Divide by w to receive point coordinates
376  __m256 point = _mm256_div_ps(multResult,
377  _mm256_shuffle_ps(multResult,multResult, _MM_SHUFFLE(3,3,3,3)));
378 
379  // Write result to memory
380  _mm256_store_ps(outputPtr, point);
381 
382  outputPtr += 8;
383  x+=2;
384  }
385  }
386  }
387 
388  return &pointMap[0];
389 }
390 #endif
391 
392 #ifdef __SSE2__
393 float* Reconstruct3D::Pimpl::createPointMapSSE2(const unsigned short* dispMap, int width,
394  int height, int rowStride, const float* q, unsigned short minDisparity,
395  int subpixelFactor, unsigned short maxDisparity) {
396 
397  // Create column vectors of q
398  const __m128 qCol0 = _mm_setr_ps(q[0], q[4], q[8], q[12]);
399  const __m128 qCol1 = _mm_setr_ps(q[1], q[5], q[9], q[13]);
400  const __m128 qCol2 = _mm_setr_ps(q[2], q[6], q[10], q[14]);
401  const __m128 qCol3 = _mm_setr_ps(q[3], q[7], q[11], q[15]);
402 
403  // More constants that we need
404  const __m128i minDispVector = _mm_set1_epi16(minDisparity);
405  const __m128i maxDispVector = _mm_set1_epi16(maxDisparity);
406  const __m128 scaleVector = _mm_set1_ps(1.0/double(subpixelFactor));
407  const __m128i zeroVector = _mm_set1_epi16(0);
408 
409  float* outputPtr = &pointMap[0];
410 
411  for(int y = 0; y < height; y++) {
412  const unsigned char* rowStart = &reinterpret_cast<const unsigned char*>(dispMap)[y*rowStride];
413  const unsigned char* rowEnd = &reinterpret_cast<const unsigned char*>(dispMap)[y*rowStride + 2*width];
414 
415  int x = 0;
416  for(const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 16) {
417  __m128i disparities = _mm_load_si128(reinterpret_cast<const __m128i*>(ptr));
418 
419  // Find invalid disparities and set them to 0
420  __m128i validMask = _mm_cmplt_epi16(disparities, maxDispVector);
421  disparities = _mm_and_si128(validMask, disparities);
422 
423  // Clamp to minimum disparity
424  disparities = _mm_max_epi16(disparities, minDispVector);
425 
426  // Convert to floats and scale with 1/subpixelFactor
427  __m128 floatDisp = _mm_cvtepi32_ps(_mm_unpacklo_epi16(disparities, zeroVector));
428  __m128 dispScaled = _mm_mul_ps(floatDisp, scaleVector);
429 
430  // Copy to array
431 #ifdef _MSC_VER
432  __declspec(align(16)) float dispArray[8];
433 #else
434  float dispArray[8]__attribute__((aligned(16)));
435 #endif
436  _mm_store_ps(&dispArray[0], dispScaled);
437 
438  // Same for other half
439  floatDisp = _mm_cvtepi32_ps(_mm_unpackhi_epi16(disparities, zeroVector));
440  dispScaled = _mm_mul_ps(floatDisp, scaleVector);
441  _mm_store_ps(&dispArray[4], dispScaled);
442 
443  // Iterate over disparities and perform matrix multiplication for each
444  for(int i=0; i<8; i++) {
445  // Create vectors and perform matrix multiplication
446  __m128 u1 = _mm_set1_ps(static_cast<float>(x));
447  __m128 u2 = _mm_set1_ps(static_cast<float>(y));
448  __m128 u3 = _mm_set1_ps(dispArray[i]);
449  __m128 u4 = _mm_set1_ps(1.0f);
450 
451  __m128 prod1 = _mm_mul_ps(u1, qCol0);
452  __m128 prod2 = _mm_mul_ps(u2, qCol1);
453  __m128 prod3 = _mm_mul_ps(u3, qCol2);
454  __m128 prod4 = _mm_mul_ps(u4, qCol3);
455 
456  __m128 multResult = _mm_add_ps(_mm_add_ps(prod1, prod2), _mm_add_ps(prod3, prod4));
457 
458  // Divide by w to receive point coordinates
459  __m128 point = _mm_div_ps(multResult,
460  _mm_shuffle_ps(multResult,multResult, _MM_SHUFFLE(3,3,3,3)));
461 
462  // Write result to memory
463  _mm_store_ps(outputPtr, point);
464 
465  outputPtr += 4;
466  x++;
467  }
468  }
469  }
470 
471  return &pointMap[0];
472 }
473 #endif
474 
475 #ifdef __aarch64__
476 float* Reconstruct3D::Pimpl::createPointMapNEON(const unsigned short* dispMap, int width,
477  int height, int rowStride, const float* q, unsigned short minDisparity,
478  int subpixelFactor, unsigned short maxDisparity) {
479 
480  // Create column vectors of q
481  float32x4_t qCol0 = {q[0], q[4], q[8], q[12]};
482  float32x4_t qCol1 = {q[1], q[5], q[9], q[13]};
483  float32x4_t qCol2 = {q[2], q[6], q[10], q[14]};
484  float32x4_t qCol3 = {q[3], q[7], q[11], q[15]};
485 
486  // More constants that we need
487  uint16x8_t minDispVector = vdupq_n_u16(minDisparity);
488  uint16x8_t maxDispVector = vdupq_n_u16(maxDisparity);
489  float32x4_t scaleVector = vdupq_n_f32(1.0f / static_cast<float>(subpixelFactor));
490 
491  float* outputPtr = &pointMap[0];
492 
493  for(int y = 0; y < height; y++) {
494  const unsigned char* rowStart = &reinterpret_cast<const unsigned char*>(dispMap)[y*rowStride];
495  const unsigned char* rowEnd = &reinterpret_cast<const unsigned char*>(dispMap)[y*rowStride + 2*width];
496 
497  int x = 0;
498  for(const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 16) {
499  uint16x8_t disparities = vld1q_u16(reinterpret_cast<const uint16_t*>(ptr));
500 
501  // Find invalid disparities and set them to 0
502  uint16x8_t validMask = vcltq_u16(disparities, maxDispVector);
503  disparities = vandq_u16(validMask, disparities);
504 
505  // Clamp to minimum disparity
506  disparities = vmaxq_u16(disparities, minDispVector);
507 
508  // Convert to floats and scale with 1/subpixelFactor
509  float32x4_t floatDisp = vcvtq_f32_u32(vmovl_u16(vget_low_u16(disparities)));
510  float32x4_t dispScaled = vmulq_f32(floatDisp, scaleVector);
511 
512  // Copy to array
513  float dispArray[8];
514  vst1q_f32(&dispArray[0], dispScaled);
515 
516  // Same for other half
517  floatDisp = vcvtq_f32_u32(vmovl_u16(vget_high_u16(disparities)));
518  dispScaled = vmulq_f32(floatDisp, scaleVector);
519  vst1q_f32(&dispArray[4], dispScaled);
520 
521  // Iterate over disparities and perform matrix multiplication for each
522  for(int i=0; i<8; i++) {
523  // Create vectors and perform matrix multiplication
524  float32x4_t u1 = vdupq_n_f32(static_cast<float>(x));
525  float32x4_t u2 = vdupq_n_f32(static_cast<float>(y));
526  float32x4_t u3 = vdupq_n_f32(dispArray[i]);
527  float32x4_t u4 = vdupq_n_f32(1.0f);
528 
529  float32x4_t prod1 = vmulq_f32(u1, qCol0);
530  float32x4_t prod2 = vmulq_f32(u2, qCol1);
531  float32x4_t prod3 = vmulq_f32(u3, qCol2);
532  float32x4_t prod4 = vmulq_f32(u4, qCol3);
533 
534  float32x4_t multResult = vaddq_f32(vaddq_f32(prod1, prod2), vaddq_f32(prod3, prod4));
535 
536  // Divide by w to receive point coordinates
537  float32x4_t point = vdivq_f32(multResult, vdupq_laneq_f32(multResult, 3));
538 
539  // Write result to memory
540  vst1q_f32(outputPtr, point);
541 
542  outputPtr += 4;
543  x++;
544  }
545  }
546  }
547 
548  return &pointMap[0];
549 }
550 #endif
551 
552 void Reconstruct3D::Pimpl::writePlyFile(const char* file, const unsigned short* dispMap,
553  const unsigned char* image, int width, int height, ImageSet::ImageFormat format, int dispRowStride,
554  int imageRowStride, const float* q, double maxZ, bool binary, int subpixelFactor,
555  unsigned short maxDisparity) {
556 
557  float* pointMap = createPointMap(dispMap, width, height, dispRowStride,
558  q, 0, subpixelFactor, maxDisparity);
559 
560  // Count number of valid points
561  int pointsCount = 0;
562  if(maxZ >= 0) {
563  for(int i=0; i<width*height; i++) {
564  if(pointMap[4*i+2] <= maxZ && pointMap[4*i+2] > 0) {
565  pointsCount++;
566  }
567  }
568  } else {
569  pointsCount = width*height;
570  }
571 
572  // Write file header
573  fstream strm(file, binary ? (ios::out | ios::binary) : ios::out);
574  strm << "ply" << endl;
575 
576  if(binary) {
577  strm << "format binary_little_endian 1.0" << endl;
578  } else {
579  strm << "format ascii 1.0" << endl;
580  }
581 
582  strm << "element vertex " << pointsCount << endl
583  << "property float x" << endl
584  << "property float y" << endl
585  << "property float z" << endl;
586  if (image != nullptr) {
587  // include RGB information only if a camera image was provided
588  strm << "property uchar red" << endl
589  << "property uchar green" << endl
590  << "property uchar blue" << endl;
591  }
592  strm << "end_header" << endl;
593 
594  // Write points
595  for(int i=0; i<width*height; i++) {
596  int y = i / width;
597  int x = i % width;
598 
599  if((maxZ < 0 || pointMap[4*i+2] <= maxZ) && pointMap[4*i+2] > 0) {
600  if(binary) {
601  // Write binary format
602  strm.write(reinterpret_cast<char*>(&pointMap[4*i]), sizeof(float)*3);
603  if (image == nullptr) {
604  // disparity only, no image data
605  } else if(format == ImageSet::FORMAT_8_BIT_RGB) {
606  const unsigned char* col = &image[y*imageRowStride + 3*x];
607  strm.write(reinterpret_cast<const char*>(col), 3*sizeof(*col));
608  } else if(format == ImageSet::FORMAT_8_BIT_MONO) {
609  const unsigned char* col = &image[y*imageRowStride + x];
610  unsigned char writeData[3] = {*col, *col, *col};
611  strm.write(reinterpret_cast<const char*>(writeData), sizeof(writeData));
612  } else if(format == ImageSet::FORMAT_12_BIT_MONO) {
613  const unsigned short* col = reinterpret_cast<const unsigned short*>(&image[y*imageRowStride + 2*x]);
614  unsigned char writeData[3] = {
615  (unsigned char)(*col >> 4),
616  (unsigned char)(*col >> 4),
617  (unsigned char)(*col >> 4)
618  };
619  strm.write(reinterpret_cast<const char*>(writeData), sizeof(writeData));
620  }
621  } else {
622  // Write ASCII format
623  if(std::isfinite(pointMap[4*i + 2])) {
624  strm << pointMap[4*i]
625  << " " << pointMap[4*i + 1]
626  << " " << pointMap[4*i + 2];
627  } else {
628  strm << "NaN NaN NaN";
629  }
630 
631  if (image == nullptr) {
632  // disparity only, no image data
633  strm << endl;
634  } else if(format == ImageSet::FORMAT_8_BIT_RGB) {
635  const unsigned char* col = &image[y*imageRowStride + 3*x];
636  strm << " " << static_cast<int>(col[0])
637  << " " << static_cast<int>(col[1])
638  << " " << static_cast<int>(col[2]) << endl;
639  } else if(format == ImageSet::FORMAT_8_BIT_MONO) {
640  const unsigned char* col = &image[y*imageRowStride + x];
641  strm << " " << static_cast<int>(*col)
642  << " " << static_cast<int>(*col)
643  << " " << static_cast<int>(*col) << endl;
644  } else if(format == ImageSet::FORMAT_12_BIT_MONO) {
645  const unsigned short* col = reinterpret_cast<const unsigned short*>(&image[y*imageRowStride + 2*x]);
646  strm << " " << static_cast<int>(*col >> 4)
647  << " " << static_cast<int>(*col >> 4)
648  << " " << static_cast<int>(*col >> 4) << endl;
649  }
650  }
651  }
652  }
653 }
654 
655 void Reconstruct3D::Pimpl::writePlyFile(const char* file, const ImageSet& imageSet,
656  double maxZ, bool binary, ColorSource colSource, unsigned short maxDisparity) {
657 
658  ImageSet::ImageType colImg = getColorImage(imageSet, colSource);
659 
660  int indexDisp = imageSet.getIndexOf(ImageSet::IMAGE_DISPARITY);
661  int indexImg = imageSet.getIndexOf(colImg);
662  if(indexDisp == -1) {
663  throw std::runtime_error("No disparity channel present, cannot create point map!");
664  }
665  if(imageSet.getPixelFormat(ImageSet::IMAGE_DISPARITY) != ImageSet::FORMAT_12_BIT_MONO) {
666  throw std::runtime_error("Disparity map must have 12-bit pixel format!");
667  }
668 
669  // write Ply file, passing image data for point colors, if available
670  writePlyFile(file, reinterpret_cast<unsigned short*>(imageSet.getPixelData(indexDisp)),
671  (indexImg == -1) ? nullptr : imageSet.getPixelData(indexImg),
672  imageSet.getWidth(), imageSet.getHeight(),
673  (indexImg == -1) ? ImageSet::FORMAT_8_BIT_MONO : imageSet.getPixelFormat(indexImg),
674  imageSet.getRowStride(indexDisp),
675  (indexImg == -1) ? 0 : imageSet.getRowStride(indexImg),
676  imageSet.getQMatrix(),
677  maxZ, binary, imageSet.getSubpixelFactor(), maxDisparity);
678 }
679 
680 } // namespace
681 
visiontransfer::ImageSet::getHeight
int getHeight() const
Returns the height of each image.
Definition: imageset.cpp:533
visiontransfer::ImageSet::getWidth
int getWidth() const
Returns the width of each image.
Definition: imageset.cpp:529
visiontransfer::Reconstruct3D::projectSinglePoint
void projectSinglePoint(int imageX, int imageY, unsigned short disparity, const float *q, float &pointX, float &pointY, float &pointZ, int subpixelFactor=16)
Reconstructs the 3D location of one individual point.
Definition: reconstruct3d.cpp:113
visiontransfer::ImageSet::getRowStride
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
Definition: imageset.cpp:537
visiontransfer::ImageSet::getSubpixelFactor
int getSubpixelFactor() const
Gets the subpixel factor for this image set.
Definition: imageset.cpp:577
visiontransfer::ImageSet::getPixelData
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
Definition: imageset.cpp:553
visiontransfer::ImageSet::getIndexOf
int getIndexOf(ImageType what, bool throwIfNotFound=false) const
Returns the index of a specific image type.
Definition: imageset.cpp:613
visiontransfer::Reconstruct3D::writePlyFile
void writePlyFile(const char *file, const ImageSet &imageSet, double maxZ=(std::numeric_limits< double >::max)(), bool binary=false, ColorSource colSource=COLOR_AUTO, unsigned short maxDisparity=0xFFF)
Projects the given disparity map to 3D points and exports the result to a PLY file.
Definition: reconstruct3d.cpp:119
visiontransfer::ImageSet
A set of one to three images, but usually two (the left camera image and the disparity map)....
Definition: imageset.h:50
visiontransfer::ImageSet::FORMAT_8_BIT_RGB
@ FORMAT_8_BIT_RGB
8-bit RGB format
Definition: imageset.h:79
visiontransfer::Reconstruct3D::createZMap
float * createZMap(const ImageSet &imageSet, unsigned short minDisparity=0, unsigned short maxDisparity=0xFFF)
Converts the disparity in an image set to a depth map.
Definition: reconstruct3d.cpp:108
visiontransfer::ImageSet::ImageType
ImageType
Supported image types.
Definition: imageset.h:97
visiontransfer::ImageSet::ImageFormat
ImageFormat
Image formats that can be transferred.
Definition: imageset.h:74
visiontransfer::ImageSet::hasImageType
bool hasImageType(ImageType what) const
Returns whether a left camera image is included in the enabled data.
Definition: imageset.cpp:617
visiontransfer::ImageSet::FORMAT_12_BIT_MONO
@ FORMAT_12_BIT_MONO
Definition: imageset.h:83
visiontransfer::ImageSet::FORMAT_8_BIT_MONO
@ FORMAT_8_BIT_MONO
8-bit greyscale format
Definition: imageset.h:76
visiontransfer::ImageSet::getPixelFormat
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
Definition: imageset.cpp:545
visiontransfer::ImageSet::getQMatrix
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
Definition: imageset.cpp:561
visiontransfer::Reconstruct3D::createPointMap
float * createPointMap(const ImageSet &imageSet, unsigned short minDisparity=0, unsigned short maxDisparity=0xFFF)
Reconstructs the 3D location of each pixel in the given disparity map.
Definition: reconstruct3d.cpp:97
Allied Vision