matcher.cpp

Go to the documentation of this file.

/*
   Copyright 2012. All rights reserved.
   Institute of Measurement and Control Systems
   Karlsruhe Institute of Technology, Germany

   This file is part of libviso2.
Authors: Andreas Geiger

libviso2 is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or any later version.

libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#include "matcher.h"
#include "triangle.h"
#include "filter.h"

using namespace std;

// PUBLIC FUNCTIONS //

// constructor (with default parameters)
Matcher::Matcher(parameters param) : param(param) {

    // init match ring buffer to zero
    m1p1 = 0; n1p1 = 0;
    m1p2 = 0; n1p2 = 0;
    m2p1 = 0; n2p1 = 0;
    m2p2 = 0; n2p2 = 0;
    m1c1 = 0; n1c1 = 0;
    m1c2 = 0; n1c2 = 0;
    m2c1 = 0; n2c1 = 0;
    m2c2 = 0; n2c2 = 0;
    I1p    = 0; I2p    = 0;
    I1c    = 0; I2c    = 0;
    I1p_du = 0; I1p_dv = 0;
    I2p_du = 0; I2p_dv = 0;
    I1c_du = 0; I1c_dv = 0;
    I2c_du = 0; I2c_dv = 0;
    I1p_du_full = 0; I1p_dv_full = 0;
    I2p_du_full = 0; I2p_dv_full = 0;
    I1c_du_full = 0; I1c_dv_full = 0;
    I2c_du_full = 0; I2c_dv_full = 0;

    // margin needed to compute descriptor + sobel responses
    margin = 5+1;

    // adjust match radius on half resolution
    if (param.half_resolution)
        this->param.match_radius /= 2;
}

// deconstructor
Matcher::~Matcher() {
    if (I1p)          _mm_free(I1p);
    if (I2p)          _mm_free(I2p);
    if (I1c)          _mm_free(I1c);
    if (I2c)          _mm_free(I2c);
    if (m1p1)         _mm_free(m1p1);
    if (m1p2)         _mm_free(m1p2);
    if (I1p_du)       _mm_free(I1p_du);
    if (I1p_dv)       _mm_free(I1p_dv);
    if (I1p_du_full)  _mm_free(I1p_du_full);
    if (I1p_dv_full)  _mm_free(I1p_dv_full);
    if (m2p1)         _mm_free(m2p1);
    if (m2p2)         _mm_free(m2p2);
    if (I2p_du)       _mm_free(I2p_du);
    if (I2p_dv)       _mm_free(I2p_dv);
    if (I2p_du_full)  _mm_free(I2p_du_full);
    if (I2p_dv_full)  _mm_free(I2p_dv_full);
    if (m1c1)         _mm_free(m1c1);
    if (m1c2)         _mm_free(m1c2);
    if (I1c_du)       _mm_free(I1c_du);
    if (I1c_dv)       _mm_free(I1c_dv);
    if (I1c_du_full)  _mm_free(I1c_du_full);
    if (I1c_dv_full)  _mm_free(I1c_dv_full);
    if (m2c1)         _mm_free(m2c1);
    if (m2c2)         _mm_free(m2c2);
    if (I2c_du)       _mm_free(I2c_du);
    if (I2c_dv)       _mm_free(I2c_dv);
    if (I2c_du_full)  _mm_free(I2c_du_full);
    if (I2c_dv_full)  _mm_free(I2c_dv_full);
}

void Matcher::pushBack (uint8_t *I1,uint8_t* I2,int32_t* dims,const bool replace) {

    // image dimensions
    int32_t width  = dims[0];
    int32_t height = dims[1];
    //bytes per line
    int32_t bpl    = dims[2];

    // sanity check
    // Why not assert?!
    if (width<=0 || height<=0 || bpl<width || I1==0) {
        cerr << "ERROR: Image dimension mismatch!" << endl;
        return;
    }

    if (replace) {
        if (I1c)         _mm_free(I1c);
        if (I2c)         _mm_free(I2c);
        if (m1c1)        _mm_free(m1c1);
        if (m1c2)        _mm_free(m1c2);
        if (I1c_du)      _mm_free(I1c_du);
        if (I1c_dv)      _mm_free(I1c_dv);
        if (I1c_du_full) _mm_free(I1c_du_full);
        if (I1c_dv_full) _mm_free(I1c_dv_full);
        if (m2c1)        _mm_free(m2c1);
        if (m2c2)        _mm_free(m2c2);
        if (I2c_du)      _mm_free(I2c_du);
        if (I2c_dv)      _mm_free(I2c_dv);
        if (I2c_du_full) _mm_free(I2c_du_full);
        if (I2c_dv_full) _mm_free(I2c_dv_full);
    } else {
        if (I1p)         _mm_free(I1p);
        if (I2p)         _mm_free(I2p);
        if (m1p1)        _mm_free(m1p1);
        if (m1p2)        _mm_free(m1p2);
        if (I1p_du)      _mm_free(I1p_du);
        if (I1p_dv)      _mm_free(I1p_dv);
        if (I1p_du_full) _mm_free(I1p_du_full);
        if (I1p_dv_full) _mm_free(I1p_dv_full);
        if (m2p1)        _mm_free(m2p1);
        if (m2p2)        _mm_free(m2p2);
        if (I2p_du)      _mm_free(I2p_du);
        if (I2p_dv)      _mm_free(I2p_dv);
        if (I2p_du_full) _mm_free(I2p_du_full);
        if (I2p_dv_full) _mm_free(I2p_dv_full);
        m1p1 = m1c1; n1p1 = n1c1;
        m1p2 = m1c2; n1p2 = n1c2;
        m2p1 = m2c1; n2p1 = n2c1;
        m2p2 = m2c2; n2p2 = n2c2;
        I1p         = I1c;
        I2p         = I2c;
        I1p_du      = I1c_du;
        I1p_dv      = I1c_dv;
        I1p_du_full = I1c_du_full;
        I1p_dv_full = I1c_dv_full;
        I2p_du      = I2c_du;
        I2p_dv      = I2c_dv;
        I2p_du_full = I2c_du_full;
        I2p_dv_full = I2c_dv_full;
        dims_p[0]   = dims_c[0];
        dims_p[1]   = dims_c[1];
        dims_p[2]   = dims_c[2];
    }

    // set new dims (bytes per line must be multiple of 16)
    dims_c[0] = width;
    dims_c[1] = height;
    dims_c[2] = width + 15-(width-1)%16;

    // copy images to byte aligned memory
    // uint8_t: typedefine, unsigned int , 1 bytes
    // _mm_malloc: aligned memory, 16 is the alignment constraint
    I1c = (uint8_t*)_mm_malloc(dims_c[2]*dims_c[1]*sizeof(uint8_t),16);
    I2c = (uint8_t*)_mm_malloc(dims_c[2]*dims_c[1]*sizeof(uint8_t),16);
    if (dims_c[2]==bpl) {
        memcpy(I1c,I1,dims_c[2]*dims_c[1]*sizeof(uint8_t));
        if (I2!=0)
            memcpy(I2c,I2,dims_c[2]*dims_c[1]*sizeof(uint8_t));
    } else {
        for (int32_t v=0; v<height; v++) {
            memcpy(I1c+v*dims_c[2],I1+v*bpl,dims_c[0]*sizeof(uint8_t));
            if (I2!=0)
                memcpy(I2c+v*dims_c[2],I2+v*bpl,dims_c[0]*sizeof(uint8_t));
        }
    }

    // compute new features for current frame
    computeFeatures(I1c,dims_c,m1c1,n1c1,m1c2,n1c2,I1c_du,I1c_dv,I1c_du_full,I1c_dv_full);
    if (I2!=0)
        computeFeatures(I2c,dims_c,m2c1,n2c1,m2c2,n2c2,I2c_du,I2c_dv,I2c_du_full,I2c_dv_full);
}

void Matcher::matchFeatures(int32_t method, Matrix *Tr_delta) {

    // sanity check //

    // flow
    if (method==0) {
        if (m1p2==0 || n1p2==0 || m1c2==0 || n1c2==0)
            return;
        if (param.multi_stage)
            if (m1p1==0 || n1p1==0 || m1c1==0 || n1c1==0)
                return;

        // stereo
    } else if (method==1) {
        if (m1c2==0 || n1c2==0 || m2c2==0 || n2c2==0)
            return;
        if (param.multi_stage)
            if (m1c1==0 || n1c1==0 || m2c1==0 || n2c1==0)
                return;

        // quad matching
    } else {
        if (m1p2==0 || n1p2==0 || m2p2==0 || n2p2==0 || m1c2==0 || n1c2==0 || m2c2==0 || n2c2==0)
            return;
        if (param.multi_stage)
            if (m1p1==0 || n1p1==0 || m2p1==0 || n2p1==0 || m1c1==0 || n1c1==0 || m2c1==0 || n2c1==0)
                return;
    }

    // clear old matches
    p_matched_1.clear();
    p_matched_2.clear();

    // double pass matching
    if (param.multi_stage) {

        //printf("multi_stage\n");
        // 1st pass (sparse matches)
        // methon=2
        // use_prior false
        matching(m1p1,m2p1,m1c1,m2c1,n1p1,n2p1,n1c1,n2c1,p_matched_1,method,false,Tr_delta);
        removeOutliers(p_matched_1,method);

        // compute search range prior statistics (used for speeding up 2nd pass)
        computePriorStatistics(p_matched_1,method);

        // 2nd pass (dense matches)
        matching(m1p2,m2p2,m1c2,m2c2,n1p2,n2p2,n1c2,n2c2,p_matched_2,method,true,Tr_delta);
        /*printf("param.refinement=%d",param.refinement);*/
        //param.refinement=1 pixel; 2 subpixel
        if (param.refinement>0)
            refinement(p_matched_2,method);
        removeOutliers(p_matched_2,method);

        // single pass matching
    } else {
        matching(m1p2,m2p2,m1c2,m2c2,n1p2,n2p2,n1c2,n2c2,p_matched_2,method,false,Tr_delta);
        if (param.refinement>0)
            refinement(p_matched_2,method);
        removeOutliers(p_matched_2,method);
    }
}

