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