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