void Matcher::bucketFeatures(int32_t max_features,float bucket_width,float bucket_height) {

    // find max values
    float u_max = 0;
    float v_max = 0;
    for (vector<p_match>::iterator it = p_matched_2.begin(); it!=p_matched_2.end(); it++) {
        if (it->u1c>u_max) u_max=it->u1c;
        if (it->v1c>v_max) v_max=it->v1c;
    }

    // allocate number of buckets needed
    int32_t bucket_cols = (int32_t)floor(u_max/bucket_width)+1;
    int32_t bucket_rows = (int32_t)floor(v_max/bucket_height)+1;
    vector<p_match> *buckets = new vector<p_match>[bucket_cols*bucket_rows];

    // assign matches to their buckets
    for (vector<p_match>::iterator it=p_matched_2.begin(); it!=p_matched_2.end(); it++) {
        int32_t u = (int32_t)floor(it->u1c/bucket_width);
        int32_t v = (int32_t)floor(it->v1c/bucket_height);
        buckets[v*bucket_cols+u].push_back(*it);
    }

    // refill p_matched from buckets
    p_matched_2.clear();
    for (int32_t i=0; i<bucket_cols*bucket_rows; i++) {

        // shuffle bucket indices randomly
        std::random_shuffle(buckets[i].begin(),buckets[i].end());

        // add up to max_features features from this bucket to p_matched
        int32_t k=0;
        for (vector<p_match>::iterator it=buckets[i].begin(); it!=buckets[i].end(); it++) {
            p_matched_2.push_back(*it);
            k++;
            if (k>=max_features)
                break;
        }
    }

    // free buckets
    delete []buckets;
}

float Matcher::getGain (vector<int32_t> inliers) {

    // check if two images are provided and matched
    if (I1p==0 || I1c==0 || p_matched_2.size()==0 || inliers.size()==0)
        return 1;

    int32_t window_size = 3;
    float   gain        = 0;
    int32_t num         = 0;
    int32_t u_min,u_max,v_min,v_max;
    float   mean_prev,mean_curr;

    for (vector<int32_t>::iterator it=inliers.begin(); it!=inliers.end(); it++) {
        if (*it<(int32_t)p_matched_2.size()) {

            // mean in previous image
            u_min = min(max((int32_t)p_matched_2[*it].u1p-window_size,0),dims_p[0]);
            u_max = min(max((int32_t)p_matched_2[*it].u1p+window_size,0),dims_p[0]);
            v_min = min(max((int32_t)p_matched_2[*it].v1p-window_size,0),dims_p[1]);
            v_max = min(max((int32_t)p_matched_2[*it].v1p+window_size,0),dims_p[1]);
            mean_prev = mean(I1p,dims_p[2],u_min,u_max,v_min,v_max);

            // mean in current image
            u_min = min(max((int32_t)p_matched_2[*it].u1c-window_size,0),dims_p[0]);
            u_max = min(max((int32_t)p_matched_2[*it].u1c+window_size,0),dims_p[0]);
            v_min = min(max((int32_t)p_matched_2[*it].v1c-window_size,0),dims_p[1]);
            v_max = min(max((int32_t)p_matched_2[*it].v1c+window_size,0),dims_p[1]);
            mean_curr = mean(I1c,dims_c[2],u_min,u_max,v_min,v_max);

            if (mean_prev>10) {
                gain += mean_curr/mean_prev;
                num++;
            }
        }
    }

    if (num>0) return gain/=(float)num;
    else       return 1;
}

// PRIVATE FUNCTIONS //

void Matcher::nonMaximumSuppression (int16_t* I_f1,int16_t* I_f2,const int32_t* dims,vector<Matcher::maximum> &maxima,int32_t nms_n) {

    // extract parameters
    int32_t width  = dims[0];
    int32_t height = dims[1];
    int32_t bpl    = dims[2];
    int32_t n      = nms_n;
    int32_t tau    = param.nms_tau;

    // loop variables
    int32_t f1mini,f1minj,f1maxi,f1maxj,f2mini,f2minj,f2maxi,f2maxj;
    int32_t f1minval,f1maxval,f2minval,f2maxval,currval;
    int32_t addr;

    for (int32_t i=n+margin; i<width-n-margin;i+=n+1) {
        for (int32_t j=n+margin; j<height-n-margin;j+=n+1) {

            f1mini = i; f1minj = j; f1maxi = i; f1maxj = j;
            f2mini = i; f2minj = j; f2maxi = i; f2maxj = j;

            addr     = getAddressOffsetImage(i,j,bpl);
            f1minval = *(I_f1+addr);
            f1maxval = f1minval;
            f2minval = *(I_f2+addr);
            f2maxval = f2minval;

            for (int32_t i2=i; i2<=i+n; i2++) {
                for (int32_t j2=j; j2<=j+n; j2++) {
                    addr    = getAddressOffsetImage(i2,j2,bpl);
                    currval = *(I_f1+addr);
                    if (currval<f1minval) {
                        f1mini   = i2;
                        f1minj   = j2;
                        f1minval = currval;
                    } else if (currval>f1maxval) {
                        f1maxi   = i2;
                        f1maxj   = j2;
                        f1maxval = currval;
                    }
                    currval = *(I_f2+addr);
                    if (currval<f2minval) {
                        f2mini   = i2;
                        f2minj   = j2;
                        f2minval = currval;
                    } else if (currval>f2maxval) {
                        f2maxi   = i2;
                        f2maxj   = j2;
                        f2maxval = currval;
                    }
                }
            }

            // f1 minimum
            for (int32_t i2=f1mini-n; i2<=min(f1mini+n,width-1-margin); i2++) {
                for (int32_t j2=f1minj-n; j2<=min(f1minj+n,height-1-margin); j2++) {
                    currval = *(I_f1+getAddressOffsetImage(i2,j2,bpl));
                    if (currval<f1minval && (i2<i || i2>i+n || j2<j || j2>j+n))
                        goto failed_f1min;
                }
            }
            if (f1minval<=-tau)
                maxima.push_back(Matcher::maximum(f1mini,f1minj,f1minval,0));
failed_f1min:;

             // f1 maximum
             for (int32_t i2=f1maxi-n; i2<=min(f1maxi+n,width-1-margin); i2++) {
                 for (int32_t j2=f1maxj-n; j2<=min(f1maxj+n,height-1-margin); j2++) {
                     currval = *(I_f1+getAddressOffsetImage(i2,j2,bpl));
                     if (currval>f1maxval && (i2<i || i2>i+n || j2<j || j2>j+n))
                         goto failed_f1max;
                 }
             }
             if (f1maxval>=tau)
                 maxima.push_back(Matcher::maximum(f1maxi,f1maxj,f1maxval,1));
failed_f1max:;

             // f2 minimum
             for (int32_t i2=f2mini-n; i2<=min(f2mini+n,width-1-margin); i2++) {
                 for (int32_t j2=f2minj-n; j2<=min(f2minj+n,height-1-margin); j2++) {
                     currval = *(I_f2+getAddressOffsetImage(i2,j2,bpl));
                     if (currval<f2minval && (i2<i || i2>i+n || j2<j || j2>j+n))
                         goto failed_f2min;
                 }
             }
             if (f2minval<=-tau)
                 maxima.push_back(Matcher::maximum(f2mini,f2minj,f2minval,2));
failed_f2min:;

             // f2 maximum
             for (int32_t i2=f2maxi-n; i2<=min(f2maxi+n,width-1-margin); i2++) {
                 for (int32_t j2=f2maxj-n; j2<=min(f2maxj+n,height-1-margin); j2++) {
                     currval = *(I_f2+getAddressOffsetImage(i2,j2,bpl));
                     if (currval>f2maxval && (i2<i || i2>i+n || j2<j || j2>j+n))
                         goto failed_f2max;
                 }
             }
             if (f2maxval>=tau)
                 maxima.push_back(Matcher::maximum(f2maxi,f2maxj,f2maxval,3));
failed_f2max:;
        }
    }
}

