Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

added improved version of the variational stereo correspondence algorithm by Sergey Kosov

Browse Source
pull/13383/head
Vadim Pisarevsky 14 years ago
parent 5f0c312087
commit 7702fa4d61
3 changed files with 193 additions and 77 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      modules/contrib/include/opencv2/contrib/contrib.hpp
  2. 218
      modules/contrib/src/stereovar.cpp
  3. 47
      samples/cpp/stereo_match.cpp

5
modules/contrib/include/opencv2/contrib/contrib.hpp
Unescape View File

@ -568,7 +568,7 @@ namespace cv
{
public:
// Flags
enum {USE_INITIAL_DISPARITY = 1, USE_EQUALIZE_HIST = 2, USE_SMART_ID = 4, USE_MEDIAN_FILTERING = 8};
enum {USE_INITIAL_DISPARITY = 1, USE_EQUALIZE_HIST = 2, USE_SMART_ID = 4, USE_AUTO_PARAMS = 8, USE_MEDIAN_FILTERING = 16};
enum {CYCLE_O, CYCLE_V};
enum {PENALIZATION_TICHONOV, PENALIZATION_CHARBONNIER, PENALIZATION_PERONA_MALIK};
@ -598,7 +598,8 @@ namespace cv
CV_PROP_RW int flags;
private:
void FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level);
void autoParams();
void FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level);
void VCycle_MyFAS(Mat &I1_h, Mat &I2_h, Mat &I2x_h, Mat &u_h, int level);
void VariationalSolver(Mat &I1_h, Mat &I2_h, Mat &I2x_h, Mat &u_h, int level);
};

218
modules/contrib/src/stereovar.cpp
Unescape View File

