void computeWeights(int height, int width, int horOrVert, int *offset,
unsigned char *rgbL, float *weights) {
int i, j;
// hOffset of vOffset depending on the computed weights
int hOffset = (horOrVert == 0) ? LOAD_INT(offset) : 0;
int vOffset = (horOrVert == 1) ? LOAD_INT(offset) : 0;
// Distance coeff
float distanceCoeff = -(float) (LOAD_INT(offset)) / 36.0;
//distanceCoeff *= -1;
// Scan the pixels of the rgb image
for (j = 0; j < height; j++) {
for (i = 0; i < width; i++) {
float r0, g0, b0, r, g, b;
float weightM, weightP, weightO;
// Compute the weights
r0 = rgbL[3*(j*width+i)];
g0 = rgbL[3*(j*width+i)+1];
b0 = rgbL[3*(j*width+i)+2];
r = rgbL[3*(max(j-vOffset,0)*width+max(i-hOffset,0))];
g = rgbL[3*(max(j-vOffset,0)*width+max(i-hOffset,0))+1];
b = rgbL[3*(max(j-vOffset,0)*width+max(i-hOffset,0))+2];
weightM = sqrtf((r0-r)*(r0-r)+(g0-g)*(g0-g)+(b0-b)*(b0-b))* R_gamaC;
r = rgbL[3*(min(j+vOffset,height-1)*width+min(i+hOffset,width-1))];
g = rgbL[3*(min(j+vOffset,height-1)*width+min(i+hOffset,width-1))+1];
b = rgbL[3*(min(j+vOffset,height-1)*width+min(i+hOffset,width-1))+2];
weightP = sqrtf((r0-r)*(r0-r)+(g0-g)*(g0-g)+(b0-b)*(b0-b))* R_gamaC;
weightM = exp(distanceCoeff - weightM);
weightP = exp(distanceCoeff - weightP);
weightO = 1 / (weightM + weightP + 1);
weightM = weightM * weightO;
weightP = weightP * weightO;
// Store the three weights one after the other in the
// output buffer;
STORE_FLOAT(&weights[3*(j*width+i)+0], &weightO);
STORE_FLOAT(&weights[3*(j*width+i)+1], &weightM);
STORE_FLOAT(&weights[3*(j*width+i)+2], &weightP);
}
}
}
void costConstruction(int height, int width, float truncValue,
unsigned char *disparity, float *grayL, float *grayR,
unsigned char *cenL, unsigned char *cenR, float *disparityError) {
int i, j;
// For each disparity, scan the pixels of the left image
for (j = 0; j < height; j++) {
for (i = 0; i < width; i++) {
unsigned char censusCost;
int leftPxlIdx = j * width + i;
int rightPxlIdx = j * width
+ (((i - *disparity) > 0) ? i - *disparity : 0);
float result;
// Get the cost from the census signatures
censusCost = hammingCost(cenL + leftPxlIdx, cenR + rightPxlIdx);
// Combination method 3 -- weight addition
result =
min(fabs((float)(LOAD_FLOAT(&grayL[leftPxlIdx])-LOAD_FLOAT(&grayR[rightPxlIdx]))),truncValue)
+ censusCost / 5.0;
STORE_FLOAT(&disparityError[leftPxlIdx], &result);
}
}
}
void rgb2Gray(int size, unsigned char *rgb, float *gray){
int idx;
for (idx = 0; idx < size; idx++) {
float res = RGB2GRAY_COEF_R * (float) rgb[3*idx]
+ RGB2GRAY_COEF_G * (float) rgb[3*idx+1]
+ RGB2GRAY_COEF_B * (float) rgb[3*idx+2];
STORE_FLOAT(&gray[idx], &res);
}
}
static void ins_real P1(double, l)
{
float f = (float)l;
if (prog_code + 4 > prog_code_max) {
mem_block_t *mbp = &mem_block[A_PROGRAM];
UPDATE_PROGRAM_SIZE;
realloc_mem_block(mbp);
prog_code = mbp->block + mbp->current_size;
prog_code_max = mbp->block + mbp->max_size;
}
STORE_FLOAT(prog_code, f);
}