inline void Matcher::computeDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr) {

    // get address indices
    int32_t addr_m1 = getAddressOffsetImage(u,v-1,bpl);
    int32_t addr_m3 = addr_m1-2*bpl;
    int32_t addr_m5 = addr_m3-2*bpl;
    int32_t addr_p1 = addr_m1+2*bpl;
    int32_t addr_p3 = addr_p1+2*bpl;
    int32_t addr_p5 = addr_p3+2*bpl;

    // compute descriptor
    uint32_t k = 0;
    desc_addr[k++] = I_du[addr_m1-3];
    desc_addr[k++] = I_dv[addr_m1-3];
    /*printf("k0=%d,k1=%d",I_du[addr_m1-3],I_dv[addr_m1-3]);*/
    desc_addr[k++] = I_du[addr_p1-3];
    desc_addr[k++] = I_dv[addr_p1-3];
    desc_addr[k++] = I_du[addr_m1-1];
    desc_addr[k++] = I_dv[addr_m1-1];
    desc_addr[k++] = I_du[addr_p1-1];
    desc_addr[k++] = I_dv[addr_p1-1];
    desc_addr[k++] = I_du[addr_m1+3];
    desc_addr[k++] = I_dv[addr_m1+3];
    desc_addr[k++] = I_du[addr_p1+3];
    desc_addr[k++] = I_dv[addr_p1+3];
    desc_addr[k++] = I_du[addr_m1+1];
    desc_addr[k++] = I_dv[addr_m1+1];
    desc_addr[k++] = I_du[addr_p1+1];
    desc_addr[k++] = I_dv[addr_p1+1];
    desc_addr[k++] = I_du[addr_m5-1];
    desc_addr[k++] = I_dv[addr_m5-1];
    desc_addr[k++] = I_du[addr_p5-1];
    desc_addr[k++] = I_dv[addr_p5-1];
    desc_addr[k++] = I_du[addr_m5+1];
    desc_addr[k++] = I_dv[addr_m5+1];
    desc_addr[k++] = I_du[addr_p5+1];
    desc_addr[k++] = I_dv[addr_p5+1];
    desc_addr[k++] = I_du[addr_m3-5];
    desc_addr[k++] = I_dv[addr_m3-5];
    desc_addr[k++] = I_du[addr_p3-5];
    desc_addr[k++] = I_dv[addr_p3-5];
    desc_addr[k++] = I_du[addr_m3+5];
    desc_addr[k++] = I_dv[addr_m3+5];
    desc_addr[k++] = I_du[addr_p3+5];
    desc_addr[k++] = I_dv[addr_p3+5];
    //k=32
}

inline void Matcher::computeSmallDescriptor (const uint8_t* I_du,const uint8_t* I_dv,const int32_t &bpl,const int32_t &u,const int32_t &v,uint8_t *desc_addr) {

    // get address indices
    int32_t addr2 = getAddressOffsetImage(u,v,bpl);
    int32_t addr1 = addr2-bpl;
    int32_t addr0 = addr1-bpl;
    int32_t addr3 = addr2+bpl;
    int32_t addr4 = addr3+bpl;

    // compute ELAS-descriptor
    uint32_t k = 0;
    desc_addr[k++] = I_du[addr0];
    desc_addr[k++] = I_du[addr1-2];
    desc_addr[k++] = I_du[addr1];
    desc_addr[k++] = I_du[addr1+2];
    desc_addr[k++] = I_du[addr2-1];
    desc_addr[k++] = I_du[addr2];
    desc_addr[k++] = I_du[addr2];
    desc_addr[k++] = I_du[addr2+1];
    desc_addr[k++] = I_du[addr3-2];
    desc_addr[k++] = I_du[addr3];
    desc_addr[k++] = I_du[addr3+2];
    desc_addr[k++] = I_du[addr4];
    desc_addr[k++] = I_dv[addr1];
    desc_addr[k++] = I_dv[addr2-1];
    desc_addr[k++] = I_dv[addr2+1];
    desc_addr[k++] = I_dv[addr3];
}

void Matcher::computeDescriptors (uint8_t* I_du,uint8_t* I_dv,const int32_t bpl,std::vector<Matcher::maximum> &maxima) {

    // loop variables
    int32_t u,v;
    uint8_t *desc_addr;

    // for all maxima do
    for (vector<Matcher::maximum>::iterator it=maxima.begin(); it!=maxima.end(); it++) {
        u = (*it).u;
        v = (*it).v;
        desc_addr = (uint8_t*)(&((*it).d1));
        computeDescriptor(I_du,I_dv,bpl,u,v,desc_addr);
    }
}

inline uint8_t Matcher::saturate (int16_t in) {
    if (in<0)   return 0;
    if (in>255) return 255;
    return in;
}

void Matcher::filterImageAll (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,int16_t* I_f1,int16_t* I_f2,const int* dims) {

    // get bpl and height
    const int32_t height = dims[1];
    const int32_t bpl    = dims[2];

    // init sobel pointers
    const uint8_t* p00 = I + 0;
    const uint8_t* p01 = I + 1;
    const uint8_t* p02 = I + 2;
    const uint8_t* p03 = I + 3;
    const uint8_t* p04 = I + 4;
    const uint8_t* p10 = I + 0 + bpl;
    const uint8_t* p11 = I + 1 + bpl;
    const uint8_t* p12 = I + 2 + bpl;
    const uint8_t* p13 = I + 3 + bpl;
    const uint8_t* p14 = I + 4 + bpl;
    const uint8_t* p20 = I + 0 + 2*bpl;
    const uint8_t* p21 = I + 1 + 2*bpl;
    const uint8_t* p22 = I + 2 + 2*bpl;
    const uint8_t* p23 = I + 3 + 2*bpl;
    const uint8_t* p24 = I + 4 + 2*bpl;
    const uint8_t* p30 = I + 0 + 3*bpl;
    const uint8_t* p31 = I + 1 + 3*bpl;
    const uint8_t* p32 = I + 2 + 3*bpl;
    const uint8_t* p33 = I + 3 + 3*bpl;
    const uint8_t* p34 = I + 4 + 3*bpl;
    const uint8_t* p40 = I + 0 + 4*bpl;
    const uint8_t* p41 = I + 1 + 4*bpl;
    const uint8_t* p42 = I + 2 + 4*bpl;
    const uint8_t* p43 = I + 3 + 4*bpl;
    const uint8_t* p44 = I + 4 + 4*bpl;

    // init output pointers
    uint8_t* result_du = I_du +   bpl + 1;
    uint8_t* result_dv = I_dv +   bpl + 1;
    int16_t* result_f1 = I_f1 + 2*bpl + 2;
    int16_t* result_f2 = I_f2 + 2*bpl + 2;

    // stop here
    const uint8_t* end_input = I + bpl*height;

    // compute filter responses (border pixels are invalid)
    for( ; p44 != end_input; p00++, p01++, p02++, p03++, p04++,
            p10++, p11++, p12++, p13++, p14++,
            p20++, p21++, p22++, p23++, p24++,
            p30++, p31++, p32++, p33++, p34++,
            p40++, p41++, p42++, p43++, p44++,
            result_du++, result_dv++, result_f1++, result_f2++ ) {
        int16_t temp_du = - *p00 - 2 * *p10 - *p20 + *p02 + 2 * *p12 + *p22;
        int16_t temp_dv = - *p00 - 2 * *p01 - *p02 + *p20 + 2 * *p21 + *p22;
        *result_du = saturate( temp_du/4 + 128 );
        *result_dv = saturate( temp_dv/4 + 128 );
        *result_f1 = - *p00 - *p01 -     *p02 - *p03 - *p04
            - *p10 + *p11 +     *p12 + *p13 - *p14
            - *p20 + *p21 + 8 * *p22 + *p23 - *p24
            - *p30 + *p31 +     *p32 + *p33 - *p34
            - *p40 - *p41 -     *p42 - *p43 - *p44;
        *result_f2 = - *p00 - *p01 + *p03 + *p04
            - *p10 - *p11 + *p13 + *p14
            + *p30 + *p31 - *p33 - *p34
            + *p40 + *p41 - *p43 - *p44;
    }
}

void Matcher::filterImageSobel (uint8_t* I,uint8_t* I_du,uint8_t* I_dv,const int* dims) {

    // get image width and height
    const int32_t height = dims[1];
    const int32_t bpl    = dims[2];

    // init sobel pointers
    const uint8_t* p00 = I + 0;
    const uint8_t* p01 = I + 1;
    const uint8_t* p02 = I + 2;
    const uint8_t* p10 = I + 0 + bpl;
    const uint8_t* p11 = I + 1 + bpl;
    const uint8_t* p12 = I + 2 + bpl;
    const uint8_t* p20 = I + 0 + 2*bpl;
    const uint8_t* p21 = I + 1 + 2*bpl;
    const uint8_t* p22 = I + 2 + 2*bpl;

    // init output pointers
    uint8_t* result_du = I_du + bpl + 1;
    uint8_t* result_dv = I_dv + bpl + 1;

    // stop here
    const uint8_t* end_input = I + bpl*height;

    // compute filter responses (border pixels are invalid)
    for( ; p22 != end_input; p00++, p01++, p02++,
            p10++, p11++, p12++,
            p20++, p21++, p22++,
            result_du++, result_dv++) {
        int16_t temp_du = - *p00 - 2 * *p10 - *p20 + *p02 + 2 * *p12 + *p22;
        int16_t temp_dv = - *p00 - 2 * *p01 - *p02 + *p20 + 2 * *p21 + *p22;
        *result_du = saturate( temp_du/4 + 128 );
        *result_dv = saturate( temp_dv/4 + 128 );
    }
}