@ -53,7 +53,7 @@
namespace cv
{
StereoVar::StereoVar() : levels(3), pyrScale(0.5), nIt(3), minDisp(0), maxDisp(16), poly_n(5), poly_sigma(1.2), fi(1000.0f), lambda(0.0f), penalization(PENALIZATION_TICHONOV), cycle(CYCLE_V), flags(USE_SMART_ID)
StereoVar::StereoVar() : levels(3), pyrScale(0.5), nIt(5), minDisp(0), maxDisp(16), poly_n(3), poly_sigma(0), fi(25.0f), lambda(0.03f), penalization(PENALIZATION_TICHONOV), cycle(CYCLE_V), flags(USE_SMART_ID | USE_AUTO_PARAMS)
{
}
@ -65,111 +65,172 @@ StereoVar::~StereoVar()
{
}
static Mat diffX(Mat &img)
static Mat diffX(Mat &src)
{
// TODO try pointers or assm
register int x, y;
Mat dst(img.size(), img.type());
dst.setTo(0);
for (x = 0; x < img.cols - 1; x++)
for (y = 0; y < img.rows; y++)
dst.at<float>(y, x) = img.at<float>(y, x + 1) - img.at<float>(y ,x);
return dst;
register int x, y, cols = src.cols - 1;
Mat dst(src.size(), src.type());
for(y = 0; y < src.rows; y++){
const float* pSrc = src.ptr<float>(y);
float* pDst = dst.ptr<float>(y);
for (x = 0; x <= cols - 8; x += 8) {
__m128 a0 = _mm_loadu_ps(pSrc + x);
__m128 b0 = _mm_loadu_ps(pSrc + x + 1);
__m128 a1 = _mm_loadu_ps(pSrc + x + 4);
__m128 b1 = _mm_loadu_ps(pSrc + x + 5);
b0 = _mm_sub_ps(b0, a0);
b1 = _mm_sub_ps(b1, a1);
_mm_storeu_ps(pDst + x, b0);
_mm_storeu_ps(pDst + x + 4, b1);
}
for( ; x < cols; x++) pDst[x] = pSrc[x+1] - pSrc[x];
pDst[cols] = 0.f;
}
return dst;
}
static Mat Gradient(Mat &img)
static Mat getGradient(Mat &src)
{
Mat sobel, sobelX, sobelY;
img.copyTo(sobelX);
img.copyTo(sobelY);
Sobel(img, sobelX, sobelX.type(), 1, 0, 1);
Sobel(img, sobelY, sobelY.type(), 0, 1, 1);
sobelX = abs(sobelX);
sobelY = abs(sobelY);
add(sobelX, sobelY, sobel);
sobelX.release();
sobelY.release();
return sobel;
register int x, y;
Mat dst(src.size(), src.type());
dst.setTo(0);
for (y = 0; y < src.rows - 1; y++) {
float *pSrc = src.ptr<float>(y);
float *pSrcF = src.ptr<float>(y + 1);
float *pDst = dst.ptr<float>(y);
for (x = 0; x < src.cols - 1; x++)
pDst[x] = fabs(pSrc[x + 1] - pSrc[x]) + fabs(pSrcF[x] - pSrc[x]);
}
return dst;
}
static float g_c(Mat z, int x, int y, float l)
static Mat getG_c(Mat &src, float l)
{
return 0.5f*l / sqrtf(l*l + z.at<float>(y,x)*z.at<float>(y,x));
Mat dst(src.size(), src.type());
for (register int y = 0; y < src.rows; y++) {
float *pSrc = src.ptr<float>(y);
float *pDst = dst.ptr<float>(y);
for (register int x = 0; x < src.cols; x++)
pDst[x] = 0.5f*l / sqrtf(l*l + pSrc[x]*pSrc[x]);
}
return dst;
}
static float g_p(Mat z, int x, int y, float l)
static Mat getG_p(Mat &src, float l)
{
return 0.5f*l*l / (l*l + z.at<float>(y,x)*z.at<float>(y,x)) ;
Mat dst(src.size(), src.type());
for (register int y = 0; y < src.rows; y++) {
float *pSrc = src.ptr<float>(y);
float *pDst = dst.ptr<float>(y);
for (register int x = 0; x < src.cols; x++)
pDst[x] = 0.5f*l*l / (l*l + pSrc[x]*pSrc[x]);
}
return dst;
}
void StereoVar::VariationalSolver(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level)
{
register int n, x, y;
float gl = 1, gr = 1, gu = 1, gd = 1, gc = 4;
Mat g_c, g_p;
Mat U;
Mat Sobel;
u.copyTo(U);
int N = nIt;
float l = lambda;
float Fi = fi;
double scale = pow(pyrScale, (double) level);
if (flags & USE_SMART_ID) {
if (flags & USE_SMART_ID) {
double scale = pow(pyrScale, (double) level) * (1 + pyrScale);
N = (int) (N / scale);
Fi /= (float) scale;
l *= (float) scale;
}
double scale = pow(pyrScale, (double) level);
Fi /= (float) scale;
l *= (float) scale;
int width = u.cols - 1;
int height = u.rows - 1;
for (n = 0; n < N; n++) {
if (penalization != PENALIZATION_TICHONOV) {if(!Sobel.empty()) Sobel.release(); Sobel = Gradient(U);}
for (x = 1; x < u.cols - 1; x++) {
for (y = 1 ; y < u.rows - 1; y++) {
if (penalization != PENALIZATION_TICHONOV) {
Mat gradient = getGradient(U);
switch (penalization) {
case PENALIZATION_CHARBONNIER: g_c = getG_c(gradient, l); break;
case PENALIZATION_PERONA_MALIK: g_p = getG_p(gradient, l); break;
}
gradient.release();
}
for (y = 1 ; y < height; y++) {
float *pU = U.ptr<float>(y);
float *pUu = U.ptr<float>(y + 1);
float *pUd = U.ptr<float>(y - 1);
float *pu = u.ptr<float>(y);
float *pI1 = I1.ptr<float>(y);
float *pI2 = I2.ptr<float>(y);
float *pI2x = I2x.ptr<float>(y);
float *pG_c = NULL, *pG_cu = NULL, *pG_cd = NULL;
float *pG_p = NULL, *pG_pu = NULL, *pG_pd = NULL;
switch (penalization) {
case PENALIZATION_CHARBONNIER:
pG_c = g_c.ptr<float>(y);
pG_cu = g_c.ptr<float>(y + 1);
pG_cd = g_c.ptr<float>(y - 1);
break;
case PENALIZATION_PERONA_MALIK:
pG_p = g_p.ptr<float>(y);
pG_pu = g_p.ptr<float>(y + 1);
pG_pd = g_p.ptr<float>(y - 1);
break;
}
for (x = 1; x < width; x++) {
switch (penalization) {
case PENALIZATION_CHARBONNIER:
gc = g_c(Sobel, x, y, l);
gl = gc + g_c(Sobel, x - 1, y, l);
gr = gc + g_c(Sobel, x + 1, y, l);
gu = gc + g_c(Sobel, x, y + 1, l);
gd = gc + g_c(Sobel, x, y - 1, l);
gc = gl + gr + gu + gd;
gc = pG_c[x];
gl = gc + pG_c[x - 1];
gr = gc + pG_c[x + 1];
gu = gc + pG_cu[x];
gd = gc + pG_cd[x];
gc = gl + gr + gu + gd;
break;
case PENALIZATION_PERONA_MALIK:
gc = g_p(Sobel, x, y, l);
gl = gc + g_p(Sobel, x - 1, y, l);
gr = gc + g_p(Sobel, x + 1, y, l);
gu = gc + g_p(Sobel, x, y + 1, l);
gd = gc + g_p(Sobel, x, y - 1, l);
gc = pG_p[x];
gl = gc + pG_p[x - 1];
gr = gc + pG_p[x + 1];
gu = gc + pG_pu[x];
gd = gc + pG_pd[x];
gc = gl + gr + gu + gd;
break;
}
float fi = Fi;
if (maxDisp > minDisp) {
if (U.at<float>(y,x) > maxDisp * scale) {fi*=1000; U.at<float>(y,x) = static_cast<float>(maxDisp * scale);}
if (U.at<float>(y,x) < minDisp * scale) {fi*=1000; U.at<float>(y,x) = static_cast<float>(minDisp * scale);}
if (pU[x] > maxDisp * scale) {fi *= 1000; pU[x] = static_cast<float>(maxDisp * scale);}
if (pU[x] < minDisp * scale) {fi *= 1000; pU[x] = static_cast<float>(minDisp * scale);}
}
int A = (int) (U.at<float>(y,x));
int neg = 0; if (U.at<float>(y,x) <= 0) neg = -1;
int A = static_cast<int>(pU[x]);
int neg = 0; if (pU[x] <= 0) neg = -1;
if (x + A >= u.cols)
u.at<float>(y, x) = U.at<float>(y, u.cols - A - 1);
if (x + A > width)
pu[x] = pU[width - A];
else if (x + A + neg < 0)
u.at<float>(y, x) = U.at<float>(y, - A + 2);
pu[x] = pU[- A + 2];
else {
u.at<float>(y, x) = A + (I2x.at<float>(y, x + A + neg) * (I1.at<float>(y, x) - I2.at<float>(y, x + A))
+ fi * (gr * U.at<float>(y, x + 1) + gl * U.at<float>(y, x - 1) + gu * U.at<float>(y + 1, x) + gd * U.at<float>(y - 1, x) - gc * A))
/ (I2x.at<float>(y, x + A + neg) * I2x.at<float>(y, x + A + neg) + gc * fi) ;
pu[x] = A + (pI2x[x + A + neg] * (pI1[x] - pI2[x + A])
+ fi * (gr * pU[x + 1] + gl * pU[x - 1] + gu * pUu[x] + gd * pUd[x] - gc * A))
/ (pI2x[x + A + neg] * pI2x[x + A + neg] + gc * fi) ;
}
}//y
}// x
pu[0] = pu[1];
pu[width] = pu[width - 1];
}// y
for (x = 0; x <= width; x++) {
u.at<float>(0, x) = u.at<float>(1, x);
u.at<float>(u.rows - 1, x) = u.at<float>(u.rows - 2, x);
}//x
for (y = 0; y < u.rows; y++) {
u.at<float>(y, 0) = u.at<float>(y, 1);
u.at<float>(y, u.cols - 1) = u.at<float>(y, u.cols - 2);
u.at<float>(height, x) = u.at<float>(height - 1, x);
}
u.copyTo(U);
if (!g_c.empty()) g_c.release();
if (!g_p.empty()) g_p.release();
}//n
}
@ -251,15 +312,43 @@ void StereoVar::FMG(Mat &I1, Mat &I2, Mat &I2x, Mat &u, int level)
u_h.release();
level--;
if ((flags & USE_AUTO_PARAMS) && (level < levels / 3)) {
penalization = PENALIZATION_PERONA_MALIK;
fi *= 100;
flags -= USE_AUTO_PARAMS;
autoParams();
}
if (flags & USE_MEDIAN_FILTERING) medianBlur(u, u, 3);
if (level >= 0) FMG(I1, I2, I2x, u, level);
}
void StereoVar::autoParams()
{
int maxD = MAX(labs(maxDisp), labs(minDisp));
if (!maxD) pyrScale = 0.85;
else if (maxD < 8) pyrScale = 0.5;
else if (maxD < 64) pyrScale = 0.5 + static_cast<double>(maxD - 8) * 0.00625;
else pyrScale = 0.85;
if (maxD) {
levels = 0;
while ( pow(pyrScale, levels) * maxD > 1.5) levels ++;
levels++;
}
switch(penalization) {
case PENALIZATION_TICHONOV: cycle = CYCLE_V; break;
case PENALIZATION_CHARBONNIER: cycle = CYCLE_O; break;
case PENALIZATION_PERONA_MALIK: cycle = CYCLE_O; break;
}
}
void StereoVar::operator ()( const Mat& left, const Mat& right, Mat& disp )
{
CV_Assert(left.size() == right.size() && left.type() == right.type());
CvSize imgSize = left.size();
int MaxD = MAX(std::abs(minDisp), std::abs(maxDisp));
int MaxD = MAX(labs(minDisp), labs(maxDisp));
int SignD = 1; if (MIN(minDisp, maxDisp) < 0) SignD = -1;
if (minDisp >= maxDisp) {MaxD = 256; SignD = 1;}
@ -290,6 +379,11 @@ void StereoVar::operator ()( const Mat& left, const Mat& right, Mat& disp )
GaussianBlur(rightgray, rightgray, cvSize(poly_n, poly_n), poly_sigma);
}
if (flags & USE_AUTO_PARAMS) {
penalization = PENALIZATION_TICHONOV;
autoParams();
}
Mat I1, I2;
leftgray.convertTo(I1, CV_32FC1);
rightgray.convertTo(I2, CV_32FC1);

47
samples/cpp/stereo_match.cpp
Unescape View File

@ -20,7 +20,7 @@ void print_help()
{
printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n");
printf("\nUsage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh|var] [--blocksize=<block_size>]\n"
"[--max-disparity=<max_disparity>] [-i <intrinsic_filename>] [-e <extrinsic_filename>]\n"
"[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i <intrinsic_filename>] [-e <extrinsic_filename>]\n"
"[--no-display] [-o <disparity_image>] [-p <point_cloud_file>]\n");
}
@ -40,18 +40,18 @@ void saveXYZ(const char* filename, const Mat& mat)
fclose(fp);
}
int main(int argc, char** argv)
{
const char* algorithm_opt = "--algorithm=";
const char* maxdisp_opt = "--max-disparity=";
const char* blocksize_opt = "--blocksize=";
const char* nodisplay_opt = "--no-display=";
const char* scale_opt = "--scale=";
if(argc < 3)
{
print_help();
return 0;
return 0;
}
const char* img1_filename = 0;
const char* img2_filename = 0;
@ -64,6 +64,7 @@ int main(int argc, char** argv)
int alg = STEREO_SGBM;
int SADWindowSize = 0, numberOfDisparities = 0;
bool no_display = false;
float scale = 1.f;
StereoBM bm;
StereoSGBM sgbm;
@ -111,6 +112,14 @@ int main(int argc, char** argv)
return -1;
}
}
else if( strncmp(argv[i], scale_opt, strlen(scale_opt)) == 0 )
{
if( sscanf( argv[i] + strlen(scale_opt), "%f", &scale ) != 1 || scale < 0 )
{
printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
return -1;
}
}
else if( strcmp(argv[i], nodisplay_opt) == 0 )
no_display = true;
else if( strcmp(argv[i], "-i" ) == 0 )
@ -149,6 +158,17 @@ int main(int argc, char** argv)
int color_mode = alg == STEREO_BM ? 0 : -1;
Mat img1 = imread(img1_filename, color_mode);
Mat img2 = imread(img2_filename, color_mode);
if( scale != 1.f )
{
Mat temp1, temp2;
int method = scale < 1 ? INTER_AREA : INTER_CUBIC;
resize(img1, temp1, Size(), scale, scale, method);
img1 = temp1;
resize(img2, temp2, Size(), scale, scale, method);
img2 = temp2;
}
Size img_size = img1.size();
Rect roi1, roi2;
@ -224,18 +244,18 @@ int main(int argc, char** argv)
sgbm.disp12MaxDiff = 1;
sgbm.fullDP = alg == STEREO_HH;
var.levels = 6;
var.pyrScale = 0.6;
var.nIt = 3;
var.minDisp = -numberOfDisparities;
var.levels = 3; // ignored with USE_AUTO_PARAMS
var.pyrScale = 0.5; // ignored with USE_AUTO_PARAMS
var.nIt = 25;
var.minDisp = -numberOfDisparities;
var.maxDisp = 0;
var.poly_n = 3;
var.poly_sigma = 0.0;
var.fi = 5.0f;
var.lambda = 0.1;
var.penalization = var.PENALIZATION_TICHONOV;
var.cycle = var.CYCLE_V;
var.flags = var.USE_SMART_ID | var.USE_INITIAL_DISPARITY | 1 * var.USE_MEDIAN_FILTERING ;
var.fi = 15.0f;
var.lambda = 0.03f;
var.penalization = var.PENALIZATION_TICHONOV; // ignored with USE_AUTO_PARAMS
var.cycle = var.CYCLE_V; // ignored with USE_AUTO_PARAMS
var.flags = var.USE_SMART_ID | var.USE_AUTO_PARAMS | var.USE_INITIAL_DISPARITY | var.USE_MEDIAN_FILTERING ;
Mat disp, disp8;
//Mat img1p, img2p, dispp;
@ -245,8 +265,9 @@ int main(int argc, char** argv)
int64 t = getTickCount();
if( alg == STEREO_BM )
bm(img1, img2, disp);
else if( alg == STEREO_VAR )
else if( alg == STEREO_VAR ) {
var(img1, img2, disp);
}
else if( alg == STEREO_SGBM || alg == STEREO_HH )
sgbm(img1, img2, disp);
t = getTickCount() - t;

Write Preview
Loading…
Cancel
Save