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