void Matcher::getHalfResolutionDimensions(const int32_t *dims,int32_t *dims_half) {
    dims_half[0] = dims[0]/2;
    dims_half[1] = dims[1]/2;
    dims_half[2] = dims_half[0]+15-(dims_half[0]-1)%16;
}

uint8_t* Matcher::createHalfResolutionImage(uint8_t *I,const int32_t* dims) {
    int32_t dims_half[3];
    getHalfResolutionDimensions(dims,dims_half);
    uint8_t* I_half = (uint8_t*)_mm_malloc(dims_half[2]*dims_half[1]*sizeof(uint8_t),16);
    for (int32_t v=0; v<dims_half[1]; v++)
        for (int32_t u=0; u<dims_half[0]; u++)
            I_half[v*dims_half[2]+u] =  (uint8_t)(((int32_t)I[(v*2+0)*dims[2]+u*2+0]+
                        (int32_t)I[(v*2+0)*dims[2]+u*2+1]+
                        (int32_t)I[(v*2+1)*dims[2]+u*2+0]+
                        (int32_t)I[(v*2+1)*dims[2]+u*2+1])/4);
    return I_half;
}

void Matcher::computeFeatures (uint8_t *I,const int32_t* dims,int32_t* &max1,int32_t &num1,int32_t* &max2,int32_t &num2,uint8_t* &I_du,uint8_t* &I_dv,uint8_t* &I_du_full,uint8_t* &I_dv_full) {

    int16_t *I_f1;
    int16_t *I_f2;

    int32_t dims_matching[3];
    memcpy(dims_matching,dims,3*sizeof(int32_t));

    // allocate memory for sobel images and filter images
    // Default half_resolution is true
    if (!param.half_resolution) {
        I_du = (uint8_t*)_mm_malloc(dims[2]*dims[1]*sizeof(uint8_t*),16);
        I_dv = (uint8_t*)_mm_malloc(dims[2]*dims[1]*sizeof(uint8_t*),16);
        I_f1 = (int16_t*)_mm_malloc(dims[2]*dims[1]*sizeof(int16_t),16);
        I_f2 = (int16_t*)_mm_malloc(dims[2]*dims[1]*sizeof(int16_t),16);
        filter::sobel5x5(I,I_du,I_dv,dims[2],dims[1]);
        filter::blob5x5(I,I_f1,dims[2],dims[1]);
        filter::checkerboard5x5(I,I_f2,dims[2],dims[1]);
    } else {
        //分辨率减半,像素的颜色取周边四邻均值
        uint8_t* I_matching = createHalfResolutionImage(I,dims);
        //dims_matching has been half of dims
        getHalfResolutionDimensions(dims,dims_matching);
        I_du      = (uint8_t*)_mm_malloc(dims_matching[2]*dims_matching[1]*sizeof(uint8_t*),16);
        I_dv      = (uint8_t*)_mm_malloc(dims_matching[2]*dims_matching[1]*sizeof(uint8_t*),16);
        I_f1      = (int16_t*)_mm_malloc(dims_matching[2]*dims_matching[1]*sizeof(int16_t),16);
        I_f2      = (int16_t*)_mm_malloc(dims_matching[2]*dims_matching[1]*sizeof(int16_t),16);
        I_du_full = (uint8_t*)_mm_malloc(dims[2]*dims[1]*sizeof(uint8_t*),16);
        I_dv_full = (uint8_t*)_mm_malloc(dims[2]*dims[1]*sizeof(uint8_t*),16);
        filter::sobel5x5(I_matching,I_du,I_dv,dims_matching[2],dims_matching[1]);
        filter::sobel5x5(I,I_du_full,I_dv_full,dims[2],dims[1]);
        filter::blob5x5(I_matching,I_f1,dims_matching[2],dims_matching[1]);
        filter::checkerboard5x5(I_matching,I_f2,dims_matching[2],dims_matching[1]);
        _mm_free(I_matching);
    }

    // extract sparse maxima (1st pass) via non-maximum suppression
    vector<Matcher::maximum> maxima1;
    if (param.multi_stage) {
        int32_t nms_n_sparse = param.nms_n*3;
        if (nms_n_sparse>10)
            nms_n_sparse = max(param.nms_n,10);
        nonMaximumSuppression(I_f1,I_f2,dims_matching,maxima1,nms_n_sparse);
        computeDescriptors(I_du,I_dv,dims_matching[2],maxima1);
    }

    // extract dense maxima (2nd pass) via non-maximum suppression
    vector<Matcher::maximum> maxima2;
    nonMaximumSuppression(I_f1,I_f2,dims_matching,maxima2,param.nms_n);
    computeDescriptors(I_du,I_dv,dims_matching[2],maxima2);

    // release filter images
    _mm_free(I_f1);
    _mm_free(I_f2);

    // get number of interest points and init maxima pointer to NULL
    num1 = maxima1.size();
    num2 = maxima2.size();
    //printf("n1=%d,n2=%d\n",num1,num2);
    max1 = 0;
    max2 = 0;

    int32_t s = 1;
    if (param.half_resolution)
        s = 2;

    // return sparse maxima as 16-bytes aligned memory
    if (num1!=0) {
        max1 = (int32_t*)_mm_malloc(sizeof(Matcher::maximum)*num1,16);
        int32_t k=0;
        for (vector<Matcher::maximum>::iterator it=maxima1.begin(); it!=maxima1.end(); it++) {
            *(max1+k++) = it->u*s;  *(max1+k++) = it->v*s;  *(max1+k++) = 0;        *(max1+k++) = it->c;
            *(max1+k++) = it->d1;   *(max1+k++) = it->d2;   *(max1+k++) = it->d3;   *(max1+k++) = it->d4;
            *(max1+k++) = it->d5;   *(max1+k++) = it->d6;   *(max1+k++) = it->d7;   *(max1+k++) = it->d8;
        }
    }

    // return dense maxima as 16-bytes aligned memory
    if (num2!=0) {
        max2 = (int32_t*)_mm_malloc(sizeof(Matcher::maximum)*num2,16);
        int32_t k=0;
        for (vector<Matcher::maximum>::iterator it=maxima2.begin(); it!=maxima2.end(); it++) {
            *(max2+k++) = it->u*s;  *(max2+k++) = it->v*s;  *(max2+k++) = 0;        *(max2+k++) = it->c;
            *(max2+k++) = it->d1;   *(max2+k++) = it->d2;   *(max2+k++) = it->d3;   *(max2+k++) = it->d4;
            *(max2+k++) = it->d5;   *(max2+k++) = it->d6;   *(max2+k++) = it->d7;   *(max2+k++) = it->d8;
        }
    }
}

