? ? ? ? ? ? Ddynamic[j][k] = 0; ?
? ? ? ? } ?
? ? } ?
? ? minD = -maxD; ?
? ? maxD = 0; ?
? ??
? ? for (int i = SH2 ; i < imageHeight -SH2;i++) ?
? ? { ?
? ? ? ? for (int j = imageWidth-SW2-1; j >= SW2;j--) ?
? ? ? ? { ?
? ? ? ? ? ? for (int k = j + maxD; k >= j + minD;k--) ?
? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? difPixel = 0; ?
? ? ? ? ? ? ? ? for (m = -SH2;m <= SH2;m++) ?
? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? for (n = -SW2; n <= SW2;n++) ?
? ? ? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? ? ? pLeftPixel = (unsigned char*)leftImage->imageData + ??
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?(i+m)*leftImage->widthStep + j+n; ?
? ? ? ? ? ? ? ? ? ? ? ? leftPixel ?= *pLeftPixel; ?
? ? ? ? ? ? ? ? ? ? ? ? if (k < SW2 || k ?>= imageWidth -SW2) ?
? ? ? ? ? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ?difPixel += leftPixel; ?
? ? ? ? ? ? ? ? ? ? ? ? }else { ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
? ? ? ? ? ? ? ? ? ? ? ? ? ? pRightPixel= (unsigned char*)rightImage->imageData+ ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?(i+m)*rightImage->widthStep + k+n; ??
? ? ? ? ? ? ? ? ? ? ? ? ? ? rightPixel = *pRightPixel; ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? difPixel += abs(leftPixel - rightPixel); ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? //cout<
? ? ? ? ? ? ? ? ? ? ? ? } ?
? ? ? ? ? ? ? ? ? ? } ?
? ? ? ? ? ? ? ? } ?
??
? ? ? ? ? ? ? ? if (difPixel <= max12Diff) ?
? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? Ddynamic[j][k] = Ddynamic[j+1][k+1] +1; ??
? ? ? ? ? ? ? ? }else if (Ddynamic[j+1][k] > Ddynamic[j][k+1]) ?
? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? Ddynamic[j][k] = Ddynamic[j+1][k]; ?
? ? ? ? ? ? ? ? }else{ ?
? ? ? ? ? ? ? ? ? ? Ddynamic[j][k] = Ddynamic[j][k+1]; ?
? ? ? ? ? ? ? ? } ?
??
? ? ? ? ? ? } ?
? ? ? ? ? ? //cout<<"\n"; ?
? ? ? ? } ?
? ? ? ? //逆向搜索，找出最佳路径 ?
??
? ? ? ? m = 0; ?
? ? ? ? n = 0; ?
? ? ? ? int m2 = m, n2 = n; ?
? ? ? ? while( m < imageWidth && n < imageWidth) ?
? ? ? ? { ?
? ? ? ? ? ? if ((m == m2 + 1 && n >= n2 +1) || ( m > m2 +1 && n == n2 + 1)) ?
? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? pPixel = (unsigned char *)leftDepth->imageData + i*leftDepth->widthStep + m; ?
? ? ? ? ? ? ? ? *pPixel = (m-n)*10; ?
? ? ? ? ? ? ? ? //标记有效匹配点 ?
? ? ? ? ? ? ? ? pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + m; ?
? ? ? ? ? ? ? ? *pPixel = 255; ?
??
? ? ? ? ? ? ? ? m2 = m; ?
? ? ? ? ? ? ? ? n2 = n; ?
? ? ? ? ? ? } ?
? ? ? ? ? ? if (Ddynamic[m+1][n+1] >= Ddynamic[m][n+1] && Ddynamic[m+1][n+1] >= Ddynamic[m+1][n]) ?
? ? ? ? ? ? { ?
??
? ? ? ? ? ? ? ? m++; ??
? ? ? ? ? ? ? ? n++; ?
? ? ? ? ? ? }else if (Ddynamic[m+1][n] >= Ddynamic[m][n +1] && Ddynamic[m+1][n] >= Ddynamic[m+1][n+1]) ?
? ? ? ? ? ? { ??
? ? ? ? ? ? ? ? m++; ?
? ? ? ? ? ? } ?
? ? ? ? ? ? else ?
? ? ? ? ? ? { ??
? ? ? ? ? ? ? ? n++; ?
? ? ? ? ? ? } ?
??
? ? ? ? } ?
? ? ? ? //cvWaitKey(0); ?
? ? } ?
? ? int t3 = clock(); ?
? ? //refine the depth image ?7*7中值滤波 ?
? ? //统计未能匹配点的个数 ?
? ? count = 0; ?
? ? for (int i = 0 ;i< imageHeight;i++) ?
? ? { ?
? ? ? ? for (int j= 0; j< imageWidth;j++) ?
? ? ? ? { ?
? ? ? ? ? ? pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j; ?
? ? ? ? ? ? pixel = *pPixel; ?
? ? ? ? ? ? if (pixel == 0) ?
? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? count++; ?
? ? ? ? ? ? } ?
? ? ? ? } ?
? ? } ?
??
? ? cout<<"Left Count: ?"<
??
? ? //将关键信息计入文件 ?
? ? fprintf(pFile,"\nDP:\nClock: ? %dms",t3-t1); ?
? ? fprintf(pFile,"\nInvalid Point: ? %f",(double)count/(imageWidth*imageHeight)); ?
? ? // cvWaitKey(0); ?
? ? leftFilter = cvCloneImage(leftDepth); ?
? ? //7*7中值滤波 ?
? ? count = 0; ?
? ? for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++) ?
? ? { ?
? ? ? ? for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++) ?
? ? ? ? { ?
? ? ? ? ? ? pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j; ?
? ? ? ? ? ? pixel = *pPixel; ?
? ? ? ? ? ? if (pixel == 0) ?
? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? count = 0; ?
? ? ? ? ? ? ? ? for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++) ?
? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++) ?
? ? ? ? ? ? ? ? ? ? { ?
? ? ? ? ? ? ? ? ? ? ? ? pPixel2 = (unsigned char *)leftDepth->imageData + m