зеркало из https://github.com/microsoft/AirSim.git
Added SGM code
This commit is contained in:
Matthias
Родитель
77f84fff2c
Коммит
1ec2c4b31b
|
|
@ -0,0 +1,4 @@
|
||||||
|
input/
|
||||||
|
output/
|
||||||
|
obj/
|
||||||
|
lib/
|
||||||
|
|
@ -0,0 +1,120 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#include "dsimage.h"
|
||||||
|
|
||||||
|
void getDispMap2(DSI &dv1, DSI &dv2, int confThreshold, float * dispMap, unsigned char * confMap)
|
||||||
|
{
|
||||||
|
int cols = (int)dv1.m_cols;
|
||||||
|
int rows = (int)dv1.m_rows;
|
||||||
|
int planes = (int)dv1.m_planes;
|
||||||
|
|
||||||
|
// first row
|
||||||
|
{
|
||||||
|
uint64_t offset = 0;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
for (int x = 0; x < cols; x++)
|
||||||
|
{
|
||||||
|
pDisp[x] = FLT_MAX;
|
||||||
|
pConf[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// last row
|
||||||
|
{
|
||||||
|
uint64_t offset = (rows - 1) * cols;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
for (int x = 0; x < cols; x++)
|
||||||
|
{
|
||||||
|
pDisp[x] = FLT_MAX;
|
||||||
|
pConf[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int y = 0; y < rows; y++)
|
||||||
|
{
|
||||||
|
uint64_t offset = y * cols;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
{
|
||||||
|
pDisp[0] = FLT_MAX;
|
||||||
|
pConf[0] = 0;
|
||||||
|
pDisp[cols - 1] = FLT_MAX;
|
||||||
|
pConf[cols - 1] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int y = 1; y < rows - 1; y++)
|
||||||
|
{
|
||||||
|
uint64_t offset = y * cols;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
|
||||||
|
#pragma omp parallel for schedule(dynamic,1)
|
||||||
|
|
||||||
|
for (int x = 1; x < cols - 1; x++)
|
||||||
|
{
|
||||||
|
int bestplane = (int)planes - 1;
|
||||||
|
short minval = SHRT_MAX;
|
||||||
|
short secondminval = SHRT_MAX;
|
||||||
|
short * pV1 = dv1(x, y);
|
||||||
|
short * pV2 = dv2(x, y);
|
||||||
|
for (int d = 0; d < planes; d++)
|
||||||
|
{
|
||||||
|
short val = pV1[d] + pV2[d];
|
||||||
|
if (val < minval)
|
||||||
|
{
|
||||||
|
minval = val;
|
||||||
|
bestplane = d;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int d = 0; d < planes; d++)
|
||||||
|
{
|
||||||
|
if (abs(d - bestplane) > 2)
|
||||||
|
{
|
||||||
|
short val = pV1[d] + pV2[d];
|
||||||
|
if (val < secondminval)
|
||||||
|
{
|
||||||
|
secondminval = val;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float distinctiveness1 = float(minval) / float(secondminval + 1e-9f);
|
||||||
|
float conf = (float) __min(__max(20.0f * log(1.0f / (distinctiveness1*distinctiveness1)), 0.0f), 255.0f);
|
||||||
|
int Dim = (int)planes;
|
||||||
|
if (conf >= confThreshold)
|
||||||
|
{
|
||||||
|
// Local quadratic fit of cost and subpixel refinement.
|
||||||
|
double rDisp = bestplane;
|
||||||
|
double rCost = minval;
|
||||||
|
if (bestplane >= 1 && bestplane < planes - 1)
|
||||||
|
{
|
||||||
|
double yl = pV1[bestplane - 1] + pV2[bestplane - 1];
|
||||||
|
double xc = bestplane;
|
||||||
|
double yc = minval;
|
||||||
|
double yu = pV1[bestplane + 1] + pV2[bestplane + 1];
|
||||||
|
double d2 = yu - yc + yl - yc;
|
||||||
|
double d1 = 0.5 * (yu - yl);
|
||||||
|
if (fabs(d2) > fabs(d1))
|
||||||
|
{
|
||||||
|
rDisp = xc - d1 / d2;
|
||||||
|
rCost = yc + 0.5 * d1 * (rDisp - xc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
pDisp[x] = (float)(rDisp - Dim);
|
||||||
|
pConf[x] = (unsigned char)conf;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pDisp[x] = FLT_MAX;
|
||||||
|
pConf[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,223 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#ifndef dsimage_h
|
||||||
|
#define dsimage_h
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <limits.h>
|
||||||
|
#include <float.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
|
||||||
|
class DSI
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
DSI()
|
||||||
|
{
|
||||||
|
m_cols = 0;
|
||||||
|
m_rows = 0;
|
||||||
|
m_planes = 0;
|
||||||
|
m_data = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
void create(uint64_t cols, uint64_t rows, uint64_t planes)
|
||||||
|
{
|
||||||
|
m_cols = cols;
|
||||||
|
m_rows = rows;
|
||||||
|
m_planes = planes;
|
||||||
|
|
||||||
|
uint64_t pixelCount = m_cols * m_rows * m_planes;
|
||||||
|
m_data = (short*)_aligned_malloc(pixelCount * sizeof(short), 16);
|
||||||
|
if (!m_data)
|
||||||
|
{
|
||||||
|
printf("[ERROR] not enough memory!\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setzero()
|
||||||
|
{
|
||||||
|
uint64_t pixelCount = m_cols * m_rows * m_planes;
|
||||||
|
memset(m_data, 0, pixelCount*sizeof(short));
|
||||||
|
}
|
||||||
|
|
||||||
|
short operator()(uint64_t x, uint64_t y, uint64_t z) const
|
||||||
|
{
|
||||||
|
return m_data[(x + y * m_cols)*m_planes + z];
|
||||||
|
}
|
||||||
|
|
||||||
|
short& operator()(uint64_t x, uint64_t y, uint64_t z)
|
||||||
|
{
|
||||||
|
return m_data[(x + y * m_cols)*m_planes + z];
|
||||||
|
}
|
||||||
|
|
||||||
|
short* operator()(uint64_t x, uint64_t y) const
|
||||||
|
{
|
||||||
|
return &(m_data[(x + y * m_cols)*m_planes]);
|
||||||
|
}
|
||||||
|
|
||||||
|
void getDispMap(int confThreshold, int doSubPixRefinement, float * dispMap, unsigned char * confMap)
|
||||||
|
{
|
||||||
|
// first row
|
||||||
|
{
|
||||||
|
uint64_t offset = 0;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
for (int x = 0; x < m_cols; x++)
|
||||||
|
{
|
||||||
|
pDisp[x] = FLT_MAX;
|
||||||
|
pConf[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// last row
|
||||||
|
{
|
||||||
|
uint64_t offset = (m_rows-1) * m_cols;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
for (int x = 0; x < m_cols; x++)
|
||||||
|
{
|
||||||
|
pDisp[x] = FLT_MAX;
|
||||||
|
pConf[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int y = 0; y < m_rows; y++)
|
||||||
|
{
|
||||||
|
uint64_t offset = y * m_cols;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
{
|
||||||
|
pDisp[0] = FLT_MAX;
|
||||||
|
pConf[0] = 0;
|
||||||
|
pDisp[m_cols - 1] = FLT_MAX;
|
||||||
|
pConf[m_cols - 1] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int y = 1; y < m_rows-1; y++)
|
||||||
|
{
|
||||||
|
uint64_t offset = y * m_cols;
|
||||||
|
float *pDisp = &(dispMap[offset]);
|
||||||
|
unsigned char *pConf = &(confMap[offset]);
|
||||||
|
|
||||||
|
#pragma omp parallel for schedule(dynamic,1)
|
||||||
|
|
||||||
|
for (int x = 1; x < m_cols-1; x++)
|
||||||
|
{
|
||||||
|
int bestplane = (int)m_planes - 1;
|
||||||
|
short minval = SHRT_MAX;
|
||||||
|
short secondminval = SHRT_MAX;
|
||||||
|
short * pV = (*this)(x, y);
|
||||||
|
for (int d = 0; d < m_planes; d++)
|
||||||
|
{
|
||||||
|
short val = pV[d];
|
||||||
|
if (val < minval)
|
||||||
|
{
|
||||||
|
minval = val;
|
||||||
|
bestplane = d;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int d = 0; d < m_planes; d++)
|
||||||
|
{
|
||||||
|
if (abs(d - bestplane) > 2)
|
||||||
|
{
|
||||||
|
short val = pV[d];
|
||||||
|
if (val < secondminval)
|
||||||
|
{
|
||||||
|
secondminval = val;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float distinctiveness1 = float(minval) / float(secondminval + 1e-9f);
|
||||||
|
float conf = (float) __min(__max(20.0f * log(1.0f / (distinctiveness1*distinctiveness1)), 0.0f), 255.0f);
|
||||||
|
int Dim = (int)m_planes;
|
||||||
|
if (conf >= confThreshold)
|
||||||
|
{
|
||||||
|
// Local quadratic fit of cost and subpixel refinement.
|
||||||
|
double rDisp = bestplane;
|
||||||
|
double rCost = minval;
|
||||||
|
if (doSubPixRefinement)
|
||||||
|
{
|
||||||
|
if (bestplane >= 1 && bestplane < m_planes - 1)
|
||||||
|
{
|
||||||
|
double yl = pV[bestplane - 1];
|
||||||
|
double xc = bestplane;
|
||||||
|
double yc = minval;
|
||||||
|
double yu = pV[bestplane + 1];
|
||||||
|
double d2 = yu - yc + yl - yc;
|
||||||
|
double d1 = 0.5 * (yu - yl);
|
||||||
|
if (fabs(d2) > fabs(d1))
|
||||||
|
{
|
||||||
|
rDisp = xc - d1 / d2;
|
||||||
|
rCost = yc + 0.5 * d1 * (rDisp - xc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
pDisp[x] = (float)(rDisp - Dim);
|
||||||
|
pConf[x] = (unsigned char)conf;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pDisp[x] = FLT_MAX;
|
||||||
|
pConf[x] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
~DSI()
|
||||||
|
{
|
||||||
|
free();
|
||||||
|
}
|
||||||
|
|
||||||
|
void free()
|
||||||
|
{
|
||||||
|
if (m_data != NULL)
|
||||||
|
_aligned_free(m_data);
|
||||||
|
m_data = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t m_cols;
|
||||||
|
uint64_t m_rows;
|
||||||
|
uint64_t m_planes;
|
||||||
|
short *m_data;
|
||||||
|
};
|
||||||
|
|
||||||
|
void getDispMap2(DSI &dv1, DSI &dv2, int confThreshold, float * dispMap, unsigned char * confMap);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
void dump(vt::CByteImg& left, vt::CByteImg& right, int flag)
|
||||||
|
{
|
||||||
|
int w = m_cols;
|
||||||
|
int h = m_rows;
|
||||||
|
for (int d = 0; d < m_planes; d++)
|
||||||
|
{
|
||||||
|
vt::CByteImg D;
|
||||||
|
D.Create(w, h);
|
||||||
|
D.Fill(byte(255));
|
||||||
|
for (int i = 0; i < h; i++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < w; j++)
|
||||||
|
{
|
||||||
|
{
|
||||||
|
D(j, i) = (*this)(j, i, d);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
vt::wstring fn;
|
||||||
|
fn.format_with_resize(L"dsi%d/disp-%04d.png", flag, d);
|
||||||
|
vt::VtSaveImage(fn, D);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,619 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "sgmstereo.h"
|
||||||
|
#include "dsimage.h"
|
||||||
|
#include "emmintrin.h"
|
||||||
|
|
||||||
|
SGMStereo::SGMStereo(int _w, int _h, int minDisparity, int maxDisparity, int numDirections, int sgmConfidenceThreshold, int doSubPixRefinement,
|
||||||
|
float smoothness,
|
||||||
|
float penalty1,
|
||||||
|
float penalty2,
|
||||||
|
float alpha,
|
||||||
|
int doSequential)
|
||||||
|
{
|
||||||
|
m_w = _w;
|
||||||
|
m_h = _h;
|
||||||
|
m_smoothness = smoothness;
|
||||||
|
m_penalty1 = penalty1;
|
||||||
|
m_penalty2 = penalty2;
|
||||||
|
m_alpha = alpha;
|
||||||
|
m_minDisparity = minDisparity;
|
||||||
|
m_maxDisparity = maxDisparity;
|
||||||
|
m_numDirections = numDirections;
|
||||||
|
m_sgmConfidenceThreshold = sgmConfidenceThreshold;
|
||||||
|
m_doSubPixRefinement = doSubPixRefinement;
|
||||||
|
m_doSequential = doSequential;
|
||||||
|
|
||||||
|
if (minDisparity >= maxDisparity)
|
||||||
|
{
|
||||||
|
printf("[ERROR] Invalid Disparity Range [%d -- %d] ...\n", minDisparity, maxDisparity);
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int dispRange = maxDisparity - minDisparity;
|
||||||
|
|
||||||
|
m_dsi.create(_w, _h, dispRange);
|
||||||
|
|
||||||
|
if (m_doSequential)
|
||||||
|
{
|
||||||
|
messages.create(_w, _h, dispRange);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
messages_hor.create(_w, _h, dispRange);
|
||||||
|
messages_ver.create(_w, _h, dispRange);
|
||||||
|
}
|
||||||
|
|
||||||
|
float rec_penalty2 = 1.0f / m_penalty2;
|
||||||
|
wLUT = new float[256];
|
||||||
|
for (int i = 0; i < 256; i++)
|
||||||
|
{
|
||||||
|
wLUT[i] = m_penalty1 + m_alpha * exp(-i * rec_penalty2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void SGMStereo::calculateDSI_sse(unsigned char *L, unsigned char * R)
|
||||||
|
{
|
||||||
|
int cols = m_w;
|
||||||
|
int rows = m_h;
|
||||||
|
|
||||||
|
#pragma omp parallel for schedule(dynamic,1)
|
||||||
|
|
||||||
|
for (int y = 1; y < rows - 1; y++)
|
||||||
|
{
|
||||||
|
int off1 = cols * y;
|
||||||
|
int off0 = off1 - cols;
|
||||||
|
int off2 = off1 + cols;
|
||||||
|
for (int x = 1; x < cols - 1; x++)
|
||||||
|
{
|
||||||
|
float u_00 = (float)L[off0 + x - 1];
|
||||||
|
float u_10 = (float)L[off0 + x];
|
||||||
|
float u_20 = (float)L[off0 + x + 1];
|
||||||
|
float u_01 = (float)L[off1 + x - 1];
|
||||||
|
float u_11 = (float)L[off1 + x];
|
||||||
|
float u_21 = (float)L[off1 + x + 1];
|
||||||
|
float u_02 = (float)L[off2 + x - 1];
|
||||||
|
float u_12 = (float)L[off2 + x];
|
||||||
|
float u_22 = (float)L[off2 + x + 1];
|
||||||
|
|
||||||
|
for (int disp = m_minDisparity; disp < m_maxDisparity; disp += 4)
|
||||||
|
{
|
||||||
|
if (x + disp - 1 >= 0 && x + disp + 5 < m_w)
|
||||||
|
{
|
||||||
|
float a[6], b[6], c[6];
|
||||||
|
for (int k = -1; k < 5; k++)
|
||||||
|
{
|
||||||
|
a[k + 1] = (float)R[off0 + x + disp + k];
|
||||||
|
b[k + 1] = (float)R[off1 + x + disp + k];
|
||||||
|
c[k + 1] = (float)R[off2 + x + disp + k];
|
||||||
|
}
|
||||||
|
|
||||||
|
__m128 v_00 = _mm_set_ps(a[0], a[1], a[2], a[3]);
|
||||||
|
__m128 v_10 = _mm_set_ps(a[1], a[2], a[3], a[4]);
|
||||||
|
__m128 v_20 = _mm_set_ps(a[2], a[3], a[4], a[5]);
|
||||||
|
__m128 v_01 = _mm_set_ps(b[0], b[1], b[2], b[3]);
|
||||||
|
__m128 v_11 = _mm_set_ps(b[1], b[2], b[3], b[4]);
|
||||||
|
__m128 v_21 = _mm_set_ps(b[2], b[3], b[4], b[5]);
|
||||||
|
__m128 v_02 = _mm_set_ps(c[0], c[1], c[2], c[3]);
|
||||||
|
__m128 v_12 = _mm_set_ps(c[1], c[2], c[3], c[4]);
|
||||||
|
__m128 v_22 = _mm_set_ps(c[2], c[3], c[4], c[5]);
|
||||||
|
|
||||||
|
float sum_1 = (u_00 + u_10 + u_20 + u_01 + u_11 + u_21 + u_02 + u_12 + u_22) / 9.0f;
|
||||||
|
__m128 sum_2 = _mm_div_ps(_mm_add_ps(v_00, _mm_add_ps(v_10, _mm_add_ps(v_20, _mm_add_ps(v_01, _mm_add_ps(v_11, _mm_add_ps(v_21, _mm_add_ps(v_02, _mm_add_ps(v_12, v_22)))))))), _mm_set1_ps(9.0f));
|
||||||
|
__m128 val_1, val_2, sum_v1_x_v1, sum_v2_x_v2, sum_v1_x_v2;
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_00 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_00, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_mul_ps(val_1, val_1);
|
||||||
|
sum_v2_x_v2 = _mm_mul_ps(val_2, val_2);
|
||||||
|
sum_v1_x_v2 = _mm_mul_ps(val_1, val_2);
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_10 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_10, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_20 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_20, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_01 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_01, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_11 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_11, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_21 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_21, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_02 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_02, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_12 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_12, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
val_1 = _mm_set1_ps(u_22 - sum_1);
|
||||||
|
val_2 = _mm_sub_ps(v_22, sum_2);
|
||||||
|
sum_v1_x_v1 = _mm_add_ps(sum_v1_x_v1, _mm_mul_ps(val_1, val_1));
|
||||||
|
sum_v2_x_v2 = _mm_add_ps(sum_v2_x_v2, _mm_mul_ps(val_2, val_2));
|
||||||
|
sum_v1_x_v2 = _mm_add_ps(sum_v1_x_v2, _mm_mul_ps(val_1, val_2));
|
||||||
|
|
||||||
|
__m128 ncc1 = _mm_div_ps(sum_v1_x_v2, _mm_sqrt_ps(_mm_max_ps(_mm_mul_ps(sum_v1_x_v1, sum_v2_x_v2), _mm_set1_ps(0.01f))));
|
||||||
|
__m128 score = _mm_min_ps(_mm_mul_ps(_mm_sub_ps(_mm_set1_ps(1.0f), ncc1), _mm_set1_ps(255.0f)), _mm_set1_ps(255.0f));
|
||||||
|
float scores[4];
|
||||||
|
_mm_storeu_ps(scores, score);
|
||||||
|
short *pDSI = m_dsi(x, y);
|
||||||
|
int indexDisp = disp - m_minDisparity;
|
||||||
|
pDSI[indexDisp + 3] = (short)scores[0];
|
||||||
|
pDSI[indexDisp + 2] = (short)scores[1];
|
||||||
|
pDSI[indexDisp + 1] = (short)scores[2];
|
||||||
|
pDSI[indexDisp] = (short)scores[3];
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
short *pDSI = m_dsi(x, y);
|
||||||
|
int indexDisp = disp - m_minDisparity;
|
||||||
|
pDSI[indexDisp + 3] = (short)255;
|
||||||
|
pDSI[indexDisp + 2] = (short)255;
|
||||||
|
pDSI[indexDisp + 1] = (short)255;
|
||||||
|
pDSI[indexDisp] = (short)255;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void subtractMinVal(short *pMessage, int size)
|
||||||
|
{
|
||||||
|
__m128i* pM = (__m128i*)pMessage;
|
||||||
|
__m128i minval = *pM;
|
||||||
|
for (int i = 1; i < size / 8; i++)
|
||||||
|
{
|
||||||
|
pM++;
|
||||||
|
minval = _mm_min_epi16(*pM, minval);
|
||||||
|
}
|
||||||
|
|
||||||
|
union u
|
||||||
|
{
|
||||||
|
__m128i m;
|
||||||
|
short s[8];
|
||||||
|
} x;
|
||||||
|
|
||||||
|
x.m = minval;
|
||||||
|
short val = __min(__min(__min(x.s[0], x.s[1]), __min(x.s[2], x.s[3])), __min(__min(x.s[4], x.s[5]), __min(x.s[6], x.s[7])));
|
||||||
|
|
||||||
|
minval = _mm_set1_epi16(val);
|
||||||
|
|
||||||
|
for (int i = 0; i < size / 8; i++)
|
||||||
|
{
|
||||||
|
*pM = _mm_sub_epi16(*pM, minval);
|
||||||
|
pM--;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void SGMStereo::messagePassing(short *pData, short *pBuffer1, short *pDMessage, int size, float weight, short smoothness)
|
||||||
|
{
|
||||||
|
short pen1 = smoothness;
|
||||||
|
short pen2 = (short)(smoothness*weight);
|
||||||
|
|
||||||
|
#if 1
|
||||||
|
__m128i* pB1 = (__m128i*)pBuffer1;
|
||||||
|
__m128i* pDM = (__m128i*)pDMessage;
|
||||||
|
__m128i* pD = (__m128i*)pData;
|
||||||
|
__m128i penalty1 = _mm_set1_epi16(pen1);
|
||||||
|
__m128i penalty2 = _mm_set1_epi16(pen2);
|
||||||
|
__m128i buffer1, buffer2, buffer3;
|
||||||
|
|
||||||
|
subtractMinVal(pBuffer1, size);
|
||||||
|
|
||||||
|
short newval(255*64), newval2(255*64);
|
||||||
|
for (int i = 0; i < size/8; i++)
|
||||||
|
{
|
||||||
|
buffer1 = _mm_slli_si128(*pB1, 2);
|
||||||
|
buffer3 = _mm_insert_epi16(buffer1, newval, 0);
|
||||||
|
buffer1 = _mm_add_epi16(buffer3,penalty1);
|
||||||
|
|
||||||
|
buffer2 = _mm_srli_si128(*pB1, 2);
|
||||||
|
if (i*8+8 < size)
|
||||||
|
{
|
||||||
|
newval = pBuffer1[i*8+7];
|
||||||
|
newval2 = pBuffer1[i*8+8];
|
||||||
|
}
|
||||||
|
else
|
||||||
|
newval2 = 255*64;
|
||||||
|
|
||||||
|
buffer3 = _mm_insert_epi16(buffer2, newval2, 7);
|
||||||
|
buffer2 = _mm_add_epi16(buffer3,penalty1);
|
||||||
|
buffer3 = _mm_min_epi16(buffer1, _mm_min_epi16(buffer2, _mm_min_epi16(*pB1, penalty2)));
|
||||||
|
|
||||||
|
*pB1 = _mm_add_epi16(buffer3, *pD);
|
||||||
|
*pDM = _mm_add_epi16(*pDM, *pB1);
|
||||||
|
|
||||||
|
pB1++;
|
||||||
|
pDM++;
|
||||||
|
pD++;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
int limit = 32768;
|
||||||
|
short mem[2048];
|
||||||
|
short minval = limit - 1;
|
||||||
|
|
||||||
|
for (int i = 0; i < size; i++)
|
||||||
|
{
|
||||||
|
minval = __min(minval, pBuffer1[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < size; i++)
|
||||||
|
{
|
||||||
|
pBuffer1[i] -= minval;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < size; i++)
|
||||||
|
{
|
||||||
|
short minval1 = __min(pBuffer1[__max(i - 1, 0)], pBuffer1[__min(i + 1, size - 1)]) + pen1;
|
||||||
|
short minval2 = __min(pBuffer1[i], pen2);
|
||||||
|
mem[i] = __min(minval1, minval2);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < size; i++)
|
||||||
|
{
|
||||||
|
short val = mem[i] + pData[i];
|
||||||
|
if (val + pDMessage[i] >= limit)
|
||||||
|
{
|
||||||
|
pDMessage[i] = limit - 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pDMessage[i] += val;
|
||||||
|
}
|
||||||
|
pBuffer1[i] = val;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void SGMStereo::scanlineOptimization(DSI &dv, DSI &msgs, unsigned char* img, float *lut, int dx_, int dy_)
|
||||||
|
{
|
||||||
|
int cols = (int)dv.m_cols;
|
||||||
|
int rows = (int)dv.m_rows;
|
||||||
|
int planes = (int)dv.m_planes;
|
||||||
|
|
||||||
|
std::vector<float> startx;
|
||||||
|
std::vector<float> starty;
|
||||||
|
startx.reserve(2 * rows + 2 * cols);
|
||||||
|
starty.reserve(2 * rows + 2 * cols);
|
||||||
|
|
||||||
|
for (int ys = 0; ys < rows; ys++)
|
||||||
|
{
|
||||||
|
for (int xs = 0; xs < cols; xs++)
|
||||||
|
{
|
||||||
|
if ((xs == 0 && dx_ == 1) || (ys == 0 && dy_ == 1) || (ys == rows-1 && dy_ == -1) || (xs == cols-1 && dx_ == -1))
|
||||||
|
{
|
||||||
|
startx.push_back(float(xs));
|
||||||
|
starty.push_back(float(ys));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
short * buffervec = (short*)_aligned_malloc(planes * sizeof(short), 16);
|
||||||
|
|
||||||
|
float dist = sqrt((float)(dx_*dx_+dy_*dy_));
|
||||||
|
|
||||||
|
for (int j = 0; j < (int)startx.size(); j++)
|
||||||
|
{
|
||||||
|
int x = (int)startx[j];
|
||||||
|
int y = (int)starty[j];
|
||||||
|
//uint64_t x64 = (uint64_t)x;
|
||||||
|
//uint64_t y64 = (uint64_t)y;
|
||||||
|
|
||||||
|
int dx = dx_;
|
||||||
|
int dy = dy_;
|
||||||
|
|
||||||
|
for (int i = 0; i < planes; i++)
|
||||||
|
{
|
||||||
|
buffervec[i] = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
short smoothness = (short)(m_smoothness / dist);
|
||||||
|
bool forward = true;
|
||||||
|
|
||||||
|
int oldColor = 0;
|
||||||
|
do
|
||||||
|
{
|
||||||
|
int newIntensity = img[y*cols + x];
|
||||||
|
int diff = abs(newIntensity - oldColor);
|
||||||
|
oldColor = newIntensity;
|
||||||
|
float weight = lut[diff];
|
||||||
|
|
||||||
|
messagePassing(dv(x,y), buffervec, msgs(x,y), planes, weight, smoothness);
|
||||||
|
|
||||||
|
y+=dy;
|
||||||
|
x+=dx;
|
||||||
|
|
||||||
|
if (forward && (y == rows || x == cols || y < 0))
|
||||||
|
{
|
||||||
|
dx*=-1;
|
||||||
|
dy*=-1;
|
||||||
|
y+=dy;
|
||||||
|
x+=dx;
|
||||||
|
for (int i = 0; i < planes; i++)
|
||||||
|
buffervec[i] = 0;
|
||||||
|
forward = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
while (forward || (y >= 0 && x >= 0 && y < rows && x < cols));
|
||||||
|
|
||||||
|
dx*=-1;
|
||||||
|
dy*=-1;
|
||||||
|
}
|
||||||
|
|
||||||
|
_aligned_free(buffervec);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void SGMStereo::scanlineOptimization_hor(DSI &dv, DSI &msgs, unsigned char *img, float *lut)
|
||||||
|
{
|
||||||
|
int cols = (int)dv.m_cols;
|
||||||
|
int rows = (int)dv.m_rows;
|
||||||
|
int planes = (int)dv.m_planes;
|
||||||
|
int bufsize = planes * sizeof(short);
|
||||||
|
short * buf = (short*)_aligned_malloc(bufsize, 16);
|
||||||
|
short smoothness = (short)(m_smoothness);
|
||||||
|
|
||||||
|
for (int y = 0; y < rows; y++)
|
||||||
|
{
|
||||||
|
int offset = y * cols;
|
||||||
|
int oldIntensity = 0;
|
||||||
|
memset(buf, 0, bufsize);
|
||||||
|
for (int x = 0; x < cols; x++)
|
||||||
|
{
|
||||||
|
int newIntensity = img[offset + x];
|
||||||
|
int diff = abs(newIntensity - oldIntensity);
|
||||||
|
oldIntensity = newIntensity;
|
||||||
|
float weight = lut[diff];
|
||||||
|
messagePassing(dv(x, y), buf, msgs(x, y), int(planes), weight, smoothness);
|
||||||
|
}
|
||||||
|
oldIntensity = 0;
|
||||||
|
memset(buf, 0, bufsize);
|
||||||
|
for (int x = cols-1; x >= 0; x--)
|
||||||
|
{
|
||||||
|
int newIntensity = img[offset + x];
|
||||||
|
int diff = abs(newIntensity - oldIntensity);
|
||||||
|
oldIntensity = newIntensity;
|
||||||
|
float weight = lut[diff];
|
||||||
|
messagePassing(dv(x, y), buf, msgs(x, y), planes, weight, smoothness);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
_aligned_free(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
void SGMStereo::scanlineOptimization_vert(DSI &dv, DSI &msgs, unsigned char *img, float *lut)
|
||||||
|
{
|
||||||
|
int cols = (int)dv.m_cols;
|
||||||
|
int rows = (int)dv.m_rows;
|
||||||
|
int planes = (int)dv.m_planes;
|
||||||
|
int bufsize = planes * sizeof(short);
|
||||||
|
short * buf = (short*)_aligned_malloc(bufsize, 16);
|
||||||
|
short smoothness = (short)(m_smoothness);
|
||||||
|
for (int x = 0; x < cols; x++)
|
||||||
|
{
|
||||||
|
int offset = x;
|
||||||
|
int oldIntensity = 0;
|
||||||
|
memset(buf, 0, bufsize);
|
||||||
|
for (int y = 0; y < rows; y++)
|
||||||
|
{
|
||||||
|
int newIntensity = img[offset];
|
||||||
|
int diff = abs(newIntensity - oldIntensity);
|
||||||
|
oldIntensity = newIntensity;
|
||||||
|
float weight = lut[diff];
|
||||||
|
messagePassing(dv(x, y), buf, msgs(x, y), planes, weight, smoothness);
|
||||||
|
offset += cols;
|
||||||
|
}
|
||||||
|
|
||||||
|
offset = cols * (rows - 1) + x;
|
||||||
|
oldIntensity = 0;
|
||||||
|
memset(buf, 0, bufsize);
|
||||||
|
for (int y = rows - 1; y > 0; y--)
|
||||||
|
{
|
||||||
|
int newIntensity = img[offset];
|
||||||
|
int diff = abs(newIntensity - oldIntensity);
|
||||||
|
oldIntensity = newIntensity;
|
||||||
|
float weight = lut[diff];
|
||||||
|
messagePassing(dv(x, y), buf, msgs(x, y), planes, weight, smoothness);
|
||||||
|
offset -= cols;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
_aligned_free(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void SGMStereo::Run(
|
||||||
|
unsigned char * iLeft,
|
||||||
|
unsigned char * iRight,
|
||||||
|
float* dispMap,
|
||||||
|
unsigned char* confMap)
|
||||||
|
{
|
||||||
|
calculateDSI_sse(iLeft, iRight);
|
||||||
|
|
||||||
|
if (m_doSequential)
|
||||||
|
{
|
||||||
|
messages.setzero();
|
||||||
|
scanlineOptimization_hor(m_dsi, messages, iLeft, wLUT);
|
||||||
|
scanlineOptimization_vert(m_dsi, messages, iLeft, wLUT);
|
||||||
|
if (m_numDirections == 8)
|
||||||
|
{
|
||||||
|
scanlineOptimization(m_dsi, messages, iLeft, wLUT, 1, 1);
|
||||||
|
scanlineOptimization(m_dsi, messages, iLeft, wLUT, 1, -1);
|
||||||
|
}
|
||||||
|
messages.getDispMap(m_sgmConfidenceThreshold, m_doSubPixRefinement, dispMap, confMap);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
messages_hor.setzero();
|
||||||
|
messages_ver.setzero();
|
||||||
|
|
||||||
|
#pragma omp parallel for
|
||||||
|
for (int k = 0; k < 2; k++)
|
||||||
|
{
|
||||||
|
if (k==0)
|
||||||
|
scanlineOptimization_hor(m_dsi, messages_hor, iLeft, wLUT);
|
||||||
|
if (k==1)
|
||||||
|
scanlineOptimization_vert(m_dsi, messages_ver, iLeft, wLUT);
|
||||||
|
}
|
||||||
|
getDispMap2(messages_hor, messages_ver, m_sgmConfidenceThreshold, dispMap, confMap);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void SGMStereo::free()
|
||||||
|
{
|
||||||
|
m_dsi.free();
|
||||||
|
|
||||||
|
if (m_doSequential)
|
||||||
|
{
|
||||||
|
messages.free();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
messages_hor.free();
|
||||||
|
messages_ver.free();
|
||||||
|
}
|
||||||
|
|
||||||
|
delete[] wLUT;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
void SGMStereo::calculateDSI(CByteImg& refImage, CByteImg& nbrImage)
|
||||||
|
{
|
||||||
|
int cols = refImage.Width();
|
||||||
|
int rows = refImage.Height();
|
||||||
|
|
||||||
|
const int nw = nbrImage.Width();
|
||||||
|
const int nh = nbrImage.Height();
|
||||||
|
|
||||||
|
int disparityRange = m_maxDisparity - m_minDisparity;
|
||||||
|
|
||||||
|
printf("\n Calculate DSI (%4d x %4d x %4d), disparity-range=(%d %d)\n\n", cols, rows, disparityRange, m_minDisparity, m_maxDisparity);
|
||||||
|
|
||||||
|
m_dsi.create(cols, rows, disparityRange, 255);
|
||||||
|
|
||||||
|
for (int disp = m_minDisparity; disp < m_maxDisparity; disp++)
|
||||||
|
{
|
||||||
|
#pragma omp parallel for schedule(dynamic,1)
|
||||||
|
for (int y = 1; y < rows - 1; y++)
|
||||||
|
{
|
||||||
|
float L10_ = refImage(-1 + 1, -1 + y);
|
||||||
|
float L20_ = refImage(0 + 1, -1 + y);
|
||||||
|
float L11_ = refImage(-1 + 1, 0 + y);
|
||||||
|
float L21_ = refImage(0 + 1, 0 + y);
|
||||||
|
float L12_ = refImage(-1 + 1, +1 + y);
|
||||||
|
float L22_ = refImage(0 + 1, +1 + y);
|
||||||
|
|
||||||
|
float R10_ = 0;
|
||||||
|
float R20_ = 0;
|
||||||
|
float R11_ = 0;
|
||||||
|
float R21_ = 0;
|
||||||
|
float R12_ = 0;
|
||||||
|
float R22_ = 0;
|
||||||
|
if (disp >= 0 && disp < nw - 1)
|
||||||
|
{
|
||||||
|
R10_ = nbrImage(-1 + disp + 1, -1 + y);
|
||||||
|
R20_ = nbrImage(0 + disp + 1, -1 + y);
|
||||||
|
R11_ = nbrImage(-1 + disp + 1, 0 + y);
|
||||||
|
R21_ = nbrImage(0 + disp + 1, 0 + y);
|
||||||
|
R12_ = nbrImage(-1 + disp + 1, +1 + y);
|
||||||
|
R22_ = nbrImage(0 + disp + 1, +1 + y);
|
||||||
|
}
|
||||||
|
for (int x = 1; x < cols - 1; x++)
|
||||||
|
{
|
||||||
|
float L00_ = L10_;
|
||||||
|
float L01_ = L11_;
|
||||||
|
float L02_ = L12_;
|
||||||
|
|
||||||
|
L10_ = L20_;
|
||||||
|
L11_ = L21_;
|
||||||
|
L12_ = L22_;
|
||||||
|
|
||||||
|
L20_ = refImage(+1 + x, -1 + y);
|
||||||
|
L21_ = refImage(+1 + x, 0 + y);
|
||||||
|
L22_ = refImage(+1 + x, +1 + y);
|
||||||
|
|
||||||
|
float Lavg = (L00_ + L10_ + L20_ + L01_ + L11_ + L21_ + L02_ + L12_ + L22_) / 9.0f;
|
||||||
|
|
||||||
|
float L00 = L00_ - Lavg; float L10 = L10_ - Lavg; float L20 = L20_ - Lavg;
|
||||||
|
float L01 = L01_ - Lavg; float L11 = L11_ - Lavg; float L21 = L21_ - Lavg;
|
||||||
|
float L02 = L02_ - Lavg; float L12 = L12_ - Lavg; float L22 = L22_ - Lavg;
|
||||||
|
|
||||||
|
float LL = L00 * L00 + L10 * L10 + L20 * L20 + L01 * L01 + L11 * L11 + L21 * L21 + L02 * L02 + L12 * L12 + L22 * L22;
|
||||||
|
|
||||||
|
float R00_ = R10_;
|
||||||
|
float R01_ = R11_;
|
||||||
|
float R02_ = R12_;
|
||||||
|
|
||||||
|
R10_ = R20_;
|
||||||
|
R11_ = R21_;
|
||||||
|
R12_ = R22_;
|
||||||
|
|
||||||
|
if (1 + x + disp < 0 || +1 + x + disp >= nw)
|
||||||
|
{
|
||||||
|
int indexDisp = disp - m_minDisparity;
|
||||||
|
m_dsi(x, y, indexDisp) = short(255);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
R20_ = nbrImage(+1 + x + disp, -1 + y);
|
||||||
|
R21_ = nbrImage(+1 + x + disp, 0 + y);
|
||||||
|
R22_ = nbrImage(+1 + x + disp, +1 + y);
|
||||||
|
|
||||||
|
float Ravg = (R00_ + R10_ + R20_ + R01_ + R11_ + R21_ + R02_ + R12_ + R22_) / 9.0f;
|
||||||
|
|
||||||
|
float R00 = R00_ - Ravg; float R10 = R10_ - Ravg; float R20 = R20_ - Ravg;
|
||||||
|
float R01 = R01_ - Ravg; float R11 = R11_ - Ravg; float R21 = R21_ - Ravg;
|
||||||
|
float R02 = R02_ - Ravg; float R12 = R12_ - Ravg; float R22 = R22_ - Ravg;
|
||||||
|
|
||||||
|
float RR = R00 * R00 + R10 * R10 + R20 * R20 + R01 * R01 + R11 * R11 + R21 * R21 + R02 * R02 + R12 * R12 + R22 * R22;
|
||||||
|
|
||||||
|
float LR = L00 * R00 + L10 * R10 + L20 * R20 + L01 * R01 + L11 * R11 + L21 * R21 + L02 * R02 + L12 * R12 + L22 * R22;
|
||||||
|
|
||||||
|
float ncc = LR / VtMax(sqrt(LL * RR), 1e-2f);
|
||||||
|
int indexDisp = disp - m_minDisparity;
|
||||||
|
int cost = (int)VtMin((1.0f - ncc) * 255.0f, 255.0f);
|
||||||
|
m_dsi(x, y, indexDisp) = cost;
|
||||||
|
|
||||||
|
} //for x
|
||||||
|
} //for y
|
||||||
|
} //for disp
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,51 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#ifndef sgm_stereo_h
|
||||||
|
#define sgm_stereo_h
|
||||||
|
|
||||||
|
#include "dsimage.h"
|
||||||
|
|
||||||
|
class SGMStereo
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
void calculateDSI_sse(unsigned char *refImage, unsigned char * nbrImage);
|
||||||
|
void messagePassing(short *pData, short *pBuffer1, short *pDMessage, int size, float weight, short smoothness);
|
||||||
|
void scanlineOptimization(DSI &dv, DSI &messages, unsigned char * img, float *lut, int dx_, int dy_);
|
||||||
|
void scanlineOptimization_hor(DSI &dv, DSI &messages, unsigned char *img, float *lut);
|
||||||
|
void scanlineOptimization_vert(DSI &dv, DSI &messages, unsigned char *img, float *lut);
|
||||||
|
|
||||||
|
DSI m_dsi, messages;
|
||||||
|
|
||||||
|
DSI messages_hor, messages_ver;
|
||||||
|
|
||||||
|
float * wLUT;
|
||||||
|
|
||||||
|
int m_w, m_h;
|
||||||
|
|
||||||
|
float m_smoothness;
|
||||||
|
float m_penalty1;
|
||||||
|
float m_penalty2;
|
||||||
|
float m_alpha;
|
||||||
|
int m_minDisparity;
|
||||||
|
int m_maxDisparity;
|
||||||
|
int m_numDirections;
|
||||||
|
int m_sgmConfidenceThreshold;
|
||||||
|
int m_doSubPixRefinement;
|
||||||
|
int m_doSequential;
|
||||||
|
|
||||||
|
public:
|
||||||
|
SGMStereo(int _w, int _h, int minDisparity, int maxDisparity, int numDirections, int sgmConfidenceThreshold, int doSubPixRefinement,
|
||||||
|
float smoothness,
|
||||||
|
float penalty1,
|
||||||
|
float penalty2,
|
||||||
|
float alpha,
|
||||||
|
int doSequential);
|
||||||
|
|
||||||
|
void Run(unsigned char * iLeft, unsigned char * iRight, float* dispMap, unsigned char* confMap);
|
||||||
|
|
||||||
|
void free();
|
||||||
|
};
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,146 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Debug|x64">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|x64">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="dsimage.cpp" />
|
||||||
|
<ClCompile Include="sgmstereo.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="sgmstereo.h" />
|
||||||
|
<ClInclude Include="dsimage.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<ProjectGuid>{A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}</ProjectGuid>
|
||||||
|
<RootNamespace>Features</RootNamespace>
|
||||||
|
<SccProjectName>SAK</SccProjectName>
|
||||||
|
<SccAuxPath>SAK</SccAuxPath>
|
||||||
|
<SccLocalPath>SAK</SccLocalPath>
|
||||||
|
<SccProvider>SAK</SccProvider>
|
||||||
|
<ProjectName>sgmstereo</ProjectName>
|
||||||
|
<WindowsTargetPlatformVersion>10.0.17134.0</WindowsTargetPlatformVersion>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup>
|
||||||
|
<_ProjectFileVersion>10.0.30319.1</_ProjectFileVersion>
|
||||||
|
<OutDir Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<OutDir Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<IntDir Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<OutDir Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<OutDir Condition="'$(Configuration)|$(Platform)'=='Release|x64'">..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<IntDir Condition="'$(Configuration)|$(Platform)'=='Release|x64'">..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<AdditionalIncludeDirectories>VRLib\inc\VRLibCore;VRLib\inc\VRLibCV;VRLib\inc\VRLibImageIO;LibJpeg\inc;LibTiff\inc;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<MinimalRebuild>true</MinimalRebuild>
|
||||||
|
<BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks>
|
||||||
|
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
|
||||||
|
<OpenMPSupport>true</OpenMPSupport>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||||
|
<PreprocessorDefinitions>VRLIB_USES_LIBTIFF;VRLIB_USES_LIBJPEG;NOMINMAX;_UNICODE;UNICODE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<TreatWarningAsError>false</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<AdditionalIncludeDirectories>%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks>
|
||||||
|
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
|
||||||
|
<OpenMPSupport>true</OpenMPSupport>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||||
|
<PreprocessorDefinitions>NOMINMAX;_UNICODE;UNICODE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<TreatWarningAsError>false</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<WholeProgramOptimization>false</WholeProgramOptimization>
|
||||||
|
<AdditionalIncludeDirectories>VRLib\inc\VRLibCore;VRLib\inc\VRLibCV;VRLib\inc\VRLibImageIO;LibJpeg\inc;LibTiff\inc;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
|
||||||
|
<OpenMPSupport>true</OpenMPSupport>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||||
|
<PreprocessorDefinitions>VRLIB_USES_LIBTIFF;VRLIB_USES_LIBJPEG;NOMINMAX;_UNICODE;UNICODE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<TreatWarningAsError>false</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<WholeProgramOptimization>false</WholeProgramOptimization>
|
||||||
|
<AdditionalIncludeDirectories>%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
|
||||||
|
<OpenMPSupport>true</OpenMPSupport>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||||
|
<PreprocessorDefinitions>NOMINMAX;_UNICODE;UNICODE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,71 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdio.h> /* printf */
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
struct SGMOptions
|
||||||
|
{
|
||||||
|
std::string inputDir; // folder contains input images
|
||||||
|
std::string outputDir; // folder for output features files and match files
|
||||||
|
int onlyStereo; // only stereo processing
|
||||||
|
|
||||||
|
int sgmConfidenceThreshold; // value between [0 and 255] confidence threshold cutoff
|
||||||
|
int maxImageDimensionWidth;
|
||||||
|
int minDisparity;
|
||||||
|
int maxDisparity;
|
||||||
|
int numDirections; // 4 or 8
|
||||||
|
int doSequential; // do sequential message passing (or horizontal and vertical in parallel).
|
||||||
|
int doVis; // if 1, then output visualization.
|
||||||
|
int doOut; // if 1, then write output
|
||||||
|
float smoothness;
|
||||||
|
float penalty1;
|
||||||
|
float penalty2;
|
||||||
|
float alpha;
|
||||||
|
int doSubPixRefinement;
|
||||||
|
|
||||||
|
SGMOptions()
|
||||||
|
{
|
||||||
|
inputDir = "";
|
||||||
|
outputDir = "C:/Github/AirSimSGM/SGM/output";
|
||||||
|
sgmConfidenceThreshold = 16;
|
||||||
|
maxImageDimensionWidth = -1;
|
||||||
|
minDisparity = 0;
|
||||||
|
maxDisparity = 192;
|
||||||
|
numDirections = 4;
|
||||||
|
doSequential = 0;
|
||||||
|
doVis = 1;
|
||||||
|
doOut = 0;
|
||||||
|
smoothness = 200.0f;
|
||||||
|
penalty1 = 1.0;
|
||||||
|
penalty2 = 8.0;
|
||||||
|
alpha = 10.0;
|
||||||
|
onlyStereo = 0;
|
||||||
|
doSubPixRefinement = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Print()
|
||||||
|
{
|
||||||
|
printf("\n\n****************** Parameter List *********************\n");
|
||||||
|
wprintf(L" sgmConfidenceThreshold = %d\n", sgmConfidenceThreshold);
|
||||||
|
wprintf(L" maxImageDimensionWidth = %d\n", maxImageDimensionWidth);
|
||||||
|
wprintf(L" minDisparity = %d\n", minDisparity);
|
||||||
|
wprintf(L" maxDisparity = %d\n", maxDisparity);
|
||||||
|
wprintf(L" numDirections = %d\n", numDirections);
|
||||||
|
wprintf(L" smoothness = %f\n", smoothness);
|
||||||
|
wprintf(L" penalty1 = %f\n", penalty1);
|
||||||
|
wprintf(L" penalty2 = %f\n", penalty2);
|
||||||
|
wprintf(L" alpha = %f\n", alpha);
|
||||||
|
wprintf(L" doSequential = %d\n", doSequential);
|
||||||
|
wprintf(L" doVis = %d\n", doVis);
|
||||||
|
wprintf(L" doOut = %d\n", doOut);
|
||||||
|
wprintf(L" onlyStereo = %d\n", onlyStereo);
|
||||||
|
wprintf(L" doSubPixRefinement = %d\n", doSubPixRefinement);
|
||||||
|
printf("*********************************************************\n\n\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,131 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#include "StateStereo.h"
|
||||||
|
#include "sgmstereo.h"
|
||||||
|
#include <stdio.h> /* printf */
|
||||||
|
#include <ctime> /* clock_t, clock, CLOCKS_PER_SEC */
|
||||||
|
|
||||||
|
CStateStereo::CStateStereo()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
CStateStereo::~CStateStereo()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void CStateStereo::Initialize(SGMOptions& params, int m, int n)
|
||||||
|
{
|
||||||
|
inputFrameWidth = n;
|
||||||
|
inputFrameHeight = m;
|
||||||
|
|
||||||
|
outputDir = params.outputDir;
|
||||||
|
minDisp = params.minDisparity;
|
||||||
|
maxDisp = params.maxDisparity;
|
||||||
|
ndisps = maxDisp - minDisp;
|
||||||
|
confThreshold = params.sgmConfidenceThreshold;
|
||||||
|
|
||||||
|
// ensure that disparity range is a multiple of 8
|
||||||
|
int rem = ndisps % 8;
|
||||||
|
int dv = ndisps / 8;
|
||||||
|
if (rem != 0)
|
||||||
|
{
|
||||||
|
ndisps = 8 * (dv+1);
|
||||||
|
maxDisp = ndisps + minDisp;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (params.maxImageDimensionWidth == -1 || inputFrameWidth <= params.maxImageDimensionWidth)
|
||||||
|
{
|
||||||
|
downSampleFactor = 1.0f;
|
||||||
|
processingFrameWidth = inputFrameWidth;
|
||||||
|
processingFrameHeight = inputFrameHeight;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
downSampleFactor = params.maxImageDimensionWidth / (1.0f * inputFrameWidth);
|
||||||
|
processingFrameWidth = params.maxImageDimensionWidth;
|
||||||
|
processingFrameHeight = (int)(downSampleFactor * inputFrameHeight + 0.5f);
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("process at %d x %d resolution, ndisps = %d\n", processingFrameWidth, processingFrameHeight, ndisps);
|
||||||
|
|
||||||
|
dispMap = new float[processingFrameWidth * processingFrameHeight];
|
||||||
|
confMap = new unsigned char[processingFrameWidth * processingFrameHeight];
|
||||||
|
|
||||||
|
sgmStereo = new SGMStereo(processingFrameWidth, processingFrameHeight, -maxDisp, -minDisp, params.numDirections, params.sgmConfidenceThreshold,
|
||||||
|
params.doSubPixRefinement,
|
||||||
|
params.smoothness,
|
||||||
|
params.penalty1,
|
||||||
|
params.penalty2,
|
||||||
|
params.alpha,
|
||||||
|
params.doSequential);
|
||||||
|
}
|
||||||
|
|
||||||
|
void CStateStereo::CleanUp()
|
||||||
|
{
|
||||||
|
if (sgmStereo != NULL)
|
||||||
|
{
|
||||||
|
sgmStereo->free();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void CStateStereo::ProcessFrameAirSim(int frameCounter, float& dtime, const std::vector<uint8_t>& left_image, const std::vector<uint8_t>& right_image)
|
||||||
|
{
|
||||||
|
unsigned char *iL, *iR;
|
||||||
|
|
||||||
|
// sgm stereo
|
||||||
|
if (processingFrameWidth != inputFrameWidth || processingFrameHeight != inputFrameHeight)
|
||||||
|
{
|
||||||
|
printf("[ERROR]: Frame resolution = (%d x %d) is not equal to initialization ...\n", processingFrameWidth, processingFrameHeight);
|
||||||
|
}
|
||||||
|
|
||||||
|
int nP = processingFrameWidth * processingFrameHeight;
|
||||||
|
iL = new unsigned char[nP];
|
||||||
|
iR = new unsigned char[nP];
|
||||||
|
|
||||||
|
int channels = (int)left_image.size()/nP;
|
||||||
|
|
||||||
|
for (int i = 0; i < nP; i++)
|
||||||
|
{
|
||||||
|
{
|
||||||
|
int idx = channels*i;
|
||||||
|
iL[i] = (left_image[idx] + left_image[idx+1] + left_image[idx+2]) / 3;
|
||||||
|
iR[i] = (right_image[idx] + right_image[idx+1] + right_image[idx+2]) / 3;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::clock_t start;
|
||||||
|
start = std::clock();
|
||||||
|
|
||||||
|
sgmStereo->Run(iL, iR, dispMap, confMap);
|
||||||
|
float duration = ( std::clock() - start ) / (float) CLOCKS_PER_SEC;
|
||||||
|
dtime += duration;
|
||||||
|
|
||||||
|
printf("Frame %06d: %5.1f ms, Average fps: %lf\n", frameCounter, duration*1000, 1.0 / (dtime / double(frameCounter+1)));
|
||||||
|
|
||||||
|
delete[] iL;
|
||||||
|
delete[] iR;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
float CStateStereo::GetLeftDisparity(float x, float y)
|
||||||
|
{
|
||||||
|
int ix = (int)(x * processingFrameWidth + 0.5f);
|
||||||
|
int iy = (int)(y * processingFrameHeight + 0.5f);
|
||||||
|
ix = __max(ix, 0);
|
||||||
|
ix = __min(ix, processingFrameWidth - 1);
|
||||||
|
iy = __max(iy, 0);
|
||||||
|
iy = __min(iy, processingFrameHeight - 1);
|
||||||
|
int off = iy*processingFrameWidth + ix;
|
||||||
|
float d = dispMap[off];
|
||||||
|
unsigned char c = confMap[off];
|
||||||
|
if (fabs(d) < ndisps && c >= confThreshold)
|
||||||
|
{
|
||||||
|
return 1.0f - (fabs(d)/float(ndisps));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
return -1.0f;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||||
|
// Licensed under the MIT License.
|
||||||
|
// by Sudipta Sinha
|
||||||
|
// adapted for AirSim by Matthias Mueller
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "../sgmstereo/sgmstereo.h"
|
||||||
|
#include "SGMOptions.h"
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
class CStateStereo
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
float downSampleFactor;
|
||||||
|
std::string outputDir;
|
||||||
|
int inputFrameWidth, inputFrameHeight;
|
||||||
|
int ndisps;
|
||||||
|
int minDisp;
|
||||||
|
int maxDisp;
|
||||||
|
int confThreshold;
|
||||||
|
|
||||||
|
SGMStereo * sgmStereo;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
int processingFrameWidth, processingFrameHeight;
|
||||||
|
|
||||||
|
CStateStereo();
|
||||||
|
~CStateStereo();
|
||||||
|
void Initialize(SGMOptions& params, int m = 144, int n = 256);
|
||||||
|
void CleanUp();
|
||||||
|
void ProcessFrameAirSim(int frameCounter, float& dtime, const std::vector<uint8_t>& left_image, const std::vector<uint8_t>& right_image);
|
||||||
|
float GetLeftDisparity(float x, float y);
|
||||||
|
|
||||||
|
float* dispMap;
|
||||||
|
unsigned char* confMap;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,179 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Debug|x64">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|x64">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="SGMOptions.h" />
|
||||||
|
<ClInclude Include="StateStereo.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="StateStereo.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<ProjectGuid>{E512EB59-4EAB-49D1-9174-0CAF1B40CED0}</ProjectGuid>
|
||||||
|
<Keyword>Win32Proj</Keyword>
|
||||||
|
<RootNamespace>epnp</RootNamespace>
|
||||||
|
<ProjectName>stereoPipeline</ProjectName>
|
||||||
|
<WindowsTargetPlatformVersion>10.0.17134.0</WindowsTargetPlatformVersion>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>v141</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="PropertySheets">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
<IncludePath>..\..\..\..\d\d;..\..\..\..\VisionTools\inc;$(IncludePath)</IncludePath>
|
||||||
|
<OutDir>..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
<IncludePath>..\..\..\..\d\d;..\..\..\..\VisionTools\inc;$(IncludePath)</IncludePath>
|
||||||
|
<OutDir>..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
<IncludePath>$(SolutionDir)dep\sunflower\inc;$(SolutionDir)dep\VisionTools\inc;$(SolutionDir)dep\d\d;$(IncludePath)</IncludePath>
|
||||||
|
<OutDir>..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
<IncludePath>$(IncludePath)</IncludePath>
|
||||||
|
<IntDir>..\..\obj\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<OutDir>..\..\lib\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<PrecompiledHeader>Create</PrecompiledHeader>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<AdditionalIncludeDirectories>..\sgmstereo;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<AdditionalIncludeDirectories>..\sgmstereo;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
|
||||||
|
<OpenMPSupport>true</OpenMPSupport>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<PrecompiledHeader>Create</PrecompiledHeader>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<AdditionalIncludeDirectories>..\sgmstereo;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<AdditionalLibraryDirectories>C:\OpenCV2.2\lib</AdditionalLibraryDirectories>
|
||||||
|
<AdditionalDependencies>opencv_core220.lib; %(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level4</WarningLevel>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<AdditionalIncludeDirectories>..\sgmstereo;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
<RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
|
||||||
|
<OpenMPSupport>true</OpenMPSupport>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<AdditionalLibraryDirectories>C:\OpenCV2.2\lib</AdditionalLibraryDirectories>
|
||||||
|
<AdditionalDependencies>opencv_core220.lib; %(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
Загрузка…
Ссылка в новой задаче