void Matcher::computePriorStatistics (vector<Matcher::p_match> &p_matched,int32_t method) {

    // compute number of bins
    int32_t u_bin_num = (int32_t)ceil((float)dims_c[0]/(float)param.match_binsize);
    int32_t v_bin_num = (int32_t)ceil((float)dims_c[1]/(float)param.match_binsize);
    int32_t bin_num   = v_bin_num*u_bin_num;

    // number of matching stages
    int32_t num_stages = 2;
    if (method==2)
        num_stages = 4;

    // allocate bin accumulator memory
    vector<Matcher::delta> *delta_accu = new vector<Matcher::delta>[bin_num];

    // fill bin accumulator
    Matcher::delta delta_curr;
    for (vector<Matcher::p_match>::iterator it=p_matched.begin(); it!=p_matched.end(); it++) {

        // method flow: compute position delta
        if (method==0) {
            delta_curr.val[0] = it->u1p - it->u1c;
            delta_curr.val[1] = it->v1p - it->v1c;
            delta_curr.val[2] = it->u1c - it->u1p;
            delta_curr.val[3] = it->v1c - it->v1p;

            // method stereo: compute position delta
        } else if (method==1) {
            delta_curr.val[0] = it->u2c - it->u1c;
            delta_curr.val[1] = 0;
            delta_curr.val[2] = it->u1c - it->u2c;
            delta_curr.val[3] = 0;

            // method quad matching: compute position delta
        } else {
            /*
               delta_curr.val[0] = it->u1p - it->u1c;
               delta_curr.val[1] = it->v1p - it->v1c;
               delta_curr.val[2] = it->u2p - it->u1p;
               delta_curr.val[3] = 0;
               delta_curr.val[4] = it->u2c - it->u2p;
               delta_curr.val[5] = it->v2c - it->v2p;
               delta_curr.val[6] = it->u1c - it->u2c;
               delta_curr.val[7] = 0;*/
            delta_curr.val[0] = it->u2p - it->u1p;
            delta_curr.val[1] = 0;
            delta_curr.val[2] = it->u2c - it->u2p;
            delta_curr.val[3] = it->v2c - it->v2p;
            delta_curr.val[4] = it->u1c - it->u2c;
            delta_curr.val[5] = 0;
            delta_curr.val[6] = it->u1p - it->u1c;
            delta_curr.val[7] = it->v1p - it->v1c;
        }

        // compute row and column of bin to which this observation belongs
        int32_t u_bin_min,u_bin_max,v_bin_min,v_bin_max;

        // flow + stereo: use current left image as reference
        if (method<2) {
            u_bin_min = min(max((int32_t)floor(it->u1c/(float)param.match_binsize)-1,0),u_bin_num-1);
            u_bin_max = min(max((int32_t)floor(it->u1c/(float)param.match_binsize)+1,0),u_bin_num-1);
            v_bin_min = min(max((int32_t)floor(it->v1c/(float)param.match_binsize)-1,0),v_bin_num-1);
            v_bin_max = min(max((int32_t)floor(it->v1c/(float)param.match_binsize)+1,0),v_bin_num-1);

            // quad matching: use current previous image as reference
        } else {
            u_bin_min = min(max((int32_t)floor(it->u1p/(float)param.match_binsize)-1,0),u_bin_num-1);
            u_bin_max = min(max((int32_t)floor(it->u1p/(float)param.match_binsize)+1,0),u_bin_num-1);
            v_bin_min = min(max((int32_t)floor(it->v1p/(float)param.match_binsize)-1,0),v_bin_num-1);
            v_bin_max = min(max((int32_t)floor(it->v1p/(float)param.match_binsize)+1,0),v_bin_num-1);
        }

        // add to accumulator
        for (int32_t v_bin=v_bin_min; v_bin<=v_bin_max; v_bin++)
            for (int32_t u_bin=u_bin_min; u_bin<=u_bin_max; u_bin++)
                delta_accu[v_bin*u_bin_num+u_bin].push_back(delta_curr);
    }

    // clear ranges
    ranges.clear();

    // for all bins compute statistics
    for (int32_t v_bin=0; v_bin<v_bin_num; v_bin++) {
        for (int32_t u_bin=0; u_bin<u_bin_num; u_bin++) {

            // use full range in case there are no observations
            delta delta_min(-param.match_radius);
            delta delta_max(+param.match_radius);

            // otherwise determine delta min and delta max
            if (delta_accu[v_bin*u_bin_num+u_bin].size()>0) {

                // init displacements 'delta' to 'infinite'
                delta_min = delta(+1000000);
                delta_max = delta(-1000000);

                // find minimum and maximum displacements
                for (vector<Matcher::delta>::iterator it=delta_accu[v_bin*u_bin_num+u_bin].begin();
                        it!=delta_accu[v_bin*u_bin_num+u_bin].end(); it++) {
                    for (int32_t i=0; i<num_stages*2; i++) {
                        if (it->val[i]<delta_min.val[i]) delta_min.val[i] = it->val[i];
                        if (it->val[i]>delta_max.val[i]) delta_max.val[i] = it->val[i];
                    }
                }
            }

            // set search range for this bin
            // stage 0-3
            range r;
            for (int32_t i=0; i<num_stages; i++) {

                // bound minimum search range to 20x20
                float delta_u = delta_max.val[i*2+0]-delta_min.val[i*2+0];
                if (delta_u<20) {
                    delta_min.val[i*2+0] -= ceil((20-delta_u)/2);
                    delta_max.val[i*2+0] += ceil((20-delta_u)/2);
                }
                float delta_v = delta_max.val[i*2+1]-delta_min.val[i*2+1];
                if (delta_v<20) {
                    delta_min.val[i*2+1] -= ceil((20-delta_v)/2);
                    delta_max.val[i*2+1] += ceil((20-delta_v)/2);
                }

                // set range for this bin
                r.u_min[i] = delta_min.val[i*2+0];
                r.u_max[i] = delta_max.val[i*2+0];
                r.v_min[i] = delta_min.val[i*2+1];
                r.v_max[i] = delta_max.val[i*2+1];
            }
            ranges.push_back(r);
        }
    }

    // free bin accumulator memory
    delete []delta_accu;
}

void Matcher::createIndexVector (int32_t* m,int32_t n,vector<int32_t> *k,const int32_t &u_bin_num,const int32_t &v_bin_num) {

    // descriptor step size
    int32_t step_size = sizeof(Matcher::maximum)/sizeof(int32_t);

    // for all points do
    for (int32_t i=0; i<n; i++) {

        // extract coordinates and class
        int32_t u = *(m+step_size*i+0); // u-coordinate
        int32_t v = *(m+step_size*i+1); // v-coordinate
        int32_t c = *(m+step_size*i+3); // class,0123
        // compute row and column of bin to which this observation belongs
        int32_t u_bin = min((int32_t)floor((float)u/(float)param.match_binsize),u_bin_num-1);
        int32_t v_bin = min((int32_t)floor((float)v/(float)param.match_binsize),v_bin_num-1);

        // save index
        k[(c*v_bin_num+v_bin)*u_bin_num+u_bin].push_back(i);
    }
}

inline void Matcher::findMatch (int32_t* m1,const int32_t &i1,int32_t* m2,const int32_t &step_size,vector<int32_t> *k2,
        const int32_t &u_bin_num,const int32_t &v_bin_num,const int32_t &stat_bin,
        int32_t& min_ind,int32_t stage,bool flow,bool use_prior,double u_,double v_) {

    // init and load image coordinates + feature
    //min_ind输出
    min_ind          = 0;
    double  min_cost = 10000000;
    int32_t u1       = *(m1+step_size*i1+0);
    int32_t v1       = *(m1+step_size*i1+1);
    int32_t c        = *(m1+step_size*i1+3);
    //这俩是特征描述符
    __m128i xmm1     = _mm_load_si128((__m128i*)(m1+step_size*i1+4));
    __m128i xmm2     = _mm_load_si128((__m128i*)(m1+step_size*i1+8));

    float u_min,u_max,v_min,v_max;

    // restrict search range with prior
    if (use_prior) {
        u_min = u1+ranges[stat_bin].u_min[stage];
        u_max = u1+ranges[stat_bin].u_max[stage];
        v_min = v1+ranges[stat_bin].v_min[stage];
        v_max = v1+ranges[stat_bin].v_max[stage];

        // otherwise: use full search space
    } else {
        u_min = u1-param.match_radius;
        u_max = u1+param.match_radius;
        v_min = v1-param.match_radius;
        v_max = v1+param.match_radius;
    }

    // if stereo search => constrain to 1d
    // param.match_disp_tolerance=2
    if (!flow) {
        v_min = v1-param.match_disp_tolerance;
        v_max = v1+param.match_disp_tolerance;
    }
    /*printf("use_prior=%d,flow=%d\n",use_prior,flow);*/
    // bins of interest,bin_size=50, match_radius=200
    int32_t u_bin_min = min(max((int32_t)floor(u_min/(float)param.match_binsize),0),u_bin_num-1);
    int32_t u_bin_max = min(max((int32_t)floor(u_max/(float)param.match_binsize),0),u_bin_num-1);
    int32_t v_bin_min = min(max((int32_t)floor(v_min/(float)param.match_binsize),0),v_bin_num-1);
    int32_t v_bin_max = min(max((int32_t)floor(v_max/(float)param.match_binsize),0),v_bin_num-1);

    // for all bins of interest do
    for (int32_t u_bin=u_bin_min; u_bin<=u_bin_max; u_bin++) {
        for (int32_t v_bin=v_bin_min; v_bin<=v_bin_max; v_bin++) {
            int32_t k2_ind = (c*v_bin_num+v_bin)*u_bin_num+u_bin;
            for (vector<int32_t>::const_iterator i2_it=k2[k2_ind].begin(); i2_it!=k2[k2_ind].end(); i2_it++) {
                //1-xx
                /*printf("size=%d\n",k2[k2_ind].size());*/
                int32_t u2   = *(m2+step_size*(*i2_it)+0);
                int32_t v2   = *(m2+step_size*(*i2_it)+1);
                if (u2>=u_min && u2<=u_max && v2>=v_min && v2<=v_max) {
                    __m128i xmm3 = _mm_load_si128((__m128i*)(m2+step_size*(*i2_it)+4));
                    __m128i xmm4 = _mm_load_si128((__m128i*)(m2+step_size*(*i2_it)+8));
                    //Computes the absolute difference of the 16 unsigned 8-bit integers from a and the 16 unsigned 8-bit integers from b.
                    xmm3 = _mm_sad_epu8 (xmm1,xmm3);
                    xmm4 = _mm_sad_epu8 (xmm2,xmm4);
                    xmm4 = _mm_add_epi16(xmm3,xmm4);
                    double cost = (double)(_mm_extract_epi16(xmm4,0)+_mm_extract_epi16(xmm4,4));
                    /*printf("%f,%f,%f,%f",_mm_extract_epi16(xmm4,0),_mm_extract_epi16(xmm4,2),_mm_extract_epi16(xmm4,3),_mm_extract_epi16(xmm4,4));*/
                    /*printf("cost=%f",cost);*/
                    /*printf("%f,%f\n",_mm_extract_epi16(xmm4,1),_mm_extract_epi16(xmm4,3));*/
                    if (u_>=0 && v_>=0) {
                        double du = (double)u2-u_;
                        double dv = (double)v2-v_;
                        double dist = sqrt(du*du+dv*dv);
                        cost += 4*dist;
                    }

                    if (cost<min_cost) {
                        min_ind  = *i2_it;
                        min_cost = cost;
                    }
                }
            }
        }
    }
}

