15 #include "reconstruct3d.h" 16 #include "visiontransfer/alignedallocator.h" 27 #include <immintrin.h> 29 #include <emmintrin.h> 42 class Reconstruct3D::Pimpl {
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);
50 float* createPointMap(
const ImageSet& imageSet,
unsigned short minDisparity,
unsigned short maxDisparity = 0xFFF);
52 float* createZMap(
const ImageSet& imageSet,
unsigned short minDisparity,
unsigned short maxDisparity);
54 void projectSinglePoint(
int imageX,
int imageY,
unsigned short disparity,
const float* q,
55 float& pointX,
float& pointY,
float& pointZ,
int subpixelFactor);
57 void writePlyFile(
const char* file,
const unsigned short* dispMap,
59 int dispRowStride,
int imageRowStride,
const float* q,
60 double maxZ,
bool binary,
int subpixelFactor,
unsigned short maxDisparity);
62 void writePlyFile(
const char* file,
const ImageSet& imageSet,
63 double maxZ,
bool binary, ColorSource colSource,
unsigned short maxDisparity);
66 std::vector<float, AlignedAllocator<float> > pointMap;
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);
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);
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);
83 Reconstruct3D::Reconstruct3D()
87 Reconstruct3D::~Reconstruct3D() {
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);
99 return pimpl->createPointMap(imageSet, minDisparity, maxDisparity);
103 unsigned short maxDisparity) {
104 return pimpl->createZMap(imageSet, minDisparity, maxDisparity);
108 const float* q,
float& pointX,
float& pointY,
float& pointZ,
int subpixelFactor) {
109 pimpl->projectSinglePoint(imageX, imageY, disparity, q, pointX, pointY, pointZ,
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);
122 double maxZ,
bool binary,
ColorSource colSource,
unsigned short maxDisparity) {
123 pimpl->writePlyFile(file, imageSet, maxZ, binary, colSource, maxDisparity);
128 Reconstruct3D::Pimpl::Pimpl() {
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) {
136 if(pointMap.size() <
static_cast<unsigned int>(4*width*height)) {
137 pointMap.resize(4*width*height);
142 bool angledCameraFallback = (q[15] != 0.0 && minDisparity == 0);
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);
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);
157 return createPointMapFallback(dispMap, width, height, rowStride, q,
158 minDisparity, subpixelFactor, maxDisparity);
162 float* Reconstruct3D::Pimpl::createPointMap(
const ImageSet& imageSet,
unsigned short minDisparity,
unsigned short maxDisparity) {
164 throw std::runtime_error(
"ImageSet does not contain a disparity map!");
168 throw std::runtime_error(
"Disparity map must have 12-bit pixel format!");
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) {
180 float* outputPtr = &pointMap[0];
181 int stride = rowStride / 2;
184 if(minDisparity == 0) {
187 dInvalid = std::numeric_limits<double>::infinity();
190 dInvalid = double(minDisparity) / double(subpixelFactor);
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];
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]);
204 if(intDisp >= maxDisparity) {
207 d = double(intDisp) / double(subpixelFactor);
210 double w = qw + q[14]*d;
212 *outputPtr =
static_cast<float>((qx + q[2]*d)/w);
215 *outputPtr =
static_cast<float>((qy + q[6]*d)/w);
218 *outputPtr =
static_cast<float>((qz + q[10]*d)/w);
230 float* Reconstruct3D::Pimpl::createZMap(
const ImageSet& imageSet,
unsigned short minDisparity,
231 unsigned short maxDisparity) {
233 if(pointMap.size() <
static_cast<unsigned int>(imageSet.
getWidth()*imageSet.
getHeight())) {
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));
244 if(minDisparity == 0) {
247 dInvalid = std::numeric_limits<double>::infinity();
250 dInvalid = double(minDisparity) / double(subpixelFactor);
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];
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]);
262 if(intDisp >= maxDisparity) {
265 d = double(intDisp) / double(subpixelFactor);
268 double w = qw + q[14]*d;
270 *outputPtr =
static_cast<float>((qz + q[10]*d)/w);
279 void Reconstruct3D::Pimpl::projectSinglePoint(
int imageX,
int imageY,
unsigned short disparity,
280 const float* q,
float& pointX,
float& pointY,
float& pointZ,
int subpixelFactor) {
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);
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) {
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]);
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);
306 float* outputPtr = &pointMap[0];
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];
313 for(
const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 32) {
314 __m256i disparities = _mm256_load_si256(reinterpret_cast<const __m256i*>(ptr));
317 __m256i validMask = _mm256_cmpgt_epi16(maxDispVector, disparities);
318 disparities = _mm256_and_si256(validMask, disparities);
321 disparities = _mm256_max_epi16(disparities, minDispVector);
324 __m256i disparitiesMixup = _mm256_permute4x64_epi64(disparities, 0xd8);
327 __m256 floatDisp = _mm256_cvtepi32_ps(_mm256_unpacklo_epi16(disparitiesMixup, zeroVector));
328 __m256 dispScaled = _mm256_mul_ps(floatDisp, scaleVector);
332 __declspec(align(32))
float dispArray[16];
334 float dispArray[16]__attribute__((aligned(32)));
336 _mm256_store_ps(&dispArray[0], dispScaled);
339 floatDisp = _mm256_cvtepi32_ps(_mm256_unpackhi_epi16(disparitiesMixup, zeroVector));
340 dispScaled = _mm256_mul_ps(floatDisp, scaleVector);
341 _mm256_store_ps(&dispArray[8], dispScaled);
344 for(
int i=0; i<16; i+=2) {
346 __m256 vec = _mm256_setr_ps(x, y, dispArray[i], 1.0,
347 x+1, y, dispArray[i+1], 1.0);
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));
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);
360 __m256 multResult = _mm256_add_ps(_mm256_add_ps(prod1, prod2), _mm256_add_ps(prod3, prod4));
363 __m256 point = _mm256_div_ps(multResult,
364 _mm256_shuffle_ps(multResult,multResult, _MM_SHUFFLE(3,3,3,3)));
367 _mm256_store_ps(outputPtr, point);
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) {
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]);
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);
396 float* outputPtr = &pointMap[0];
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];
403 for(
const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 16) {
404 __m128i disparities = _mm_load_si128(reinterpret_cast<const __m128i*>(ptr));
407 __m128i validMask = _mm_cmplt_epi16(disparities, maxDispVector);
408 disparities = _mm_and_si128(validMask, disparities);
411 disparities = _mm_max_epi16(disparities, minDispVector);
414 __m128 floatDisp = _mm_cvtepi32_ps(_mm_unpacklo_epi16(disparities, zeroVector));
415 __m128 dispScaled = _mm_mul_ps(floatDisp, scaleVector);
419 __declspec(align(16))
float dispArray[8];
421 float dispArray[8]__attribute__((aligned(16)));
423 _mm_store_ps(&dispArray[0], dispScaled);
426 floatDisp = _mm_cvtepi32_ps(_mm_unpackhi_epi16(disparities, zeroVector));
427 dispScaled = _mm_mul_ps(floatDisp, scaleVector);
428 _mm_store_ps(&dispArray[4], dispScaled);
431 for(
int i=0; i<8; i++) {
433 __m128 vec = _mm_setr_ps(static_cast<float>(x), static_cast<float>(y), dispArray[i], 1.0);
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));
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);
446 __m128 multResult = _mm_add_ps(_mm_add_ps(prod1, prod2), _mm_add_ps(prod3, prod4));
449 __m128 point = _mm_div_ps(multResult,
450 _mm_shuffle_ps(multResult,multResult, _MM_SHUFFLE(3,3,3,3)));
453 _mm_store_ps(outputPtr, point);
465 void Reconstruct3D::Pimpl::writePlyFile(
const char* file,
const unsigned short* dispMap,
467 int imageRowStride,
const float* q,
double maxZ,
bool binary,
int subpixelFactor,
468 unsigned short maxDisparity) {
470 float* pointMap =
createPointMap(dispMap, width, height, dispRowStride,
471 q, 0, subpixelFactor, maxDisparity);
476 for(
int i=0; i<width*height; i++) {
477 if(pointMap[4*i+2] <= maxZ && pointMap[4*i+2] > 0) {
482 pointsCount = width*height;
486 fstream strm(file, binary ? (ios::out | ios::binary) : ios::out);
487 strm <<
"ply" << endl;
490 strm <<
"format binary_little_endian 1.0" << endl;
492 strm <<
"format ascii 1.0" << endl;
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) {
501 strm <<
"property uchar red" << endl
502 <<
"property uchar green" << endl
503 <<
"property uchar blue" << endl;
505 strm <<
"end_header" << endl;
508 for(
int i=0; i<width*height; i++) {
512 if((maxZ < 0 || pointMap[4*i+2] <= maxZ) && pointMap[4*i+2] > 0) {
515 strm.write(reinterpret_cast<char*>(&pointMap[4*i]),
sizeof(
float)*3);
516 if (image ==
nullptr) {
519 const unsigned char* col = &image[y*imageRowStride + 3*x];
520 strm.write(reinterpret_cast<const char*>(col), 3*
sizeof(*col));
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));
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)
532 strm.write(reinterpret_cast<const char*>(writeData),
sizeof(writeData));
536 if(std::isfinite(pointMap[4*i + 2])) {
537 strm << pointMap[4*i]
538 <<
" " << pointMap[4*i + 1]
539 <<
" " << pointMap[4*i + 2];
541 strm <<
"NaN NaN NaN";
544 if (image ==
nullptr) {
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;
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;
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;
568 void Reconstruct3D::Pimpl::writePlyFile(
const char* file,
const ImageSet& imageSet,
569 double maxZ,
bool binary,
ColorSource colSource,
unsigned short maxDisparity) {
573 int indexDisp = imageSet.
getIndexOf(ImageSet::IMAGE_DISPARITY);
575 if(indexDisp == -1) {
576 throw std::runtime_error(
"No disparity channel present, cannot create point map!");
579 throw std::runtime_error(
"Disparity map must have 12-bit pixel format!");
584 (indexImg == -1) ? nullptr : imageSet.
getPixelData(indexImg),
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
int getIndexOf(ImageType what, bool throwIfNotFound=false) const
Returns the index of a specific image type.
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.
int getSubpixelFactor() const
Gets the subpixel factor for this image set.
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.
ImageType
Supported image types.
ImageFormat
Image formats that can be transferred.
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
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.
A set of one to three images, but usually two (the left camera image and the disparity map)...
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
int getWidth() const
Returns the width of each image.