public StereoVisionEdges()
{
algorithm_type = EDGES;
camera = new StereoVisionEdgesCam[2];
for (int cam = 0; cam < 2; cam++)
{
camera[cam] = new StereoVisionEdgesCam();
camera[cam].init(320, 240);
}
}
/* Match features from this camera with features from the opposite one.
* It is assumed that matching is performed on the left camera CPU */
public int svs_match(
StereoVisionEdgesCam other,
int ideal_no_of_matches, /* ideal number of matches to be returned */
int max_disparity_percent, /* max disparity as a percent of image width */
int learnDesc, /* descriptor match weight */
int learnLuma, /* luminance match weight */
int learnDisp, /* disparity weight */
int learnPrior, /* prior weight */
int use_priors) /* if non-zero then use priors, assuming time between frames is small */
{
int x, xL = 0, xR, L, R, y, no_of_feats, no_of_feats_left,
no_of_feats_right, row, col = 0, bit, disp_diff;
int luma_diff, disp_prior = 0, min_disp, max_disp = 0, max_disp_pixels,
meanL, meanR, disp = 0, fL = 0, fR = 0, bestR = 0;
uint descLanti, descR, desc_match;
uint correlation, anticorrelation, total;
uint descL, n, match_prob, best_prob;
int idx, max, curr_idx = 0, search_idx, winner_idx = 0;
int no_of_possible_matches = 0, matches = 0;
int itt, prev_matches, row_offset, col_offset;
int p, pmax=3;
uint meandescL, meandescR;
short[] meandesc = new short[SVS_DESCRIPTOR_PIXELS];
/* create arrays */
if (svs_matches == null) {
svs_matches = new uint[SVS_MAX_MATCHES * 4];
valid_quadrants = new byte[SVS_MAX_MATCHES];
peaks_history = new ushort[4*SVS_PEAKS_HISTORY];
disparity_priors = new int[SVS_MAX_IMAGE_WIDTH * SVS_MAX_IMAGE_HEIGHT / (16*SVS_VERTICAL_SAMPLING)];
}
/* convert max disparity from percent to pixels */
max_disp_pixels = max_disparity_percent * (int)imgWidth / 100;
min_disp = -10;
max_disp = max_disp_pixels;
row = 0;
for (y = 4; y < (int) imgHeight - 4; y += SVS_VERTICAL_SAMPLING, row++) {
/* number of features on left and right rows */
no_of_feats_left = features_per_row[row];
no_of_feats_right = other.features_per_row[row];
/* compute mean descriptor for the left row
* this will be used to create eigendescriptors */
meandescL = (uint)0;
for (int i = SVS_DESCRIPTOR_PIXELS-1; i>=0;i--) meandesc[i]=0;
for (L = 0; L < no_of_feats_left; L++) {
descL = descriptor[fL + L];
n = 1;
for (bit = 0; bit < SVS_DESCRIPTOR_PIXELS; bit++, n *= 2) {
uint v = descL & n;
if (v != 0)
meandesc[bit]++;
else
meandesc[bit]--;
}
}
n = 1;
for (bit = 0; bit < SVS_DESCRIPTOR_PIXELS; bit++, n *= 2) {
if (meandesc[bit] >= 0)
meandescL |= (uint)n;
}
/* compute mean descriptor for the right row
* this will be used to create eigendescriptors */
meandescR = 0;
for (int i = SVS_DESCRIPTOR_PIXELS-1; i>=0;i--) meandesc[i]=0;
for (R = 0; R < no_of_feats_right; R++) {
descR = other.descriptor[fR + R];
n = 1;
for (bit = 0; bit < SVS_DESCRIPTOR_PIXELS; bit++, n *= 2) {
uint v = descR & n;
if (v != 0)
meandesc[bit]++;
else
meandesc[bit]--;
}
}
n = 1;
for (bit = 0; bit < SVS_DESCRIPTOR_PIXELS; bit++, n *= 2) {
if (meandesc[bit] > 0)
meandescR |= (uint)n;
}
/* features along the row in the left camera */
for (L = 0; L < no_of_feats_left; L++) {
/* x coordinate of the feature in the left camera */
xL = feature_x[fL + L];
if (use_priors != 0) {
disp_prior = disparity_priors[(row * imgWidth + xL) / 16];
}
/* mean luminance and eigendescriptor for the left camera feature */
meanL = mean[fL + L];
descL = descriptor[fL + L] & meandescL;
/* invert bits of the descriptor for anti-correlation matching */
n = descL;
descLanti = 0;
for (bit = 0; bit < SVS_DESCRIPTOR_PIXELS; bit++) {
/* Shift result vector to higher significance. */
descLanti <<= 1;
/* Get least significant input bit. */
descLanti |= (uint)(n & 1);
/* Shift input vector to lower significance. */
n >>= 1;
}
total = 0;
/* features along the row in the right camera */
for (R = 0; R < no_of_feats_right; R++) {
/* set matching score to zero */
row_peaks[R] = 0;
/* x coordinate of the feature in the right camera */
xR = other.feature_x[fR + R];
/* compute disparity */
disp = xL - xR;
/* is the disparity within range? */
if ((disp >= min_disp) && (disp < max_disp)) {
if (disp < 0)
disp = 0;
/* mean luminance for the right camera feature */
meanR = other.mean[fR + R];
/* is the mean luminance similar? */
luma_diff = meanR - meanL;
/* right camera feature eigendescriptor */
descR = other.descriptor[fR + R] & meandescR;
/* bitwise descriptor correlation match */
desc_match = descL & descR;
/* count the number of correlation bits */
correlation = BitsSetTable256[desc_match & 0xff]
+ BitsSetTable256[(desc_match >> 8) & 0xff]
+ BitsSetTable256[(desc_match >> 16) & 0xff]
+ BitsSetTable256[desc_match >> 24];
/* bitwise descriptor anti-correlation match */
desc_match = descLanti & descR;
/* count the number of anti-correlation bits */
anticorrelation = BitsSetTable256[desc_match & 0xff]
+ BitsSetTable256[(desc_match >> 8) & 0xff]
+ BitsSetTable256[(desc_match >> 16) & 0xff]
+ BitsSetTable256[desc_match >> 24];
if (luma_diff < 0)
luma_diff = -luma_diff;
int score =
10000 + (max_disp * learnDisp)
+ (((int) correlation
+ (int) (SVS_DESCRIPTOR_PIXELS
- anticorrelation))
* learnDesc) - (luma_diff
* learnLuma) - (disp * learnDisp);
if (use_priors!=0) {
disp_diff = disp - disp_prior;
if (disp_diff < 0)
disp_diff = -disp_diff;
score -= disp_diff * learnPrior;
}
if (score < 0)
score = 0;
/* store overall matching score */
row_peaks[R] = (uint) score;
total += row_peaks[R];
} else {
if ((disp < min_disp) && (disp > -max_disp)) {
row_peaks[R] = (uint) ((max_disp - disp)
* learnDisp);
total += row_peaks[R];
}
}
}
/* non-zero total matching score */
if (total > 0) {
/* several candidate disparities per feature
observing the principle of least commitment */
for (p = 0; p < pmax; p++) {
/* convert matching scores to probabilities */
best_prob = 0;
for (R = 0; R < no_of_feats_right; R++) {
if (row_peaks[R] > 0) {
match_prob = row_peaks[R] * 1000 / total;
if (match_prob > best_prob) {
best_prob = match_prob;
bestR = R;
}
}
}
if ((best_prob > 0) && (best_prob < 1000)
&& (no_of_possible_matches < SVS_MAX_FEATURES)) {
/* x coordinate of the feature in the right camera */
xR = other.feature_x[fR + bestR];
/* possible disparity */
disp = xL - xR;
if (disp >= -10) {
if (disp < 0)
disp = 0;
/* add the best result to the list of possible matches */
svs_matches[no_of_possible_matches * 4] = best_prob;
svs_matches[no_of_possible_matches * 4 + 1]
= (uint) xL;
svs_matches[no_of_possible_matches * 4 + 2]
= (uint) y;
svs_matches[no_of_possible_matches * 4 + 3]
= (uint) disp;
if (p > 0) {
svs_matches[no_of_possible_matches * 4 + 1] += imgWidth;
}
no_of_possible_matches++;
row_peaks[bestR] = 0;
}
}
}
}
}
/* increment feature indexes */
fL += no_of_feats_left;
fR += no_of_feats_right;
}
// clear priors
int priors_length = (int)(imgWidth * imgHeight / (16*SVS_VERTICAL_SAMPLING));
if (no_of_possible_matches > 20) {
for (int i=priors_length-1;i>=0;i--) disparity_priors[i]=0;
/* filter the results */
svs_filter_plane(no_of_possible_matches, max_disp);
/* sort matches in descending order of probability */
if (no_of_possible_matches < ideal_no_of_matches) {
ideal_no_of_matches = no_of_possible_matches;
}
curr_idx = 0;
search_idx = 0;
for (matches = 0; matches < ideal_no_of_matches; matches++, curr_idx
+= 4) {
match_prob = svs_matches[curr_idx];
winner_idx = -1;
search_idx = curr_idx + 4;
max = no_of_possible_matches * 4;
while (search_idx < max) {
if (svs_matches[search_idx] > match_prob) {
match_prob = svs_matches[search_idx];
winner_idx = search_idx;
}
search_idx += 4;
}
if (winner_idx > -1) {
/* swap */
best_prob = svs_matches[winner_idx];
xL = (int)svs_matches[winner_idx + 1];
y = (int)svs_matches[winner_idx + 2];
disp = (int)svs_matches[winner_idx + 3];
svs_matches[winner_idx] = svs_matches[curr_idx];
svs_matches[winner_idx + 1] = svs_matches[curr_idx + 1];
svs_matches[winner_idx + 2] = svs_matches[curr_idx + 2];
svs_matches[winner_idx + 3] = svs_matches[curr_idx + 3];
svs_matches[curr_idx] = best_prob;
svs_matches[curr_idx + 1] = (uint)xL;
svs_matches[curr_idx + 2] = (uint)y;
svs_matches[curr_idx + 3] = (uint)disp;
/* update your priors */
if (svs_matches[winner_idx + 1] >= imgWidth) {
svs_matches[winner_idx + 1] -= imgWidth;
}
else {
row = y / SVS_VERTICAL_SAMPLING;
for (row_offset = -3; row_offset <= 3; row_offset++) {
for (col_offset = -1; col_offset <= 1; col_offset++) {
idx = (((row + row_offset) * (int)imgWidth + xL) / 16)
+ col_offset;
if ((idx > -1) && (idx < priors_length)) {
if (disparity_priors[idx] == 0)
disparity_priors[idx] = disp;
else
disparity_priors[idx] = (disp
+ disparity_priors[idx]) / 2;
}
}
}
}
}
if (svs_matches[curr_idx] == 0) {
break;
}
}
/* attempt to assign disparities to vertical features */
for (int i = 0; i < SVS_MAX_MATCHES; i++) valid_quadrants[i]=0;
itt = 0;
prev_matches = matches;
for (itt = 0; itt < 10; itt++) {
fL = 0;
col = 0;
for (x = 4; x < (int) imgWidth - 4; x += SVS_HORIZONTAL_SAMPLING, col++) {
no_of_feats = features_per_col[col];
/* features along the row in the left camera */
for (L = 0; L < no_of_feats; L++) {
if (valid_quadrants[fL + L] == 0) {
/* y coordinate of the feature in the left camera */
y = feature_y[fL + L];
/* lookup disparity from priors */
row = y / SVS_VERTICAL_SAMPLING;
disp_prior
= disparity_priors[(row * imgWidth + x) / 16];
if ((disp_prior > 0) && (matches < SVS_MAX_MATCHES)) {
curr_idx = matches * 4;
svs_matches[curr_idx] = 1000;
svs_matches[curr_idx + 1] = (uint)x;
svs_matches[curr_idx + 2] = (uint)y;
svs_matches[curr_idx + 3] = (uint)disp_prior;
matches++;
/* update your priors */
for (row_offset = -3; row_offset <= 3; row_offset++) {
for (col_offset = -1; col_offset <= 1; col_offset++) {
idx = (((row + row_offset) * (int)imgWidth + x)
/ 16) + col_offset;
if ((idx > -1) && (idx < priors_length)) {
if (disparity_priors[idx] == 0)
disparity_priors[idx] = disp_prior;
}
}
}
valid_quadrants[fL + L] = 1;
}
}
}
fL += no_of_feats;
}
if (prev_matches == matches)
break;
prev_matches = matches;
}
}
return (matches);
}