void Matcher::matching (int32_t *m1p,int32_t *m2p,int32_t *m1c,int32_t *m2c,
        int32_t n1p,int32_t n2p,int32_t n1c,int32_t n2c,
        vector<Matcher::p_match> &p_matched,int32_t method,bool use_prior,Matrix *Tr_delta) {

    // descriptor step size (number of int32_t elements in struct)
    int32_t step_size = sizeof(Matcher::maximum)/sizeof(int32_t);

    // compute number of bins
    int32_t u_bin_num = (int32_t)ceil((float)dims_c[0]/(float)param.match_binsize);
    int32_t v_bin_num = (int32_t)ceil((float)dims_c[1]/(float)param.match_binsize);
    int32_t bin_num   = 4*v_bin_num*u_bin_num; // 4 classes

    // allocate memory for index vectors (needed for efficient search)
    vector<int32_t> *k1p = new vector<int32_t>[bin_num];
    vector<int32_t> *k2p = new vector<int32_t>[bin_num];
    vector<int32_t> *k1c = new vector<int32_t>[bin_num];
    vector<int32_t> *k2c = new vector<int32_t>[bin_num];

    // loop variables
    int32_t* M = (int32_t*)calloc(dims_c[0]*dims_c[1],sizeof(int32_t));
    int32_t i1p,i2p,i1c,i2c,i1c2,i1p2;
    int32_t u1p,v1p,u2p,v2p,u1c,v1c,u2c,v2c;

    double t00,t01,t02,t03,t10,t11,t12,t13,t20,t21,t22,t23;
    //前两次运行tr_delta = null
    if (Tr_delta) {
        t00 = Tr_delta->val[0][0];
        t01 = Tr_delta->val[0][1];
        t02 = Tr_delta->val[0][2];
        t03 = Tr_delta->val[0][3];
        t10 = Tr_delta->val[1][0];
        t11 = Tr_delta->val[1][1];
        t12 = Tr_delta->val[1][2];
        t13 = Tr_delta->val[1][3];
        t20 = Tr_delta->val[2][0];
        t21 = Tr_delta->val[2][1];
        t22 = Tr_delta->val[2][2];
        t23 = Tr_delta->val[2][3];
    }

    // method: flow
    if (method==0) {

        //printf("flow\n");
        // create position/class bin index vectors
        createIndexVector(m1p,n1p,k1p,u_bin_num,v_bin_num);
        createIndexVector(m1c,n1c,k1c,u_bin_num,v_bin_num);

        // for all points do
        for (i1c=0; i1c<n1c; i1c++) {

            // coordinates in previous left image
            u1c = *(m1c+step_size*i1c+0);
            v1c = *(m1c+step_size*i1c+1);

            // compute row and column of statistics bin to which this observation belongs
            int32_t u_bin = min((int32_t)floor((float)u1c/(float)param.match_binsize),u_bin_num-1);
            int32_t v_bin = min((int32_t)floor((float)v1c/(float)param.match_binsize),v_bin_num-1);
            int32_t stat_bin = v_bin*u_bin_num+u_bin;

            // match forward/backward
            findMatch(m1c,i1c,m1p,step_size,k1p,u_bin_num,v_bin_num,stat_bin,i1p, 0,true,use_prior);
            findMatch(m1p,i1p,m1c,step_size,k1c,u_bin_num,v_bin_num,stat_bin,i1c2,1,true,use_prior);

            // circle closure success?
            if (i1c2==i1c) {

                // extract coordinates
                u1p = *(m1p+step_size*i1p+0);
                v1p = *(m1p+step_size*i1p+1);

                // add match if this pixel isn't matched yet
                if (*(M+getAddressOffsetImage(u1c,v1c,dims_c[0]))==0) {
                    p_matched.push_back(Matcher::p_match(u1p,v1p,i1p,-1,-1,-1,u1c,v1c,i1c,-1,-1,-1));
                    *(M+getAddressOffsetImage(u1c,v1c,dims_c[0])) = 1;
                }
            }
        }

        // method: stereo
    } else if (method==1) {

        //printf("stereo\n");
        // create position/class bin index vectors
        createIndexVector(m1c,n1c,k1c,u_bin_num,v_bin_num);
        createIndexVector(m2c,n2c,k2c,u_bin_num,v_bin_num);

        // for all points do
        for (i1c=0; i1c<n1c; i1c++) {

            // coordinates in previous left image
            u1c = *(m1c+step_size*i1c+0);
            v1c = *(m1c+step_size*i1c+1);

            // compute row and column of statistics bin to which this observation belongs
            int32_t u_bin = min((int32_t)floor((float)u1c/(float)param.match_binsize),u_bin_num-1);
            int32_t v_bin = min((int32_t)floor((float)v1c/(float)param.match_binsize),v_bin_num-1);
            int32_t stat_bin = v_bin*u_bin_num+u_bin;

            // match left/right
            findMatch(m1c,i1c,m2c,step_size,k2c,u_bin_num,v_bin_num,stat_bin,i2c, 0,false,use_prior);
            findMatch(m2c,i2c,m1c,step_size,k1c,u_bin_num,v_bin_num,stat_bin,i1c2,1,false,use_prior);

            // circle closure success?
            if (i1c2==i1c) {

                // extract coordinates
                u2c = *(m2c+step_size*i2c+0);
                v2c = *(m2c+step_size*i2c+1);

                // if disparity is positive
                if (u1c>=u2c) {

                    // add match if this pixel isn't matched yet
                    if (*(M+getAddressOffsetImage(u1c,v1c,dims_c[0]))==0) {
                        p_matched.push_back(Matcher::p_match(-1,-1,-1,-1,-1,-1,u1c,v1c,i1c,u2c,v2c,i2c));
                        *(M+getAddressOffsetImage(u1c,v1c,dims_c[0])) = 1;
                    }
                }
            }
        }

        // method: quad matching
    } else {

        //printf("quad\n");
        // create position/class bin index vectors
        createIndexVector(m1p,n1p,k1p,u_bin_num,v_bin_num);
        createIndexVector(m2p,n2p,k2p,u_bin_num,v_bin_num);
        createIndexVector(m1c,n1c,k1c,u_bin_num,v_bin_num);
        createIndexVector(m2c,n2c,k2c,u_bin_num,v_bin_num);

        // for all points do
        for (i1p=0; i1p<n1p; i1p++) {

            // coordinates
            // 特征描述符的个数,一个描述符32byte
            u1p = *(m1p+step_size*i1p+0);
            v1p = *(m1p+step_size*i1p+1);

            // compute row and column of statistics bin to which this observation belongs
            int32_t u_bin = min((int32_t)floor((float)u1p/(float)param.match_binsize),u_bin_num-1);
            int32_t v_bin = min((int32_t)floor((float)v1p/(float)param.match_binsize),v_bin_num-1);
            int32_t stat_bin = v_bin*u_bin_num+u_bin;

            // match in circle
            // 第一次使用1/2分辨率时use_prio=false,第二次全分辨率=true
            // 这个false指的是flaw,0指的是stage
            findMatch(m1p,i1p,m2p,step_size,k2p,u_bin_num,v_bin_num,stat_bin,i2p, 0,false,use_prior);

            u2p = *(m2p+step_size*i2p+0);
            v2p = *(m2p+step_size*i2p+1);

            if (Tr_delta) {

                double d = max((double)u1p-(double)u2p,1.0);
                double x1p = ((double)u1p-param.cu)*param.base/d;
                double y1p = ((double)v1p-param.cv)*param.base/d;
                double z1p = param.f*param.base/d;
                /*printf("%f,%f,%f\n",x1p,y1p,z1p);*/

                //在右摄像头坐标中的位置
                double x2c = t00*x1p + t01*y1p + t02*z1p + t03 - param.base;
                double y2c = t10*x1p + t11*y1p + t12*z1p + t13;
                double z2c = t20*x1p + t21*y1p + t22*z1p + t23;

                double u2c_ = param.f*x2c/z2c+param.cu;
                double v2c_ = param.f*y2c/z2c+param.cv;

                findMatch(m2p,i2p,m2c,step_size,k2c,u_bin_num,v_bin_num,stat_bin,i2c, 1,true ,use_prior,u2c_,v2c_);
            }
            else
            {
                findMatch(m2p,i2p,m2c,step_size,k2c,u_bin_num,v_bin_num,stat_bin,i2c, 1,true ,use_prior);
            }


            findMatch(m2c,i2c,m1c,step_size,k1c,u_bin_num,v_bin_num,stat_bin,i1c, 2,false,use_prior);
            if (Tr_delta)
                findMatch(m1c,i1c,m1p,step_size,k1p,u_bin_num,v_bin_num,stat_bin,i1p2,3,true ,use_prior,u1p,v1p);
            else
                findMatch(m1c,i1c,m1p,step_size,k1p,u_bin_num,v_bin_num,stat_bin,i1p2,3,true ,use_prior);

            // circle closure success?
            if (i1p2==i1p) {

                // extract coordinates
                u2c = *(m2c+step_size*i2c+0); v2c = *(m2c+step_size*i2c+1);
                u1c = *(m1c+step_size*i1c+0); v1c = *(m1c+step_size*i1c+1);

                // if disparities are positive
                if (u1p>=u2p && u1c>=u2c) //&& param.f*param.base/(u1p-u2p)<3.0 && param.f*param.base/(u1c-u2c)<3.0 )
                {
                    // add match
                    p_matched.push_back(Matcher::p_match(u1p,v1p,i1p,u2p,v2p,i2p,
                                u1c,v1c,i1c,u2c,v2c,i2c));
                }
            }
        }

        // old version:
        /*
        // for all points do
        for (i1c=0; i1c<n1c; i1c++) {

        // coordinates
        u1c = *(m1c+step_size*i1c+0);
        v1c = *(m1c+step_size*i1c+1);

        // compute row and column of statistics bin to which this observation belongs
        int32_t u_bin = min((int32_t)floor((float)u1c/(float)param.match_binsize),u_bin_num-1);
        int32_t v_bin = min((int32_t)floor((float)v1c/(float)param.match_binsize),v_bin_num-1);
        int32_t stat_bin = v_bin*u_bin_num+u_bin;

        // match in circle
        findMatch(m1c,i1c,m1p,step_size,k1p,u_bin_num,v_bin_num,stat_bin,i1p, 0,true ,use_prior,Tr_delta);
        findMatch(m1p,i1p,m2p,step_size,k2p,u_bin_num,v_bin_num,stat_bin,i2p, 1,false,use_prior,Tr_delta);
        findMatch(m2p,i2p,m2c,step_size,k2c,u_bin_num,v_bin_num,stat_bin,i2c, 2,true ,use_prior,Tr_delta);
        findMatch(m2c,i2c,m1c,step_size,k1c,u_bin_num,v_bin_num,stat_bin,i1c2,3,false,use_prior,Tr_delta);

        // circle closure success?
        if (i1c2==i1c) {

        // extract coordinates
        u1p = *(m1p+step_size*i1p+0); v1p = *(m1p+step_size*i1p+1);
        u2p = *(m2p+step_size*i2p+0); v2p = *(m2p+step_size*i2p+1);
        u2c = *(m2c+step_size*i2c+0); v2c = *(m2c+step_size*i2c+1);

        // if disparities are positive
        if (u1p>=u2p && u1c>=u2c) {

        // add match if this pixel isn't matched yet
        if (*(M+getAddressOffsetImage(u1c,v1c,dims_c[0]))==0) {
        p_matched.push_back(Matcher::p_match(u1p,v1p,i1p,u2p,v2p,i2p,
        u1c,v1c,i1c,u2c,v2c,i2c));
         *(M+getAddressOffsetImage(u1c,v1c,dims_c[0])) = 1;
         }

         }
         }
         }
         */
    }

    // free memory
    free(M);
    delete []k1p;
    delete []k2p;
    delete []k1c;
    delete []k2c;
}

