15 #include "reconstruct3d.h" 16 #include "visiontransfer/alignedallocator.h" 26 #include <immintrin.h> 28 #include <emmintrin.h> 39 class Reconstruct3D::Pimpl {
43 float* createPointMap(
const unsigned short* dispMap,
int width,
int height,
44 int rowStride,
const float* q,
unsigned short minDisparity);
46 float* createPointMap(
const ImagePair& imagePair,
unsigned short minDisparity);
48 void projectSinglePoint(
int imageX,
int imageY,
unsigned short disparity,
const float* q,
49 float& pointX,
float& pointY,
float& pointZ);
51 void writePlyFile(
const char* file,
const unsigned short* dispMap,
53 int dispRowStride,
int imageRowStride,
const float* q,
54 double maxZ,
bool binary);
56 void writePlyFile(
const char* file,
const ImagePair& imagePair,
57 double maxZ,
bool binary);
60 std::vector<float, AlignedAllocator<float> > pointMap;
62 float* createPointMapFallback(
const unsigned short* dispMap,
int width,
int height,
63 int rowStride,
const float* q,
unsigned short minDisparity);
65 float* createPointMapSSE2(
const unsigned short* dispMap,
int width,
int height,
66 int rowStride,
const float* q,
unsigned short minDisparity);
68 float* createPointMapAVX2(
const unsigned short* dispMap,
int width,
int height,
69 int rowStride,
const float* q,
unsigned short minDisparity);
74 Reconstruct3D::Reconstruct3D()
78 Reconstruct3D::~Reconstruct3D() {
83 int rowStride,
const float* q,
unsigned short minDisparity) {
84 return pimpl->createPointMap(dispMap, width, height, rowStride, q, minDisparity);
88 return pimpl->createPointMap(imagePair, minDisparity);
92 const float* q,
float& pointX,
float& pointY,
float& pointZ) {
93 pimpl->projectSinglePoint(imageX, imageY, disparity, q, pointX, pointY, pointZ);
98 int imageRowStride,
const float* q,
double maxZ,
bool binary) {
99 pimpl->writePlyFile(file, dispMap, image, width, height, format, dispRowStride,
100 imageRowStride, q, maxZ, binary);
104 double maxZ,
bool binary) {
105 pimpl->writePlyFile(file, imagePair, maxZ, binary);
110 Reconstruct3D::Pimpl::Pimpl() {
113 float* Reconstruct3D::Pimpl::createPointMap(
const unsigned short* dispMap,
int width,
114 int height,
int rowStride,
const float* q,
unsigned short minDisparity) {
117 if(pointMap.size() !=
static_cast<unsigned int>(4*width*height)) {
118 pointMap.resize(4*width*height);
122 return createPointMapAVX2(dispMap, width, height, rowStride, q, minDisparity);
124 return createPointMapSSE2(dispMap, width, height, rowStride, q, minDisparity);
126 return createPointMapFallback(dispMap, width, height, rowStride, q, minDisparity);
130 float* Reconstruct3D::Pimpl::createPointMap(
const ImagePair& imagePair,
unsigned short minDisparity) {
132 throw std::runtime_error(
"Disparity map must have 12-bit pixel format!");
139 float* Reconstruct3D::Pimpl::createPointMapFallback(
const unsigned short* dispMap,
int width,
140 int height,
int rowStride,
const float* q,
unsigned short minDisparity) {
142 float* outputPtr = &pointMap[0];
143 int stride = rowStride / 2;
145 for(
int y = 0; y < height; y++) {
146 double qx = q[1]*y + q[3];
147 double qy = q[5]*y + q[7];
148 double qz = q[9]*y + q[11];
149 double qw = q[13]*y + q[15];
151 for(
int x = 0; x < width; x++) {
152 unsigned short intDisp = std::max(minDisparity, dispMap[y*stride + x]);
153 if(intDisp >= 0xFFF) {
154 intDisp = minDisparity;
157 double d = intDisp / 16.0;
158 double w = qw + q[14]*d;
160 *outputPtr =
static_cast<float>((qx + q[2]*d)/w);
163 *outputPtr =
static_cast<float>((qy + q[6]*d)/w);
166 *outputPtr =
static_cast<float>((qz + q[10]*d)/w);
178 void Reconstruct3D::Pimpl::projectSinglePoint(
int imageX,
int imageY,
unsigned short disparity,
179 const float* q,
float& pointX,
float& pointY,
float& pointZ) {
181 double d = disparity / 16.0;
182 double w = q[15] + q[14]*d;
183 pointX =
static_cast<float>((imageX*q[0] + q[3])/w);
184 pointY =
static_cast<float>((imageY*q[5] + q[7])/w);
185 pointZ =
static_cast<float>(q[11]/w);
189 float* Reconstruct3D::Pimpl::createPointMapAVX2(
const unsigned short* dispMap,
int width,
190 int height,
int rowStride,
const float* q,
unsigned short minDisparity) {
193 const __m256 qCol0 = _mm256_setr_ps(q[0], q[4], q[8], q[12], q[0], q[4], q[8], q[12]);
194 const __m256 qCol1 = _mm256_setr_ps(q[1], q[5], q[9], q[13], q[1], q[5], q[9], q[13]);
195 const __m256 qCol2 = _mm256_setr_ps(q[2], q[6], q[10], q[14], q[2], q[6], q[10], q[14]);
196 const __m256 qCol3 = _mm256_setr_ps(q[3], q[7], q[11], q[15], q[3], q[7], q[11], q[15]);
199 const __m256i minDispVector = _mm256_set1_epi16(minDisparity);
200 const __m256i maxDispVector = _mm256_set1_epi16(0xFFF);
201 const __m256 scaleVector = _mm256_set1_ps(1.0/16.0);
202 const __m256i zeroVector = _mm256_set1_epi16(0);
204 float* outputPtr = &pointMap[0];
206 for(
int y = 0; y < height; y++) {
207 const unsigned char* rowStart = &
reinterpret_cast<const unsigned char*
>(dispMap)[y*rowStride];
208 const unsigned char* rowEnd = &
reinterpret_cast<const unsigned char*
>(dispMap)[y*rowStride + 2*width];
211 for(
const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 32) {
212 __m256i disparities = _mm256_load_si256(reinterpret_cast<const __m256i*>(ptr));
215 __m256i validMask = _mm256_cmpgt_epi16(maxDispVector, disparities);
216 disparities = _mm256_and_si256(validMask, disparities);
219 disparities = _mm256_max_epi16(disparities, minDispVector);
222 __m256i disparitiesMixup = _mm256_permute4x64_epi64(disparities, 0xd8);
225 __m256 floatDisp = _mm256_cvtepi32_ps(_mm256_unpacklo_epi16(disparitiesMixup, zeroVector));
226 __m256 dispScaled = _mm256_mul_ps(floatDisp, scaleVector);
230 __declspec(align(32))
float dispArray[16];
232 float dispArray[16]__attribute__((aligned(32)));
234 _mm256_store_ps(&dispArray[0], dispScaled);
237 floatDisp = _mm256_cvtepi32_ps(_mm256_unpackhi_epi16(disparitiesMixup, zeroVector));
238 dispScaled = _mm256_mul_ps(floatDisp, scaleVector);
239 _mm256_store_ps(&dispArray[8], dispScaled);
242 for(
int i=0; i<16; i+=2) {
244 __m256 vec = _mm256_setr_ps(x, y, dispArray[i], 1.0,
245 x+1, y, dispArray[i+1], 1.0);
248 __m256 u1 = _mm256_shuffle_ps(vec,vec, _MM_SHUFFLE(0,0,0,0));
249 __m256 u2 = _mm256_shuffle_ps(vec,vec, _MM_SHUFFLE(1,1,1,1));
250 __m256 u3 = _mm256_shuffle_ps(vec,vec, _MM_SHUFFLE(2,2,2,2));
251 __m256 u4 = _mm256_shuffle_ps(vec,vec, _MM_SHUFFLE(3,3,3,3));
253 __m256 prod1 = _mm256_mul_ps(u1, qCol0);
254 __m256 prod2 = _mm256_mul_ps(u2, qCol1);
255 __m256 prod3 = _mm256_mul_ps(u3, qCol2);
256 __m256 prod4 = _mm256_mul_ps(u4, qCol3);
258 __m256 multResult = _mm256_add_ps(_mm256_add_ps(prod1, prod2), _mm256_add_ps(prod3, prod4));
261 __m256 point = _mm256_div_ps(multResult,
262 _mm256_shuffle_ps(multResult,multResult, _MM_SHUFFLE(3,3,3,3)));
265 _mm256_store_ps(outputPtr, point);
278 float* Reconstruct3D::Pimpl::createPointMapSSE2(
const unsigned short* dispMap,
int width,
279 int height,
int rowStride,
const float* q,
unsigned short minDisparity) {
282 const __m128 qCol0 = _mm_setr_ps(q[0], q[4], q[8], q[12]);
283 const __m128 qCol1 = _mm_setr_ps(q[1], q[5], q[9], q[13]);
284 const __m128 qCol2 = _mm_setr_ps(q[2], q[6], q[10], q[14]);
285 const __m128 qCol3 = _mm_setr_ps(q[3], q[7], q[11], q[15]);
288 const __m128i minDispVector = _mm_set1_epi16(minDisparity);
289 const __m128i maxDispVector = _mm_set1_epi16(0xFFF);
290 const __m128 scaleVector = _mm_set1_ps(1.0/16.0);
291 const __m128i zeroVector = _mm_set1_epi16(0);
293 float* outputPtr = &pointMap[0];
295 for(
int y = 0; y < height; y++) {
296 const unsigned char* rowStart = &
reinterpret_cast<const unsigned char*
>(dispMap)[y*rowStride];
297 const unsigned char* rowEnd = &
reinterpret_cast<const unsigned char*
>(dispMap)[y*rowStride + 2*width];
300 for(
const unsigned char* ptr = rowStart; ptr != rowEnd; ptr += 16) {
301 __m128i disparities = _mm_load_si128(reinterpret_cast<const __m128i*>(ptr));
304 __m128i validMask = _mm_cmplt_epi16(disparities, maxDispVector);
305 disparities = _mm_and_si128(validMask, disparities);
308 disparities = _mm_max_epi16(disparities, minDispVector);
311 __m128 floatDisp = _mm_cvtepi32_ps(_mm_unpacklo_epi16(disparities, zeroVector));
312 __m128 dispScaled = _mm_mul_ps(floatDisp, scaleVector);
316 __declspec(align(16))
float dispArray[8];
318 float dispArray[8]__attribute__((aligned(16)));
320 _mm_store_ps(&dispArray[0], dispScaled);
323 floatDisp = _mm_cvtepi32_ps(_mm_unpackhi_epi16(disparities, zeroVector));
324 dispScaled = _mm_mul_ps(floatDisp, scaleVector);
325 _mm_store_ps(&dispArray[4], dispScaled);
328 for(
int i=0; i<8; i++) {
330 __m128 vec = _mm_setr_ps(static_cast<float>(x), static_cast<float>(y), dispArray[i], 1.0);
333 __m128 u1 = _mm_shuffle_ps(vec,vec, _MM_SHUFFLE(0,0,0,0));
334 __m128 u2 = _mm_shuffle_ps(vec,vec, _MM_SHUFFLE(1,1,1,1));
335 __m128 u3 = _mm_shuffle_ps(vec,vec, _MM_SHUFFLE(2,2,2,2));
336 __m128 u4 = _mm_shuffle_ps(vec,vec, _MM_SHUFFLE(3,3,3,3));
338 __m128 prod1 = _mm_mul_ps(u1, qCol0);
339 __m128 prod2 = _mm_mul_ps(u2, qCol1);
340 __m128 prod3 = _mm_mul_ps(u3, qCol2);
341 __m128 prod4 = _mm_mul_ps(u4, qCol3);
343 __m128 multResult = _mm_add_ps(_mm_add_ps(prod1, prod2), _mm_add_ps(prod3, prod4));
346 __m128 point = _mm_div_ps(multResult,
347 _mm_shuffle_ps(multResult,multResult, _MM_SHUFFLE(3,3,3,3)));
350 _mm_store_ps(outputPtr, point);
362 void Reconstruct3D::Pimpl::writePlyFile(
const char* file,
const unsigned short* dispMap,
364 int imageRowStride,
const float* q,
double maxZ,
bool binary) {
366 float* pointMap =
createPointMap(dispMap, width, height, dispRowStride, q, 0);
371 for(
int i=0; i<width*height; i++) {
372 if(pointMap[4*i+2] <= maxZ) {
377 pointsCount = width*height;
381 fstream strm(file, binary ? (ios::out | ios::binary) : ios::out);
382 strm <<
"ply" << endl;
385 strm <<
"format binary_little_endian 1.0" << endl;
387 strm <<
"format ascii 1.0" << endl;
390 strm <<
"element vertex " << pointsCount << endl
391 <<
"property float x" << endl
392 <<
"property float y" << endl
393 <<
"property float z" << endl
394 <<
"property uchar red" << endl
395 <<
"property uchar green" << endl
396 <<
"property uchar blue" << endl
397 <<
"end_header" << endl;
400 for(
int i=0; i<width*height; i++) {
404 if(maxZ < 0 || pointMap[4*i+2] <= maxZ) {
407 strm.write(reinterpret_cast<char*>(&pointMap[4*i]),
sizeof(
float)*3);
409 const unsigned char* col = &image[y*imageRowStride + 3*x];
410 strm.write(reinterpret_cast<const char*>(col), 3*
sizeof(*col));
412 const unsigned char* col = &image[y*imageRowStride + x];
413 unsigned char writeData[3] = {*col, *col, *col};
414 strm.write(reinterpret_cast<const char*>(writeData),
sizeof(writeData));
416 const unsigned short* col =
reinterpret_cast<const unsigned short*
>(&image[y*imageRowStride + 2*x]);
417 unsigned char writeData[3] = {
418 (
unsigned char)(*col >> 4),
419 (
unsigned char)(*col >> 4),
420 (
unsigned char)(*col >> 4)
422 strm.write(reinterpret_cast<const char*>(writeData),
sizeof(writeData));
426 if(std::isfinite(pointMap[4*i + 2])) {
427 strm << pointMap[4*i]
428 <<
" " << pointMap[4*i + 1]
429 <<
" " << pointMap[4*i + 2];
431 strm <<
"NaN NaN NaN";
435 const unsigned char* col = &image[y*imageRowStride + 3*x];
436 strm <<
" " <<
static_cast<int>(col[0])
437 <<
" " << static_cast<int>(col[1])
438 <<
" " <<
static_cast<int>(col[2]) << endl;
440 const unsigned char* col = &image[y*imageRowStride + x];
441 strm <<
" " <<
static_cast<int>(*col)
442 <<
" " <<
static_cast<int>(*col)
443 <<
" " <<
static_cast<int>(*col) << endl;
445 const unsigned short* col =
reinterpret_cast<const unsigned short*
>(&image[y*imageRowStride + 2*x]);
446 strm <<
" " <<
static_cast<int>(*col >> 4)
447 <<
" " << static_cast<int>(*col >> 4)
448 <<
" " <<
static_cast<int>(*col >> 4) << endl;
455 void Reconstruct3D::Pimpl::writePlyFile(
const char* file,
const ImagePair& imagePair,
456 double maxZ,
bool binary) {
458 throw std::runtime_error(
"Disparity map must have 12-bit pixel format!");
int getHeight() const
Returns the height of each image.
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.
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
ImageFormat
Image formats that can be transferred.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
int getWidth() const
Returns the width of each image.
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.
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
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.
A set of two images, which are usually the left camera image and the disparity map.