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