void Matcher::removeOutliers (vector<Matcher::p_match> &p_matched,int32_t method) {

    // do we have enough points for outlier removal?
    if (p_matched.size()<=3)
        return;

    // input/output structure for triangulation
    struct triangulateio in, out;

    // inputs
    in.numberofpoints = p_matched.size();
    in.pointlist = (float*)malloc(in.numberofpoints*2*sizeof(float));
    int32_t k=0;

    // create copy of p_matched, init vector with number of support points
    // and fill triangle point vector for delaunay triangulation
    vector<Matcher::p_match> p_matched_copy;
    vector<int32_t> num_support;
    for (vector<Matcher::p_match>::iterator it=p_matched.begin(); it!=p_matched.end(); it++) {
        p_matched_copy.push_back(*it);
        num_support.push_back(0);
        in.pointlist[k++] = it->u1c;
        in.pointlist[k++] = it->v1c;
    }

    // input parameters
    in.numberofpointattributes = 0;
    in.pointattributelist      = NULL;
    in.pointmarkerlist         = NULL;
    in.numberofsegments        = 0;
    in.numberofholes           = 0;
    in.numberofregions         = 0;
    in.regionlist              = NULL;

    // outputs
    out.pointlist              = NULL;
    out.pointattributelist     = NULL;
    out.pointmarkerlist        = NULL;
    out.trianglelist           = NULL;
    out.triangleattributelist  = NULL;
    out.neighborlist           = NULL;
    out.segmentlist            = NULL;
    out.segmentmarkerlist      = NULL;
    out.edgelist               = NULL;
    out.edgemarkerlist         = NULL;

    // do triangulation (z=zero-based, n=neighbors, Q=quiet, B=no boundary markers)
    // attention: not using the B switch or using the n switch creates a memory leak (=> use valgrind!)
    char parameters[] = "zQB";
    triangulate(parameters, &in, &out, NULL);

    // for all triangles do
    for (int32_t i=0; i<out.numberoftriangles; i++) {

        // extract triangle corner points
        int32_t p1 = out.trianglelist[i*3+0];
        int32_t p2 = out.trianglelist[i*3+1];
        int32_t p3 = out.trianglelist[i*3+2];

        // method: flow
        if (method==0) {

            // 1. corner disparity and flow
            float p1_flow_u = p_matched_copy[p1].u1c-p_matched_copy[p1].u1p;
            float p1_flow_v = p_matched_copy[p1].v1c-p_matched_copy[p1].v1p;

            // 2. corner disparity and flow
            float p2_flow_u = p_matched_copy[p2].u1c-p_matched_copy[p2].u1p;
            float p2_flow_v = p_matched_copy[p2].v1c-p_matched_copy[p2].v1p;

            // 3. corner disparity and flow
            float p3_flow_u = p_matched_copy[p3].u1c-p_matched_copy[p3].u1p;
            float p3_flow_v = p_matched_copy[p3].v1c-p_matched_copy[p3].v1p;

            // consistency of 1. edge
            if (fabs(p1_flow_u-p2_flow_u)+fabs(p1_flow_v-p2_flow_v)<param.outlier_flow_tolerance) {
                num_support[p1]++;
                num_support[p2]++;
            }

            // consistency of 2. edge
            if (fabs(p2_flow_u-p3_flow_u)+fabs(p2_flow_v-p3_flow_v)<param.outlier_flow_tolerance) {
                num_support[p2]++;
                num_support[p3]++;
            }

            // consistency of 3. edge
            if (fabs(p1_flow_u-p3_flow_u)+fabs(p1_flow_v-p3_flow_v)<param.outlier_flow_tolerance) {
                num_support[p1]++;
                num_support[p3]++;
            }

            // method: stereo
        } else if (method==1) {

            // 1. corner disparity and flow
            float p1_disp   = p_matched_copy[p1].u1c-p_matched_copy[p1].u2c;

            // 2. corner disparity and flow
            float p2_disp   = p_matched_copy[p2].u1c-p_matched_copy[p2].u2c;

            // 3. corner disparity and flow
            float p3_disp   = p_matched_copy[p3].u1c-p_matched_copy[p3].u2c;

            // consistency of 1. edge
            if (fabs(p1_disp-p2_disp)<param.outlier_disp_tolerance) {
                num_support[p1]++;
                num_support[p2]++;
            }

            // consistency of 2. edge
            if (fabs(p2_disp-p3_disp)<param.outlier_disp_tolerance) {
                num_support[p2]++;
                num_support[p3]++;
            }

            // consistency of 3. edge
            if (fabs(p1_disp-p3_disp)<param.outlier_disp_tolerance) {
                num_support[p1]++;
                num_support[p3]++;
            }

            // method: quad matching
        } else {

            // 1. corner disparity and flow
            float p1_flow_u = p_matched_copy[p1].u1c-p_matched_copy[p1].u1p;
            float p1_flow_v = p_matched_copy[p1].v1c-p_matched_copy[p1].v1p;
            float p1_disp   = p_matched_copy[p1].u1p-p_matched_copy[p1].u2p;

            // 2. corner disparity and flow
            float p2_flow_u = p_matched_copy[p2].u1c-p_matched_copy[p2].u1p;
            float p2_flow_v = p_matched_copy[p2].v1c-p_matched_copy[p2].v1p;
            float p2_disp   = p_matched_copy[p2].u1p-p_matched_copy[p2].u2p;

            // 3. corner disparity and flow
            float p3_flow_u = p_matched_copy[p3].u1c-p_matched_copy[p3].u1p;
            float p3_flow_v = p_matched_copy[p3].v1c-p_matched_copy[p3].v1p;
            float p3_disp   = p_matched_copy[p3].u1p-p_matched_copy[p3].u2p;

            // consistency of 1. edge
            if (fabs(p1_disp-p2_disp)<param.outlier_disp_tolerance && fabs(p1_flow_u-p2_flow_u)+fabs(p1_flow_v-p2_flow_v)<param.outlier_flow_tolerance) {
                num_support[p1]++;
                num_support[p2]++;
            }

            // consistency of 2. edge
            if (fabs(p2_disp-p3_disp)<param.outlier_disp_tolerance && fabs(p2_flow_u-p3_flow_u)+fabs(p2_flow_v-p3_flow_v)<param.outlier_flow_tolerance) {
                num_support[p2]++;
                num_support[p3]++;
            }

            // consistency of 3. edge
            if (fabs(p1_disp-p3_disp)<param.outlier_disp_tolerance && fabs(p1_flow_u-p3_flow_u)+fabs(p1_flow_v-p3_flow_v)<param.outlier_flow_tolerance) {
                num_support[p1]++;
                num_support[p3]++;
            }
        }
    }

    // refill p_matched
    p_matched.clear();
    for (int i=0; i<in.numberofpoints; i++)
        if (num_support[i]>=4)
            p_matched.push_back(p_matched_copy[i]);

    // free memory used for triangulation
    free(in.pointlist);
    free(out.pointlist);
    free(out.trianglelist);
}

