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);
        }