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