bool Matcher::parabolicFitting(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1,
        const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2,
        const float &u1,const float &v1,
        float       &u2,float       &v2,
        Matrix At,Matrix AtA,
        uint8_t* desc_buffer) {

    // check if parabolic fitting is feasible (descriptors are within margin)
    if (u2-3<margin || u2+3>dims2[0]-1-margin || v2-3<margin || v2+3>dims2[1]-1-margin)
        return false;

    // compute reference descriptor
    __m128i xmm1,xmm2;
    computeSmallDescriptor(I1_du,I1_dv,dims1[2],(int32_t)u1,(int32_t)v1,desc_buffer);
    xmm1 = _mm_load_si128((__m128i*)(desc_buffer));

    // compute cost matrix
    int32_t cost[49];
    for (int32_t dv=0; dv<7; dv++) {
        for (int32_t du=0; du<7; du++) {
            computeSmallDescriptor(I2_du,I2_dv,dims2[2],(int32_t)u2+du-3,(int32_t)v2+dv-3,desc_buffer);
            xmm2 = _mm_load_si128((__m128i*)(desc_buffer));
            xmm2 = _mm_sad_epu8(xmm1,xmm2);
            cost[dv*7+du] = _mm_extract_epi16(xmm2,0)+_mm_extract_epi16(xmm2,4);
        }
    }

    // compute minimum
    int32_t min_ind  = 0;
    int32_t min_cost = cost[0];
    for (int32_t i=1; i<49; i++) {
        if (cost[i]<min_cost) {
            min_ind   = i;
            min_cost  = cost[i];
        }
    }

    // get indices
    int32_t du = min_ind%7;
    int32_t dv = min_ind/7;

    // if minimum is at borders => remove this match
    if (du==0 || du==6 || dv==0 || dv==6)
        return false;

    // solve least squares system
    Matrix c(9,1);
    for (int32_t i=-1; i<=+1; i++) {
        for (int32_t j=-1; j<=+1; j++) {
            int32_t cost_curr = cost[(dv+i)*7+(du+j)];
            // if (i!=0 && j!=0 && cost_curr<=min_cost+150)
            // return false;
            c.val[(i+1)*3+(j+1)][0] = cost_curr;
        }
    }
    Matrix b = At*c;
    if (!b.solve(AtA))
        return false;

    // extract relative coordinates
    float divisor = (b.val[2][0]*b.val[2][0]-4.0*b.val[0][0]*b.val[1][0]);
    if (fabs(divisor)<1e-8 || fabs(b.val[2][0])<1e-8)
        return false;
    float ddv = (2.0*b.val[0][0]*b.val[4][0]-b.val[2][0]*b.val[3][0])/divisor;
    float ddu = -(b.val[4][0]+2.0*b.val[1][0]*ddv)/b.val[2][0];
    if (fabs(ddu)>=1.0 || fabs(ddv)>=1.0)
        return false;

    // update target
    u2 += (float)du-3.0+ddu;
    v2 += (float)dv-3.0+ddv;

    // return true on success
    return true;
}

void Matcher::relocateMinimum(const uint8_t* I1_du,const uint8_t* I1_dv,const int32_t* dims1,
        const uint8_t* I2_du,const uint8_t* I2_dv,const int32_t* dims2,
        const float &u1,const float &v1,
        float       &u2,float       &v2,
        uint8_t* desc_buffer) {

    // check if parabolic fitting is feasible (descriptors are within margin)
    if (u2-2<margin || u2+2>dims2[0]-1-margin || v2-2<margin || v2+2>dims2[1]-1-margin)
        return;

    // compute reference descriptor
    __m128i xmm1,xmm2;
    computeSmallDescriptor(I1_du,I1_dv,dims1[2],(int32_t)u1,(int32_t)v1,desc_buffer);
    xmm1 = _mm_load_si128((__m128i*)(desc_buffer));

    // compute cost matrix
    int32_t cost[25];
    for (int32_t dv=0; dv<5; dv++) {
        for (int32_t du=0; du<5; du++) {
            computeSmallDescriptor(I2_du,I2_dv,dims2[2],(int32_t)u2+du-2,(int32_t)v2+dv-2,desc_buffer);
            xmm2 = _mm_load_si128((__m128i*)(desc_buffer));
            xmm2 = _mm_sad_epu8(xmm1,xmm2);
            cost[dv*5+du] = _mm_extract_epi16(xmm2,0)+_mm_extract_epi16(xmm2,4);
        }
    }

    // compute minimum
    int32_t min_ind  = 0;
    int32_t min_cost = cost[0];
    for (int32_t i=1; i<25; i++) {
        if (cost[i]<min_cost) {
            min_ind   = i;
            min_cost  = cost[i];
        }
    }

    // update target
    u2 += (float)(min_ind%5)-2.0;
    v2 += (float)(min_ind/5)-2.0;
}

void Matcher::refinement (vector<Matcher::p_match> &p_matched,int32_t method) {

    // allocate aligned memory (32 bytes for 1 descriptors)
    uint8_t* desc_buffer = (uint8_t*)_mm_malloc(32*sizeof(uint8_t),16);

    // copy vector (for refill)
    vector<Matcher::p_match> p_matched_copy = p_matched;
    p_matched.clear();

    // create matrices for least square fitting
    FLOAT A_data[9*6] =
    {
        1, 1, 1,-1,-1, 1,
        0, 1, 0, 0,-1, 1,
        1, 1,-1, 1,-1, 1,
        1, 0, 0,-1, 0, 1,
        0, 0, 0, 0, 0, 1,
        1, 0, 0, 1, 0, 1,
        1, 1,-1,-1, 1, 1,
        0, 1, 0, 0, 1, 1,
        1, 1, 1, 1, 1, 1
    };
    Matrix A(9,6,A_data);
    Matrix At  = ~A;
    Matrix AtA = At*A;

    uint8_t* I1p_du_fit = I1p_du;
    uint8_t* I1p_dv_fit = I1p_dv;
    uint8_t* I2p_du_fit = I2p_du;
    uint8_t* I2p_dv_fit = I2p_dv;
    uint8_t* I1c_du_fit = I1c_du;
    uint8_t* I1c_dv_fit = I1c_dv;
    uint8_t* I2c_du_fit = I2c_du;
    uint8_t* I2c_dv_fit = I2c_dv;
    if (param.half_resolution) {
        I1p_du_fit = I1p_du_full;
        I1p_dv_fit = I1p_dv_full;
        I2p_du_fit = I2p_du_full;
        I2p_dv_fit = I2p_dv_full;
        I1c_du_fit = I1c_du_full;
        I1c_dv_fit = I1c_dv_full;
        I2c_du_fit = I2c_du_full;
        I2c_dv_fit = I2c_dv_full;
    }

    // for all matches do
    for (vector<Matcher::p_match>::iterator it=p_matched_copy.begin(); it!=p_matched_copy.end(); it++) {

        // method: flow or quad matching
        if (method==0 || method==2) {
            if (param.refinement==2) {
                if (!parabolicFitting(I1c_du_fit,I1c_dv_fit,dims_c,I1p_du_fit,I1p_dv_fit,dims_p,
                            it->u1c,it->v1c,it->u1p,it->v1p,At,AtA,desc_buffer))
                    continue;
            } else {
                relocateMinimum(I1c_du_fit,I1c_dv_fit,dims_c,I1p_du_fit,I1p_dv_fit,dims_p,
                        it->u1c,it->v1c,it->u1p,it->v1p,desc_buffer);
            }
        }

        // method: stereo or quad matching
        if (method==1 || method==2) {
            if (param.refinement==2) {
                if (!parabolicFitting(I1c_du_fit,I1c_dv_fit,dims_c,I2c_du_fit,I2c_dv_fit,dims_c,
                            it->u1c,it->v1c,it->u2c,it->v2c,At,AtA,desc_buffer))
                    continue;
            } else {
                relocateMinimum(I1c_du_fit,I1c_dv_fit,dims_c,I2c_du_fit,I2c_dv_fit,dims_c,
                        it->u1c,it->v1c,it->u2c,it->v2c,desc_buffer);
            }
        }

        // method: quad matching
        if (method==2) {
            if (param.refinement==2) {
                if (!parabolicFitting(I1c_du_fit,I1c_dv_fit,dims_c,I2p_du_fit,I2p_dv_fit,dims_p,
                            it->u1c,it->v1c,it->u2p,it->v2p,At,AtA,desc_buffer))
                    continue;
            } else {
                relocateMinimum(I1c_du_fit,I1c_dv_fit,dims_c,I2p_du_fit,I2p_dv_fit,dims_p,
                        it->u1c,it->v1c,it->u2p,it->v2p,desc_buffer);
            }
        }

        // add this match
        p_matched.push_back(*it);
    }

    // free memory
    _mm_free(desc_buffer);
}

float Matcher::mean(const uint8_t* I,const int32_t &bpl,const int32_t &u_min,const int32_t &u_max,const int32_t &v_min,const int32_t &v_max) {
    float mean = 0;
    for (int32_t v=v_min; v<=v_max; v++)
        for (int32_t u=u_min; u<=u_max; u++)
            mean += (float)*(I+getAddressOffsetImage(u,v,bpl));
    return
        mean /= (float)((u_max-u_min+1)*(v_max-v_min+1));
}