c++图像处理程序编译成.so动态链接库供Java调用（包含jni文件与引用第三方动态链接库opencv）

一、linux编译so文件需要准备的环境

1、安装JDK（注意：不能安装openjdk,因为openjdk没有include目录，编译时需要用到include目录的头文件）

2、安装gcc和g++  ( yum install gcc-c++) 、（yum install cmake3）

3、本案例使用到第三方opencv动态链接库，所以需要安装opencv
3.1：首先官网下载opencv,地址：https://opencv.org/releases/

3.2：tar -zxvf opencv-4.5.0.zip -C /opt/module/

3.3:cd opencv-4.5.0  ->mkdir build->cd build->cmake -D CMAKE_BUILD_TYPE=RELEASE -D OPENCV_GENERATE_PKGCONFIG=ON -D CMAKE_INSTALL_PREFIX=/usr/local ..(此操作不能丢失..,编译时间比较久)

3.4：make install,头文件被放在/usr/local/include/opencv4/opencv2/目录下，我们的库文件被放在/usr/local/lib64/目录下

 

 查看opencv的动态库文件存在地址

 

 3.5:查看opencv安装版本

 3.6：建立软连接：sudo ln -s /usr/local/lib64/pkgconfig/opencv4.pc /usr/share/pkgconfig/

 3.7：pkg-config --cflags --libs opencv4

 

4、为防止生成的so文件找不到依赖库，使用vim /etc/ld.so.conf.d/opencv4.conf，保存并使用此命令让其生效：sudo ldconfig（/etc/Id.so.conf详解：https://www.cnblogs.com/chris-cp/p/3591306.html）

 注意出现：undefined symbol,就是因为找不到opencv的一些so依赖库导致的

/usr/local/lib64

5、需要使用到的cpp文件

5.1：com_htzs_insar_jni_Registration.cpp

#include<stdio.h>
#include"com_htzs_insar_jni_Registration.h"
#include"ComplexMat.h"
#include"Utils.h"
#include"Registration.h"

using namespace InSAR;
using namespace cv;
static const char* const Pointclasspath = "com/htzs/insar/jni/Point";
static const char* const Fitclasspath = "com/htzs/insar/jni/Fit";
/** @brief 抛出异常

@param env                JNIEnv指针
@param msg                异常情况
*/
void ThrowException(JNIEnv* env, const char* msg)
{
    jclass cls = env->FindClass("com/htzs/insar/jni/InSAR_JNIException");
    if (!cls) return;
    jmethodID methodID = env->GetMethodID(cls, "<init>", "(Ljava/lang/String;)V");
    if (!methodID) return;
    char buffer[512] = { 0 };
    sprintf(buffer, "InSAR_JNIException !: %s", msg);
    jstring str = env->NewStringUTF(buffer);
    jthrowable thr = (jthrowable)env->NewObject(cls, methodID, str);
    env->Throw(thr);
    env->DeleteLocalRef(str);
    env->DeleteLocalRef(thr);
}
/** @brief 插值并计算偏移量

@param lineSize                     每行元素数量
@param master_real                  主图像实部数组
@param master_imag                  主图像虚部数组
@param slave_real                   辅图像实部数组
@param slave_imag                   辅图像虚部数组
@return offset                      偏移量
*/
JNIEXPORT jobject JNICALL Java_com_htzs_insar_jni_Registration_calcOffset(
    JNIEnv* env,
    jclass,
    jint lineSize,
    jshortArray master_real,
    jshortArray master_imag,
    jshortArray slave_real,
    jshortArray slave_imag
)
{
    char msg[512];
    //获取主辅图像数据
    jint num = env->GetArrayLength(master_real);
    jint rows = num / lineSize;
    ComplexMat master(rows, lineSize), slave(rows, lineSize);
    master.convertTo(master, CV_16S);
    slave.convertTo(slave, CV_16S);
    jshort* data = NULL;
    data = env->GetShortArrayElements(master_real, NULL);
    memcpy(master.re.data, data, sizeof(short) * num);
    env->ReleaseShortArrayElements(master_real, data, 0);

    data = env->GetShortArrayElements(master_imag, NULL);
    memcpy(master.im.data, data, sizeof(short) * num);
    env->ReleaseShortArrayElements(master_imag, data, 0);

    data = env->GetShortArrayElements(slave_real, NULL);
    memcpy(slave.re.data, data, sizeof(short) * num);
    env->ReleaseShortArrayElements(slave_real, data, 0);

    data = env->GetShortArrayElements(slave_imag, NULL);
    memcpy(slave.im.data, data, sizeof(short) * num);
    env->ReleaseShortArrayElements(slave_imag, data, 0);

    //short型转换至double型
    master.convertTo(master, CV_64F);
    slave.convertTo(slave, CV_64F);

    //插值8倍并计算偏移量

    Registration regis; Utils util;
    ComplexMat master_tmp, slave_tmp;
    Mat coherence; double coh = 0.0; int offset_row, offset_col;
    int ret;

    //粗配准
    ComplexMat master_temp, slave_temp;
    int move_r, move_c;
    ret = regis.registration_pixel(master, slave, &move_r, &move_c);
    if (ret < 0)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    ret = util.complex_coherence(master, slave, coherence);
    if (ret < 0)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    coh = mean(coherence)[0];
    int nr = master.GetRows(); int nc = master.GetCols();
    if (nr == 1 || nc == 1)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    nr = nr % 2 == 0 ? nr : nr - 1;
    nc = nc % 2 == 0 ? nc : nc - 1;
    master = master(Range(0, nr), Range(0, nc));
    slave = slave(Range(0, nr), Range(0, nc));

    ret = regis.interp_paddingzero(master, master_temp, 8);
    if (ret < 0)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    ret = regis.interp_paddingzero(slave, slave_temp, 8);
    if (ret < 0)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    ret = regis.real_coherent(master_temp, slave_temp, &offset_row, &offset_col);
    if (ret < 0)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }


    jclass Point = env->FindClass(Pointclasspath);
    jmethodID methodID = env->GetMethodID(Point, "<init>", "()V");
    jobject p = env->NewObject(Point, methodID);

    jfieldID fid = env->GetFieldID(Point, "row", "D");
    jfieldID fid2 = env->GetFieldID(Point, "col", "D");
    jfieldID fid3 = env->GetFieldID(Point, "coherence", "D");
    env->SetDoubleField(p, fid, (double)offset_row / 8.0 + (double)move_r);
    env->SetDoubleField(p, fid2, (double)offset_col / 8.0 + (double)move_c);
    env->SetDoubleField(p, fid3, coh);

    return p;

}


/** @brief 根据各子块中心坐标和偏移量拟合整张图每个像素的偏移量

@param offset                            各子块偏移量数组
@param center_coordinate                 各子块中心坐标数组
@return Fit                              偏移量拟合系数（a0-a5,b0-b5）
*/
JNIEXPORT jobject JNICALL Java_com_htzs_insar_jni_Registration_fitting
(JNIEnv* env, jclass job, jobjectArray offset_array, jobjectArray center_coordinate_array, jdouble coh_thresh)
{
    //读取子块中心坐标和偏移量数据
    char msg[512];
    jint num_obj = env->GetArrayLength(offset_array);
    if (num_obj < 7)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    Mat offset_r, offset_c, offset_coord_row, offset_coord_col, coh_array;
    offset_r.create(num_obj, 1, CV_64F); offset_c.create(num_obj, 1, CV_64F);
    offset_coord_row.create(num_obj, 1, CV_64F); offset_coord_col.create(num_obj, 1, CV_64F); coh_array.create(num_obj, 1, CV_64F);
    jclass Point = env->FindClass(Pointclasspath);
    jfieldID offset_row_ID = env->GetFieldID(Point, "row", "D");
    jfieldID offset_col_ID = env->GetFieldID(Point, "col", "D");
    jfieldID coh_ID = env->GetFieldID(Point, "coherence", "D");
    jobject offset;
    jdouble offset_col, offset_row, coh;
    double count = 0;
    coh_thresh = coh_thresh > 0.85 ? 0.85 : coh_thresh;
    for (jint i = 0; i < num_obj; i++)
    {
        offset = env->GetObjectArrayElement(offset_array, i);
        offset_row = env->GetDoubleField(offset, offset_row_ID);
        offset_col = env->GetDoubleField(offset, offset_col_ID);
        coh = env->GetDoubleField(offset, coh_ID);
        env->DeleteLocalRef(offset);

        offset_r.at<double>(i, 0) = offset_row;
        offset_c.at<double>(i, 0) = offset_col;
        coh_array.at<double>(i, 0) = coh;
        if (coh > coh_thresh) count++;

        offset = env->GetObjectArrayElement(center_coordinate_array, i);
        offset_row = env->GetDoubleField(offset, offset_row_ID);
        offset_col = env->GetDoubleField(offset, offset_col_ID);
        env->DeleteLocalRef(offset);

        offset_coord_row.at<double>(i, 0) = offset_row;
        offset_coord_col.at<double>(i, 0) = offset_col;
    }
    if (count < 7)
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }


    //利用相关系数筛掉配准异常子块的偏移量

    Mat offset_coord_row_sifted, offset_coord_col_sifted, offset_r_sifted, offset_c_sifted;
    offset_coord_row_sifted.create(count, 1, CV_64F);
    offset_coord_col_sifted.create(count, 1, CV_64F);
    offset_r_sifted.create(count, 1, CV_64F);
    offset_c_sifted.create(count, 1, CV_64F);
    count = 0;
    for (int i = 0; i < num_obj; i++)
    {
        if (coh_array.at<double>(i, 0) > coh_thresh)
        {
            offset_coord_row_sifted.at<double>(count, 0) = offset_coord_row.at<double>(i, 0);
            offset_coord_col_sifted.at<double>(count, 0) = offset_coord_col.at<double>(i, 0);
            offset_r_sifted.at<double>(count, 0) = offset_r.at<double>(i, 0);
            offset_c_sifted.at<double>(count, 0) = offset_c.at<double>(i, 0);
            count++;
        }
    }
    offset_coord_row_sifted.copyTo(offset_coord_row);
    offset_coord_col_sifted.copyTo(offset_coord_col);
    offset_r_sifted.copyTo(offset_r);
    offset_c_sifted.copyTo(offset_c);
    num_obj = count;
    Mat A = Mat::ones(num_obj, 6, CV_64F);
    Mat temp, A_t;
    offset_coord_col.copyTo(A(Range(0, num_obj), Range(1, 2)));

    offset_coord_row.copyTo(A(Range(0, num_obj), Range(2, 3)));

    temp = offset_coord_col.mul(offset_coord_row);
    temp.copyTo(A(Range(0, num_obj), Range(3, 4)));

    temp = offset_coord_col.mul(offset_coord_col);
    temp.copyTo(A(Range(0, num_obj), Range(4, 5)));

    temp = offset_coord_row.mul(offset_coord_row);
    temp.copyTo(A(Range(0, num_obj), Range(5, 6)));

    transpose(A, A_t);

    Mat b_r, b_c, coef_r, coef_c, error_r, error_c, b_t, a, a_t;

    A.copyTo(a);
    cv::transpose(a, a_t);
    offset_r.copyTo(b_r);
    b_r = A_t * b_r;

    offset_c.copyTo(b_c);
    b_c = A_t * b_c;

    A = A_t * A;

    double rms1 = -1.0; double rms2 = -1.0;
    Mat eye = Mat::zeros(num_obj, num_obj, CV_64F);
    for (int i = 0; i < num_obj; i++)
    {
        eye.at<double>(i, i) = 1.0;
    }
    if (cv::invert(A, error_r, cv::DECOMP_LU) > 0)
    {
        cv::transpose(offset_r, b_t);
        error_r = b_t * (eye - a * error_r * a_t) * offset_r;
        rms1 = sqrt(error_r.at<double>(0, 0) / double(num_obj));
    }
    if (cv::invert(A, error_c, cv::DECOMP_LU) > 0)
    {
        cv::transpose(offset_c, b_t);
        error_c = b_t * (eye - a * error_c * a_t) * offset_c;
        rms2 = sqrt(error_c.at<double>(0, 0) / double(num_obj));
    }
    if (!cv::solve(A, b_r, coef_r, cv::DECOMP_NORMAL))
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }
    if (!cv::solve(A, b_c, coef_c, cv::DECOMP_NORMAL))
    {
        sprintf(msg, "(line %d in file %s)!\n", __LINE__, __FILE__);
        ThrowException(env, msg);
        return jobject();
    }

    jclass Fit = env->FindClass(Fitclasspath);
    jmethodID init_ID = env->GetMethodID(Fit, "<init>", "()V");
    jobject fit = env->NewObject(Fit, init_ID);
    jfieldID a0 = env->GetFieldID(Fit, "a0", "D");
    jfieldID a1 = env->GetFieldID(Fit, "a1", "D");
    jfieldID a2 = env->GetFieldID(Fit, "a2", "D");
    jfieldID a3 = env->GetFieldID(Fit, "a3", "D");
    jfieldID a4 = env->GetFieldID(Fit, "a4", "D");
    jfieldID a5 = env->GetFieldID(Fit, "a5", "D");
    jfieldID b0 = env->GetFieldID(Fit, "b0", "D");
    jfieldID b1 = env->GetFieldID(Fit, "b1", "D");
    jfieldID b2 = env->GetFieldID(Fit, "b2", "D");
    jfieldID b3 = env->GetFieldID(Fit, "b3", "D");
    jfieldID b4 = env->GetFieldID(Fit, "b4", "D");
    jfieldID b5 = env->GetFieldID(Fit, "b5", "D");

    env->SetDoubleField(fit, a0, coef_r.at<double>(0, 0));
    env->SetDoubleField(fit, a1, coef_r.at<double>(1, 0));
    env->SetDoubleField(fit, a2, coef_r.at<double>(2, 0));
    env->SetDoubleField(fit, a3, coef_r.at<double>(3, 0));
    env->SetDoubleField(fit, a4, coef_r.at<double>(4, 0));
    env->SetDoubleField(fit, a5, coef_r.at<double>(5, 0));
    env->SetDoubleField(fit, b0, coef_c.at<double>(0, 0));
    env->SetDoubleField(fit, b1, coef_c.at<double>(1, 0));
    env->SetDoubleField(fit, b2, coef_c.at<double>(2, 0));
    env->SetDoubleField(fit, b3, coef_c.at<double>(3, 0));
    env->SetDoubleField(fit, b4, coef_c.at<double>(4, 0));
    env->SetDoubleField(fit, b5, coef_c.at<double>(5, 0));

    return fit;
}

/** @brief 根据偏移量拟合系数插值得到配准后的SAR图像

@param lineSize                       每行元素个数
@param real                           实部数据
@param imag                           虚部数据
@param center                         子块中心点坐标
@param fit                            12个偏移量拟合系数
@param overlapSize                    瓦片四周重叠部分长度
@return 配准后的SAR复图像（实部为前一半数据，虚部为后一半数据）
*/
JNIEXPORT jshortArray JNICALL Java_com_htzs_insar_jni_Registration_registration
(JNIEnv* env, jclass job, jint lineSize, jshortArray real, jshortArray imag, jobject center, jobject fit, jint overlapSize)
{
    jint num = env->GetArrayLength(real);
    jint rows = num / lineSize;
    jint cols = lineSize;
    ComplexMat slave(rows, lineSize);
    slave.convertTo(slave, CV_16S);
    jshort* data = env->GetShortArrayElements(real, NULL);
    memcpy(slave.re.data, data, sizeof(short) * num);
    env->ReleaseShortArrayElements(real, data, 0);
    data = env->GetShortArrayElements(imag, NULL);
    memcpy(slave.im.data, data, sizeof(short) * num);
    env->ReleaseShortArrayElements(imag, data, 0);

    jclass Point = env->FindClass(Pointclasspath);
    jfieldID row_ID = env->GetFieldID(Point, "row", "D");
    jfieldID col_ID = env->GetFieldID(Point, "col", "D");

    double center_rows, center_cols;
    center_rows = env->GetDoubleField(center, row_ID);
    center_cols = env->GetDoubleField(center, col_ID);

    int row_start = (int)round(center_rows - (double)rows / 2.0);
    int col_start = (int)round(center_cols - (double)cols / 2.0);

    jclass Fit = env->FindClass(Fitclasspath);
    jfieldID a0 = env->GetFieldID(Fit, "a0", "D");
    jfieldID a1 = env->GetFieldID(Fit, "a1", "D");
    jfieldID a2 = env->GetFieldID(Fit, "a2", "D");
    jfieldID a3 = env->GetFieldID(Fit, "a3", "D");
    jfieldID a4 = env->GetFieldID(Fit, "a4", "D");
    jfieldID a5 = env->GetFieldID(Fit, "a5", "D");
    jfieldID b0 = env->GetFieldID(Fit, "b0", "D");
    jfieldID b1 = env->GetFieldID(Fit, "b1", "D");
    jfieldID b2 = env->GetFieldID(Fit, "b2", "D");
    jfieldID b3 = env->GetFieldID(Fit, "b3", "D");
    jfieldID b4 = env->GetFieldID(Fit, "b4", "D");
    jfieldID b5 = env->GetFieldID(Fit, "b5", "D");

    Mat coef_r(6, 1, CV_64F), coef_c(6, 1, CV_64F);
    coef_r.at<double>(0, 0) = env->GetDoubleField(fit, a0);
    coef_r.at<double>(1, 0) = env->GetDoubleField(fit, a1);
    coef_r.at<double>(2, 0) = env->GetDoubleField(fit, a2);
    coef_r.at<double>(3, 0) = env->GetDoubleField(fit, a3);
    coef_r.at<double>(4, 0) = env->GetDoubleField(fit, a4);
    coef_r.at<double>(5, 0) = env->GetDoubleField(fit, a5);

    coef_c.at<double>(0, 0) = env->GetDoubleField(fit, b0);
    coef_c.at<double>(1, 0) = env->GetDoubleField(fit, b1);
    coef_c.at<double>(2, 0) = env->GetDoubleField(fit, b2);
    coef_c.at<double>(3, 0) = env->GetDoubleField(fit, b3);
    coef_c.at<double>(4, 0) = env->GetDoubleField(fit, b4);
    coef_c.at<double>(5, 0) = env->GetDoubleField(fit, b5);

    ComplexMat slave_tmp;
    slave_tmp = slave;
#pragma omp parallel for schedule(guided)
    for (int i = overlapSize; i < rows - overlapSize; i++)
    {
        double x, y, ii, jj; Mat tmp(1, 6, CV_64F); Mat result;
        int mm, nn, mm1, nn1;
        double offset_rows, offset_cols, upper, lower;
        for (int j = overlapSize; j < cols - overlapSize; j++)
        {
            x = (double)j + (double)col_start;
            y = (double)i + (double)row_start;
            ii = (double)i; jj = (double)j;
            tmp.at<double>(0, 0) = 1.0;
            tmp.at<double>(0, 1) = x;
            tmp.at<double>(0, 2) = y;
            tmp.at<double>(0, 3) = x * y;
            tmp.at<double>(0, 4) = x * x;
            tmp.at<double>(0, 5) = y * y;
            result = tmp * coef_r;
            offset_rows = result.at<double>(0, 0);
            result = tmp * coef_c;
            offset_cols = result.at<double>(0, 0);

            ii += offset_rows;
            jj += offset_cols;

            mm = (int)floor(ii); nn = (int)floor(jj);
            if (mm < 0 || nn < 0 || mm > rows - 1 || nn > cols - 1)
            {
                slave_tmp.re.at<short>(i, j) = 0.0;
                slave_tmp.im.at<short>(i, j) = 0.0;
            }
            else
            {
                mm1 = mm + 1; nn1 = nn + 1;
                mm1 = mm1 >= rows - 1 ? rows - 1 : mm1;
                nn1 = nn1 >= cols - 1 ? cols - 1 : nn1;
                //实部插值
                upper = slave.re.at<short>(mm, nn) + double(slave.re.at<short>(mm, nn1) - slave.re.at<short>(mm, nn)) * (jj - (double)nn);
                lower = slave.re.at<short>(mm1, nn) + double(slave.re.at<short>(mm1, nn1) - slave.re.at<short>(mm1, nn)) * (jj - (double)nn);
                slave_tmp.re.at<short>(i, j) = upper + (lower - upper) * (ii - (double)mm);
                //虚部插值
                upper = slave.im.at<short>(mm, nn) + double(slave.im.at<short>(mm, nn1) - slave.im.at<short>(mm, nn)) * (jj - (double)nn);
                lower = slave.im.at<short>(mm1, nn) + double(slave.im.at<short>(mm1, nn1) - slave.im.at<short>(mm1, nn)) * (jj - (double)nn);
                slave_tmp.im.at<short>(i, j) = upper + (lower - upper) * (ii - (double)mm);
            }

        }
    }
    slave = slave_tmp(Range(overlapSize, rows - overlapSize), Range(overlapSize, cols - overlapSize));
    int size = slave.GetCols() * slave.GetRows();
    jshortArray arr = env->NewShortArray(size * 2);
    env->SetShortArrayRegion(arr, 0, size, (short*)slave.re.data);
    env->SetShortArrayRegion(arr, size, size, (short*)slave.im.data);
    return arr;
}

5.2：ComplexMat.cpp

#include"ComplexMat.h"

using InSAR::ComplexMat;
ComplexMat::ComplexMat()
{
    Mat tmp = Mat::zeros(1, 1, CV_64F);
    tmp.copyTo(this->re);
    tmp.copyTo(this->im);
}

ComplexMat::ComplexMat(Mat& re, Mat& im)
{
    re.copyTo(this->re);
    im.copyTo(this->im);
}

ComplexMat::ComplexMat(int rows, int cols)
{
    if (rows > 0 && cols > 0)
    {
        Mat tmp = Mat::zeros(rows, cols, CV_64F);
        tmp.copyTo(this->im);
        tmp.copyTo(this->re);
    }
}

ComplexMat::ComplexMat(const ComplexMat& b)
{
    b.re.copyTo(this->re);
    b.im.copyTo(this->im);
}

ComplexMat::~ComplexMat()
{

}

int ComplexMat::type() const
{
    if (re.type() != im.type()) return -1;
    return this->re.type();
}

int ComplexMat::Mul(const ComplexMat& Src, ComplexMat& Dst, bool bConj) const
{
    ComplexMat result;
    if (this->GetRows() < 1 ||
        this->GetCols() < 1 ||
        Src.GetRows() < 1 ||
        Src.GetCols() < 1 ||
        this->GetCols() != Src.GetCols() ||
        this->GetRows() != Src.GetRows() ||
        this->type() != Src.type())
    {
        fprintf(stderr, "ComplexMat::Mul(): input check failed!\n\n");
        return -1;
    }

    if (bConj)
    {
        result.re = this->re.mul(Src.re) + this->im.mul(Src.im);
        result.im = Src.re.mul(this->im) - this->re.mul(Src.im);
    }
    else
    {
        result.re = this->re.mul(Src.re) - this->im.mul(Src.im);
        result.im = Src.re.mul(this->im) + this->re.mul(Src.im);
    }
    Dst.SetRe(result.re);
    Dst.SetIm(result.im);
    return 0;
}

ComplexMat ComplexMat::operator*(const ComplexMat& b) const
{
    ComplexMat result;
    if (this->GetRows() < 1 ||
        this->GetCols() < 1 ||
        b.GetRows() < 1 ||
        b.GetCols() < 1 ||
        (this->GetCols() != b.GetCols()) && b.GetCols() != 1 ||
        (this->GetRows() != b.GetRows()) && b.GetRows() != 1 ||
        this->type() != b.type())
    {
        fprintf(stderr, "ComplexMat::Mul(): input check failed!\n\n");
        return ComplexMat();
    }
    if (b.GetCols() == 1 && b.GetRows() == 1)
    {
        result.re = this->re * b.re.at<double>(0, 0) - this->im * b.im.at<double>(0, 0);
        result.im = this->im * b.re.at<double>(0, 0) + this->re * b.im.at<double>(0, 0);
    }
    else
    {
        result.re = this->re.mul(b.re) - this->im.mul(b.im);
        result.im = b.re.mul(this->im) + this->re.mul(b.im);
    }
    return result;
}

ComplexMat ComplexMat::operator*(const Mat& a) const
{
    if (a.cols != this->GetCols() ||
        a.rows != this->GetRows() ||
        a.type() != this->type() ||
        a.channels() != 1)
    {
        fprintf(stderr, "ComplexMat::operator*(const Mat& a): input check failed!\n\n");
        return ComplexMat();
    }
    Mat out_re, out_im;
    ComplexMat out;
    out_re = this->re.mul(a);
    out_im = this->im.mul(a);
    out.SetRe(out_re);
    out.SetIm(out_im);
    return out;
}

ComplexMat ComplexMat::operator*(const double& a) const
{
    Mat out_re, out_im;
    ComplexMat out;
    out_re = this->re * a;
    out_im = this->im * a;
    out.SetRe(out_re);
    out.SetIm(out_im);
    return out;
}

ComplexMat ComplexMat::operator()(cv::Range _rowRange, cv::Range _colRange) const
{
    if (_rowRange.start < 0 ||
        _rowRange.end > this->GetRows() ||
        _colRange.start < 0 ||
        _colRange.end > this->GetCols())
    {
        fprintf(stderr, "ComplexMat::operator()(cv::Range _rowRange, cv::Range _colRange): \n Range exceeds legal value!\n\n");
        return ComplexMat();
    }
    ComplexMat out;
    this->re(cv::Range(_rowRange.start, _rowRange.end), cv::Range(_colRange.start, _colRange.end)).copyTo(out.re);
    this->im(cv::Range(_rowRange.start, _rowRange.end), cv::Range(_colRange.start, _colRange.end)).copyTo(out.im);
    return out;
}

int ComplexMat::SetValue(cv::Range _rowRange, cv::Range _colRange, ComplexMat& src)
{
    if ((_rowRange.end - _rowRange.start) != src.GetRows() ||
        (_colRange.end - _colRange.start) != src.GetCols() ||
        _rowRange.start < 0 ||
        _rowRange.end > this->GetRows() ||
        _colRange.start < 0 ||
        _colRange.end > this->GetCols() ||
        src.type() != this->type() ||
        (src.type() != CV_64F && src.type() != CV_16S)
        )
    {
        fprintf(stderr, "ComplexMat::SetValue(): input check failed!\n\n");
        return -1;
    }
    if (src.type() == CV_64F)
    {
        for (int i = _rowRange.start; i < _rowRange.end; i++)
        {
            for (int j = _colRange.start; j < _colRange.end; j++)
            {

                this->re.at<double>(i, j) = src.re.at<double>(i - _rowRange.start, j - _colRange.start);
                this->im.at<double>(i, j) = src.im.at<double>(i - _rowRange.start, j - _colRange.start);
            }
        }
    }
    if (src.type() == CV_16S)
    {
        for (int i = _rowRange.start; i < _rowRange.end; i++)
        {
            for (int j = _colRange.start; j < _colRange.end; j++)
            {

                this->re.at<short>(i, j) = src.re.at<short>(i - _rowRange.start, j - _colRange.start);
                this->im.at<short>(i, j) = src.im.at<short>(i - _rowRange.start, j - _colRange.start);
            }
        }
    }

    return 0;
}

Mat ComplexMat::GetIm() const
{
    return this->im;
}

Mat ComplexMat::GetMod() const
{
    Mat tmp;
    magnitude(this->re, this->im, tmp);
    return tmp;
}

Mat ComplexMat::GetPhase()
{
    int nr = this->GetRows();
    int nc = this->GetCols();
    if (nr < 1 || nc < 1)
    {
        return Mat::zeros(1, 1, CV_64F);
    }
    Mat phase(nr, nc, CV_64F);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        for (int j = 0; j < nc; j++)
        {
            phase.at <double>(i, j) = atan2(this->im.at<double>(i, j), this->re.at<double>(i, j));
        }
    }
    return phase;
}

Mat ComplexMat::GetRe() const
{
    return this->re;
}

void ComplexMat::SetIm(Mat& im)
{
    im.copyTo(this->im);

}

void ComplexMat::SetRe(Mat& re)
{
    re.copyTo(this->re);
}

int ComplexMat::GetCols() const
{
    if (re.cols != im.cols) return -1;
    return this->re.cols;
}

int ComplexMat::GetRows() const
{
    if (re.rows != im.rows) return -1;
    return this->re.rows;
}

ComplexMat ComplexMat::operator+(const ComplexMat& b) const
{
    if (this->GetCols() != b.GetCols() ||
        b.GetRows() != b.GetRows() ||
        this->type() != b.type() ||
        this->GetCols() < 1 ||
        this->GetRows() < 1
        )
    {
        fprintf(stderr, "ComplexMat operator+: input check failed!\n\n");
        return *this;
    }
    ComplexMat out;
    Mat out_re, out_im;
    out_re = this->re + b.re;
    out_im = this->im + b.im;
    out.SetIm(out_im);
    out.SetRe(out_re);
    return out;
}

ComplexMat ComplexMat::operator=(const ComplexMat& b)
{
    b.re.copyTo(this->re);
    b.im.copyTo(this->im);
    return *this;
}

ComplexMat ComplexMat::sum(int dim) const
{
    ComplexMat out;
    Mat re, im;
    int nr = this->GetRows();
    int nc = this->GetCols();
    if (nr < 0 || nc < 0)
    {
        fprintf(stderr, "ComplexMat::sum(): rows or cols < 0!\n\n");
        return ComplexMat();
    }
    if (dim == 1)
    {
        re = Mat::zeros(nr, 1, CV_64F);
        im = Mat::zeros(nr, 1, CV_64F);
#pragma omp parallel for schedule(guided)
        for (int i = 0; i < nr; i++)
        {
            double tmp_re, tmp_im;
            tmp_re = 0.0; tmp_im = 0.0;
            for (int j = 0; j < nc; j++)
            {
                tmp_re += this->re.at<double>(i, j);
                tmp_im += this->im.at<double>(i, j);
            }
            re.at<double>(i, 0) = tmp_re;
            im.at<double>(i, 0) = tmp_im;
        }
    }
    else
    {
        re = Mat::zeros(1, nc, CV_64F);
        im = Mat::zeros(1, nc, CV_64F);

#pragma omp parallel for schedule(guided)
        for (int j = 0; j < nc; j++)
        {
            double tmp_re, tmp_im;
            tmp_re = 0.0; tmp_im = 0.0;
            for (int i = 0; i < nr; i++)
            {
                tmp_re += this->re.at<double>(i, j);
                tmp_im += this->im.at<double>(i, j);
            }
            re.at<double>(0, j) = tmp_re;
            im.at<double>(0, j) = tmp_im;
        }
    }
    out.SetRe(re);
    out.SetIm(im);
    return out;
}

ComplexMat ComplexMat::conj() const
{
    ComplexMat out;
    Mat im, re;
    this->re.copyTo(re);
    this->im.copyTo(im);
    im = -im;
    out.SetRe(re);
    out.SetIm(im);
    return out;
}

int ComplexMat::countNonzero() const
{
    if (this->re.rows == 0 ||
        this->im.rows == 0 ||
        this->re.cols == 0 ||
        this->im.cols == 0
        )
    {
        return 0;
    }
    int count = 0;
    int nr = GetRows();
    int nc = GetCols();
    for (int i = 0; i < nr; i++)
    {
        for (int j = 0; j < nc; j++)
        {
            if (fabs(this->re.at<double>(i, j)) > DBL_EPSILON || fabs(this->im.at<double>(i, j)) > DBL_EPSILON)
            {
                count++;
            }
        }
    }
    return count;
}

bool ComplexMat::isempty() const
{
    if (this->GetRows() < 1 || this->GetCols() < 1) return true;
    return false;
}

void ComplexMat::convertTo(ComplexMat& out, int type) const
{
    if (this->type() == type)
    {
        out = *this;
    }
    else
    {
        this->re.convertTo(out.re, type);
        this->im.convertTo(out.im, type);
    }
}

5.3：Registration.cpp

#include<string.h>
#include<math.h>
#include"Registration.h"

using namespace cv;
using namespace InSAR;
inline bool return_check(int ret, const char* detail_info, const char* error_head)
{
    if (ret < 0)
    {
        fprintf(stderr, "%s %s\n\n", error_head, detail_info);
        return true;
    }
    else
    {
        return false;
    }
}

inline bool parallel_check(volatile bool parallel_flag, const char* detail_info, const char* parallel_error_head)
{
    if (!parallel_flag)
    {
        fprintf(stderr, "%s %s\n\n", parallel_error_head, detail_info);
        return true;
    }
    else
    {
        return false;
    }
}

inline bool parallel_flag_change(volatile bool parallel_flag, int ret)
{
    if (ret < 0)
    {
        parallel_flag = false;
        return true;
    }
    else
    {
        return false;
    }
}

Registration::Registration()
{
    memset(this->error_head, 0, 256);
    memset(this->parallel_error_head, 0, 256);
    strcpy(this->error_head, "REGISTRATION_DLL_ERROR: error happens when using ");
    strcpy(this->parallel_error_head, "REGISTRATION_DLL_ERROR: error happens when using parallel computing in function: ");
}

Registration::~Registration()
{
}
int Registration::fft2(Mat& Src, Mat& Dst)
{
    if (Src.rows < 1 ||
        Src.cols < 1 ||
        Src.channels() != 1 ||
        Src.type() != CV_64F)
    {
        fprintf(stderr, "fft2(): input check failed!\n\n");
        return -1;
    }
    Mat planes[] = { Mat_<double>(Src), Mat::zeros(Src.size(), CV_64F) };
    Mat complexImg;
    merge(planes, 2, complexImg);
    dft(complexImg, Dst);
    return 0;
}

int Registration::fftshift2(Mat& matrix)
{
    if (matrix.rows < 2 ||
        matrix.cols < 2 ||
        matrix.channels() != 1)
    {
        fprintf(stderr, "fftshift2(): input check failed!\n\n");
        return -1;
    }
    matrix = matrix(Rect(0, 0, matrix.cols & -2, matrix.rows & -2));
    int cx = matrix.cols / 2;
    int cy = matrix.rows / 2;
    Mat tmp;
    Mat q0(matrix, Rect(0, 0, cx, cy));
    Mat q1(matrix, Rect(cx, 0, cx, cy));
    Mat q2(matrix, Rect(0, cy, cx, cy));
    Mat q3(matrix, Rect(cx, cy, cx, cy));

    q0.copyTo(tmp);
    q3.copyTo(q0);
    tmp.copyTo(q3);

    q1.copyTo(tmp);
    q2.copyTo(q1);
    tmp.copyTo(q2);
    return 0;
}

int Registration::real_coherent(ComplexMat& Master, ComplexMat& Slave, int* offset_row, int* offset_col)
{
    if (Master.GetRows() < 1 ||
        Master.GetCols() < 1 ||
        Master.GetRows() != Slave.GetRows() ||
        Master.GetCols() != Slave.GetCols())
    {
        fprintf(stderr, "real_coherent(): input check failed!\n\n");
        return -1;
    }
    int ret;
    Mat img1;
    Mat img2;
    img1 = Master.GetMod();
    img2 = Slave.GetMod();
    Mat im1fft;
    Mat im2fft;
    ret = fft2(img1, im1fft);
    if (return_check(ret, "fft2(*, *)", error_head)) return -1;
    ret = fft2(img2, im2fft);
    if (return_check(ret, "fft2(*, *)", error_head)) return -1;
    Mat spectrum;
    mulSpectrums(im1fft, im2fft, spectrum, 0, true);

    Mat result;
    idft(spectrum, result, DFT_REAL_OUTPUT);//需要显示图像时可以用DFT_SCALE

    ret = fftshift2(result);
    if (return_check(ret, "fftshift2(*)", error_head)) return -1;
    normalize(result, result, 0, 1, NORM_MINMAX);

    int r = result.rows / 2;
    int c = result.cols / 2;
    Point peak_loc;
    minMaxLoc(result, NULL, NULL, NULL, &peak_loc);

    *offset_row = r - peak_loc.y;
    *offset_col = c - peak_loc.x;
    return 0;
}

int Registration::registration_pixel(ComplexMat& Master, ComplexMat& Slave, int* move_r, int* move_c)
{
    if (Master.GetRows() < 1 ||
        Master.GetCols() < 1 ||
        Master.GetRows() != Slave.GetRows() ||
        Master.GetCols() != Slave.GetCols())
    {
        fprintf(stderr, "registration_pixel(): input check failed!\n\n");
        return -1;
    }
    int offset_rows, offset_cols, ret;
    offset_cols = 0;
    offset_rows = 0;
    ret = real_coherent(Master, Slave, &offset_rows, &offset_cols);//相关函数求取偏移量
    if (move_r)*move_r = offset_rows;
    if (move_c)*move_c = offset_cols;
    if (return_check(ret, "real_coherent(*, *, *, *)", error_head)) return -1;
    ////搬移与裁剪

    //行偏移（竖直移动）
    ComplexMat image_master_mid;
    ComplexMat image_slave_mid;
    int nr = Slave.GetRows();
    int nc = Slave.GetCols();
    //////////////////////////////检查粗配准偏移量是否超过图像大小////////////////////////////////////
    if ((offset_rows > 0 ? offset_rows : -offset_rows) >= nr ||
        (offset_cols > 0 ? offset_cols : -offset_cols) >= nc)
    {
        fprintf(stderr, "registration_pixel(): registration offset exceed size of images!\n\n");
        return -1;
    }
    if (offset_rows >= 0)
    {
        //实部
        Slave.re(Range(offset_rows, nr), Range(0, nc)).copyTo(image_slave_mid.re);//辅图像向上搬移

        Master.re(Range(0, nr - offset_rows), Range(0, nc)).copyTo(image_master_mid.re);//裁剪主图像


        //虚部
        Slave.im(Range(offset_rows, nr), Range(0, nc)).copyTo(image_slave_mid.im);//辅图像向上搬移

        Master.im(Range(0, nr - offset_rows), Range(0, nc)).copyTo(image_master_mid.im);//裁剪

    }

    else
    {
        //实部
        Slave.re(Range(0, nr + offset_rows), Range(0, nc)).copyTo(image_slave_mid.re);//辅图像向下搬移

        Master.re(Range(-offset_rows, nr), Range(0, nc)).copyTo(image_master_mid.re);//裁剪


        //虚部
        Slave.im(Range(0, nr + offset_rows), Range(0, nc)).copyTo(image_slave_mid.im);//辅图像向下搬移

        Master.im(Range(-offset_rows, nr), Range(0, nc)).copyTo(image_master_mid.im);//裁剪

    }

    ComplexMat image_master_regis;
    ComplexMat image_slave_regis;
    int nr1 = image_master_mid.re.rows;
    int nc1 = image_master_mid.re.cols;
    //列偏移（水平移动）
    if (offset_cols >= 0)//辅图像向左搬移
    {
        //实部
        image_slave_mid.re(Range(0, nr1), Range(offset_cols, nc1)).copyTo(image_slave_regis.re);

        image_master_mid.re(Range(0, nr1), Range(0, nc1 - offset_cols)).copyTo(image_master_regis.re);

        //虚部
        image_slave_mid.im(Range(0, nr1), Range(offset_cols, nc1)).copyTo(image_slave_regis.im);

        image_master_mid.im(Range(0, nr1), Range(0, nc1 - offset_cols)).copyTo(image_master_regis.im);

    }

    else//辅图像向右搬移
    {
        //实部
        image_slave_mid.re(Range(0, nr1), Range(0, nc1 + offset_cols)).copyTo(image_slave_regis.re);

        image_master_mid.re(Range(0, nr1), Range(-offset_cols, nc1)).copyTo(image_master_regis.re);

        //虚部
        image_slave_mid.im(Range(0, nr1), Range(0, nc1 + offset_cols)).copyTo(image_slave_regis.im);

        image_master_mid.im(Range(0, nr1), Range(-offset_cols, nc1)).copyTo(image_master_regis.im);
    }

    Master.re = image_master_regis.re;

    Master.im = image_master_regis.im;

    Slave.re = image_slave_regis.re;

    Slave.im = image_slave_regis.im;
    return 0;
}

int Registration::interp_paddingzero(ComplexMat& InputMatrix, ComplexMat& OutputMatrix, int interp_times)
{
    if (InputMatrix.GetRows() < 2 ||
        InputMatrix.GetCols() < 2 ||
        interp_times < 2)
    {
        fprintf(stderr, "interp_paddingzero(): input check failed!\n\n");
        return -1;
    }

    int nr = InputMatrix.GetRows();
    int nc = InputMatrix.GetCols();

    OutputMatrix.re = Mat::zeros(interp_times * nr, interp_times * nc, CV_64F);

    OutputMatrix.im = Mat::zeros(interp_times * nr, interp_times * nc, CV_64F);
    Mat re, im;
    InputMatrix.re.copyTo(re);
    InputMatrix.im.copyTo(im);
    Mat planes[] = { Mat_<double>(re), Mat_<double>(im) };

    Mat complexImg;

    merge(planes, 2, complexImg);

    dft(complexImg, complexImg, DFT_COMPLEX_OUTPUT);

    split(complexImg, planes);

    planes[0](Range(0, nr / 2), Range(0, nc / 2)).copyTo(OutputMatrix.re(Range(0, nr / 2), Range(0, nc / 2)));
    planes[1](Range(0, nr / 2), Range(0, nc / 2)).copyTo(OutputMatrix.im(Range(0, nr / 2), Range(0, nc / 2)));

    planes[0](Range(nr / 2, nr), Range(0, nc / 2)).copyTo(OutputMatrix.re(Range(nr * interp_times - nr / 2, nr * interp_times), Range(0, nc / 2)));
    planes[1](Range(nr / 2, nr), Range(0, nc / 2)).copyTo(OutputMatrix.im(Range(nr * interp_times - nr / 2, nr * interp_times), Range(0, nc / 2)));

    planes[0](Range(0, nr / 2), Range(nc / 2, nc)).copyTo(OutputMatrix.re(Range(0, nr / 2), Range(nc * interp_times - nc / 2, nc * interp_times)));
    planes[1](Range(0, nr / 2), Range(nc / 2, nc)).copyTo(OutputMatrix.im(Range(0, nr / 2), Range(nc * interp_times - nc / 2, nc * interp_times)));

    planes[0](Range(nr / 2, nr), Range(nc / 2, nc)).copyTo(OutputMatrix.re(Range(nr * interp_times - nr / 2, nr * interp_times), Range(nc * interp_times - nc / 2, nc * interp_times)));
    planes[1](Range(nr / 2, nr), Range(nc / 2, nc)).copyTo(OutputMatrix.im(Range(nr * interp_times - nr / 2, nr * interp_times), Range(nc * interp_times - nc / 2, nc * interp_times)));

    Mat planes1[] = { Mat_<double>(OutputMatrix.re), Mat_<double>(OutputMatrix.im) };

    merge(planes1, 2, complexImg);

    idft(complexImg, complexImg);

    split(complexImg, planes1);

    OutputMatrix.re = planes1[0];
    OutputMatrix.im = planes1[1];
    return 0;
}

int Registration::interp_cubic(ComplexMat& InputMatrix, ComplexMat& OutputMatrix, double offset_row, double offset_col)
{
    int nr = InputMatrix.GetRows();
    int nc = InputMatrix.GetCols();//输入矩阵尺寸
    if (nr < 2 || nc < 2 || InputMatrix.re.type() != CV_64F)
    {
        fprintf(stderr, "interp_cubic(): input check failed!\n\n");
        return -1;
    }
    ComplexMat new_image_slave;
    new_image_slave.re = Mat::zeros(nr + 3, nc + 3, CV_64F);
    new_image_slave.im = Mat::zeros(nr + 3, nc + 3, CV_64F);

    //扩充矩阵(扩展三行三列)

    //实部

    InputMatrix.re(Range(0, 1), Range(0, 1)).copyTo(new_image_slave.re(Range(0, 1), Range(0, 1)));

    InputMatrix.re(Range(0, 1), Range(0, nc)).copyTo(new_image_slave.re(Range(0, 1), Range(1, nc + 1)));

    InputMatrix.re(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(0, 1), Range(nc + 1, nc + 2)));

    InputMatrix.re(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(0, 1), Range(nc + 2, nc + 3)));

    InputMatrix.re(Range(0, nr), Range(0, 1)).copyTo(new_image_slave.re(Range(1, nr + 1), Range(0, 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.re(Range(nr + 1, nr + 2), Range(0, 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(0, 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(0, nc)).copyTo(new_image_slave.re(Range(nr + 1, nr + 2), Range(1, nc + 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(nr + 1, nr + 2), Range(nc + 2, nc + 3)));

    new_image_slave.re(Range(nr + 1, nr + 2), Range(0, nc + 3)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(0, nc + 3)));

    InputMatrix.re(Range(0, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(1, nr + 1), Range(nc + 1, nc + 2)));

    InputMatrix.re(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(nc + 1, nc + 2)));

    InputMatrix.re(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(nc + 2, nc + 3)));

    new_image_slave.re(Range(0, nr + 3), Range(nc + 1, nc + 2)).copyTo(new_image_slave.re(Range(0, nr + 3), Range(nc + 2, nc + 3)));



    InputMatrix.re(Range(0, nr), Range(0, nc)).copyTo(new_image_slave.re(Range(1, nr + 1), Range(1, nc + 1)));


    //虚部

    InputMatrix.im(Range(0, 1), Range(0, 1)).copyTo(new_image_slave.im(Range(0, 1), Range(0, 1)));

    InputMatrix.im(Range(0, 1), Range(0, nc)).copyTo(new_image_slave.im(Range(0, 1), Range(1, nc + 1)));

    InputMatrix.im(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(0, 1), Range(nc + 1, nc + 2)));

    InputMatrix.im(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(0, 1), Range(nc + 2, nc + 3)));

    InputMatrix.im(Range(0, nr), Range(0, 1)).copyTo(new_image_slave.im(Range(1, nr + 1), Range(0, 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.im(Range(nr + 1, nr + 2), Range(0, 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(0, 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(0, nc)).copyTo(new_image_slave.im(Range(nr + 1, nr + 2), Range(1, nc + 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(nr + 1, nr + 2), Range(nc + 2, nc + 3)));

    new_image_slave.im(Range(nr + 1, nr + 2), Range(0, nc + 3)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(0, nc + 3)));

    InputMatrix.im(Range(0, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(1, nr + 1), Range(nc + 1, nc + 2)));

    InputMatrix.im(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(nc + 1, nc + 2)));

    InputMatrix.im(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(nc + 2, nc + 3)));

    new_image_slave.im(Range(0, nr + 3), Range(nc + 1, nc + 2)).copyTo(new_image_slave.im(Range(0, nr + 3), Range(nc + 2, nc + 3)));

    //内点
    InputMatrix.im(Range(0, nr), Range(0, nc)).copyTo(new_image_slave.im(Range(1, nr + 1), Range(1, nc + 1)));


    //行权
    double row_weight[4];
    row_weight[0] = WeightCalculation(1.0 + offset_row);
    row_weight[1] = WeightCalculation(offset_row);
    row_weight[2] = WeightCalculation(1.0 - offset_row);
    row_weight[3] = WeightCalculation(2.0 - offset_row);

    Mat Row_weight(4, 1, CV_64F, row_weight);

    //列权
    double col_weight[4];
    col_weight[0] = WeightCalculation(1.0 + offset_col);
    col_weight[1] = WeightCalculation(offset_col);
    col_weight[2] = WeightCalculation(1.0 - offset_col);
    col_weight[3] = WeightCalculation(2.0 - offset_col);

    Mat Col_weight(1, 4, CV_64F, col_weight);


    //权矩阵
    Mat Weight = Row_weight * Col_weight;

    //实部虚部分别插值
    Mat image_slave_regis_re = Mat::zeros(nr, nc, CV_64F);
    Mat image_slave_regis_im = Mat::zeros(nr, nc, CV_64F);

    double temp;
#pragma omp parallel for schedule(guided) \
    private(temp)
    for (int i = 0; i <= nr - 1; i++)
    {
        for (int j = 0; j <= nc - 1; j++)
        {
            temp = new_image_slave.re(Range(i, i + 4), Range(j, j + 4)).dot(Weight);

            image_slave_regis_re.at<double>(i, j) = temp;


            temp = new_image_slave.im(Range(i, i + 4), Range(j, j + 4)).dot(Weight);

            image_slave_regis_im.at<double>(i, j) = temp;
        }

    }
    image_slave_regis_re.copyTo(OutputMatrix.re);
    image_slave_regis_im.copyTo(OutputMatrix.im);
    return 0;
}

int Registration::interp_cubic(ComplexMat& InputMatrix, ComplexMat& OutputMatrix, Mat& Coefficient)
{
    int nr = InputMatrix.GetRows();
    int nc = InputMatrix.GetCols();//输入矩阵尺寸
    if (nr < 2 || nc < 2 || InputMatrix.re.type() != CV_64F)
    {
        fprintf(stderr, "interp_cubic(): input check failed!\n\n");
        return -1;
    }

    ComplexMat new_image_slave;
    new_image_slave.re = Mat::zeros(nr + 3, nc + 3, CV_64F);
    new_image_slave.im = Mat::zeros(nr + 3, nc + 3, CV_64F);

    //扩充矩阵(扩展三行三列)

    //实部

    InputMatrix.re(Range(0, 1), Range(0, 1)).copyTo(new_image_slave.re(Range(0, 1), Range(0, 1)));

    InputMatrix.re(Range(0, 1), Range(0, nc)).copyTo(new_image_slave.re(Range(0, 1), Range(1, nc + 1)));

    InputMatrix.re(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(0, 1), Range(nc + 1, nc + 2)));

    InputMatrix.re(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(0, 1), Range(nc + 2, nc + 3)));

    InputMatrix.re(Range(0, nr), Range(0, 1)).copyTo(new_image_slave.re(Range(1, nr + 1), Range(0, 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.re(Range(nr + 1, nr + 2), Range(0, 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(0, 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(0, nc)).copyTo(new_image_slave.re(Range(nr + 1, nr + 2), Range(1, nc + 1)));

    InputMatrix.re(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(nr + 1, nr + 2), Range(nc + 2, nc + 3)));

    new_image_slave.re(Range(nr + 1, nr + 2), Range(0, nc + 3)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(0, nc + 3)));

    InputMatrix.re(Range(0, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(1, nr + 1), Range(nc + 1, nc + 2)));

    InputMatrix.re(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(nc + 1, nc + 2)));

    InputMatrix.re(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.re(Range(nr + 2, nr + 3), Range(nc + 2, nc + 3)));

    new_image_slave.re(Range(0, nr + 3), Range(nc + 1, nc + 2)).copyTo(new_image_slave.re(Range(0, nr + 3), Range(nc + 2, nc + 3)));



    InputMatrix.re(Range(0, nr), Range(0, nc)).copyTo(new_image_slave.re(Range(1, nr + 1), Range(1, nc + 1)));


    //虚部

    InputMatrix.im(Range(0, 1), Range(0, 1)).copyTo(new_image_slave.im(Range(0, 1), Range(0, 1)));

    InputMatrix.im(Range(0, 1), Range(0, nc)).copyTo(new_image_slave.im(Range(0, 1), Range(1, nc + 1)));

    InputMatrix.im(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(0, 1), Range(nc + 1, nc + 2)));

    InputMatrix.im(Range(0, 1), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(0, 1), Range(nc + 2, nc + 3)));

    InputMatrix.im(Range(0, nr), Range(0, 1)).copyTo(new_image_slave.im(Range(1, nr + 1), Range(0, 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.im(Range(nr + 1, nr + 2), Range(0, 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(0, 1)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(0, 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(0, nc)).copyTo(new_image_slave.im(Range(nr + 1, nr + 2), Range(1, nc + 1)));

    InputMatrix.im(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(nr + 1, nr + 2), Range(nc + 2, nc + 3)));

    new_image_slave.im(Range(nr + 1, nr + 2), Range(0, nc + 3)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(0, nc + 3)));

    InputMatrix.im(Range(0, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(1, nr + 1), Range(nc + 1, nc + 2)));

    InputMatrix.im(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(nc + 1, nc + 2)));

    InputMatrix.im(Range(nr - 1, nr), Range(nc - 1, nc)).copyTo(new_image_slave.im(Range(nr + 2, nr + 3), Range(nc + 2, nc + 3)));

    new_image_slave.im(Range(0, nr + 3), Range(nc + 1, nc + 2)).copyTo(new_image_slave.im(Range(0, nr + 3), Range(nc + 2, nc + 3)));

    //内点
    InputMatrix.im(Range(0, nr), Range(0, nc)).copyTo(new_image_slave.im(Range(1, nr + 1), Range(1, nc + 1)));





    Mat image_slave_regis_re = Mat::zeros(nr, nc, CV_64F);
    Mat image_slave_regis_im = Mat::zeros(nr, nc, CV_64F);


    int ret;
    volatile bool parallel_flag = true;
#pragma omp parallel for schedule(guided) \
    private(ret)
    for (int i = 0; i <= nr - 1; i++)
    {
        if (!parallel_flag) continue;

        for (int j = 0; j <= nc - 1; j++)
        {
            if (!parallel_flag) continue;
            double temp, offset_row, offset_col;
            Mat Row_weight = Mat::zeros(4, 1, CV_64F);
            Mat Col_weight = Mat::zeros(1, 4, CV_64F);
            offset_row = 0;
            offset_col = 0;
            ret = every_subpixel_move(i + 1, j + 1, Coefficient, &offset_row, &offset_col);
            if (ret < 0)
            {
                parallel_flag = false;
                continue;
            }
            Row_weight.at<double>(0, 0) = WeightCalculation(offset_row + 1.0);
            Row_weight.at<double>(1, 0) = WeightCalculation(offset_row);
            Row_weight.at<double>(2, 0) = WeightCalculation(1.0 - offset_row);
            Row_weight.at<double>(3, 0) = WeightCalculation(2.0 - offset_row);

            Col_weight.at<double>(0, 0) = WeightCalculation(1.0 + offset_col);
            Col_weight.at<double>(0, 1) = WeightCalculation(offset_col);
            Col_weight.at<double>(0, 2) = WeightCalculation(1.0 - offset_col);
            Col_weight.at<double>(0, 3) = WeightCalculation(2.0 - offset_col);
            temp = new_image_slave.re(Range(i, i + 4), Range(j, j + 4)).dot(Row_weight * Col_weight);

            image_slave_regis_re.at<double>(i, j) = temp;


            temp = new_image_slave.im(Range(i, i + 4), Range(j, j + 4)).dot(Row_weight * Col_weight);

            image_slave_regis_im.at<double>(i, j) = temp;
        }

    }
    if (parallel_check(parallel_flag, "interp_cubic()", parallel_error_head)) return -1;
    image_slave_regis_re.copyTo(OutputMatrix.re);
    image_slave_regis_im.copyTo(OutputMatrix.im);
    return 0;
}

double Registration::WeightCalculation(double offset)
{
    double weight = 0;
    if (offset > 0)
    {
        offset = offset;
    }
    else
    {
        offset = -offset;
    }

    if (offset < 1.0)
    {
        weight = 1.0 - 2.0 * offset * offset + offset * offset * offset;
    }
    else if (offset >= 1.0 && offset <= 2.0)
    {
        weight = 4.0 - 8.0 * offset + 5.0 * offset * offset - offset * offset * offset;
    }
    else
    {
        weight = 0.0;
    }

    return weight;
}

int Registration::registration_subpixel(ComplexMat& Master, ComplexMat& Slave, int blocksize, int interp_times)
{
    if (Master.GetRows() < 1 ||
        Master.GetCols() < 1 ||
        Master.GetRows() != Slave.GetRows() ||
        Master.GetCols() != Slave.GetCols() ||
        blocksize < 1 ||
        interp_times < 1)
    {
        fprintf(stderr, "%s\n\n", "registration_subpixel(): input check failed!\n\n");
        return -1;
    }

    int nsubr = Master.GetRows() / blocksize; //子块行数
    int nsubc = Master.GetCols() / blocksize; //子块列数
    int nsub = nsubr * nsubc;  //子块总数
    if (nsubc < 1 || nsubr < 1)
    {
        fprintf(stderr, "%s\n\n", "registration_subpixel: subblockszie, nsubc < 1 || nsubr < 1");
        return -1;
    }
    Mat sub_r_offset = Mat::zeros(nsub, 1, CV_64F); //行亚像素偏移量
    Mat sub_c_offset = Mat::zeros(nsub, 1, CV_64F); //列亚像素偏移量

    Mat m = Mat::zeros(nsub, 1, CV_32S); //子块中心行坐标
    Mat n = Mat::zeros(nsub, 1, CV_32S); //子块中心列坐标
    Mat indx = Mat::zeros(nsub, 1, CV_32S); //索引


    int count = 0;
    int ret;
    Mat coherence;
    Utils util;
    volatile bool parallel_flag = true;
#pragma omp parallel for schedule(guided) \
    private(ret)
    for (int i = 0; i < nsubc; i++)
    {
        if (!parallel_flag) continue;

        for (int j = 0; j < nsubr; j++)
        {
            if (!parallel_flag) continue;
            ComplexMat temp_in_slave, temp_out_slave, temp_in_master, temp_out_master;
            int offset_row, offset_col;
            //辅图像子块插值
            Slave.re(Range(j * blocksize, (j + 1) * blocksize), Range(i * blocksize, (i + 1) * blocksize)).copyTo(temp_in_slave.re);
            Slave.im(Range(j * blocksize, (j + 1) * blocksize), Range(i * blocksize, (i + 1) * blocksize)).copyTo(temp_in_slave.im);
            //util.cvmat2bin("E:\\zgb1\\InSAR\\InSAR\\bin\\re.bin", temp_in_slave.re);
            ret = interp_paddingzero(temp_in_slave, temp_out_slave, interp_times);
            if (ret < 0)
            {
                parallel_flag = false;
                continue;
            }

            //主图像子块插值
            Master.re(Range(j * blocksize, (j + 1) * blocksize), Range(i * blocksize, (i + 1) * blocksize)).copyTo(temp_in_master.re);
            Master.im(Range(j * blocksize, (j + 1) * blocksize), Range(i * blocksize, (i + 1) * blocksize)).copyTo(temp_in_master.im);
            ret = interp_paddingzero(temp_in_master, temp_out_master, interp_times);
            if (ret < 0)
            {
                parallel_flag = false;
                continue;
            }

            //实相关函数求取亚像素偏移量
            ret = real_coherent(temp_out_master, temp_out_slave, &offset_row, &offset_col);
            if (ret < 0)
            {
                parallel_flag = false;
                continue;
            }
            double offset_row_sub, offset_col_sub, mean_coh;
            //ret = util.real_coherence(temp_out_master, temp_out_slave, coherence);
            //if (ret < 0)
            //{
            //    parallel_flag = false;
            //    continue;
            //}
            //mean_coh = mean(coherence)[0];
            offset_row_sub = double(offset_row) / double(interp_times);
            offset_col_sub = double(offset_col) / double(interp_times);
            sub_r_offset.at<double>(i * nsubr + j, 0) = offset_row_sub;
            sub_c_offset.at<double>(i * nsubr + j, 0) = offset_col_sub;

            //子块中心坐标
            n.at<int>(i * nsubr + j, 0) = blocksize / 2 + i * blocksize;
            m.at<int>(i * nsubr + j, 0) = blocksize / 2 + j * blocksize;

            ret = interp_cubic(temp_in_slave, temp_in_slave, sub_r_offset.at<double>(i * nsubr + j, 0), sub_c_offset.at<double>(i * nsubr + j, 0));//子辅图像插值
            if (ret < 0)
            {
                parallel_flag = false;
                continue;
            }
            ret = util.real_coherence(temp_in_master, temp_in_slave, coherence);
            if (ret < 0)
            {
                parallel_flag = false;
                continue;
            }
            if (mean(coherence)[0] > 0.4)
            {
                indx.at<int>(i * nsubr + j, 0) = 1;
            }

        }
    }
    if (parallel_check(parallel_flag, "registration_subpixel()", parallel_error_head)) return -1;
    int NoneZero = countNonZero(indx);//非零元素个数
    count = 0;
    if (NoneZero == 0)
    {
        fprintf(stderr, "registration_subpixel(): NoneZero == 0\n\n");
        return -1;
    }
    Mat sub_r_offset_sifted = Mat::zeros(NoneZero, 1, CV_64F); //筛选后行亚像素偏移量
    Mat sub_c_offset_sifted = Mat::zeros(NoneZero, 1, CV_64F); //筛选后列亚像素偏移量

    Mat m_sifted = Mat::zeros(NoneZero, 1, CV_32S); //筛选后子块中心行坐标
    Mat n_sifted = Mat::zeros(NoneZero, 1, CV_32S); //筛选后子块中心列坐标


    for (int k = 0; k < nsub; k++)
    {
        if (indx.at<int>(k, 0) > 0)
        {

            sub_r_offset_sifted.at<double>(count, 0) = sub_r_offset.at<double>(k, 0);
            sub_c_offset_sifted.at<double>(count, 0) = sub_c_offset.at<double>(k, 0);
            m_sifted.at<int>(count, 0) = m.at<int>(k, 0);
            n_sifted.at<int>(count, 0) = n.at<int>(k, 0);

            count++;
        }
    }


    Mat para;
    ret = all_subpixel_move(m_sifted, n_sifted, sub_r_offset_sifted, sub_c_offset_sifted, para);//拟合辅图像偏移量
    //测试
    //cout << para << "\n";
    //
    if (return_check(ret, "all_subpixel_move(*, *, *, *)", error_head)) return -1;
    ret = interp_cubic(Slave, Slave, para);
    if (return_check(ret, "interp_cubic(*, *, *)", error_head)) return -1;
    return 0;
}

int Registration::coregistration_subpixel(ComplexMat& master, ComplexMat& slave, int blocksize, int interp_times)
{
    if (master.isempty() ||
        slave.GetCols() != master.GetCols() ||
        slave.GetRows() != slave.GetRows() ||
        blocksize * 5 > (slave.GetCols() < slave.GetRows() ? slave.GetCols() : slave.GetRows()) ||
        blocksize < 8 || interp_times < 1
        )
    {
        fprintf(stderr, "coregistration_pixel(): input check failed!\n");
        return -1;
    }

    /*---------------------------------------*/
    /*              求取偏移量矩阵           */
    /*---------------------------------------*/

    Utils util;
    int m = (master.GetRows()) / blocksize;
    int n = (master.GetCols()) / blocksize;
    Mat offset_r = Mat::zeros(m, n, CV_64F); Mat offset_c = Mat::zeros(m, n, CV_64F);
    Mat offset_coord_row = Mat::zeros(m, n, CV_64F);
    Mat offset_coord_col = Mat::zeros(m, n, CV_64F);
    //子块中心坐标
    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < n; j++)
        {
            offset_coord_row.at<double>(i, j) = ((double)blocksize) / 2 * (double)(2 * i + 1);
            offset_coord_col.at<double>(i, j) = ((double)blocksize) / 2 * (double)(2 * j + 1);
        }
    }
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < m; i++)
    {
        ComplexMat master_sub, slave_sub, master_sub_interp, slave_sub_interp;
        int offset_row, offset_col;
        for (int j = 0; j < n; j++)
        {
            //主图像子块插值
            master.re(Range(i * blocksize, (i + 1) * blocksize), Range(j * blocksize, (j + 1) * blocksize)).copyTo(master_sub.re);
            master.im(Range(i * blocksize, (i + 1) * blocksize), Range(j * blocksize, (j + 1) * blocksize)).copyTo(master_sub.im);
            interp_paddingzero(master_sub, master_sub_interp, interp_times);
            //辅图像子块插值
            slave.re(Range(i * blocksize, (i + 1) * blocksize), Range(j * blocksize, (j + 1) * blocksize)).copyTo(slave_sub.re);
            slave.im(Range(i * blocksize, (i + 1) * blocksize), Range(j * blocksize, (j + 1) * blocksize)).copyTo(slave_sub.im);
            interp_paddingzero(slave_sub, slave_sub_interp, interp_times);
            //求取偏移量
            real_coherent(master_sub_interp, slave_sub_interp, &offset_row, &offset_col);
            offset_r.at<double>(i, j) = (double)offset_row / (double)interp_times;
            offset_c.at<double>(i, j) = (double)offset_col / (double)interp_times;

        }
    }

    /*---------------------------------------*/
    /*    拟合偏移量（将坐标做归一化处理）   */
    /*---------------------------------------*/

    /*
    * 拟合公式为 offser_row/offser_col = a0 + a1*x + a2*y + a3*x*y + a4*x*x + a5*y*y + a6*x*x*y + a7*x*y*y + a8*x*x*x + a9*y*y*y
    */
    //util.cvmat2bin("E:\\working_dir\\projects\\software\\InSAR\\bin\\offset_r.bin", offset_r);
    //util.cvmat2bin("E:\\working_dir\\projects\\software\\InSAR\\bin\\offset_c.bin", offset_c);

    //剔除outliers
    Mat sentinel = Mat::zeros(m, n, CV_64F);
    int ix, iy, count = 0, c = 0; double delta, thresh = 2.0;
    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < n; j++)
        {
            count = 0;
            //上
            ix = j;
            iy = i - 1; iy = iy < 0 ? 0 : iy;
            delta = fabs(offset_c.at<double>(i, j) - offset_c.at<double>(iy, ix));
            delta += fabs(offset_r.at<double>(i, j) - offset_r.at<double>(iy, ix));
            if (fabs(delta) >= thresh) count++;
            //下
            ix = j;
            iy = i + 1; iy = iy > m - 1 ? m - 1 : iy;
            delta = fabs(offset_c.at<double>(i, j) - offset_c.at<double>(iy, ix));
            delta += fabs(offset_r.at<double>(i, j) - offset_r.at<double>(iy, ix));
            if (fabs(delta) >= thresh) count++;
            //左
            ix = j - 1; ix = ix < 0 ? 0 : ix;
            iy = i;
            delta = fabs(offset_c.at<double>(i, j) - offset_c.at<double>(iy, ix));
            delta += fabs(offset_r.at<double>(i, j) - offset_r.at<double>(iy, ix));
            if (fabs(delta) >= thresh) count++;
            //右
            ix = j + 1; ix = ix > n - 1 ? n - 1 : ix;
            iy = i;
            delta = fabs(offset_c.at<double>(i, j) - offset_c.at<double>(iy, ix));
            delta += fabs(offset_r.at<double>(i, j) - offset_r.at<double>(iy, ix));
            if (fabs(delta) >= thresh) count++;

            if (count > 2) { sentinel.at<double>(i, j) = 1.0; c++; }
        }
    }
    Mat offset_c_0, offset_r_0, offset_coord_row_0, offset_coord_col_0;
    offset_c_0 = Mat::zeros(m * n - c, 1, CV_64F);
    offset_r_0 = Mat::zeros(m * n - c, 1, CV_64F);
    offset_coord_row_0 = Mat::zeros(m * n - c, 1, CV_64F);
    offset_coord_col_0 = Mat::zeros(m * n - c, 1, CV_64F);
    count = 0;
    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < n; j++)
        {
            if (sentinel.at<double>(i, j) < 0.5)
            {
                offset_r_0.at<double>(count, 0) = offset_r.at<double>(i, j);
                offset_c_0.at<double>(count, 0) = offset_c.at<double>(i, j);
                offset_coord_row_0.at<double>(count, 0) = offset_coord_row.at<double>(i, j);
                offset_coord_col_0.at<double>(count, 0) = offset_coord_col.at<double>(i, j);
                count++;
            }
        }
    }


    offset_c = offset_c_0;
    offset_r = offset_r_0;
    offset_coord_row = offset_coord_row_0;
    offset_coord_col = offset_coord_col_0;
    m = 1; n = count;
    if (count < 11)
    {
        fprintf(stderr, "coregistration_pixel(): insufficient valide sub blocks!\n");
        return -1;
    }
    double offset_x = (double)master.GetCols() / 2;
    double offset_y = (double)master.GetRows() / 2;
    double scale_x = (double)master.GetCols();
    double scale_y = (double)master.GetRows();
    offset_coord_row -= offset_y;
    offset_coord_col -= offset_x;
    offset_coord_row /= scale_y;
    offset_coord_col /= scale_x;
    Mat A = Mat::ones(m * n, 10, CV_64F);
    Mat temp, A_t;
    offset_coord_col.copyTo(A(Range(0, m * n), Range(1, 2)));

    offset_coord_row.copyTo(A(Range(0, m * n), Range(2, 3)));

    temp = offset_coord_col.mul(offset_coord_row);
    temp.copyTo(A(Range(0, m * n), Range(3, 4)));

    temp = offset_coord_col.mul(offset_coord_col);
    temp.copyTo(A(Range(0, m * n), Range(4, 5)));

    temp = offset_coord_row.mul(offset_coord_row);
    temp.copyTo(A(Range(0, m * n), Range(5, 6)));

    temp = offset_coord_col.mul(offset_coord_col);
    temp = temp.mul(offset_coord_row);
    temp.copyTo(A(Range(0, m * n), Range(6, 7)));

    temp = offset_coord_row.mul(offset_coord_row);
    temp = temp.mul(offset_coord_col);
    temp.copyTo(A(Range(0, m * n), Range(7, 8)));

    temp = offset_coord_col.mul(offset_coord_col);
    temp = temp.mul(offset_coord_col);
    temp.copyTo(A(Range(0, m * n), Range(8, 9)));

    temp = offset_coord_row.mul(offset_coord_row);
    temp = temp.mul(offset_coord_row);
    temp.copyTo(A(Range(0, m * n), Range(9, 10)));

    transpose(A, A_t);

    Mat b_r, b_c, coef_r, coef_c, error_r, error_c, b_t, a, a_t;

    A.copyTo(a);
    cv::transpose(a, a_t);
    offset_r.copyTo(b_r);
    b_r = A_t * b_r;

    offset_c.copyTo(b_c);
    b_c = A_t * b_c;

    A = A_t * A;

    double rms1 = -1.0; double rms2 = -1.0;
    Mat eye = Mat::zeros(m * n, m * n, CV_64F);
    for (int i = 0; i < m * n; i++)
    {
        eye.at<double>(i, i) = 1.0;
    }
    if (cv::invert(A, error_r, cv::DECOMP_LU) > 0)
    {
        cv::transpose(offset_r, b_t);
        error_r = b_t * (eye - a * error_r * a_t) * offset_r;
        rms1 = sqrt(error_r.at<double>(0, 0) / double(m * n));
    }
    if (cv::invert(A, error_c, cv::DECOMP_LU) > 0)
    {
        cv::transpose(offset_c, b_t);
        error_c = b_t * (eye - a * error_c * a_t) * offset_c;
        rms2 = sqrt(error_c.at<double>(0, 0) / double(m * n));
    }
    if (!cv::solve(A, b_r, coef_r, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "coregistration_subpixel(): matrix defficiency!\n");
        return -1;
    }
    if (!cv::solve(A, b_c, coef_c, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "coregistration_subpixel(): matrix defficiency!\n");
        return -1;
    }

    /*---------------------------------------*/
    /*    双线性插值获取重采样后的辅图像     */
    /*---------------------------------------*/
    int rows = master.GetRows(); int cols = master.GetCols();
    ComplexMat slave_tmp;
    slave_tmp = master;
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < rows; i++)
    {
        double x, y, ii, jj; Mat tmp(1, 10, CV_64F); Mat result;
        int mm, nn, mm1, nn1;
        double offset_rows, offset_cols, upper, lower;
        for (int j = 0; j < cols; j++)
        {
            jj = (double)j;
            ii = (double)i;
            x = (jj - offset_x) / scale_x;
            y = (ii - offset_y) / scale_y;
            tmp.at<double>(0, 0) = 1.0;
            tmp.at<double>(0, 1) = x;
            tmp.at<double>(0, 2) = y;
            tmp.at<double>(0, 3) = x * y;
            tmp.at<double>(0, 4) = x * x;
            tmp.at<double>(0, 5) = y * y;
            tmp.at<double>(0, 6) = x * x * y;
            tmp.at<double>(0, 7) = x * y * y;
            tmp.at<double>(0, 8) = x * x * x;
            tmp.at<double>(0, 9) = y * y * y;
            result = tmp * coef_r;
            offset_rows = result.at<double>(0, 0);
            result = tmp * coef_c;
            offset_cols = result.at<double>(0, 0);

            ii += offset_rows;
            jj += offset_cols;

            mm = (int)floor(ii); nn = (int)floor(jj);
            if (mm < 0 || nn < 0 || mm > rows - 1 || nn > cols - 1)
            {
                slave_tmp.re.at<double>(i, j) = 0.0;
                slave_tmp.im.at<double>(i, j) = 0.0;
            }
            else
            {
                mm1 = mm + 1; nn1 = nn + 1;
                mm1 = mm1 >= rows - 1 ? rows - 1 : mm1;
                nn1 = nn1 >= cols - 1 ? cols - 1 : nn1;
                //实部插值
                upper = slave.re.at<double>(mm, nn) + (slave.re.at<double>(mm, nn1) - slave.re.at<double>(mm, nn)) * (jj - (double)nn);
                lower = slave.re.at<double>(mm1, nn) + (slave.re.at<double>(mm1, nn1) - slave.re.at<double>(mm1, nn)) * (jj - (double)nn);
                slave_tmp.re.at<double>(i, j) = upper + (lower - upper) * (ii - (double)mm);
                //虚部插值
                upper = slave.im.at<double>(mm, nn) + (slave.im.at<double>(mm, nn1) - slave.im.at<double>(mm, nn)) * (jj - (double)nn);
                lower = slave.im.at<double>(mm1, nn) + (slave.im.at<double>(mm1, nn1) - slave.im.at<double>(mm1, nn)) * (jj - (double)nn);
                slave_tmp.im.at<double>(i, j) = upper + (lower - upper) * (ii - (double)mm);
            }

        }
    }
    slave = slave_tmp;
    return 0;
}

int Registration::every_subpixel_move(int i, int j, Mat& coefficient, double* offset_row, double* offset_col)
{
    if (i < 1 || j < 1 || coefficient.cols < 1 || coefficient.rows < 12 || coefficient.type() != CV_64F)
    {
        fprintf(stderr, "every_subpixel_move(): input check failed!\n\n");
        return -1;
    }
    double a1, a2, b1, b2, c1, c2, d1, d2, e1, e2, f1, f2;
    a1 = coefficient.at<double>(0, 0);
    b1 = coefficient.at<double>(1, 0);
    c1 = coefficient.at<double>(2, 0);
    d1 = coefficient.at<double>(3, 0);
    e1 = coefficient.at<double>(4, 0);
    f1 = coefficient.at<double>(5, 0);
    a2 = coefficient.at<double>(6, 0);
    b2 = coefficient.at<double>(7, 0);
    c2 = coefficient.at<double>(8, 0);
    d2 = coefficient.at<double>(9, 0);
    e2 = coefficient.at<double>(10, 0);
    f2 = coefficient.at<double>(11, 0);

    *offset_row = a1 + b1 * i + c1 * j + d1 * i * i + e1 * j * j + f1 * i * j;
    *offset_col = a2 + b2 * i + c2 * j + d2 * i * i + e2 * j * j + f2 * i * j;
    return 0;
}

int Registration::all_subpixel_move(Mat& Coordinate_x, Mat& Coordinate_y, Mat& offset_row, Mat& offset_col, Mat& para)
{
    if (Coordinate_x.rows < 1 ||
        Coordinate_x.cols < 1 ||
        Coordinate_x.rows != Coordinate_y.rows ||
        Coordinate_x.cols != Coordinate_y.cols ||
        offset_row.rows != offset_col.rows ||
        offset_row.cols != offset_col.cols)
    {
        fprintf(stderr, "all_subpixel_move(): input size/type check failed!\n\n");
        return -1;
    }
    int N = Coordinate_x.rows;
    Coordinate_x.convertTo(Coordinate_x, CV_64F);
    Coordinate_y.convertTo(Coordinate_y, CV_64F);
    offset_row.convertTo(offset_row, CV_64F);
    offset_col.convertTo(offset_col, CV_64F);

    Mat connect_h_x[] = { Mat::ones(N, 1, CV_64F), Coordinate_x, Coordinate_y, Coordinate_x.mul(Coordinate_x), Coordinate_y.mul(Coordinate_y),
        Coordinate_y.mul(Coordinate_x) };
    Mat matrix, matrix_t;
    hconcat(connect_h_x, 6, matrix);
    Mat para1;
    transpose(matrix, matrix_t);
    if (!solve(matrix_t * matrix, matrix_t * offset_row, para1, DECOMP_LU))
    {
        fprintf(stderr, "all_subpixel_move(): cant' solve least square problem!\n");
        return -1;
    }

    Mat connect_h_y[] = { Mat::ones(N, 1, CV_64F), Coordinate_x, Coordinate_y, Coordinate_x.mul(Coordinate_x), Coordinate_y.mul(Coordinate_y),
        Coordinate_y.mul(Coordinate_x) };
    hconcat(connect_h_y, 6, matrix);
    transpose(matrix, matrix_t);
    Mat para2;
    if (!solve(matrix_t * matrix, matrix_t * offset_col, para2, DECOMP_LU))
    {
        fprintf(stderr, "all_subpixel_move(): cant' solve least square problem!\n");
        return -1;
    }
    Mat connect[] = { para1, para2 };
    vconcat(connect, 2, para);
    return 0;
}

int Registration::gcps_sift(int rows, int cols, int move_rows, int move_cols, Mat& gcps)
{
    if (fabs(move_rows) > rows ||
        fabs(move_cols) > cols ||
        rows < 1 ||
        cols < 1 ||
        gcps.cols != 5 ||
        gcps.type() != CV_64F ||
        gcps.channels() != 1 ||
        gcps.rows < 3)
    {
        fprintf(stderr, "gcps_sift(): input check failed!\n\n");
        return -1;
    }
    Mat gcps_tmp, GCP;
    gcps(Range(0, 2), Range(0, 5)).copyTo(GCP);
    gcps.copyTo(gcps_tmp);
    Mat index = Mat::zeros(gcps.rows, 1, CV_64F);
    int count = 0;
    //////////////////////////Scenario 1///////////////////////////////
    if (move_rows > 0 && move_cols > 0)
    {
        for (int i = 0; i < gcps_tmp.rows; i++)
        {
            if (gcps_tmp.at<double>(i, 0) <= double(rows - move_rows) &&
                gcps_tmp.at<double>(i, 1) <= double(cols - move_cols))
            {
                index.at<double>(i, 0) = 1.0;
                count++;
            }
        }
        if (count == 0)
        {
            fprintf(stderr, "all ground control points have been sifted out!\n\n");
            return -1;
        }
        gcps_tmp = Mat::zeros(count, gcps.cols, CV_64F);
        count = 0;
        for (int i = 0; i < gcps.rows; i++)
        {
            if (index.at<double>(i, 0) > 0.5)
            {
                gcps(Range(i, i + 1), Range(0, gcps.cols)).copyTo(gcps_tmp(Range(count, count + 1), Range(0, gcps.cols)));
                count++;
            }
        }
    }
    //////////////////////////Scenario 2///////////////////////////////
    if (move_rows > 0 && move_cols <= 0)
    {
        for (int i = 0; i < gcps_tmp.rows; i++)
        {
            if (gcps_tmp.at<double>(i, 0) <= double(rows - move_rows) &&
                gcps_tmp.at<double>(i, 1) >= double(1 - move_cols))
            {
                index.at<double>(i, 0) = 1.0;
                gcps_tmp.at<double>(i, 1) = gcps_tmp.at<double>(i, 1) + move_cols;
                count++;
            }
        }
        if (count == 0)
        {
            fprintf(stderr, "all ground control points have been sifted out!\n\n");
            return -1;
        }
        Mat gcps_tmp1 = Mat::zeros(count, gcps.cols, CV_64F);
        count = 0;
        for (int i = 0; i < gcps.rows; i++)
        {
            if (index.at<double>(i, 0) > 0.5)
            {
                gcps_tmp(Range(i, i + 1), Range(0, gcps.cols)).copyTo(gcps_tmp1(Range(count, count + 1), Range(0, gcps.cols)));
                count++;
            }
        }
        gcps = gcps_tmp1;
    }
    //////////////////////////Scenario 3///////////////////////////////
    if (move_rows <= 0 && move_cols > 0)
    {
        for (int i = 0; i < gcps_tmp.rows; i++)
        {
            if (gcps_tmp.at<double>(i, 0) >= double(1 - move_rows) &&
                gcps_tmp.at<double>(i, 1) <= double(cols - move_cols))
            {
                index.at<double>(i, 0) = 1.0;
                gcps_tmp.at<double>(i, 0) = gcps_tmp.at<double>(i, 0) + move_rows;
                count++;
            }
        }
        if (count == 0)
        {
            fprintf(stderr, "all ground control points have been sifted out!\n\n");
            return -1;
        }
        Mat gcps_tmp1 = Mat::zeros(count, gcps.cols, CV_64F);
        count = 0;
        for (int i = 0; i < gcps.rows; i++)
        {
            if (index.at<double>(i, 0) > 0.5)
            {
                gcps_tmp(Range(i, i + 1), Range(0, gcps.cols)).copyTo(gcps_tmp1(Range(count, count + 1), Range(0, gcps.cols)));
                count++;
            }
        }
        gcps = gcps_tmp1;
    }
    //////////////////////////Scenario 4///////////////////////////////
    if (move_rows <= 0 && move_cols <= 0)
    {
        for (int i = 0; i < gcps_tmp.rows; i++)
        {
            if (gcps_tmp.at<double>(i, 0) >= double(1 - move_rows) &&
                gcps_tmp.at<double>(i, 1) >= double(1 - move_cols))
            {
                index.at<double>(i, 0) = 1.0;
                gcps_tmp.at<double>(i, 0) = gcps_tmp.at<double>(i, 0) + move_rows;
                gcps_tmp.at<double>(i, 1) = gcps_tmp.at<double>(i, 1) + move_cols;
                count++;
            }
        }
        if (count == 0)
        {
            fprintf(stderr, "all ground control points have been sifted out!\n\n");
            return -1;
        }
        Mat gcps_tmp1 = Mat::zeros(count, gcps.cols, CV_64F);
        count = 0;
        for (int i = 0; i < gcps.rows; i++)
        {
            if (index.at<double>(i, 0) > 0.5)
            {
                gcps_tmp(Range(i, i + 1), Range(0, gcps.cols)).copyTo(gcps_tmp1(Range(count, count + 1), Range(0, gcps.cols)));
                count++;
            }
        }
        gcps = gcps_tmp1;
    }
    Mat GCPS(2 + gcps.rows, 5, CV_64F);
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 5; j++)
        {
            GCPS.at<double>(i, j) = GCP.at<double>(i, j);
        }
    }
    for (int i = 2; i < GCPS.rows; i++)
    {
        for (int j = 0; j < 5; j++)
        {
            GCPS.at<double>(i, j) = gcps.at<double>(i - 2, j);
        }
    }
    GCPS.copyTo(gcps);
    return 0;
}

5.4：Utils.cpp

// Utils.cpp : 定义 DLL 应用程序的导出函数。
//
#include<complex.h>
#include<fstream>
#include<iostream>
#include<queue>
#include"Utils.h"


using namespace cv;
using namespace std;
using namespace InSAR;


/*宏定义*/
#define RETURN_MSG \
{ \
    if( fp ) fclose( fp ); \
    return( -1 ); \
}

#define GET_NEXT_LINE \
{ \
    if( !fgets( instring, 256, fp ) ) \
        ch = 0; \
        else \
        ch = *instring; \
}
inline bool return_check(int ret, const char* detail_info, const char* error_head)
{
    if (ret < 0)
    {
        fprintf(stderr, "%s %s\n\n", error_head, detail_info);
        return true;
    }
    else
    {
        return false;
    }
}

inline bool read_check(long ret, long ret_ref, const char* detail_info, const char* error_head)
{
    if (ret != ret_ref)
    {
        fprintf(stderr, "%s %s\n\n", error_head, detail_info);
        return true;
    }
    return false;
}

inline bool parallel_check(volatile bool parallel_flag, const char* detail_info, const char* parallel_error_head)
{
    if (!parallel_flag)
    {
        fprintf(stderr, "%s %s\n\n", parallel_error_head, detail_info);
        return true;
    }
    else
    {
        return false;
    }
}

inline bool parallel_flag_change(volatile bool parallel_flag, int ret)
{
    if (ret < 0)
    {
        parallel_flag = false;
        return true;
    }
    else
    {
        return false;
    }
}
Utils::Utils()
{
    memset(this->error_head, 0, 256);
    memset(this->parallel_error_head, 0, 256);
    strcpy(this->error_head, "UTILS_DLL_ERROR: error happens when using ");
    strcpy(this->parallel_error_head, "UTILS_DLL_ERROR: error happens when using parallel computing in function: ");
}

Utils::~Utils()
{
}

int Utils::get_mode_index(const Mat& input, int* out)
{
    if (input.empty() || input.type() != CV_32S || out == NULL)
    {
        fprintf(stderr, "get_mode_index(): input check failed!\n");
        return -1;
    }
    Mat temp; input.copyTo(temp);
    int nr = temp.rows; int nc = temp.cols;
    temp = temp.reshape(0, 1);
    cv::sort(temp, temp, cv::SORT_EVERY_ROW + cv::SORT_ASCENDING);
    int total = nr * nc;
    int max_count = 0; int max_count_ix = 0; int count = 0, i = 0, j = 0;
    while (i < total - 1)
    {
        count = 0;
        for (j = i; j < total - 1; j++)
        {
            if (temp.at<int>(0, j) == temp.at<int>(0, j + 1)) count++;
            else break;
        }
        if (max_count < count)
        {
            max_count = count;
            max_count_ix = j;
        }
        j++;
        i = j;
    }
    *out = temp.at<int>(0, max_count_ix);
    return 0;
}

int Utils::diff(Mat& Src, Mat& diff1, Mat& diff2, bool same)
{
    int nr = Src.rows;
    int nc = Src.cols;
    if (nr < 2 || nc < 2 || Src.type() != CV_64F)
    {
        fprintf(stderr, "diff(): input check failed!\n\n");
        return -1;
    }

    diff1 = Src(Range(1, nr), Range(0, nc)) - Src(Range(0, nr - 1), Range(0, nc));
    diff2 = Src(Range(0, nr), Range(1, nc)) - Src(Range(0, nr), Range(0, nc - 1));
    if (same)
    {
        copyMakeBorder(diff1, diff1, 0, 1, 0, 0, BORDER_CONSTANT, Scalar(0.0));
        copyMakeBorder(diff2, diff2, 0, 0, 0, 1, BORDER_CONSTANT, Scalar(0.0));
    }
    return 0;
}



int Utils::generate_phase(const ComplexMat& Master, const ComplexMat& Slave, Mat& phase)
{
    if (Master.GetRows() < 1 ||
        Master.GetCols() < 1 ||
        Slave.GetRows() != Master.GetRows() ||
        Slave.GetCols() != Master.GetCols())
    {
        fprintf(stderr, "generate_phase(): input check failed!\n\n");
        return -1;
    }
    ComplexMat tmp;
    int ret = Master.Mul(Slave, tmp, true);
    phase = tmp.GetPhase();
    return 0;
}

int Utils::write_DIMACS(const char* DIMACS_file_problem, triangle* tri, int num_triangle, vector<tri_node>& nodes, tri_edge* edges, long num_edges, Mat& cost)
{
    if (DIMACS_file_problem == NULL ||
        tri == NULL ||
        num_triangle < 1 ||
        nodes.size() < 3 ||
        edges == NULL ||
        num_edges < 3 ||
        cost.rows < 2 ||
        cost.cols < 2 ||
        cost.channels() != 1 ||
        cost.type() != CV_64F
        )
    {
        fprintf(stderr, "write_DIMACS(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(DIMACS_file_problem, "wt");
    if (fp == NULL)
    {
        fprintf(stderr, "write_DIMACS(): can't open %s\n", DIMACS_file_problem);
        return -1;
    }

    int ret, num_nodes;

    num_nodes = nodes.size();
    long num_arcs = 0;
    for (int i = 0; i < num_triangle; i++)
    {
        if ((tri + i) != NULL)
        {
            if ((tri + i)->neigh1 > 0) num_arcs++;
            if ((tri + i)->neigh2 > 0) num_arcs++;
            if ((tri + i)->neigh3 > 0) num_arcs++;
        }
    }

    //统计正负残差点并写入节点信息
    int positive, negative, total;
    positive = 0;
    negative = 0;
    double thresh = 0.7;
    for (int i = 0; i < num_triangle; i++)
    {
        if ((tri + i)->residue > thresh)
        {
            positive++;
        }
        if ((tri + i)->residue < -thresh)
        {
            negative++;
        }
    }
    bool b_balanced = (positive == negative);
    if (negative == 0 || positive == 0)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "write_DIMACS(): no residue point!\n\n");
        return -1;
    }
    fprintf(fp, "c This is MCF problem file.\n");
    fprintf(fp, "c Problem line(nodes, links)\n");
    //统计边缘三角形个数
    long boundry_tri = 0;
    for (int i = 0; i < num_triangle; i++)
    {
        if ((tri + i) != NULL)
        {
            if ((edges + (tri + i)->edge1 - 1)->isBoundry ||
                (edges + (tri + i)->edge2 - 1)->isBoundry ||
                (edges + (tri + i)->edge3 - 1)->isBoundry)
            {
                boundry_tri++;
            }
        }
    }
    long n;
    if (!b_balanced)
    {
        n = num_triangle + 1;
        fprintf(fp, "p min %ld %ld\n", n, num_arcs + boundry_tri * 2);
    }
    else
    {
        n = num_triangle;
        fprintf(fp, "p min %ld %ld\n", n, num_arcs);
    }
    fprintf(fp, "c Node descriptor lines\n");
    positive = 0;
    negative = 0;
    int count = 0;
    bool b_positive, b_negative, is_residue;
    double sum = 0.0;
    for (int i = 0; i < num_triangle; i++)
    {
        if ((tri + i) != NULL && (tri + i)->residue > thresh)
        {
            fprintf(fp, "n %d %lf\n", i + 1, (tri + i)->residue);
            sum += (tri + i)->residue;
        }
        if ((tri + i) != NULL && (tri + i)->residue < -thresh)
        {
            fprintf(fp, "n %d %lf\n", i + 1, (tri + i)->residue);
            sum += (tri + i)->residue;
        }
    }
    //写入大地节点
    if (!b_balanced)
    {
        fprintf(fp, "n %d %lf\n", num_triangle + 1, -sum);
    }

    //写入流费用
    fprintf(fp, "c Arc descriptor lines(from, to, minflow, maxflow, cost)\n");
    int rows, cols;
    int lower_bound = 0;
    int upper_bound = 5;
    double cost_mean;
    int nr = cost.rows;
    int nc = cost.cols;
    for (int i = 0; i < num_triangle; i++)
    {
        if ((tri + i) != NULL &&
            (tri + i)->p1 >= 1 &&
            (tri + i)->p1 <= num_nodes &&
            (tri + i)->p2 >= 1 &&
            (tri + i)->p2 <= num_nodes &&
            (tri + i)->p3 >= 1 &&
            (tri + i)->p3 <= num_nodes
            )
        {
            cost_mean = 0.0;
            nodes[(tri + i)->p1 - 1].get_pos(&rows, &cols);
            if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
            nodes[(tri + i)->p2 - 1].get_pos(&rows, &cols);
            if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
            nodes[(tri + i)->p3 - 1].get_pos(&rows, &cols);
            if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
            cost_mean = cost_mean / 3;
            if ((tri + i)->neigh1 > 0) fprintf(fp, "a %d %d %d %d %lf\n", i + 1, (tri + i)->neigh1, lower_bound, upper_bound, cost_mean);
            if ((tri + i)->neigh2 > 0) fprintf(fp, "a %d %d %d %d %lf\n", i + 1, (tri + i)->neigh2, lower_bound, upper_bound, cost_mean);
            if ((tri + i)->neigh3 > 0) fprintf(fp, "a %d %d %d %d %lf\n", i + 1, (tri + i)->neigh3, lower_bound, upper_bound, cost_mean);
        }
    }
    if (!b_balanced)
    {
        //写入边界流费用
        for (int i = 0; i < num_triangle; i++)
        {
            if ((tri + i) != NULL &&
                (((edges + (tri + i)->edge1 - 1)->isBoundry) ||
                    ((edges + (tri + i)->edge2 - 1)->isBoundry) ||
                    ((edges + (tri + i)->edge3 - 1)->isBoundry)
                    ))
            {
                cost_mean = 0.0;
                nodes[(tri + i)->p1 - 1].get_pos(&rows, &cols);
                if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
                nodes[(tri + i)->p2 - 1].get_pos(&rows, &cols);
                if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
                nodes[(tri + i)->p3 - 1].get_pos(&rows, &cols);
                if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
                cost_mean = cost_mean / 3;
                fprintf(fp, "a %d %d %d %d %lf\n", i + 1, num_triangle + 1, lower_bound, upper_bound, cost_mean);
                fprintf(fp, "a %d %d %d %d %lf\n", num_triangle + 1, i + 1, lower_bound, upper_bound, cost_mean);
            }
        }
    }
    if (fp) fclose(fp);
    fp = NULL;
    return 0;
}

int Utils::write_DIMACS(
    const char* DIMACS_file_problem,
    vector<triangle>& triangle,
    vector<tri_node>& nodes,
    vector<tri_edge>& edges,
    const Mat& cost
)
{
    if (DIMACS_file_problem == NULL ||
        triangle.size() < 1 ||
        nodes.size() < 3 ||
        edges.size() < 3 ||
        cost.rows < 2 ||
        cost.cols < 2 ||
        cost.channels() != 1 ||
        cost.type() != CV_64F
        )
    {
        fprintf(stderr, "write_DIMACS(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(DIMACS_file_problem, "wt");
    if (fp == NULL)
    {
        fprintf(stderr, "write_DIMACS(): can't open %s\n", DIMACS_file_problem);
        return -1;
    }

    int ret, num_nodes;
    int num_triangle = triangle.size();
    num_nodes = nodes.size();
    long num_arcs = 0;
    for (int i = 0; i < num_triangle; i++)
    {
        if (triangle[i].neigh1 > 0) num_arcs++;
        if (triangle[i].neigh2 > 0) num_arcs++;
        if (triangle[i].neigh3 > 0) num_arcs++;
    }

    //统计正负残差点并写入节点信息
    int positive, negative, total;
    positive = 0;
    negative = 0;
    double thresh = 0.7;
    for (int i = 0; i < num_triangle; i++)
    {
        if (triangle[i].residue > thresh)
        {
            positive++;
        }
        if (triangle[i].residue < -thresh)
        {
            negative++;
        }
    }
    bool b_balanced = (positive == negative);
    if (negative == 0 && positive == 0)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "write_DIMACS(): no residue point!\n\n");
        return -1;
    }
    fprintf(fp, "c This is MCF problem file.\n");
    fprintf(fp, "c Problem line(nodes, links)\n");
    //统计边缘三角形个数
    long boundry_tri = 0;
    for (int i = 0; i < num_triangle; i++)
    {
        if (edges[triangle[i].edge1 - 1].isBoundry ||
            edges[triangle[i].edge2 - 1].isBoundry ||
            edges[triangle[i].edge3 - 1].isBoundry)
        {
            boundry_tri++;
        }
    }
    long n;
    if (!b_balanced)
    {
        n = num_triangle + 1;
        fprintf(fp, "p min %ld %ld\n", n, num_arcs + boundry_tri * 2);
    }
    else
    {
        n = num_triangle;
        fprintf(fp, "p min %ld %ld\n", n, num_arcs);
    }
    fprintf(fp, "c Node descriptor lines\n");
    positive = 0;
    negative = 0;
    int count = 0;
    bool b_positive, b_negative, is_residue;
    double sum = 0.0;
    for (int i = 0; i < num_triangle; i++)
    {
        if (triangle[i].residue > thresh)
        {
            fprintf(fp, "n %d %lf\n", i + 1, triangle[i].residue);
            sum += triangle[i].residue;
        }
        if (triangle[i].residue < -thresh)
        {
            fprintf(fp, "n %d %lf\n", i + 1, triangle[i].residue);
            sum += triangle[i].residue;
        }
    }
    //写入大地节点
    if (!b_balanced)
    {
        fprintf(fp, "n %d %lf\n", num_triangle + 1, -sum);
    }

    //写入流费用
    fprintf(fp, "c Arc descriptor lines(from, to, minflow, maxflow, cost)\n");
    int rows, cols;
    int lower_bound = 0;
    int upper_bound = 1;
    double cost_mean;
    int nr = cost.rows;
    int nc = cost.cols;
    for (int i = 0; i < num_triangle; i++)
    {
        if (triangle[i].p1 >= 1 &&
            triangle[i].p1 <= num_nodes &&
            triangle[i].p2 >= 1 &&
            triangle[i].p2 <= num_nodes &&
            triangle[i].p3 >= 1 &&
            triangle[i].p3 <= num_nodes
            )
        {
            cost_mean = 0.0;
            nodes[triangle[i].p1 - 1].get_pos(&rows, &cols);
            if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
            nodes[triangle[i].p2 - 1].get_pos(&rows, &cols);
            if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
            nodes[triangle[i].p3 - 1].get_pos(&rows, &cols);
            if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
            cost_mean = cost_mean / 3;
            if (triangle[i].neigh1 > 0) fprintf(fp, "a %d %d %d %d %lf\n", i + 1, triangle[i].neigh1, lower_bound, upper_bound, cost_mean);
            if (triangle[i].neigh2 > 0) fprintf(fp, "a %d %d %d %d %lf\n", i + 1, triangle[i].neigh2, lower_bound, upper_bound, cost_mean);
            if (triangle[i].neigh3 > 0) fprintf(fp, "a %d %d %d %d %lf\n", i + 1, triangle[i].neigh3, lower_bound, upper_bound, cost_mean);
        }
    }
    if (!b_balanced)
    {
        //写入边界流费用
        for (int i = 0; i < num_triangle; i++)
        {
            if (edges[triangle[i].edge1 - 1].isBoundry ||
                edges[triangle[i].edge2 - 1].isBoundry ||
                edges[triangle[i].edge3 - 1].isBoundry)
            {
                cost_mean = 0.0;
                nodes[triangle[i].p1 - 1].get_pos(&rows, &cols);
                if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
                nodes[triangle[i].p2 - 1].get_pos(&rows, &cols);
                if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
                nodes[triangle[i].p3 - 1].get_pos(&rows, &cols);
                if (rows >= 0 && rows <= nr - 1) cost_mean += cost.at<double>(rows, cols);
                cost_mean = cost_mean / 3;
                fprintf(fp, "a %d %d %d %d %lf\n", i + 1, num_triangle + 1, lower_bound, upper_bound, cost_mean);
                fprintf(fp, "a %d %d %d %d %lf\n", num_triangle + 1, i + 1, lower_bound, upper_bound, cost_mean);
            }
        }
    }
    if (fp) fclose(fp);
    fp = NULL;
    return 0;
}

int Utils::read_DIMACS(const char* DIMACS_file_solution, Mat& k1, Mat& k2, int rows, int cols)
{
    if (rows < 2 || cols < 2)
    {
        fprintf(stderr, "read_DIMACS(): input check failed!\n\n");
        return -1;
    }
    char instring[256];
    char ch;
    double obj_value = 0;
    int i, row_index, col_index, symbol;
    long from, to;
    double flow = 0;
    k1 = Mat::zeros(rows - 1, cols, CV_64F);
    k2 = Mat::zeros(rows, cols - 1, CV_64F);
    long earth_node_indx = (rows - 1) * (cols - 1) + 1;
    FILE* fp = NULL;
    fp = fopen( DIMACS_file_solution, "rt");
    if (fp == NULL)
    {
        fprintf(stderr, "read_DIMACS(): can't open file %s\n\n", DIMACS_file_solution);
        return -1;
    }
    /////////////////////读取注释///////////////////////////
    GET_NEXT_LINE;
    while (ch != 's' && ch)
    {
        if (ch != 'c')
        {
            if (fp) fclose(fp);
            fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
            return -1;
        }
        GET_NEXT_LINE;
    }
    /////////////////////读取优化目标值/////////////////////
    for (i = 1; i < 81; i++)
    {
        if (isspace((int)instring[i]) > 0)
        {
            i++;
            break;
        }
    }
    if (sscanf(&(instring[i]), "%lf", &obj_value) != 1)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
        return -1;
    }
    if (obj_value < 0.0)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): this problem can't be solved(unbounded or infeasible)!\n\n");
        return -1;
    }
    ////////////////////读取MCF结果/////////////////////////
    GET_NEXT_LINE;
    while (ch && ch == 'f')
    {
        if (sscanf(&(instring[2]), "%ld %ld %lf", &from, &to, &flow) != 3 ||
            flow < 0.0 || from < 0 || to < 0)
        {
            if (fp) fclose(fp);
            fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
            return -1;
        }
        //是否为接地弧
        if (from == earth_node_indx || to == earth_node_indx)
        {
            if (from == earth_node_indx)
            {
                //top
                if (to <= cols - 1)
                {
                    row_index = 0;
                    col_index = to - 1;
                    symbol = 1;
                    k2.at<double>(row_index, col_index) = k2.at<double>(row_index, col_index) + symbol * flow;
                }
                //bottom
                else if (to > (rows - 2) * (cols - 1))
                {
                    symbol = -1;
                    row_index = rows - 1;
                    col_index = to - (rows - 2) * (cols - 1) - 1;
                    k2.at<double>(row_index, col_index) = k2.at<double>(row_index, col_index) + symbol * flow;
                }
                //right
                else if (to % (cols - 1) == 0 && to < (rows - 2) * (cols - 1) && to >(cols - 1))
                {
                    symbol = 1;
                    row_index = to / (cols - 1) - 1;
                    col_index = cols - 1;
                    k1.at<double>(row_index, col_index) = k1.at<double>(row_index, col_index) + symbol * flow;
                }
                //left
                else
                {
                    symbol = -1;
                    row_index = to / (cols - 1);
                    col_index = 0;
                    k1.at<double>(row_index, col_index) = k1.at<double>(row_index, col_index) + symbol * flow;
                }
            }
            else
            {
                /*long tmp;
                tmp = from;
                from = to;
                to = tmp;*/
                //top
                if (from <= cols - 1)
                {
                    symbol = -1;
                    row_index = 0;
                    col_index = from - 1;
                    k2.at<double>(row_index, col_index) = k2.at<double>(row_index, col_index) + symbol * flow;
                }
                //bottom
                else if (from >= (rows - 2) * (cols - 1))
                {
                    symbol = 1;
                    row_index = rows - 1;
                    col_index = from - (rows - 2) * (cols - 1) - 1;
                    k2.at<double>(row_index, col_index) = k2.at<double>(row_index, col_index) + symbol * flow;
                }
                //right
                else if (from % (cols - 1) == 0 && from < (rows - 2) * (cols - 1) && from >(cols - 1))
                {
                    symbol = -1;
                    row_index = from / (cols - 1) - 1;
                    col_index = cols - 1;
                    k1.at<double>(row_index, col_index) = k1.at<double>(row_index, col_index) + symbol * flow;
                }
                //left
                else
                {
                    symbol = 1;
                    row_index = from / (cols - 1);
                    col_index = 0;
                    k1.at<double>(row_index, col_index) = k1.at<double>(row_index, col_index) + symbol * flow;
                }
            }
        }
        else
        {
            if (abs(from - to) > 1)
            {
                symbol = from > to ? 1 : -1;
                row_index = (from > to ? to : from) / long(cols - 1);
                col_index = (from > to ? to : from) % long(cols - 1);
                if (col_index == 0)
                {
                    col_index = cols - 1;
                    row_index--;
                }
                col_index--;
                k2.at<double>(row_index + 1, col_index) = k2.at<double>(row_index + 1, col_index) + symbol * flow;
            }
            else
            {
                symbol = from > to ? 1 : -1;
                row_index = (from > to ? to : from) / long(cols - 1);
                col_index = (from > to ? to : from) % long(cols - 1);
                k1.at<double>(row_index, col_index) = k1.at<double>(row_index, col_index) + symbol * flow;
            }
        }
        GET_NEXT_LINE;
    }
    if (ch != 'c')
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
        return -1;
    }
    if (fp)
    {
        fclose(fp);
    }
    return 0;
}

int Utils::write_DIMACS(const char* DIMACS_file_problem, Mat& residue, Mat& coherence, double thresh)
{

    if (residue.cols < 2 ||
        residue.rows < 2 ||
        coherence.cols < 2 ||
        coherence.rows < 2 ||
        residue.type() != CV_64F ||
        coherence.type() != CV_64F ||
        (coherence.rows - residue.rows) != 1 ||
        (coherence.cols - residue.cols) != 1 ||
        thresh < 0.0)
    {
        fprintf(stderr, "write_DIMACS(): input check failed!\n\n");
        return -1;
    }
    long nr = residue.rows;
    long nc = residue.cols;
    long i, j;
    long node_index = 1;
    double sum = 0.0;
    //统计正负残差点数
    long positive, negative, total, Arcs_num, Nodes_num;
    positive = 0;
    negative = 0;
    for (i = 0; i < nr; i++)
    {
        for (j = 0; j < nc; j++)
        {
            if (residue.at<double>(i, j) > thresh)
            {
                positive++;
            }
            if (residue.at<double>(i, j) < -thresh)
            {
                negative++;
            }
        }
    }
    bool b_balanced = (positive == negative);
    if (!b_balanced)
    {
        Nodes_num = residue.rows * residue.cols + 1;
        Arcs_num = 2 * (residue.rows - 1) * residue.cols + 2 * residue.rows * (residue.cols - 1) +
            2 * 2 * residue.cols + 2 * 2 * (residue.rows - 2);
    }
    else
    {
        Nodes_num = residue.rows * residue.cols;
        Arcs_num = 2 * (residue.rows - 1) * residue.cols + 2 * residue.rows * (residue.cols - 1);
    }
    ofstream fout;
    FILE* fp = NULL;
    fp = fopen(DIMACS_file_problem, "wt");
    if (!fp)
    {
        fprintf(stderr, "write_DIMACS(): cant't open file %s\n\n", DIMACS_file_problem);
        return -1;
    }
    fprintf(fp, "c This is a DIMACS file, describing Minimum Cost Flow problem.\n");
    fprintf(fp, "c Problem line (nodes, links)\n");
    fprintf(fp, "p min %ld %ld\n", Nodes_num, Arcs_num);
    fprintf(fp, "c Node descriptor lines (supply+ or demand-)\n");


    /*
    * 写入节点的度（残差值1，-1）
    */

    for (i = 0; i < nr; i++)
    {
        for (j = 0; j < nc; j++)
        {
            if (residue.at<double>(i, j) > thresh)
            {
                node_index = i * nc + j + 1;
                fprintf(fp, "n %ld %lf\n", node_index, residue.at<double>(i, j));
                sum += residue.at<double>(i, j);
            }
            if (residue.at<double>(i, j) < -thresh)
            {
                node_index = i * nc + j + 1;
                fprintf(fp, "n %ld %lf\n", node_index, residue.at<double>(i, j));
                sum += residue.at<double>(i, j);
            }
        }
    }

    /*写接地节点*/

    node_index = nc * nr + 1;
    if (!b_balanced)
    {
        fprintf(fp, "n %ld %lf\n", node_index, -sum);
    }


    long earth_node_index = node_index;

    /*
    * 写入每个有向弧的费用（流费用）
    */
    long lower_bound = 0;
    long upper_bound = 5;
    double mean_coherence1, mean_coherence2, mean_coherence3, mean_coherence4;
    fprintf(fp, "c Arc descriptor lines (from, to, minflow, maxflow, cost)\n");
    /*接地节点的有向弧流费用*/
    if (!b_balanced)
    {
        //top
        for (i = 0; i < nc; i++)
        {
            node_index = i + 1;
            mean_coherence1 = coherence.at<double>(0, i);
            mean_coherence2 = mean_coherence1;
            fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
                node_index, earth_node_index, lower_bound, upper_bound, mean_coherence1,
                earth_node_index, node_index, lower_bound, upper_bound, mean_coherence2);
        }
        //bottom
        for (i = 0; i < nc; i++)
        {
            node_index = nc * (nr - 1) + i + 1;
            mean_coherence1 = coherence.at<double>(nr - 1, i);
            mean_coherence2 = mean_coherence1;
            fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
                node_index, earth_node_index, lower_bound, upper_bound, mean_coherence1,
                earth_node_index, node_index, lower_bound, upper_bound, mean_coherence2);
        }
        //left
        for (i = 1; i < nr - 1; i++)
        {
            node_index = nc * i + 1;
            mean_coherence1 = coherence.at<double>(i, 0);
            mean_coherence2 = mean_coherence1;
            fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
                node_index, earth_node_index, lower_bound, upper_bound, mean_coherence1,
                earth_node_index, node_index, lower_bound, upper_bound, mean_coherence2);
        }
        //right
        for (i = 1; i < nr - 1; i++)
        {
            node_index = nc * (i + 1);
            mean_coherence1 = coherence.at<double>(i, nc - 1);
            mean_coherence2 = mean_coherence1;
            fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
                node_index, earth_node_index, lower_bound, upper_bound, mean_coherence1,
                earth_node_index, node_index, lower_bound, upper_bound, mean_coherence2);
        }
    }

    /*非接地节点的有向弧流费用*/
    for (i = 0; i < nr - 1; i++)
    {
        for (j = 0; j < nc - 1; j++)
        {
            node_index = i * nc + j + 1;
            /*正向*/
            mean_coherence1 = mean(coherence(Range(i, i + 2), Range(j, j + 3))).val[0];
            /*逆向*/
            mean_coherence2 = mean(coherence(Range(i, i + 2), Range(j, j + 3))).val[0];


            /*正向*/
            mean_coherence3 = mean(coherence(Range(i, i + 3), Range(j, j + 2))).val[0];
            /*逆向*/
            mean_coherence4 = mean(coherence(Range(i, i + 3), Range(j, j + 2))).val[0];
            fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
                node_index, node_index + 1, lower_bound, upper_bound, mean_coherence1,
                node_index + 1, node_index, lower_bound, upper_bound, mean_coherence2,
                node_index, node_index + nc, lower_bound, upper_bound, mean_coherence3,
                node_index + nc, node_index, lower_bound, upper_bound, mean_coherence4);

        }
    }



    for (j = 0; j < nc - 1; j++)
    {
        node_index = (nr - 1) * nc + j + 1;
        /*正向*/
        mean_coherence1 = mean(coherence(Range(nr - 1, nr + 1), Range(j, j + 3))).val[0];
        /*逆向*/
        mean_coherence2 = mean(coherence(Range(nr - 1, nr + 1), Range(j, j + 3))).val[0];
        fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
            node_index, node_index + 1, lower_bound, upper_bound, mean_coherence1,
            node_index + 1, node_index, lower_bound, upper_bound, mean_coherence2);
    }

    for (i = 0; i < nr - 1; i++)
    {
        node_index = (i + 1) * nc;
        /*正向*/
        mean_coherence1 = mean(coherence(Range(i, i + 3), Range(nc - 1, nc + 1))).val[0];
        /*逆向*/
        mean_coherence2 = mean(coherence(Range(i, i + 3), Range(nc - 1, nc + 1))).val[0];
        fprintf(fp, "a %ld %ld %ld %ld %lf\na %ld %ld %ld %ld %lf\n",
            node_index, node_index + nc, lower_bound, upper_bound, mean_coherence1,
            node_index + nc, node_index, lower_bound, upper_bound, mean_coherence2);
    }
    fprintf(fp, "c ");
    fprintf(fp, "c End of file");
    if (fp) fclose(fp);
    fp = NULL;
    return 0;
}

int Utils::cumsum(Mat& phase, int dim)
{
    /*
    cumulates along the dimension specified by dim
    dim = 1,按列计算
    dim = 2,按行计算
    */
    int rows = phase.rows;
    int cols = phase.cols;
    int i, j;
    if (rows > 0 && cols > 0)
    {
        switch (dim)
        {
        case 1:
            if (phase.rows < 2 ||
                phase.cols < 1 ||
                phase.type() != CV_64F)
            {
                fprintf(stderr, "cumsum(): input check failed!\n\n");
                return -1;
            }
            for (j = 0; j < cols; j++)
            {
                for (i = 1; i < rows; i++)
                {
                    phase.at<double>(i, j) = phase.at<double>(i - 1, j) + phase.at<double>(i, j);
                }
            }
            break;
        case 2:
            if (phase.rows < 1 ||
                phase.cols < 2 ||
                phase.type() != CV_64F)
            {
                fprintf(stderr, "cumsum(): input check failed!\n\n");
                return -1;
            }
            for (i = 0; i < rows; i++)
            {
                for (j = 1; j < cols; j++)
                {
                    phase.at<double>(i, j) = phase.at<double>(i, j - 1) + phase.at<double>(i, j);
                }
            }
            break;
        default:
            break;
        }
    }
    return 0;
}

int Utils::cross(Mat& vec1, Mat& vec2, Mat& out)
{
    if (vec1.cols != 3 ||
        vec1.rows < 1 ||
        vec1.type() != CV_64F ||
        vec1.channels() != 1 ||
        vec1.cols != vec2.cols ||
        vec1.rows != vec2.rows ||
        vec2.channels() != 1 ||
        vec2.type() != CV_64F
        )
    {
        fprintf(stderr, "cross(): input check failed!\n\n");
        return -1;
    }
    Mat out_tmp = Mat::zeros(vec1.rows, vec1.cols, CV_64F);
    int rows = vec1.rows;
    for (int i = 0; i < rows; i++)
    {
        out_tmp.at<double>(i, 0) = vec1.at<double>(i, 1) * vec2.at<double>(i, 2) -
            vec1.at<double>(i, 2) * vec2.at<double>(i, 1);//a(2) * b(3) - a(3) * b(2)

        out_tmp.at<double>(i, 1) = vec1.at<double>(i, 2) * vec2.at<double>(i, 0) -
            vec1.at<double>(i, 0) * vec2.at<double>(i, 2);//a(3) * b(1) - a(1) * b(3)

        out_tmp.at<double>(i, 2) = vec1.at<double>(i, 0) * vec2.at<double>(i, 1) -
            vec1.at<double>(i, 1) * vec2.at<double>(i, 0);//a(1) * b(2) - a(2) * b(1)
    }
    out_tmp.copyTo(out);
    return 0;
}

int Utils::gen_mask(Mat& coherence, Mat& phase_derivatives, Mat& mask, int wnd_size, double coh_thresh, double phase_derivative_thresh)
{
    if (coherence.rows < 2 ||
        coherence.cols < 2 ||
        coherence.channels() != 1 ||
        coherence.type() != CV_64F ||
        coherence.rows != phase_derivatives.rows ||
        coherence.cols != phase_derivatives.cols ||
        phase_derivatives.channels() != 1 ||
        phase_derivatives.type() != CV_64F ||
        wnd_size < 0 ||
        wnd_size > coherence.rows ||
        coh_thresh < 0.0 ||
        coh_thresh > 1.0 ||
        phase_derivative_thresh < 0.0
        )
    {
        fprintf(stderr, "gen_mask(): input check failed!\n\n");
        return -1;
    }
    int nr = coherence.rows;
    int nc = coherence.cols;
    Mat temp_coh, temp_phase_derivatives;
    coherence.copyTo(temp_coh);
    phase_derivatives.copyTo(temp_phase_derivatives);
    int radius = (wnd_size - 1) / 2;
    cv::copyMakeBorder(temp_coh, temp_coh, radius, radius, radius, radius, cv::BORDER_DEFAULT);
    cv::copyMakeBorder(temp_phase_derivatives, temp_phase_derivatives, radius, radius, radius, radius, cv::BORDER_DEFAULT);
    Mat tmp = Mat::zeros(nr, nc, CV_32S);
    tmp.copyTo(mask);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        double mean, mean1;
        for (int j = 0; j < nc; j++)
        {
            mean = cv::mean(temp_coh(cv::Range(i, i + 2 * radius + 1), cv::Range(j, j + 2 * radius + 1)))[0];
            mean1 = cv::mean(temp_phase_derivatives(cv::Range(i, i + 2 * radius + 1), cv::Range(j, j + 2 * radius + 1)))[0];
            if (mean > coh_thresh &&
                coherence.at<double>(i, j) > coh_thresh &&
                mean1 < phase_derivative_thresh &&
                phase_derivatives.at<double>(i, j) < phase_derivative_thresh)
            {
                mask.at<int>(i, j) = 1;
            }

        }
    }
    return 0;
}

int Utils::residue_sift(Mat& residue_src, Mat& residue_dst, double thresh, long* num_residue)
{
    int rows = residue_src.rows;
    int cols = residue_src.cols;
    if (rows < 1 ||
        cols < 1 ||
        residue_src.type() != CV_64F ||
        residue_src.channels() != 1 ||
        thresh < 0.0 ||
        num_residue == NULL
        )
    {
        fprintf(stderr, "residue_sift(): input check failed!\n\n");
        return -1;
    }
    Mat tmp;
    residue_src.copyTo(tmp);
    *num_residue = 0;
    //#pragma omp parallel for schedule(guided)
    for (int i = 0; i < rows; i++)
    {
        for (int j = 0; j < cols; j++)
        {
            if (std::fabs(residue_src.at<double>(i, j)) > thresh)
            {
                tmp.at<double>(i, j) = 1.0;
                *num_residue = *num_residue + 1;
            }
            else
            {
                tmp.at<double>(i, j) = 0.0;
            }
        }
    }
    tmp.copyTo(residue_dst);
    return 0;
}

int Utils::wrap(Mat& Src, Mat& Dst)
{
    int rows = Src.rows;
    int cols = Src.cols;
    double pi = 3.1415926535;
    if (rows < 1 || cols < 1 || Src.type() != CV_64F)
    {
        fprintf(stderr, "wrap(): input check failed!\n\n");
        return -1;
    }
    Mat tmp = Mat::zeros(rows, cols, CV_64F);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < rows; i++)
    {
        for (int j = 0; j < cols; j++)
        {
            tmp.at<double>(i, j) = atan2(sin(Src.at<double>(i, j)), cos(Src.at<double>(i, j)));
        }
    }
    //Dst = tmp;
    tmp.copyTo(Dst);
    return 0;
}

int Utils::residue(Mat& phase, Mat& residuemat)
{
    int rows = phase.rows;
    int cols = phase.cols;
    int ret;
    if (rows < 2 || cols < 2 || phase.type() != CV_64F)
    {
        fprintf(stderr, "residue(): input check failed!\n\n");
        return -1;
    }
    Mat Diff_1 = phase(Range(1, rows), Range(0, cols)) - phase(Range(0, rows - 1), Range(0, cols));
    Mat Diff_2 = phase(Range(0, rows), Range(1, cols)) - phase(Range(0, rows), Range(0, cols - 1));
    ret = this->wrap(Diff_1, Diff_1);
    if (return_check(ret, "wrap(*, *)", error_head)) return -1;
    ret = this->wrap(Diff_2, Diff_2);
    if (return_check(ret, "wrap(*, *)", error_head)) return -1;

    Diff_1 = Diff_1(Range(0, Diff_1.rows), Range(1, Diff_1.cols)) -
        Diff_1(Range(0, Diff_1.rows), Range(0, Diff_1.cols - 1));

    Diff_2 = Diff_2(Range(1, Diff_2.rows), Range(0, Diff_2.cols)) -
        Diff_2(Range(0, Diff_2.rows - 1), Range(0, Diff_2.cols));

    Diff_1 = Diff_2 - Diff_1;
    double pi = 3.1415926535;
    Diff_1 = Diff_1 / (2 * pi);
    //Diff_1.copyTo(residuemat);
    residuemat = Diff_1;
    return 0;
}

int Utils::residue(triangle* tri, int num_triangle, vector<tri_node>& nodes, tri_edge* edges, int num_edges)
{
    if (tri == NULL ||
        num_triangle < 1 ||
        nodes.size() < 1 ||
        edges == NULL ||
        num_edges < 3
        )
    {
        fprintf(stderr, "residue(): input check failed!\n\n");
        return -1;
    }
    double thresh = 50.0;
    int num_nodes = nodes.size();
    int end1, end2, end3, tmp;
    double x1, y1, x2, y2, x3, y3, direction, delta12, delta23, delta31, residue, phi1, phi2, phi3, distance1,
        distance2, distance3;
    int row1, col1, row2, col2, row3, col3;
    bool b_res = false;
    for (int i = 0; i < num_triangle; i++)
    {
        if ((tri + i) != NULL)
        {
            end1 = (tri + i)->p1;
            end2 = (tri + i)->p2;
            end3 = (tri + i)->p3;
        }
        if (end1 > end2)
        {
            tmp = end1;
            end1 = end2;
            end2 = tmp;
        }

        nodes[end1 - 1].get_pos(&row1, &col1);
        nodes[end2 - 1].get_pos(&row2, &col2);
        nodes[end3 - 1].get_pos(&row3, &col3);

        nodes[end1 - 1].get_distance(nodes[end2 - 1], &distance1);
        nodes[end2 - 1].get_distance(nodes[end3 - 1], &distance2);
        nodes[end3 - 1].get_distance(nodes[end1 - 1], &distance3);
        b_res = true;
        if ((distance1 > thresh) || (distance2 > thresh) || (distance3 > thresh)) b_res = false;

        nodes[end1 - 1].get_phase(&phi1);
        nodes[end2 - 1].get_phase(&phi2);
        nodes[end3 - 1].get_phase(&phi3);

        x2 = double(col2 - col1);
        y2 = double(row1 - row2);
        x1 = double(col1 - col3);
        y1 = double(row3 - row1);
        direction = x1 * y2 - x2 * y1;

        delta12 = atan2(sin(phi2 - phi1), cos(phi2 - phi1));
        delta23 = atan2(sin(phi3 - phi2), cos(phi3 - phi2));
        delta31 = atan2(sin(phi1 - phi3), cos(phi1 - phi3));

        double res = (delta12 + delta23 + delta31) / 2.0 / PI;
        if (fabs(res) > 0.7 && !b_res)//标注边长超过阈值的残差边和残差节点
        {
            (edges + (tri + i)->edge1 - 1)->isResidueEdge = true;
            (edges + (tri + i)->edge2 - 1)->isResidueEdge = true;
            (edges + (tri + i)->edge3 - 1)->isResidueEdge = true;
            nodes[(tri + i)->p1 - 1].set_residue(true);
            nodes[(tri + i)->p2 - 1].set_residue(true);
            nodes[(tri + i)->p3 - 1].set_residue(true);
        }
        res = b_res ? res : 0.0;
        if (direction > 0.0)//在目标三角形中顺残差方向(残差方向定义为逆时针方向)
        {
            (tri + i)->residue = res;
        }
        else
        {
            (tri + i)->residue = -res;
        }
    }
    return 0;
}

int Utils::residue(vector<triangle>& triangle, vector<tri_node>& nodes, vector<tri_edge>& edges, double distance_thresh)
{
    if (triangle.size() < 1 ||
        nodes.size() < 1 ||
        edges.size() < 3
        )
    {
        fprintf(stderr, "residue(): input check failed!\n\n");
        return -1;
    }
    int num_triangle = triangle.size();
    double thresh;
    thresh = distance_thresh < 2.0 ? 2.0 : distance_thresh;
    int num_nodes = nodes.size();
    int end1, end2, end3, tmp;
    double x1, y1, x2, y2, x3, y3, direction, delta12, delta23, delta31, residue, phi1, phi2, phi3, distance1,
        distance2, distance3;
    int row1, col1, row2, col2, row3, col3;
    bool b_res = false;
    for (int i = 0; i < num_triangle; i++)
    {
        end1 = triangle[i].p1;
        end2 = triangle[i].p2;
        end3 = triangle[i].p3;
        if (end1 > end2)
        {
            tmp = end1;
            end1 = end2;
            end2 = tmp;
        }

        nodes[end1 - 1].get_pos(&row1, &col1);
        nodes[end2 - 1].get_pos(&row2, &col2);
        nodes[end3 - 1].get_pos(&row3, &col3);

        nodes[end1 - 1].get_distance(nodes[end2 - 1], &distance1);
        nodes[end2 - 1].get_distance(nodes[end3 - 1], &distance2);
        nodes[end3 - 1].get_distance(nodes[end1 - 1], &distance3);
        b_res = true;
        if ((distance1 > thresh) || (distance2 > thresh) || (distance3 > thresh)) b_res = false;

        nodes[end1 - 1].get_phase(&phi1);
        nodes[end2 - 1].get_phase(&phi2);
        nodes[end3 - 1].get_phase(&phi3);

        x2 = double(col2 - col1);
        y2 = -double(row1 - row2);
        x1 = double(col1 - col3);
        y1 = -double(row3 - row1);
        direction = x1 * y2 - x2 * y1;

        delta12 = atan2(sin(phi2 - phi1), cos(phi2 - phi1));
        delta23 = atan2(sin(phi3 - phi2), cos(phi3 - phi2));
        delta31 = atan2(sin(phi1 - phi3), cos(phi1 - phi3));

        double res = (delta12 + delta23 + delta31) / 2.0 / PI;
        if (fabs(res) > 0.7 && !b_res)//标注边长超过阈值的残差边和残差节点
        {
            edges[triangle[i].edge1 - 1].isResidueEdge = true;
            edges[triangle[i].edge2 - 1].isResidueEdge = true;
            edges[triangle[i].edge3 - 1].isResidueEdge = true;

            nodes[triangle[i].p1 - 1].set_residue(true);
            nodes[triangle[i].p2 - 1].set_residue(true);
            nodes[triangle[i].p3 - 1].set_residue(true);
        }
        res = b_res ? res : 0.0;
        if (direction < 0.0)//在目标三角形中顺残差方向(残差方向定义为逆时针方向)
        {
            triangle[i].residue = res;
        }
        else
        {
            triangle[i].residue = -res;
        }
    }
    return 0;
}

int Utils::gen_mask(Mat& coherence, Mat& mask, int wnd_size, double thresh)
{
    if (coherence.rows < 2 ||
        coherence.cols < 2 ||
        coherence.channels() != 1 ||
        coherence.type() != CV_64F ||
        wnd_size < 0 ||
        wnd_size > coherence.rows ||
        thresh < 0.0 ||
        thresh > 1.0
        )
    {
        fprintf(stderr, "gen_mask(): input check failed!\n\n");
        return -1;
    }
    int nr = coherence.rows;
    int nc = coherence.cols;
    Mat temp_coh;
    coherence.copyTo(temp_coh);
    int radius = (wnd_size + 1) / 2;
    cv::copyMakeBorder(temp_coh, temp_coh, radius, radius, radius, radius, cv::BORDER_REFLECT);
    Mat tmp = Mat::zeros(nr, nc, CV_32S);
    tmp.copyTo(mask);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        double mean;
        for (int j = 0; j < nc; j++)
        {
            mean = cv::mean(temp_coh(cv::Range(i, i + 2 * radius + 1), cv::Range(j, j + 2 * radius + 1)))[0];
            if (mean > thresh && coherence.at<double>(i, j) > thresh) mask.at<int>(i, j) = 1;
        }
    }
    return 0;
}

int Utils::real_coherence(ComplexMat& Mast, ComplexMat& Slave, Mat& coherence)
{
    int wa = 3;  //窗口方位向尺寸
    int wr = 3;  //窗口距离向尺寸

    int na = Mast.GetRows();
    int nr = Mast.GetCols();
    if ((na < 3) ||
        (nr < 3) ||
        Mast.re.type() != CV_64F ||
        Slave.re.type() != CV_64F ||
        Mast.GetCols() != Slave.GetCols() ||
        Mast.GetRows() != Slave.GetRows())
    {
        fprintf(stderr, "real_coherence(): input check failed!\n\n");
        return -1;
    }

    int win_a = (wa - 1) / 2; //方位窗半径
    int win_r = (wr - 1) / 2; //距离窗半径

    int na_new = na - 2 * win_a;
    int nr_new = nr - 2 * win_r;

    Mat Coherence(na_new, nr_new, CV_64F, Scalar::all(0));


#pragma omp parallel for schedule(guided)
    for (int i = win_a + 1; i <= na - win_a; i++)
    {
        for (int j = win_r + 1; j <= nr - win_r; j++)
        {
            Mat s1, s2, sum1, sum2;

            double up, down;
            magnitude(Mast.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), Mast.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), s1);
            magnitude(Slave.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), Slave.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), s2);
            up = sum((s1.mul(s1)).mul(s2.mul(s2)))[0];
            pow(s1, 4, s1);
            pow(s2, 4, s2);
            down = sqrt(sum(s1)[0] * sum(s2)[0]);
            if (up / (down + 1e-12) > 1.0)
            {
                Coherence.at<double>(i - 1 - win_a, j - 1 - win_r) = 1;
            }
            else
            {
                Coherence.at<double>(i - 1 - win_a, j - 1 - win_r) = up / (down + 1e-12);
            }

        }
    }
    copyMakeBorder(Coherence, Coherence, 1, 1, 1, 1, BORDER_REFLECT);
    coherence = Coherence;
    return 0;
}

int Utils::real_coherence(const ComplexMat& master_image, const ComplexMat& slave_image, int est_wndsize_rg, int est_wndsize_az, Mat& coherence)
{


    int na = master_image.GetRows();
    int nr = master_image.GetCols();
    if ((na < est_wndsize_az) ||
        (nr < est_wndsize_rg) ||
        master_image.type() != CV_64F ||
        slave_image.type() != CV_64F ||
        master_image.GetCols() != slave_image.GetCols() ||
        master_image.GetRows() != slave_image.GetRows() ||
        est_wndsize_rg % 2 == 0 ||
        est_wndsize_az % 2 == 0 ||
        est_wndsize_rg < 3 ||
        est_wndsize_az < 3
        )
    {
        fprintf(stderr, "real_coherence(): input check failed!\n\n");
        return -1;
    }

    int win_a = (est_wndsize_az - 1) / 2; //方位窗半径
    int win_r = (est_wndsize_rg - 1) / 2; //距离窗半径

    int na_new = na - 2 * win_a;
    int nr_new = nr - 2 * win_r;

    Mat Coherence(na_new, nr_new, CV_64F, Scalar::all(0));


#pragma omp parallel for schedule(guided)
    for (int i = win_a + 1; i <= na - win_a; i++)
    {
        for (int j = win_r + 1; j <= nr - win_r; j++)
        {
            Mat s1, s2, sum1, sum2;

            double up, down;
            magnitude(master_image.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), master_image.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), s1);
            magnitude(slave_image.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), slave_image.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)), s2);
            up = sum((s1.mul(s1)).mul(s2.mul(s2)))[0];
            pow(s1, 4, s1);
            pow(s2, 4, s2);
            down = sqrt(sum(s1)[0] * sum(s2)[0]);
            if (up / (down + 1e-12) > 1.0)
            {
                Coherence.at<double>(i - 1 - win_a, j - 1 - win_r) = 1;
            }
            else
            {
                Coherence.at<double>(i - 1 - win_a, j - 1 - win_r) = up / (down + 1e-12);
            }

        }
    }
    copyMakeBorder(Coherence, Coherence, win_a, win_a, win_r, win_r, BORDER_REFLECT);
    Coherence.copyTo(coherence);
    return 0;
}

int Utils::complex_coherence(ComplexMat& Mast, ComplexMat& Slave, Mat& coherence)
{
    int wa = 3;  //窗口方位向尺寸
    int wr = 3;  //窗口距离向尺寸

    int na = Mast.GetRows();
    int nr = Mast.GetCols();

    if ((na < 3) ||
        (nr < 3) ||
        Mast.re.type() != CV_64F ||
        Slave.re.type() != CV_64F ||
        Mast.GetCols() != Slave.GetCols() ||
        Mast.GetRows() != Slave.GetRows())
    {
        fprintf(stderr, "complex_coherence(): input check failed!\n\n");
        return -1;
    }

    int win_a = (wa - 1) / 2; //方位窗半径
    int win_r = (wr - 1) / 2; //距离窗半径

    int na_new = na - 2 * win_a;
    int nr_new = nr - 2 * win_r;

    Mat Coherence(na_new, nr_new, CV_64F, Scalar::all(0));
#pragma omp parallel for schedule(guided)
    for (int i = win_a + 1; i <= na - win_a; i++)
    {
        for (int j = win_r + 1; j <= nr - win_r; j++)
        {
            Mat planes_master[] = { Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F), Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F) };
            Mat planes_slave[] = { Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F), Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F) };
            Mat planes[] = { Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F), Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F) };
            Mat s1, s2;
            double up, down, sum1, sum2;
            Mast.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_master[0]);
            Mast.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_master[1]);

            Slave.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_slave[0]);
            Slave.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_slave[1]);

            merge(planes_master, 2, s1);
            merge(planes_slave, 2, s2);
            mulSpectrums(s1, s2, s1, 0, true);
            split(s1, planes);
            sum1 = sum(planes[0])[0];
            sum2 = sum(planes[1])[0];
            up = sqrt(sum1 * sum1 + sum2 * sum2);
            magnitude(planes_master[0], planes_master[1], planes_master[0]);
            magnitude(planes_slave[0], planes_slave[1], planes_slave[0]);
            sum1 = sum(planes_master[0].mul(planes_master[0]))[0];
            sum2 = sum(planes_slave[0].mul(planes_slave[0]))[0];
            down = sqrt(sum1 * sum2);
            Coherence.at<double>(i - 1 - win_a, j - 1 - win_r) = up / (down + 0.0000001);
        }
    }
    copyMakeBorder(Coherence, Coherence, 1, 1, 1, 1, BORDER_REFLECT);
    coherence = Coherence;
    return 0;
}

int Utils::complex_coherence(
    const ComplexMat& master_image,
    const ComplexMat& slave_image,
    int est_wndsize_rg,
    int est_wndsize_az,
    Mat& coherence
)
{
    int na = master_image.GetRows();
    int nr = master_image.GetCols();

    if ((na < est_wndsize_az) ||
        (nr < est_wndsize_rg) ||
        master_image.type() != CV_64F ||
        slave_image.type() != CV_64F ||
        master_image.GetCols() != slave_image.GetCols() ||
        master_image.GetRows() != slave_image.GetRows() ||
        est_wndsize_az % 2 == 0 ||
        est_wndsize_rg % 2 == 0 ||
        est_wndsize_rg < 3 ||
        est_wndsize_az < 3
        )
    {
        fprintf(stderr, "complex_coherence(): input check failed!\n\n");
        return -1;
    }

    int win_a = (est_wndsize_az - 1) / 2; //方位窗半径
    int win_r = (est_wndsize_rg - 1) / 2; //距离窗半径

    int na_new = na - 2 * win_a;
    int nr_new = nr - 2 * win_r;

    Mat Coherence(na_new, nr_new, CV_64F, Scalar::all(0));
#pragma omp parallel for schedule(guided)
    for (int i = win_a + 1; i <= na - win_a; i++)
    {
        for (int j = win_r + 1; j <= nr - win_r; j++)
        {
            Mat planes_master[] = { Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F), Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F) };
            Mat planes_slave[] = { Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F), Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F) };
            Mat planes[] = { Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F), Mat::zeros(2 * win_a + 1, 2 * win_r + 1, CV_64F) };
            Mat s1, s2;
            double up, down, sum1, sum2;
            master_image.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_master[0]);
            master_image.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_master[1]);

            slave_image.re(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_slave[0]);
            slave_image.im(Range(i - 1 - win_a, i + win_a), Range(j - 1 - win_r, j + win_r)).copyTo(planes_slave[1]);

            merge(planes_master, 2, s1);
            merge(planes_slave, 2, s2);
            mulSpectrums(s1, s2, s1, 0, true);
            split(s1, planes);
            sum1 = sum(planes[0])[0];
            sum2 = sum(planes[1])[0];
            up = sqrt(sum1 * sum1 + sum2 * sum2);
            magnitude(planes_master[0], planes_master[1], planes_master[0]);
            magnitude(planes_slave[0], planes_slave[1], planes_slave[0]);
            sum1 = sum(planes_master[0].mul(planes_master[0]))[0];
            sum2 = sum(planes_slave[0].mul(planes_slave[0]))[0];
            down = sqrt(sum1 * sum2);
            Coherence.at<double>(i - 1 - win_a, j - 1 - win_r) = up / (down + 0.0000001);
        }
    }
    copyMakeBorder(Coherence, Coherence, win_a, win_a, win_r, win_r, BORDER_REFLECT);
    Coherence.copyTo(coherence);
    return 0;
}

int Utils::phase_coherence(Mat& phase, Mat& coherence)
{
    if (phase.rows < 3 ||
        phase.cols < 3 ||
        phase.type() != CV_64F ||
        phase.channels() != 1)
    {
        fprintf(stderr, "phase_coherence(): input check failed!\n\n");
        return -1;
    }
    ComplexMat master, slave;
    Mat cos, sin;
    int ret;
    ret = this->phase2cos(phase, cos, sin);
    if (return_check(ret, "phase2cos(*, *, *)", error_head)) return -1;
    master.SetRe(cos);
    slave.SetRe(cos);
    master.SetIm(sin);
    sin = -sin;
    slave.SetIm(sin);
    ret = this->complex_coherence(master, slave, coherence);
    if (return_check(ret, "complex_coherence(*, *, *)", error_head)) return -1;
    return 0;
}

int Utils::phase_coherence(const Mat& phase, int est_wndsize_rg, int est_wndsize_az, Mat& coherence)
{
    if (phase.rows < 3 ||
        phase.cols < 3 ||
        phase.type() != CV_64F ||
        phase.channels() != 1 ||
        est_wndsize_rg % 2 == 0 ||
        est_wndsize_az % 2 == 0
        )
    {
        fprintf(stderr, "phase_coherence(): input check failed!\n\n");
        return -1;
    }
    ComplexMat master, slave;
    Mat cos, sin;
    int ret;
    ret = phase2cos(phase, cos, sin);
    if (return_check(ret, "phase2cos(*, *, *)", error_head)) return -1;
    master.SetRe(cos);
    slave.SetRe(cos);
    master.SetIm(sin);
    sin = -sin;
    slave.SetIm(sin);
    ret = complex_coherence(master, slave, est_wndsize_rg, est_wndsize_az, coherence);
    if (return_check(ret, "complex_coherence(*, *, *)", error_head)) return -1;
    return 0;
}

int Utils::phase_derivatives_variance(Mat& phase, Mat& phase_derivatives_variance, int wndsize)
{
    if (phase.cols < 2 ||
        phase.rows < 2 ||
        phase.type() != CV_64F ||
        phase.channels() != 1 ||
        wndsize < 3 ||
        wndsize % 2 == 0
        )
    {
        fprintf(stderr, "phase_derivatives_variance(): input check failed!\n\n");
        return -1;
    }

    int nr = phase.rows;
    int nc = phase.cols;
    if (wndsize > int(nr / 10) || wndsize > int(nc / 10)) wndsize = 3;
    wndsize = (wndsize - 1) / 2;
    int ret;
    phase.copyTo(phase_derivatives_variance);
    Mat derivative_row, derivative_col;
    derivative_row = phase(Range(1, nr), Range(0, nc)) - phase(Range(0, nr - 1), Range(0, nc));
    derivative_col = phase(Range(0, nr), Range(1, nc)) - phase(Range(0, nr), Range(0, nc - 1));
    ret = wrap(derivative_row, derivative_row);
    if (return_check(ret, "wrap(*, *)", error_head)) return -1;
    ret = wrap(derivative_col, derivative_col);
    if (return_check(ret, "wrap(*, *)", error_head)) return -1;
    copyMakeBorder(derivative_col, derivative_col, 0, 1, 0, 1, BORDER_DEFAULT);
    copyMakeBorder(derivative_row, derivative_row, 0, 1, 0, 1, BORDER_DEFAULT);
    copyMakeBorder(derivative_col, derivative_col, wndsize, wndsize, wndsize, wndsize, BORDER_DEFAULT);
    copyMakeBorder(derivative_row, derivative_row, wndsize, wndsize, wndsize, wndsize, BORDER_DEFAULT);
    //cv::meanStdDev()
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        double std1, std2, mean1, mean2;
        Mat tmp1, tmp2;
        for (int j = 0; j < nc; j++)
        {
            mean1 = cv::mean(derivative_row(Range(i, i + 2 * wndsize + 1), Range(j, j + 2 * wndsize + 1)))[0];
            mean2 = cv::mean(derivative_col(Range(i, i + 2 * wndsize + 1), Range(j, j + 2 * wndsize + 1)))[0];
            tmp1 = derivative_row(Range(i, i + 2 * wndsize + 1), Range(j, j + 2 * wndsize + 1)) - mean1;
            tmp2 = derivative_col(Range(i, i + 2 * wndsize + 1), Range(j, j + 2 * wndsize + 1)) - mean2;
            tmp1 = tmp1.mul(tmp1);
            tmp2 = tmp2.mul(tmp2);
            std1 = sum(tmp1)[0];
            std2 = sum(tmp2)[0];
            phase_derivatives_variance.at<double>(i, j) = (sqrt(std1) + sqrt(std2)) / ((2 * wndsize + 1) * (2 * wndsize + 1));
        }
    }
    return 0;
}

int Utils::max_integrable_distance(Mat& phase, Mat& max_integrable_distance, double conservative_thresh)
{
    if (phase.rows < 5 ||
        phase.cols < 5 ||
        phase.channels() != 1 ||
        phase.type() != CV_64F ||
        conservative_thresh < 1.41
        )
    {
        fprintf(stderr, "max_integrable_distance(): input check failed!\n\n");
        return -1;
    }
    int nr = phase.rows;
    int nc = phase.cols;
    max_integrable_distance = Mat::ones(nr, nc, CV_64F);
    max_integrable_distance = max_integrable_distance * 1.5;
#pragma omp parallel for schedule(guided)
    for (int i = 1; i < nr - 1; i++)
    {
        for (int j = 1; j < nc - 1; j++)
        {
            double max = -1.0;
            double delta, ph0;
            ph0 = phase.at<double>(i, j);

            delta = phase.at<double>(i - 1, j - 1) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.414;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i - 1, j) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.0;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i - 1, j + 1) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.414;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i, j - 1) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.0;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i, j + 1) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.0;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i + 1, j - 1) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.414;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i + 1, j) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.0;
            max = max > delta ? max : delta;

            delta = phase.at<double>(i + 1, j + 1) - ph0;
            delta = fabs(atan2(sin(delta), cos(delta))) / 1.414;
            max = max > delta ? max : delta;

            if (max < 1.0 / conservative_thresh * PI)
            {
                max = 1.0 / conservative_thresh * PI;
            }
            max_integrable_distance.at<double>(i, j) = PI / max < 1.5 ? 1.5 : PI / max;
        }
    }

    return 0;
}

int Utils::fftshift(Mat& mag)
{
    // rearrange the quadrants of Fourier image
    // so that the origin is at the image center
    if (mag.rows < 2 ||
        mag.cols < 2 ||
        mag.channels() != 1)
    {
        fprintf(stderr, "fftshift(): input check failed!\n\n");
        return -1;
    }
    mag = mag(Rect(0, 0, mag.cols & -2, mag.rows & -2));
    int cx = mag.cols / 2;
    int cy = mag.rows / 2;
    Mat tmp;
    Mat q0(mag, Rect(0, 0, cx, cy));
    Mat q1(mag, Rect(cx, 0, cx, cy));
    Mat q2(mag, Rect(0, cy, cx, cy));
    Mat q3(mag, Rect(cx, cy, cx, cy));

    q0.copyTo(tmp);
    q3.copyTo(q0);
    tmp.copyTo(q3);

    q1.copyTo(tmp);
    q2.copyTo(q1);
    tmp.copyTo(q2);
    return 0;
}

int Utils::read_DIMACS(const char* DIMACS_file_solution, tri_edge* edges, int num_edges, vector<tri_node>& nodes, triangle* tri, int num_triangle)
{
    if (DIMACS_file_solution == NULL ||
        edges == NULL ||
        num_edges < 3 ||
        nodes.size() < 3 ||
        tri == NULL ||
        num_triangle < 1
        )
    {
        fprintf(stderr, "read_DIMACS(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(DIMACS_file_solution, "rt");
    if (fp == NULL)
    {
        fprintf(stderr, "read_DIMACS(): can't open %s \n", DIMACS_file_solution);
        return -1;
    }

    char instring[256];
    char ch;
    double obj_value = 0;
    int i, tmp, end1, end2, end3, row1, col1, row2, col2, row3, col3;
    int end[3];
    double x1, y1, x2, y2, direction;
    long from, to;
    double flow = 0;
    bool flag;
    int x[3];
    int y[3];
    long* ptr_neigh = NULL;
    int num_neigh, target_edges;
    int num_nodes = nodes.size();
    /////////////////////读取注释///////////////////////////
    GET_NEXT_LINE;
    while (ch != 's' && ch)
    {
        if (ch != 'c')
        {
            if (fp) fclose(fp);
            fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
            return -1;
        }
        GET_NEXT_LINE;
    }
    /////////////////////读取优化目标值/////////////////////
    for (i = 1; i < 81; i++)
    {
        if (isspace((int)instring[i]) > 0)
        {
            i++;
            break;
        }
    }
    if (sscanf(&(instring[i]), "%lf", &obj_value) != 1)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
        return -1;
    }
    if (obj_value < 0.0)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): this problem can't be solved(unbounded or infeasible)!\n\n");
        return -1;
    }
    ////////////////////////读取MCF结果////////////////////////
    GET_NEXT_LINE;
    while (ch && ch == 'f')
    {
        if (sscanf(&(instring[2]), "%ld %ld %lf", &from, &to, &flow) != 3 ||
            flow < 0.0 || from < 0 || to < 0)
        {
            if (fp) fclose(fp);
            fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
            return -1;
        }

        if (from > 0 &&
            from <= num_triangle &&
            to > 0 &&
            to <= num_triangle)
        {
            if (/*from > 0 &&
                from <= num_triangle &&
                to > 0 &&
                to <= num_triangle &&*/
                (tri + from - 1) != NULL &&
                (tri + to - 1) != NULL
                )
            {
                end[0] = -1;
                end[1] = -1;
                x[0] = (tri + from - 1)->p1;
                x[1] = (tri + from - 1)->p2;
                x[2] = (tri + from - 1)->p3;
                y[0] = (tri + to - 1)->p1;
                y[1] = (tri + to - 1)->p2;
                y[2] = (tri + to - 1)->p3;
                i = 0; tmp = 0; flag = false;
                while (end[0] == -1 || end[1] == -1)
                {
                    if (i > 2)
                    {
                        if (fp) fclose(fp);
                        fprintf(stderr, "read_DIMACS(): illegal Delaunay triangle!\n\n");
                        return -1;
                    }
                    for (int j = 0; j < 3; j++)
                    {
                        if (x[i] == y[j])
                        {
                            end[tmp] = x[i];
                            tmp++;
                            if (i == 0) flag = true;
                            break;
                        }
                    }
                    i++;

                }
            }
            if (i == 3)
            {
                if (flag) end[2] = x[1];
                else
                {
                    end[2] = x[0];
                }
            }
            else
            {
                end[2] = x[2];
            }
            if (end[0] > end[1])
            {
                end1 = end[1];
                end2 = end[0];
            }
            else
            {
                end1 = end[0];
                end2 = end[1];
            }
            end3 = end[2];




            if (end1 > 0 && end1 <= num_nodes && end2 > 0 && end2 <= num_nodes && end3 > 0 && end3 <= num_nodes)
            {
                //找到边序号target_edges
                nodes[end1 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
                for (i = 0; i < num_neigh; i++)
                {
                    if ((ptr_neigh + i) != NULL && *(ptr_neigh + i) > 0 && *(ptr_neigh + i) <= num_edges)
                    {
                        if ((edges + *(ptr_neigh + i) - 1)->end1 == end2 || (edges + *(ptr_neigh + i) - 1)->end2 == end2)
                        {
                            target_edges = *(ptr_neigh + i);
                        }
                    }
                }

                nodes[end1 - 1].get_pos(&row1, &col1);
                nodes[end2 - 1].get_pos(&row2, &col2);
                nodes[end3 - 1].get_pos(&row3, &col3);
                x1 = double(col1 - col3);
                y1 = double(row3 - row1);
                x2 = double(col2 - col1);
                y2 = double(row1 - row2);
                direction = x1 * y2 - x2 * y1;
                if (direction > 0.0)//在目标三角形中顺残差方向
                {
                    (edges + target_edges - 1)->gain = flow;
                }
                else
                {
                    (edges + target_edges - 1)->gain = -flow;
                }
            }
        }
        GET_NEXT_LINE;
    }

    if (fp)
    {
        fclose(fp);
        fp = NULL;
    }
    return 0;
}

int Utils::read_DIMACS(
    const char* DIMACS_file_solution,
    vector<tri_edge>& edges,
    vector<tri_node>& nodes,
    vector<triangle>& triangle
)
{
    if (DIMACS_file_solution == NULL ||
        edges.size() < 3 ||
        nodes.size() < 3 ||
        triangle.size() < 1
        )
    {
        fprintf(stderr, "read_DIMACS(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(DIMACS_file_solution, "rt");
    if (fp == NULL)
    {
        fprintf(stderr, "read_DIMACS(): can't open %s \n", DIMACS_file_solution);
        return -1;
    }

    char instring[256];
    char ch;
    double obj_value = 0;
    int i, tmp, end1, end2, end3, row1, col1, row2, col2, row3, col3;
    int end[3];
    double x1, y1, x2, y2, direction;
    long from, to;
    double flow = 0;
    bool flag;
    int x[3];
    int y[3];
    long* ptr_neigh = NULL;
    int num_neigh, target_edges;
    int num_nodes = nodes.size();
    int num_triangle = triangle.size(); int num_edges = edges.size();
    /////////////////////读取注释///////////////////////////
    GET_NEXT_LINE;
    while (ch != 's' && ch)
    {
        if (ch != 'c')
        {
            if (fp) fclose(fp);
            fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
            return -1;
        }
        GET_NEXT_LINE;
    }
    /////////////////////读取优化目标值/////////////////////
    for (i = 1; i < 81; i++)
    {
        if (isspace((int)instring[i]) > 0)
        {
            i++;
            break;
        }
    }
    if (sscanf(&(instring[i]), "%lf", &obj_value) != 1)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
        return -1;
    }
    if (obj_value < 0.0)
    {
        if (fp) fclose(fp);
        fprintf(stderr, "read_DIMACS(): this problem can't be solved(unbounded or infeasible)!\n\n");
        return -1;
    }
    ////////////////////////读取MCF结果////////////////////////
    GET_NEXT_LINE;
    while (ch && ch == 'f')
    {
        if (sscanf(&(instring[2]), "%ld %ld %lf", &from, &to, &flow) != 3 ||
            flow < 0.0 || from < 0 || to < 0)
        {
            if (fp) fclose(fp);
            fprintf(stderr, "read_DIMACS(): unknown file format!\n\n");
            return -1;
        }
        //非接地边
        if (from > 0 &&
            from <= num_triangle &&
            to > 0 &&
            to <= num_triangle)
        {
            /////////寻找两个三角形的公共边//////////////
            {
                end[0] = -1;
                end[1] = -1;
                x[0] = triangle[from - 1].p1;
                x[1] = triangle[from - 1].p2;
                x[2] = triangle[from - 1].p3;
                y[0] = triangle[to - 1].p1;
                y[1] = triangle[to - 1].p2;
                y[2] = triangle[to - 1].p3;
                i = 0; tmp = 0; flag = false;
                while (end[0] == -1 || end[1] == -1)
                {
                    if (i > 2)
                    {
                        if (fp) fclose(fp);
                        fprintf(stderr, "read_DIMACS(): illegal Delaunay triangle!\n\n");
                        return -1;
                    }
                    for (int j = 0; j < 3; j++)
                    {
                        if (x[i] == y[j])
                        {
                            end[tmp] = x[i];
                            tmp++;
                            if (i == 0) flag = true;
                            break;
                        }
                    }
                    i++;

                }
            }
            if (i == 3)
            {
                if (flag) end[2] = x[1];
                else
                {
                    end[2] = x[0];
                }
            }
            else
            {
                end[2] = x[2];
            }
            if (end[0] > end[1])
            {
                end1 = end[1];
                end2 = end[0];
            }
            else
            {
                end1 = end[0];
                end2 = end[1];
            }
            end3 = end[2];
            /////////寻找两个三角形的公共边//////////////



            if (end1 > 0 && end1 <= num_nodes && end2 > 0 && end2 <= num_nodes && end3 > 0 && end3 <= num_nodes)
            {
                //找到边序号target_edges
                nodes[end1 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
                for (i = 0; i < num_neigh; i++)
                {
                    if ((ptr_neigh + i) != NULL && *(ptr_neigh + i) > 0 && *(ptr_neigh + i) <= num_edges)
                    {
                        if (edges[*(ptr_neigh + i) - 1].end1 == end2 || edges[*(ptr_neigh + i) - 1].end2 == end2)
                        {
                            target_edges = *(ptr_neigh + i);
                        }
                    }
                }

                nodes[end1 - 1].get_pos(&row1, &col1);
                nodes[end2 - 1].get_pos(&row2, &col2);
                nodes[end3 - 1].get_pos(&row3, &col3);
                x1 = double(col1 - col3);
                y1 = -double(row3 - row1);
                x2 = double(col2 - col1);
                y2 = -double(row1 - row2);
                direction = x1 * y2 - x2 * y1;
                if (direction < 0.0)//在目标三角形中顺残差方向
                {
                    edges[target_edges - 1].gain = -flow;
                }
                else
                {
                    edges[target_edges - 1].gain = flow;
                }
            }
        }
        //接地边
        if (from == num_triangle + 1 || to == num_triangle + 1)
        {
            if (from == num_triangle + 1)
            {
                if (edges[triangle[to - 1].edge1 - 1].isBoundry)target_edges = triangle[to - 1].edge1;
                else if (edges[triangle[to - 1].edge2 - 1].isBoundry) target_edges = triangle[to - 1].edge2;
                else target_edges = triangle[to - 1].edge3;


                if (edges[target_edges - 1].end1 > edges[target_edges - 1].end2)
                {
                    end1 = edges[target_edges - 1].end2;
                    end2 = edges[target_edges - 1].end1;
                }
                else
                {
                    end1 = edges[target_edges - 1].end1;
                    end2 = edges[target_edges - 1].end2;
                }

                if (triangle[to - 1].p1 != end1 && triangle[to - 1].p1 != end2) end3 = triangle[to - 1].p1;
                else if (triangle[to - 1].p2 != end1 && triangle[to - 1].p2 != end2) end3 = triangle[to - 1].p2;
                else end3 = triangle[to - 1].p3;

                nodes[end1 - 1].get_pos(&row1, &col1);
                nodes[end2 - 1].get_pos(&row2, &col2);
                nodes[end3 - 1].get_pos(&row3, &col3);
                x1 = double(col1 - col3);
                y1 = -double(row3 - row1);
                x2 = double(col2 - col1);
                y2 = -double(row1 - row2);
                direction = x1 * y2 - x2 * y1;
                if (direction < 0.0)//在目标三角形中顺残差方向
                {
                    edges[target_edges - 1].gain = flow;
                }
                else
                {
                    edges[target_edges - 1].gain = -flow;
                }
            }
            else
            {
                if (edges[triangle[from - 1].edge1 - 1].isBoundry)target_edges = triangle[from - 1].edge1;
                else if (edges[triangle[from - 1].edge2 - 1].isBoundry) target_edges = triangle[from - 1].edge2;
                else target_edges = triangle[from - 1].edge3;


                if (edges[target_edges - 1].end1 > edges[target_edges - 1].end2)
                {
                    end1 = edges[target_edges - 1].end2;
                    end2 = edges[target_edges - 1].end1;
                }
                else
                {
                    end1 = edges[target_edges - 1].end1;
                    end2 = edges[target_edges - 1].end2;
                }

                if (triangle[from - 1].p1 != end1 && triangle[from - 1].p1 != end2) end3 = triangle[from - 1].p1;
                else if (triangle[from - 1].p2 != end1 && triangle[from - 1].p2 != end2) end3 = triangle[from - 1].p2;
                else end3 = triangle[from - 1].p3;

                nodes[end1 - 1].get_pos(&row1, &col1);
                nodes[end2 - 1].get_pos(&row2, &col2);
                nodes[end3 - 1].get_pos(&row3, &col3);
                x1 = double(col1 - col3);
                y1 = -double(row3 - row1);
                x2 = double(col2 - col1);
                y2 = -double(row1 - row2);
                direction = x1 * y2 - x2 * y1;
                if (direction < 0.0)//在目标三角形中顺残差方向
                {
                    edges[target_edges - 1].gain = -flow;
                }
                else
                {
                    edges[target_edges - 1].gain = flow;
                }
            }
        }
        GET_NEXT_LINE;
    }

    if (fp)
    {
        fclose(fp);
        fp = NULL;
    }
    return 0;
}

int Utils::cvmat2bin(const char* filename, Mat& mat)
{
    int nr = mat.rows;
    int nc = mat.cols;
    if (nr < 1 || nc < 1 || mat.type() != CV_64F || mat.channels() != 1)
    {
        fprintf(stderr, "cvmat2bin(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(filename, "wb");
    if (fp == NULL)
    {
        fprintf(stderr, "can't open file: %s\n", filename);
        return -1;
    }
    fwrite(&nr, sizeof(int), 1, fp);
    fwrite(&nc, sizeof(int), 1, fp);
    fwrite((double*)mat.data, sizeof(double), nr * nc, fp);
    if (fp != NULL) fclose(fp);
    return 0;
}

int Utils::bin2cvmat(const char* filename, Mat& dst)
{

    FILE* fp = NULL;
    fp = fopen(filename, "rb");
    if (fp == NULL)
    {
        fprintf(stderr, "can't open file: %s\n", filename);
        return -1;
    }
    int rows, cols;
    fread(&rows, sizeof(int), 1, fp);
    fread(&cols, sizeof(int), 1, fp);
    if (rows < 1 || cols < 1)
    {
        fprintf(stderr, "unknown file format!\n");
        if (fp) fclose(fp);
        return -1;
    }
    Mat matrix(rows, cols, CV_64F, cv::Scalar::all(0));
    double* p = (double*)malloc(sizeof(double) * rows * cols);
    if (p == NULL)
    {
        fprintf(stderr, "failed to allocate memory for reading data from %s!\n", filename);
        if (fp) fclose(fp);
        return -1;
    }
    fread(p, sizeof(double), rows * cols, fp);
    std::memcpy(matrix.data, p, sizeof(double) * rows * cols);
    if (p != NULL) free(p);
    if (fp != NULL) fclose(fp);
    dst = matrix;
    return 0;
}

int Utils::multilook(ComplexMat& Master, ComplexMat& Slave, Mat& phase, int multilook_times)
{
    if (Master.GetRows() != Slave.GetRows() ||
        Master.GetCols() != Slave.GetCols() ||
        Master.type() != CV_64F ||
        Slave.type() != CV_64F ||
        Master.GetRows() < 1 ||
        Master.GetCols() < 1 ||
        multilook_times < 1 ||
        Master.GetRows() < multilook_times ||
        Master.GetCols() < multilook_times)
    {
        fprintf(stderr, "multilook(): input check failed!\n\n");
        return -1;
    }
    int ret;
    if (multilook_times == 1)
    {
        ret = generate_phase(Master, Slave, phase);
        if (return_check(ret, "generate_phase(*, *, *)", error_head)) return -1;
        return 0;
    }
    ComplexMat tmp;
    ret = Master.Mul(Slave, tmp, true);
    if (return_check(ret, "Master.Mul(*, *, *)", error_head)) return -1;
    int nr = tmp.GetRows();
    int nc = tmp.GetCols();
    nr = (nr - (nr % multilook_times)) / multilook_times;
    nc = (nc - (nc % multilook_times)) / multilook_times;
    Mat real = Mat::zeros(nr, nc, CV_64F);
    Mat imag = Mat::zeros(nr, nc, CV_64F);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        for (int j = 0; j < nc; j++)
        {
            real.at<double>(i, j) = cv::mean(tmp.re(Range(i * multilook_times, (i + 1) * multilook_times),
                Range(j * multilook_times, (j + 1) * multilook_times)))[0];
            imag.at<double>(i, j) = cv::mean(tmp.im(Range(i * multilook_times, (i + 1) * multilook_times),
                Range(j * multilook_times, (j + 1) * multilook_times)))[0];
        }
    }
    tmp.SetRe(real);
    tmp.SetIm(imag);
    phase = tmp.GetPhase();
    return 0;
}

int Utils::multilook(const ComplexMat& master, const ComplexMat& slave, int multilook_rg, int multilook_az, Mat& phase)
{
    if (master.GetRows() != slave.GetRows() ||
        master.GetCols() != slave.GetCols() ||
        master.type() != CV_64F ||
        slave.type() != CV_64F ||
        master.GetRows() < 1 ||
        master.GetCols() < 1 ||
        multilook_rg < 1 ||
        multilook_az < 1 ||
        master.GetRows() < multilook_az ||
        master.GetCols() < multilook_rg)
    {
        fprintf(stderr, "multilook(): input check failed!\n\n");
        return -1;
    }
    int ret;
    if (multilook_rg == 1 && multilook_az == 1)
    {
        ret = generate_phase(master, slave, phase);
        if (return_check(ret, "generate_phase(*, *, *)", error_head)) return -1;
        return 0;
    }
    ComplexMat tmp;
    ret = master.Mul(slave, tmp, true);
    if (return_check(ret, "Master.Mul(*, *, *)", error_head)) return -1;
    int nr = tmp.GetRows();
    int nc = tmp.GetCols();
    int radius_rg = multilook_rg / 2;
    int radius_az = multilook_az / 2;
    Mat real = Mat::zeros(nr, nc, CV_64F);
    Mat imag = Mat::zeros(nr, nc, CV_64F);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        int left, right, bottom, top;
        for (int j = 0; j < nc; j++)
        {
            left = j - radius_rg; left = left < 0 ? 0 : left;
            right = left + multilook_rg; right = right > nc - 1 ? nc - 1 : right;
            top = i - radius_az; top = top < 0 ? 0 : top;
            bottom = top + multilook_az; bottom = bottom > nr - 1 ? nr - 1 : bottom;
            real.at<double>(i, j) = cv::mean(tmp.re(Range(top, bottom + 1), Range(left, right + 1)))[0];
            imag.at<double>(i, j) = cv::mean(tmp.im(Range(top, bottom + 1), Range(left, right + 1)))[0];
        }
    }
    tmp.SetRe(real);
    tmp.SetIm(imag);
    tmp.GetPhase().copyTo(phase);
    return 0;
}

int Utils::phase2cos(const Mat& phase, Mat& cos, Mat& sin)
{
    if (phase.rows < 1 ||
        phase.cols < 1 ||
        phase.type() != CV_64F ||
        phase.channels() != 1)
    {
        fprintf(stderr, "phase2cos(): input check failed!\n\n");
        return -1;
    }
    int nr = phase.rows;
    int nc = phase.cols;
    Mat Cos(nr, nc, CV_64F);
    Mat Sin(nr, nc, CV_64F);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        for (int j = 0; j < nc; j++)
        {
            Cos.at<double>(i, j) = std::cos(phase.at<double>(i, j));
            Sin.at<double>(i, j) = std::sin(phase.at<double>(i, j));
        }
    }
    Cos.copyTo(cos);
    Sin.copyTo(sin);
    return 0;
}

int Utils::xyz2ell(Mat xyz, Mat& llh)
{
    if (xyz.rows != 1 ||
        xyz.cols != 3 ||
        xyz.type() != CV_64F ||
        xyz.channels() != 1)
    {
        fprintf(stderr, "xyz2ell(): input check failed!\n\n");
        return -1;
    }
    double x = xyz.at<double>(0, 0);
    double y = xyz.at<double>(0, 1);
    double z = xyz.at<double>(0, 2);
    double Rad_earth_e = 6378136.49;
    double f = 1 / 298.257223563;
    double t = Rad_earth_e * (1 - f);
    double e = sqrt((Rad_earth_e * Rad_earth_e - t * t) / (Rad_earth_e * Rad_earth_e));
    double r = x * x + y * y;
    if (fabs(r) < 1e-15)
    {
        r = 1e-14;
    }
    if (fabs(x) < 1e-15)
    {
        x = 1e-14;
    }
    double lat = atan(z / r);
    double lon = atan(y / x);
    double N, height;
    double tmp = 0.0;
    double pi = 3.1415926535;
    for (int k = 0; k < 10; k++)
    {
        tmp = (sqrt(1 - e * e * sin(lat) * sin(lat)));
        if (fabs(tmp) < 1e-15)
        {
            tmp = 1e-14;
        }
        N = Rad_earth_e / tmp;
        tmp = sin(lat);
        if (fabs(tmp) < 1e-15)
        {
            tmp = 1e-14;
        }
        height = z / tmp - N * (1 - e * e);
        tmp = (sqrt(x * x + y * y) * (N * (1 - e * e) + height));
        if (fabs(tmp) < 1e-15)
        {
            tmp = 1e-14;
        }
        lat = atan(z * (N + height) / tmp);
    }
    if (x > 0 && y < 0)
    {
        lon = -lon;
    }
    else if (x < 0 && y > 0)
    {
        lon = pi + lon;
    }
    else if (x < 0 && y < 0)
    {
        lon = -pi + lon;
    }
    lat = lat * 180 / pi;
    lon = lon * 180 / pi;
    llh = Mat::zeros(1, 3, CV_64F);
    llh.at<double>(0, 0) = lat;
    llh.at<double>(0, 1) = lon;
    llh.at<double>(0, 2) = height;
    return 0;
}

int Utils::ell2xyz(Mat llh, Mat& xyz)
{
    if (llh.cols != 3 ||
        llh.rows != 1 ||
        llh.type() != CV_64F ||
        llh.channels() != 1
        )
    {
        fprintf(stderr, "ell2xyz(): input check failed!\n\n");
        return -1;
    }
    double e2 = 0.00669438003551279091;
    double lat = llh.at<double>(0, 0);
    double lon = llh.at<double>(0, 1);
    double height = llh.at<double>(0, 2);
    lat = lat / 180.0 * PI;
    lon = lon / 180.0 * PI;
    double Ea = 6378136.49;
    double N = Ea / sqrt(1 - e2 * (sin(lat) * sin(lat)));
    double Nph = N + height;
    double x = Nph * cos(lat) * cos(lon);
    double y = Nph * cos(lat) * sin(lon);
    double z = (Nph - e2 * N) * sin(lat);
    Mat tmp = Mat::zeros(1, 3, CV_64F);
    tmp.at<double>(0, 0) = x;
    tmp.at<double>(0, 1) = y;
    tmp.at<double>(0, 2) = z;
    tmp.copyTo(xyz);
    return 0;
}

int Utils::saveSLC(const char* filename, double db, ComplexMat& SLC)
{
    if (filename == NULL ||
        db < 0 ||
        SLC.GetRows() < 1 ||
        SLC.GetCols() < 1 /*||
        SLC.type() != CV_64F*/)
    {
        fprintf(stderr, "saveSLC(): input check failed!\n\n");
        return -1;
    }
    ComplexMat tmp;
    Mat mod;
    if (SLC.type() != CV_64F && SLC.type() != CV_32F)
    {
        SLC.re.convertTo(tmp.re, CV_32F);
        SLC.im.convertTo(tmp.im, CV_32F);
        mod = tmp.GetMod();
    }
    else
    {
        mod = SLC.GetMod();
    }
    int nr = mod.rows;
    int nc = mod.cols;
    double max, min;
    if (SLC.type() == CV_64F)
    {
#pragma omp parallel for schedule(guided)
        for (int i = 0; i < nr; i++)
        {
            for (int j = 0; j < nc; j++)
            {
                mod.at<double>(i, j) = 20 * log10(mod.at<double>(i, j) + 0.000001);
            }
        }

        minMaxLoc(mod, &min, &max);
        if (fabs(max - min) < 0.00000001)
        {
            fprintf(stderr, "SLC image intensity is the same for every pixel\n\n");
            return -1;
        }
        min = max - db;
#pragma omp parallel for schedule(guided)
        for (int i = 0; i < nr; i++)
        {
            for (int j = 0; j < nc; j++)
            {
                if (mod.at<double>(i, j) <= min) mod.at<double>(i, j) = min;
            }
        }
    }
    else
    {
#pragma omp parallel for schedule(guided)
        for (int i = 0; i < nr; i++)
        {
            for (int j = 0; j < nc; j++)
            {
                mod.at<float>(i, j) = 20 * log10(mod.at<float>(i, j) + 0.000001);
            }
        }

        minMaxLoc(mod, &min, &max);
        if (fabs(max - min) < 0.00000001)
        {
            fprintf(stderr, "SLC image intensity is the same for every pixel\n\n");
            return -1;
        }
        min = max - db;
#pragma omp parallel for schedule(guided)
        for (int i = 0; i < nr; i++)
        {
            for (int j = 0; j < nc; j++)
            {
                if (mod.at<float>(i, j) <= min) mod.at<float>(i, j) = min;
            }
        }
    }
    mod = (mod - min) / (max - min) * 255.0;
    mod.convertTo(mod, CV_8U);
    bool ret = cv::imwrite(filename, mod);
    if (!ret)
    {
        fprintf(stderr, "cv::imwrite(): can't write to %s\n\n", filename);
        return -1;
    }
    return 0;
}

int Utils::savephase(const char* filename, const char* colormap, Mat phase)
{
    if (filename == NULL ||
        colormap == NULL ||
        phase.rows < 1 ||
        phase.cols < 1 ||
        phase.type() != CV_64F ||
        phase.channels() != 1)
    {
        fprintf(stderr, "savephase(): input check failed!\n\n");
        return -1;
    }
    bool gray = false;
    cv::ColormapTypes type = cv::COLORMAP_PARULA;
    if (strcmp(colormap, "jet") == 0) type = cv::COLORMAP_JET;
    if (strcmp(colormap, "hsv") == 0) type = cv::COLORMAP_HSV;
    if (strcmp(colormap, "cool") == 0) type = cv::COLORMAP_COOL;
    if (strcmp(colormap, "rainbow") == 0) type = cv::COLORMAP_RAINBOW;
    if (strcmp(colormap, "spring") == 0) type = cv::COLORMAP_SPRING;
    if (strcmp(colormap, "summer") == 0) type = cv::COLORMAP_SUMMER;
    if (strcmp(colormap, "winter") == 0) type = cv::COLORMAP_WINTER;
    if (strcmp(colormap, "autumn") == 0) type = cv::COLORMAP_AUTUMN;
    if (strcmp(colormap, "gray") == 0) gray = true;

    double min, max;
    Mat tmp;
    phase.copyTo(tmp);
    cv::minMaxLoc(tmp, &min, &max);
    if (fabs(max - min) < 0.000001)
    {
        fprintf(stderr, "phase value is the same for every pixel\n\n");
        return -1;
    }
    tmp = (tmp - min) / (max - min) * 255.0;
    tmp.convertTo(tmp, CV_8U);
    if (!gray)
    {
        cv::applyColorMap(tmp, tmp, type);
    }
    bool ret = cv::imwrite(filename, tmp);
    if (!ret)
    {
        fprintf(stderr, "cv::imwrite(): can't write to %s\n\n", filename);
        return -1;
    }
    return 0;
}

int Utils::resampling(const char* Src_file, const char* Dst_file, int dst_rows, int dst_cols)
{
    if (Src_file == NULL ||
        Dst_file == NULL ||
        dst_rows < 1 ||
        dst_cols < 1)
    {
        fprintf(stderr, "down_sampling(): input check failed!\n\n");
        return -1;
    }
    Mat img = cv::imread(Src_file);
    if (img.rows < 1 || img.cols < 1)
    {
        fprintf(stderr, "can't read from %s!\n\n", Dst_file);
        return -1;
    }
    cv::resize(img, img, cv::Size(dst_cols, dst_rows));
    if (cv::imwrite(Dst_file, img) == false)
    {
        fprintf(stderr, "failed to write %s\n\n", Dst_file);
        return -1;
    }
    return 0;
}

int Utils::amplitude_phase_blend(const char* amplitude_file, const char* phase_file, const char* blended_file, double SAR_ratio)
{
    if (amplitude_file == NULL ||
        phase_file == NULL ||
        blended_file == NULL
        )
    {
        fprintf(stderr, "amplitude_phase_blend(): input check failed!\n\n");
        return -1;
    }
    Mat amplitude = imread(amplitude_file);
    Mat phase = imread(phase_file);
    if (amplitude.rows < 1 ||
        amplitude.cols < 1
        )
    {
        fprintf(stderr, "amplitude_phase_blend(): can't open %s!\n\n", amplitude_file);
        return -1;
    }
    if (phase.rows < 1 ||
        phase.cols < 1
        )
    {
        fprintf(stderr, "amplitude_phase_blend(): can't open %s!\n", phase_file);
        return -1;
    }
    SAR_ratio = SAR_ratio < 0.5 ? 0.5 : SAR_ratio;
    SAR_ratio = SAR_ratio > 0.99 ? 0.99 : SAR_ratio;
    Mat blend;
    addWeighted(amplitude, SAR_ratio, phase, 1.0 - SAR_ratio, 0, blend);
    if (blend.rows < 1 || blend.cols < 1)
    {
        fprintf(stderr, "amplitude_phase_blend(): failed to blend !\n");
        return -1;
    }
    if (!imwrite(blended_file, blend))
    {
        fprintf(stderr, "amplitude_phase_blend(): failed to write to %s !\n", blended_file);
        return -1;
    }
    return 0;
}



int Utils::read_edges(const char* filename, tri_edge** edges, long* num_edges, int** neighbours, long num_nodes)
{
    if (filename == NULL ||
        num_edges == NULL ||
        num_nodes < 3 ||
        edges == NULL ||
        neighbours == NULL)
    {
        fprintf(stderr, "read_edges(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(filename, "rt");
    if (fp == NULL)
    {
        fprintf(stderr, "read_edges(): can't open %s\n", filename);
        return -1;
    }
    char str[1024];
    char* ptr;
    fgets(str, 1024, fp);
    *num_edges = strtol(str, &ptr, 0);
    if (*num_edges <= 0)
    {
        fprintf(stderr, "read_edges(): %s is unknown format!\n", filename);
        if (fp)
        {
            fclose(fp);
            fp = NULL;
        }
        return -1;
    }
    *edges = (tri_edge*)malloc(*num_edges * sizeof(tri_edge));
    if (*edges == NULL)
    {
        fprintf(stderr, "read_edges(): unreasonable number of edges, out of memory!\n");
        if (fp)
        {
            fclose(fp);
            fp = NULL;
        }
        return -1;
    }
    memset(*edges, 0, *num_edges * sizeof(tri_edge));
    *neighbours = (int*)malloc(sizeof(int) * num_nodes);
    if (*neighbours == NULL)
    {
        fprintf(stderr, "read_edges(): unreasonable number of nodes, out of memory!\n");
        if (fp)
        {
            fclose(fp);
            fp = NULL;
        }
        if (*edges)
        {
            free(*edges);
            *edges = NULL;
        }
        return -1;
    }
    memset(*neighbours, 0, sizeof(int) * num_nodes);
    long end1, end2, edges_number, boundry_marker;
    for (int i = 0; i < *num_edges; i++)
    {
        fgets(str, 1024, fp);
        edges_number = strtol(str, &ptr, 0);
        end1 = strtol(ptr, &ptr, 0);
        end2 = strtol(ptr, &ptr, 0);
        boundry_marker = strtol(ptr, &ptr, 0);
        (*edges + i)->end1 = end1;
        (*edges + i)->end2 = end2;
        (*edges + i)->num = i + 1;
        (*edges + i)->gain = 0;
        (*edges + i)->isResidueEdge = false;
        (*edges + i)->isBoundry = (boundry_marker == 1);
        if (end1 < 1 ||
            end1 > num_nodes ||
            end2 < 1 ||
            end2 > num_nodes)
        {
            fprintf(stderr, "read_edges(): endpoints exceed 1~num_nodes!\n");
            if (fp)
            {
                fclose(fp);
                fp = NULL;
            }
            if (*edges)
            {
                free(*edges);
                *edges = NULL;
            }
            if (*neighbours)
            {
                free(*neighbours);
                *neighbours = NULL;
            }
            return -1;
        }
        *(*neighbours + end1 - 1) = *(*neighbours + end1 - 1) + 1;//统计每个节点有多少邻接边
        *(*neighbours + end2 - 1) = *(*neighbours + end2 - 1) + 1;
    }
    if (fp)
    {
        fclose(fp);
        fp = NULL;
    }
    return 0;
}

int Utils::read_edges(const char* edge_file, vector<tri_edge>& edges, std::vector<int>& node_neighbours, long num_nodes)
{
    if (edge_file == NULL ||
        num_nodes < 3)
    {
        fprintf(stderr, "read_edges(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(edge_file, "rt");
    if (fp == NULL)
    {
        fprintf(stderr, "read_edges(): can't open %s\n", edge_file);
        return -1;
    }
    char str[1024];
    char* ptr;
    fgets(str, 1024, fp);
    long long num_edges = 0;
    num_edges = strtol(str, &ptr, 0);
    if (num_edges <= 0)
    {
        fprintf(stderr, "read_edges(): %s is unknown format!\n", edge_file);
        if (fp)
        {
            fclose(fp);
            fp = NULL;
        }
        return -1;
    }
    edges.clear(); node_neighbours.clear();
    edges.resize(num_edges);
    node_neighbours.resize(num_nodes);
    long end1, end2, edges_number, boundry_marker;
    for (int i = 0; i < num_edges; i++)
    {
        fgets(str, 1024, fp);
        edges_number = strtol(str, &ptr, 0);
        end1 = strtol(ptr, &ptr, 0);
        end2 = strtol(ptr, &ptr, 0);
        boundry_marker = strtol(ptr, &ptr, 0);
        edges[i].end1 = end1;
        edges[i].end2 = end2;
        edges[i].num = i + 1;
        edges[i].gain = 0;
        edges[i].isResidueEdge = false;
        edges[i].phase_diff = 0.0;
        edges[i].isBoundry = (boundry_marker == 1);
        if (end1 < 1 ||
            end1 > num_nodes ||
            end2 < 1 ||
            end2 > num_nodes)
        {
            fprintf(stderr, "read_edges(): endpoints exceed 1~num_nodes!\n");
            if (fp)
            {
                fclose(fp);
                fp = NULL;
            }
            return -1;
        }
        node_neighbours[end1 - 1] += 1;//统计每个节点有多少邻接边
        node_neighbours[end2 - 1] += 1;//统计每个节点有多少邻接边
    }
    if (fp)
    {
        fclose(fp);
        fp = NULL;
    }
    return 0;
}

int Utils::init_tri_node(vector<tri_node>& node_array, Mat& phase, Mat& mask, tri_edge* edges, long num_edges, int* num_neighbour, int num_nodes)
{
    if (phase.rows < 2 ||
        phase.cols < 2 ||
        phase.channels() != 1 ||
        phase.type() != CV_64F ||
        mask.rows != phase.rows ||
        mask.cols != phase.cols ||
        mask.channels() != 1 ||
        mask.type() != CV_32S ||
        edges == NULL ||
        num_edges < 3 ||
        num_neighbour == NULL ||
        num_nodes < 3)
    {
        fprintf(stderr, "init_tri_node(): input check failed!\n\n");
        return -1;
    }
    int sum = cv::countNonZero(mask);;
    if (sum != num_nodes)
    {
        fprintf(stderr, "init_tri_node(): mask and num_nodes mismatch!\n\n");
        return -1;
    }
    int rows = phase.rows;
    int cols = phase.cols;
    int count = 0;
    tri_node* ptr = NULL;
    double Phase;
    for (int i = 0; i < rows; i++)
    {
        for (int j = 0; j < cols; j++)
        {
            if (mask.at<int>(i, j) > 0)
            {
                Phase = phase.at<double>(i, j);
                ptr = new tri_node(i, j, *(num_neighbour + count), Phase);
                if (mask.at<int>(i, j) > 1)//邻接已解缠节点标记
                {
                    ptr->set_status(true);
                }
                node_array.push_back(*ptr);
                delete ptr;
                ptr = NULL;
                count++;

            }
        }
    }

    long* neighbour_ptr = NULL;
    int dummy, ret;
    tri_edge tmp;
    for (int i = 0; i < num_edges; i++)
    {
        tmp = *(edges + i);
        if (tmp.end1 < 0 ||
            tmp.end2 < 0 ||
            tmp.end1 > node_array.size() ||
            tmp.end2 > node_array.size())
        {
            fprintf(stderr, "init_tri_node(): edges' endpoint exceed legal value!\n\n");
            return -1;
        }
        ret = node_array[tmp.end1 - 1].get_neigh_ptr(&neighbour_ptr, &dummy);
        if (return_check(ret, "tri_node::get_neigh_ptr(*, *)", error_head)) return -1;
        while (neighbour_ptr != NULL && *neighbour_ptr != -1)
        {
            neighbour_ptr = neighbour_ptr + 1;
        }
        *neighbour_ptr = i + 1;

        ret = node_array[tmp.end2 - 1].get_neigh_ptr(&neighbour_ptr, &dummy);
        if (return_check(ret, "tri_node::get_neigh_ptr(*, *)", error_head)) return -1;
        while (neighbour_ptr != NULL && *neighbour_ptr != -1)
        {
            neighbour_ptr = neighbour_ptr + 1;
        }
        *neighbour_ptr = i + 1;
    }
    return 0;
}

int Utils::init_tri_node(
    vector<tri_node>& node_array,
    const Mat& phase,
    const Mat& mask,
    const vector<tri_edge>& edges,
    const vector<int>& node_neighbours,
    int num_nodes
)
{
    if (phase.rows < 2 ||
        phase.cols < 2 ||
        phase.channels() != 1 ||
        phase.type() != CV_64F ||
        mask.rows != phase.rows ||
        mask.cols != phase.cols ||
        mask.channels() != 1 ||
        mask.type() != CV_32S ||
        edges.size() < 3 ||
        (node_neighbours.size() - num_nodes) != 0 ||
        num_nodes < 3)
    {
        fprintf(stderr, "init_tri_node(): input check failed!\n\n");
        return -1;
    }
    int sum = cv::countNonZero(mask);;
    if (sum != num_nodes)
    {
        fprintf(stderr, "init_tri_node(): mask and num_nodes mismatch!\n\n");
        return -1;
    }
    long long num_edges = edges.size();
    node_array.clear();
    node_array.resize(num_nodes);
    int rows = phase.rows;
    int cols = phase.cols;
    int count = 0;
    tri_node* ptr = NULL;
    double Phase;
    for (int i = 0; i < rows; i++)
    {
        for (int j = 0; j < cols; j++)
        {
            if (mask.at<int>(i, j) > 0)
            {
                Phase = phase.at<double>(i, j);
                ptr = new tri_node(i, j, node_neighbours[count], Phase);
                if (mask.at<int>(i, j) > 1)//邻接已解缠节点标记
                {
                    ptr->set_status(true);
                }
                node_array[count] = *ptr;
                delete ptr;
                ptr = NULL;
                count++;

            }
        }
    }

    long* neighbour_ptr = NULL;
    int dummy, ret;
    tri_edge tmp;
    for (int i = 0; i < num_edges; i++)
    {
        tmp = edges[i];
        if (tmp.end1 < 0 ||
            tmp.end2 < 0 ||
            tmp.end1 > node_array.size() ||
            tmp.end2 > node_array.size())
        {
            fprintf(stderr, "init_tri_node(): edges' endpoint exceed legal value!\n\n");
            return -1;
        }
        ret = node_array[tmp.end1 - 1].get_neigh_ptr(&neighbour_ptr, &dummy);
        if (return_check(ret, "tri_node::get_neigh_ptr(*, *)", error_head)) return -1;
        while (neighbour_ptr != NULL && *neighbour_ptr != -1)
        {
            neighbour_ptr = neighbour_ptr + 1;
        }
        *neighbour_ptr = i + 1;

        ret = node_array[tmp.end2 - 1].get_neigh_ptr(&neighbour_ptr, &dummy);
        if (return_check(ret, "tri_node::get_neigh_ptr(*, *)", error_head)) return -1;
        while (neighbour_ptr != NULL && *neighbour_ptr != -1)
        {
            neighbour_ptr = neighbour_ptr + 1;
        }
        *neighbour_ptr = i + 1;
    }
    return 0;
}

int Utils::init_edge_phase_diff(vector<tri_edge>& edges, const vector<tri_node>& node_array)
{
    if (edges.size() < 3 || node_array.size() < 3)
    {
        fprintf(stderr, "init_edge_phase_diff(): input check failed!\n");
        return -1;
    }
    size_t num_nodes = node_array.size();
    size_t num_edges = edges.size();
    size_t end1, end2;
    double phi1, phi2;
    for (size_t i = 0; i < num_edges; i++)
    {
        end1 = edges[i].end1 < edges[i].end2 ? edges[i].end1 : edges[i].end2;
        end2 = edges[i].end1 < edges[i].end2 ? edges[i].end2 : edges[i].end1;
        node_array[end1 - 1].get_phase(&phi1);
        node_array[end2 - 1].get_phase(&phi2);
        phi1 = phi2 - phi1;
        phi1 = atan2(sin(phi1), cos(phi1));
        edges[i].phase_diff = phi1;
    }
    return 0;
}

int Utils::init_edges_quality(Mat& quality, tri_edge* edges, int num_edges, vector<tri_node>& nodes)
{
    if (quality.rows < 2 ||
        quality.cols < 2 ||
        quality.type() != CV_64F ||
        quality.channels() != 1 ||
        edges == NULL ||
        num_edges < 3 ||
        nodes.size() < 3
        )
    {
        fprintf(stderr, "init_edges_quality(): input check failed!\n\n");
        return -1;
    }
    int rows, cols;
    double qual = 0.0;
    for (int i = 0; i < num_edges; i++)
    {
        qual = 0.0;
        nodes[(edges + i)->end1 - 1].get_pos(&rows, &cols);
        qual += quality.at<double>(rows, cols);
        nodes[(edges + i)->end2 - 1].get_pos(&rows, &cols);
        qual += quality.at<double>(rows, cols);
        (edges + i)->quality = qual / 2.0;
    }
    return 0;
}

int Utils::init_edges_quality(const Mat& quality_map, vector<tri_edge>& edges, const vector<tri_node>& nodes)
{
    if (quality_map.rows < 2 ||
        quality_map.cols < 2 ||
        quality_map.type() != CV_64F ||
        quality_map.channels() != 1 ||
        edges.size() < 3 ||
        nodes.size() < 3
        )
    {
        fprintf(stderr, "init_edges_quality(): input check failed!\n\n");
        return -1;
    }
    int rows, cols;
    double qual = 0.0;
    size_t num_edges = edges.size();
    for (int i = 0; i < num_edges; i++)
    {
        qual = 0.0;
        nodes[edges[i].end1 - 1].get_pos(&rows, &cols);
        qual += quality_map.at<double>(rows, cols);
        nodes[edges[i].end2 - 1].get_pos(&rows, &cols);
        qual += quality_map.at<double>(rows, cols);
        edges[i].quality = qual / 2.0;
    }
    return 0;
}

int Utils::read_triangle(
    const char* ele_file,
    const char* neigh_file,
    triangle** tri,
    int* num_triangle,
    vector<tri_node>& nodes,
    tri_edge* edges,
    int num_edges
)
{
    if (ele_file == NULL ||
        neigh_file == NULL ||
        tri == NULL ||
        num_triangle == NULL ||
        nodes.size() < 3 ||
        edges == NULL ||
        num_edges < 3
        )
    {
        fprintf(stderr, "read_triangle(): input check failed!\n\n");
        return -1;
    }
    FILE* fp_ele, * fp_neigh;
    fp_ele = NULL;
    fp_neigh = NULL;
    fp_ele = fopen(ele_file, "rt");
    if (fp_ele == NULL)
    {
        fprintf(stderr, "read_triangle(): can't open %s\n", ele_file);
        return -1;
    }
    fp_neigh = fopen(neigh_file, "rt");
    if (fp_neigh == NULL)
    {
        fprintf(stderr, "read_triangle(): can't open %s\n", neigh_file);
        if (fp_ele)
        {
            fclose(fp_ele);
            fp_ele = NULL;
        }
        return -1;
    }

    char str[INPUTMAXSIZE];
    char* ptr;
    fgets(str, INPUTMAXSIZE, fp_ele);
    *num_triangle = strtol(str, &ptr, 0);
    if (*num_triangle < 1)
    {
        fprintf(stderr, "read_triangle(): number of triangles exceed legal range!\n");
        if (fp_ele)
        {
            fclose(fp_ele);
            fp_ele = NULL;
        }
        if (fp_neigh)
        {
            fclose(fp_neigh);
            fp_neigh = NULL;
        }
        return -1;
    }
    fgets(str, INPUTMAXSIZE, fp_neigh);

    *tri = (triangle*)malloc(*num_triangle * sizeof(triangle));
    if (*tri == NULL)
    {
        fprintf(stderr, "read_triangle(): out of memory!\n");
        if (fp_ele)
        {
            fclose(fp_ele);
            fp_ele = NULL;
        }
        if (fp_neigh)
        {
            fclose(fp_neigh);
            fp_neigh = NULL;
        }
        return -1;
    }
    memset(*tri, 0, sizeof(triangle) * (*num_triangle));

    int p1, p2, p3, neigh1, neigh2, neigh3, num1, num2;
    for (int i = 0; i < *num_triangle; i++)
    {
        fgets(str, INPUTMAXSIZE, fp_ele);
        num1 = strtol(str, &ptr, 0);
        p1 = strtol(ptr, &ptr, 0);
        p2 = strtol(ptr, &ptr, 0);
        p3 = strtol(ptr, &ptr, 0);

        fgets(str, INPUTMAXSIZE, fp_neigh);
        num2 = strtol(str, &ptr, 0);
        neigh1 = strtol(ptr, &ptr, 0);
        neigh2 = strtol(ptr, &ptr, 0);
        neigh3 = strtol(ptr, &ptr, 0);
        (*tri + i)->p1 = p1;
        (*tri + i)->p2 = p2;
        (*tri + i)->p3 = p3;
        (*tri + i)->neigh1 = neigh1;
        (*tri + i)->neigh2 = neigh2;
        (*tri + i)->neigh3 = neigh3;
        (*tri + i)->num = num1;
    }
    if (fp_ele)
    {
        fclose(fp_ele);
        fp_ele = NULL;
    }
    if (fp_neigh)
    {
        fclose(fp_neigh);
        fp_neigh = NULL;
    }
    //获取三角形的边序号
    long* ptr_neigh = NULL;
    int num_neigh, count;
    int edge[3];
    memset(edge, 0, sizeof(int) * 3);
    for (int j = 0; j < *num_triangle; j++)
    {
        count = 0;
        nodes[(*tri + j)->p1 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
        for (int i = 0; i < num_neigh; i++)
        {
            if ((edges + *(ptr_neigh + i) - 1)->end1 == (*tri + j)->p2 ||
                (edges + *(ptr_neigh + i) - 1)->end1 == (*tri + j)->p3 ||
                (edges + *(ptr_neigh + i) - 1)->end2 == (*tri + j)->p2 ||
                (edges + *(ptr_neigh + i) - 1)->end2 == (*tri + j)->p3
                )
            {
                edge[count] = *(ptr_neigh + i);
                count++;
            }
        }
        nodes[(*tri + j)->p2 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
        for (int i = 0; i < num_neigh; i++)
        {
            if ((edges + *(ptr_neigh + i) - 1)->end1 == (*tri + j)->p3 ||
                (edges + *(ptr_neigh + i) - 1)->end2 == (*tri + j)->p3)
            {
                edge[count] = *(ptr_neigh + i);
                //count++;
            }
        }
        (*tri + j)->edge1 = edge[0];
        (*tri + j)->edge2 = edge[1];
        (*tri + j)->edge3 = edge[2];
    }



    return 0;
}

int Utils::read_triangle(
    const char* ele_file,
    const char* neigh_file,
    vector<triangle>& triangle,
    vector<tri_node>& nodes,
    vector<tri_edge>& edges
)
{
    if (ele_file == NULL ||
        neigh_file == NULL ||
        nodes.size() < 3 ||
        edges.size() < 3
        )
    {
        fprintf(stderr, "read_triangle(): input check failed!\n\n");
        return -1;
    }
    FILE* fp_ele, * fp_neigh;
    fp_ele = NULL;
    fp_neigh = NULL;
    fp_ele = fopen(ele_file, "rt");
    if (fp_ele == NULL)
    {
        fprintf(stderr, "read_triangle(): can't open %s\n", ele_file);
        return -1;
    }
    fp_neigh = fopen(neigh_file, "rt");
    if (fp_neigh == NULL)
    {
        fprintf(stderr, "read_triangle(): can't open %s\n", neigh_file);
        if (fp_ele)
        {
            fclose(fp_ele);
            fp_ele = NULL;
        }
        return -1;
    }

    char str[INPUTMAXSIZE];
    char* ptr;
    fgets(str, INPUTMAXSIZE, fp_ele);
    long num_triangle = strtol(str, &ptr, 0);
    if (num_triangle < 1)
    {
        fprintf(stderr, "read_triangle(): number of triangles exceed legal range!\n");
        if (fp_ele)
        {
            fclose(fp_ele);
            fp_ele = NULL;
        }
        if (fp_neigh)
        {
            fclose(fp_neigh);
            fp_neigh = NULL;
        }
        return -1;
    }
    fgets(str, INPUTMAXSIZE, fp_neigh);
    triangle.clear();
    triangle.resize(num_triangle);

    int p1, p2, p3, neigh1, neigh2, neigh3, num1, num2;
    for (int i = 0; i < num_triangle; i++)
    {
        fgets(str, INPUTMAXSIZE, fp_ele);
        num1 = strtol(str, &ptr, 0);
        p1 = strtol(ptr, &ptr, 0);
        p2 = strtol(ptr, &ptr, 0);
        p3 = strtol(ptr, &ptr, 0);

        fgets(str, INPUTMAXSIZE, fp_neigh);
        num2 = strtol(str, &ptr, 0);
        neigh1 = strtol(ptr, &ptr, 0);
        neigh2 = strtol(ptr, &ptr, 0);
        neigh3 = strtol(ptr, &ptr, 0);
        triangle[i].p1 = p1;
        triangle[i].p2 = p2;
        triangle[i].p3 = p3;
        triangle[i].neigh1 = neigh1;
        triangle[i].neigh2 = neigh2;
        triangle[i].neigh3 = neigh3;
        triangle[i].num = num1;
    }
    if (fp_ele)
    {
        fclose(fp_ele);
        fp_ele = NULL;
    }
    if (fp_neigh)
    {
        fclose(fp_neigh);
        fp_neigh = NULL;
    }
    //获取三角形的边序号
    long* ptr_neigh = NULL;
    int num_neigh, count;
    int edge[3];
    memset(edge, 0, sizeof(int) * 3);
    for (int j = 0; j < num_triangle; j++)
    {
        count = 0;
        nodes[triangle[j].p1 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
        for (int i = 0; i < num_neigh; i++)
        {
            if ((edges[*(ptr_neigh + i) - 1].end1 == triangle[j].p2) ||
                (edges[*(ptr_neigh + i) - 1].end1 == triangle[j].p3) ||
                (edges[*(ptr_neigh + i) - 1].end2 == triangle[j].p2) ||
                (edges[*(ptr_neigh + i) - 1].end2 == triangle[j].p3)
                )
            {
                edge[count] = *(ptr_neigh + i);
                count++;
            }
        }
        nodes[triangle[j].p2 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
        for (int i = 0; i < num_neigh; i++)
        {
            if ((edges[*(ptr_neigh + i) - 1].end1 == triangle[j].p3) ||
                (edges[*(ptr_neigh + i) - 1].end2 == triangle[j].p3))
            {
                edge[count] = *(ptr_neigh + i);
                //count++;
            }
        }
        triangle[j].edge1 = edge[0];
        triangle[j].edge2 = edge[1];
        triangle[j].edge3 = edge[2];
    }

    return 0;
}

//int Utils::gen_delaunay(const char* filename, const char* exe_path)
//{
//    if (filename == NULL ||
//        exe_path == NULL
//        )
//    {
//        fprintf(stderr, "gen_delaunay(): input check failed!\n");
//        return -1;
//    }
//    FILE* fp = NULL;
//    fp = fopen(filename, "rt");
//    if (!fp)
//    {
//        fprintf(stderr, "gen_delaunay(): can't open %s!\n", filename);
//        return -1;
//    }
//    else
//    {
//        fclose(fp);
//        fp = NULL;
//    }
//    USES_CONVERSION;
//    LPWSTR szCommandLine = new TCHAR[256];
//    wcscpy(szCommandLine, A2W(exe_path));
//    wcscat(szCommandLine, L"\\delaunay.exe -en ");
//    wcscat(szCommandLine, A2W(filename));
//
//    STARTUPINFO si;
//    PROCESS_INFORMATION p_i;
//    ZeroMemory(&si, sizeof(si));
//    si.cb = sizeof(si);
//    ZeroMemory(&p_i, sizeof(p_i));
//    si.dwFlags = STARTF_USESHOWWINDOW;
//    si.wShowWindow = FALSE;
//    BOOL bRet = ::CreateProcess(
//        NULL,           // 不在此指定可执行文件的文件名
//        szCommandLine,      // 命令行参数
//        NULL,           // 默认进程安全性
//        NULL,           // 默认线程安全性
//        FALSE,          // 指定当前进程内的句柄不可以被子进程继承
//        CREATE_NEW_CONSOLE, // 为新进程创建一个新的控制台窗口
//        NULL,           // 使用本进程的环境变量
//        NULL,           // 使用本进程的驱动器和目录
//        &si,
//        &p_i);
//    if (bRet)
//    {
//        HANDLE hd = CreateJobObjectA(NULL, "delaunay");
//        if (hd)
//        {
//            JOBOBJECT_EXTENDED_LIMIT_INFORMATION extLimitInfo;
//            extLimitInfo.BasicLimitInformation.LimitFlags = JOB_OBJECT_LIMIT_KILL_ON_JOB_CLOSE;
//            BOOL retval = SetInformationJobObject(hd, JobObjectExtendedLimitInformation, &extLimitInfo, sizeof(extLimitInfo));
//            if (retval)
//            {
//                if (p_i.hProcess)
//                {
//                    retval = AssignProcessToJobObject(hd, p_i.hProcess);
//                }
//            }
//        }
//        WaitForSingleObject(p_i.hProcess, INFINITE);
//        if (szCommandLine != NULL) delete[] szCommandLine;
//        ::CloseHandle(p_i.hThread);
//        ::CloseHandle(p_i.hProcess);
//    }
//    else
//    {
//        fprintf(stderr, "gen_triangle(): create triangle.exe process failed!\n\n");
//        if (szCommandLine != NULL) delete[] szCommandLine;
//        return -1;
//    }
//
//
//    return 0;
//}


int Utils::write_node_file(const char* filename, const Mat& mask)
{
    if (filename == NULL ||
        mask.rows < 2 ||
        mask.cols < 2 ||
        mask.channels() != 1 ||
        mask.type() != CV_32S
        )
    {
        fprintf(stderr, "write_node_file(): input check failed!\n\n");
        return -1;
    }
    FILE* fp = NULL;
    fp = fopen(filename, "wt");
    if (fp == NULL)
    {
        fprintf(stderr, "write_node_file(): can't open %s\n", filename);
        return -1;
    }
    int nonzero = cv::countNonZero(mask);
    if (nonzero <= 0)
    {
        fprintf(stderr, "write_node_file(): no node exist!\n");
        if (fp)
        {
            fclose(fp);
            fp = NULL;
        }
        return -1;
    }
    int dim, attr1, attr2, count;
    dim = 2;
    attr1 = 0;
    attr2 = 0;
    fprintf(fp, "%d %d %d %d\n", nonzero, dim, attr1, attr2);
    int rows = mask.rows;
    int cols = mask.cols;
    count = 1;
    double x, y;
    for (int i = 0; i < rows; i++)
    {
        for (int j = 0; j < cols; j++)
        {
            if (mask.at<int>(i, j) > 0)
            {
                x = double(i + 1);
                y = double(j + 1);
                fprintf(fp, "%d %lf %lf\n", count, x, y);
                count++;
            }
        }
    }
    if (fp)
    {
        fclose(fp);
        fp = NULL;
    }
    return 0;
}

int Utils::PS_amp_dispersion(const vector<Mat>& amplitude, double thresh, Mat& mask)
{
    if (
        amplitude.size() < 3 ||
        thresh < 0.0
        )
    {
        fprintf(stderr, "PS_amp_dispersion(): input check failed!\n\n");
        return -1;
    }
    if (amplitude[0].rows < 1 || amplitude[0].cols < 1 || amplitude[0].channels() != 1 || amplitude[0].type() != CV_64F)
    {
        fprintf(stderr, "PS_amp_dispersion(): input check failed!\n\n");
        return -1;
    }
    int nr = amplitude[0].rows;
    int nc = amplitude[0].cols;
    int num_images = amplitude.size();
    Mat tmp1 = Mat::zeros(nr, nc, CV_64F);
    tmp1.copyTo(mask);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < nr; i++)
    {
        double mean, tmp;
        for (int j = 0; j < nc; j++)
        {
            mean = 0;
            tmp = 0;
            for (int k = 0; k < num_images; k++)
            {
                mean += amplitude[k].at<double>(i, j);
                //mean += (amplitude + k)->at<double>(i, j);
            }
            mean = mean / num_images;
            for (int m = 0; m < num_images; m++)
            {
                tmp += (amplitude[m].at<double>(i, j) - mean) * (amplitude[m].at<double>(i, j) - mean);
                //tmp += ((amplitude + m)->at<double>(i, j) - mean) * ((amplitude + m)->at<double>(i, j) - mean);
            }
            tmp = sqrt(tmp / num_images);
            if (mean < 1e-8) mean = 1e-8;
            tmp = tmp / mean;
            if (tmp <= thresh)
            {
                mask.at<double>(i, j) = 1.0;
            }

        }
    }
    return 0;
}

int Utils::butter_lowpass(int grid_size, int n_win, double low_pass_wavelength, Mat& lowpass)
{
    if (grid_size < 3 ||
        n_win < 3 ||
        low_pass_wavelength < 1
        )
    {
        fprintf(stderr, "butter_lowpass(): input check failed!\n\n");
        return -1;
    }
    double freq0 = 1 / low_pass_wavelength;
    double start = -(n_win) / grid_size / n_win / 2;
    double interval = 1 / grid_size / n_win;
    double end = (n_win - 2) / grid_size / n_win / 2;
    Mat freq_i = Mat::zeros(1, n_win, CV_64F);
    for (int i = 0; i < n_win; i++)
    {
        freq_i.at<double>(0, i) = start + i * interval;
    }
    freq_i = freq_i / freq0;
    cv::pow(freq_i, 10, freq_i);
    freq_i = freq_i + 1.0;
    freq_i = 1 / freq_i;
    Mat tmp;
    cv::transpose(freq_i, tmp);
    tmp = tmp * freq_i;
    int ret = fftshift2(tmp);
    if (return_check(ret, "fftshift2(*)", error_head)) return -1;
    tmp.copyTo(lowpass);
    //freq_i = -(n_win) / grid_size / n_win / 2:1 / grid_size / n_win : (n_win - 2) / grid_size / n_win / 2;
    //butter_i = 1. / (1 + (freq_i / freq0). ^ (2 * 5));
    //low_pass = butter_i'*butter_i;
    //    low_pass = fftshift(low_pass);
    return 0;
}

int Utils::circshift(Mat& out, const cv::Point& delta)
{
    Size sz = out.size();
    if (sz.height <= 0 ||
        sz.width <= 0
        )
    {
        fprintf(stderr, "circshift(): input check failed!\n\n");
        return -1;
    }
    if ((sz.height == 1 && sz.width == 1) ||
        (delta.x == 0 && delta.y == 0)
        )
    {
        return 0;
    }
    int x = delta.x;
    int y = delta.y;
    if (x > 0) x = x % sz.width;
    if (y > 0) y = y % sz.height;
    if (x < 0) x = x % sz.width + sz.width;
    if (y < 0) y = y % sz.height + sz.height;
    vector<Mat> planes;
    split(out, planes);
    for (int i = 0; i < planes.size(); i++)
    {
        Mat tmp0, tmp1, tmp2, tmp3;
        Mat q0(planes[i], Rect(0, 0, sz.width, sz.height - y));
        Mat q1(planes[i], Rect(0, sz.height - y, sz.width, y));
        q0.copyTo(tmp0);
        q1.copyTo(tmp1);
        tmp0.copyTo(planes[i](Rect(0, y, sz.width, sz.height - y)));
        tmp1.copyTo(planes[i](Rect(0, 0, sz.width, y)));


        Mat q2(planes[i], Rect(0, 0, sz.width - x, sz.height));
        Mat q3(planes[i], Rect(sz.width - x, 0, x, sz.height));
        q2.copyTo(tmp2);
        q3.copyTo(tmp3);
        tmp2.copyTo(planes[i](Rect(x, 0, sz.width - x, sz.height)));
        tmp3.copyTo(planes[i](Rect(0, 0, x, sz.height)));
    }
    merge(planes, out);
    return 0;
}

int Utils::fftshift2(Mat& out)
{
    Size sz = out.size();
    Point pt(0, 0);
    pt.x = (int)floor(sz.width / 2.0);
    pt.y = (int)floor(sz.height / 2.0);
    int ret = circshift(out, pt);
    if (return_check(ret, "circshift(*)", error_head)) return -1;
    return 0;
}

int Utils::ifftshift(Mat& out)
{
    Size sz = out.size();
    Point pt(0, 0);
    pt.x = (int)ceil(sz.width / 2.0);
    pt.y = (int)ceil(sz.height / 2.0);
    int ret = circshift(out, pt);
    if (return_check(ret, "circshift(*)", error_head)) return -1;
    return 0;
}

int Utils::fft2(Mat& Src, Mat& Dst)
{
    if (Src.rows < 1 ||
        Src.cols < 1 ||
        Src.channels() != 1 ||
        Src.type() != CV_64F)
    {
        fprintf(stderr, "fft2(): input check failed!\n\n");
        return -1;
    }
    Mat planes[] = { Mat_<double>(Src), Mat::zeros(Src.size(), CV_64F) };
    Mat complexImg;
    merge(planes, 2, complexImg);
    dft(complexImg, Dst, DFT_COMPLEX_OUTPUT);
    return 0;
}

int Utils::fft2(ComplexMat& src, ComplexMat& Dst)
{
    if (src.GetCols() < 1 ||
        src.GetRows() < 1 ||
        src.type() != CV_64F
        )
    {
        fprintf(stderr, "fft2(): input check failed!\n\n");
        return -1;
    }
    Mat re, im;
    src.GetIm().copyTo(im);
    src.GetRe().copyTo(re);
    Mat planes[] = { re, im };
    Mat complexImg;
    merge(planes, 2, complexImg);
    dft(complexImg, complexImg, DFT_COMPLEX_INPUT);
    split(complexImg, planes);
    Dst.SetRe(planes[0]);
    Dst.SetIm(planes[1]);
    return 0;
}

int Utils::ifft2(ComplexMat& src, ComplexMat& dst)
{
    if (src.GetCols() < 1 ||
        src.GetRows() < 1 ||
        src.type() != CV_64F
        )
    {
        fprintf(stderr, "ifft2(): input check failed!\n\n");
        return -1;
    }
    Mat re, im;
    im = src.GetIm();
    re = src.GetRe();
    Mat planes[] = { re, im };
    Mat complexImg;
    merge(planes, 2, complexImg);
    idft(complexImg, complexImg, DFT_COMPLEX_OUTPUT);
    split(complexImg, planes);
    dst.SetRe(planes[0]);
    dst.SetIm(planes[1]);
    return 0;
}

int Utils::std(const Mat& input, double* std)
{
    if (input.rows < 1 ||
        input.cols < 1 ||
        input.channels() != 1 ||
        input.type() != CV_64F ||
        std == NULL
        )
    {
        fprintf(stderr, "std(): input check failed!\n\n");
        return -1;
    }
    Mat data;
    double mean_v = cv::mean(input)[0];
    data = input - mean_v;
    data = data.mul(data);
    double sum_v = cv::sum(data)[0];
    int x = data.rows * data.cols - 1;
    if (x > 0)
    {
        *std = sqrt(sum_v / double(x));
    }
    else
    {
        *std = 0.0;
    }

    return 0;
}


int Utils::PS_deflat(
    vector<Mat>& interf_phase,
    Mat& interf_combination,
    vector<Mat>& pos,
    vector<Mat>& gcps,
    Mat& start_row,
    Mat& start_col,
    int mode,
    double lambda
)
{
    if (interf_phase.size() < 5 ||
        gcps.size() != pos.size() ||
        gcps.size() < 5 ||
        interf_combination.channels() != 1 ||
        interf_combination.type() != CV_32S ||
        interf_combination.cols != 2 ||
        start_row.rows != start_col.rows ||
        start_row.cols != start_col.cols ||
        //start_row.rows != gcps.size() ||
        start_col.cols != 1 ||
        start_col.channels() != 1 ||
        start_row.channels() != 1 ||
        start_col.type() != CV_32S ||
        start_row.type() != CV_32S ||
        mode < 1 ||
        mode > 2 ||
        lambda <= 0.0
        )
    {
        fprintf(stderr, "PS_deflat(): input check failed!\n\n");
        return -1;
    }
    int n_images = pos.size();
    int n_interf = interf_phase.size();
    if (n_interf != interf_combination.rows ||
        start_row.rows != n_images
        )
    {
        fprintf(stderr, "PS_deflat(): parameter check failed!\n\n");
        return -1;
    }
    volatile bool parallel_flag = true;
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < n_interf; i++)
    {
        if (!parallel_flag) continue;
        Mat GCPS, POS1, POS2, phase;
        int row_start, col_start, master_ix, slave_ix, ret;
        master_ix = interf_combination.at<int>(i, 0);
        slave_ix = interf_combination.at<int>(i, 1);
        if (master_ix < 1 ||
            master_ix > n_images ||
            slave_ix < 1 ||
            slave_ix > n_images
            )
        {
            fprintf(stderr, "PS_deflat(): image index out of range!\n");
            parallel_flag = false;
            continue;
        }
        GCPS = gcps[master_ix - 1];
        POS1 = pos[master_ix - 1];
        POS2 = pos[slave_ix - 1];
        phase = interf_phase[i];
        row_start = start_row.at<int>(master_ix - 1, 0);
        col_start = start_col.at<int>(slave_ix - 1, 0);
        ret = _PS_deflat(phase, POS1, POS2, GCPS, 1, 1, mode, lambda);
        if (ret < 0)
        {
            parallel_flag = false;
            continue;
        }
        interf_phase[i] = phase;
    }
    if (parallel_check(parallel_flag, "PS_deflat()", parallel_error_head)) return -1;
    return 0;
}

int Utils::_PS_deflat(
    Mat& phase,
    Mat& pos1,
    Mat& pos2,
    Mat& gcps,
    int start_row,
    int start_col,
    int mode,
    double lambda
)
{
    if (
        phase.rows < 1 ||
        phase.cols < 1 ||
        phase.channels() != 1 ||
        phase.type() != CV_64F ||
        pos1.rows < phase.rows ||
        pos1.cols != 6 ||
        pos1.channels() != 1 ||
        pos1.type() != CV_64F ||
        pos2.rows < phase.rows ||
        pos2.cols != 6 ||
        pos2.channels() != 1 ||
        pos2.type() != CV_64F ||
        gcps.channels() != 1 ||
        gcps.type() != CV_64F ||
        gcps.cols != 5 ||
        gcps.rows < 4 ||
        start_col < 1 ||
        start_row < 1 ||
        start_row > pos1.rows ||
        mode > 2 ||
        mode < 1 ||
        lambda <= 0.0
        )
    {
        fprintf(stderr, "_PS_deflat(): input check failed!\n\n");
        return -1;
    }
    double C = 4.0 * PI;
    if (mode == 1)
    {
        C = 4.0 * PI;
    }
    else
    {
        C = 2.0 * PI;
    }

    int ret;
    int num_gcps = gcps.rows;
    Mat R_M = Mat::zeros(num_gcps, 1, CV_64F);
    Mat R_S = Mat::zeros(num_gcps, 1, CV_64F);

    /*计算每个地面控制点的斜距*/
#pragma omp parallel for schedule(guided)
    for (int j = 0; j < num_gcps; j++)
    {
        Mat llh = Mat::zeros(1, 3, CV_64F);
        Mat xyz0, range_sat_tar1, range_sat_tar2;
        Mat fdcs1 = Mat::zeros(1, pos1.rows, CV_64F);
        Mat fdcs2 = Mat::zeros(1, pos2.rows, CV_64F);
        double range_sat_tar_norm1, range_sat_tar_norm2;

        llh.at<double>(0, 0) = gcps.at<double>(j, 0);
        llh.at<double>(0, 1) = gcps.at<double>(j, 1);
        llh.at<double>(0, 2) = gcps.at<double>(j, 2);
        ell2xyz(llh, xyz0);
        Mat xyz1 = Mat::zeros(pos1.rows, 3, CV_64F);
        Mat xyz2 = Mat::zeros(pos2.rows, 3, CV_64F);
        for (int k = 0; k < xyz1.rows; k++)
        {
            xyz0.copyTo(xyz1(Range(k, k + 1), Range(0, 3)));
        }
        for (int k = 0; k < xyz2.rows; k++)
        {
            xyz0.copyTo(xyz2(Range(k, k + 1), Range(0, 3)));
        }
        range_sat_tar1 = xyz1 - pos1(Range(0, pos1.rows), Range(0, 3));
        range_sat_tar2 = xyz2 - pos2(Range(0, pos2.rows), Range(0, 3));
        for (int k = 0; k < range_sat_tar1.rows; k++)
        {
            range_sat_tar1(Range(k, k + 1), Range(0, 3)).copyTo(xyz1);
            range_sat_tar_norm1 = sqrt(sum(xyz1.mul(xyz1))[0]);
            transpose(xyz1, xyz1);
            xyz1 = 2.0 * pos1(Range(k, k + 1), Range(3, 6)) * xyz1 / lambda / (range_sat_tar_norm1 + 1e-12);
            fdcs1.at<double>(0, k) = xyz1.at<double>(0, 0);
        }
        for (int k = 0; k < range_sat_tar2.rows; k++)
        {
            range_sat_tar2(Range(k, k + 1), Range(0, 3)).copyTo(xyz1);
            range_sat_tar_norm2 = sqrt(sum(xyz1.mul(xyz1))[0]);
            transpose(xyz1, xyz1);
            xyz1 = 2.0 * pos2(Range(k, k + 1), Range(3, 6)) * xyz1 / lambda / (range_sat_tar_norm2 + 1e-12);
            fdcs2.at<double>(0, k) = xyz1.at<double>(0, 0);
        }
        Point p1, p2;
        fdcs1 = abs(fdcs1);
        fdcs2 = abs(fdcs2);
        minMaxLoc(fdcs1, NULL, NULL, &p1, NULL);
        minMaxLoc(fdcs2, NULL, NULL, &p2, NULL);
        Mat Sat_pos_M, Sat_pos_S;
        pos1(Range(p1.x, p1.x + 1), Range(0, 3)).copyTo(Sat_pos_M);
        pos2(Range(p2.x, p2.x + 1), Range(0, 3)).copyTo(Sat_pos_S);
        Mat Slantrange_M = xyz0 - Sat_pos_M;
        Mat Slantrange_S = xyz0 - Sat_pos_S;
        R_M.at<double>(j, 0) = sqrt(sum(Slantrange_M.mul(Slantrange_M))[0]);
        R_S.at<double>(j, 0) = sqrt(sum(Slantrange_S.mul(Slantrange_S))[0]);
    }
    Mat A = Mat::zeros(num_gcps, 3, CV_64F);
    for (int j = 0; j < num_gcps; j++)
    {
        A.at<double>(j, 0) = 1.0;
        A.at<double>(j, 1) = gcps.at<double>(j, 3);
        A.at<double>(j, 2) = gcps.at<double>(j, 4);
    }
    Mat A_t;
    transpose(A, A_t);
    A = A_t * A;
    Mat b = A_t * (R_M - R_S);
    Mat x;
    if (!solve(A, b, x, DECOMP_LU))
    {
        fprintf(stderr, "_PS_deflat(): can't solve least square problem!\n");
        return -1;
    }

    Mat flat_phase = Mat::zeros(phase.rows, phase.cols, CV_64F);
    int nr = phase.rows;
    int nc = phase.cols;
#pragma omp parallel for schedule(guided)
    for (int j = 0; j < nr; j++)
    {
        for (int k = 0; k < nc; k++)
        {
            double xx = C * (x.at<double>(0, 0) + (double(j) + double(start_row)) * x.at<double>(1, 0) +
                (double(k) + double(start_col)) * x.at<double>(2, 0)) / lambda;
            phase.at<double>(j, k) = phase.at<double>(j, k) + xx;
        }
    }
    wrap(phase, phase);
    return 0;
}







int Utils::stateVec_interp(Mat& stateVec, double time_interval, Mat& stateVec_interp)
{
    if (stateVec.empty() ||
        stateVec.cols != 7 ||
        stateVec.rows < 7 ||
        time_interval < 0.0)
    {
        fprintf(stderr, "stateVec_interp(): input check failed!\n");
        return -1;
    }
    Mat statevec;
    if (stateVec.type() != CV_64F) stateVec.convertTo(statevec, CV_64F);
    else stateVec.copyTo(statevec);

    int rows = statevec.rows; int cols = statevec.cols;
    Mat time; statevec(cv::Range(0, rows), cv::Range(0, 1)).copyTo(time);
    time = time - time.at<double>(0, 0);
    Mat A = Mat::ones(rows, 6, CV_64F);
    Mat temp, b;
    //拟合x
    Mat x; statevec(cv::Range(0, rows), cv::Range(1, 2)).copyTo(x);
    time.copyTo(A(cv::Range(0, rows), cv::Range(1, 2)));
    temp = time.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(2, 3)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(3, 4)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(4, 5)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(5, 6)));
    transpose(A, temp);
    b = temp * x;
    A = temp * A;
    if (!cv::solve(A, b, x, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "stateVec_interp(): matrix defficiency!\n");
        return -1;
    }

    //拟合y
    A = Mat::ones(rows, 6, CV_64F);
    Mat y; statevec(cv::Range(0, rows), cv::Range(2, 3)).copyTo(y);
    time.copyTo(A(cv::Range(0, rows), cv::Range(1, 2)));
    temp = time.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(2, 3)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(3, 4)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(4, 5)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(5, 6)));
    transpose(A, temp);
    b = temp * y;
    A = temp * A;
    if (!cv::solve(A, b, y, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "stateVec_interp(): matrix defficiency!\n");
        return -1;
    }

    //拟合z

    A = Mat::ones(rows, 6, CV_64F);
    Mat z; statevec(cv::Range(0, rows), cv::Range(3, 4)).copyTo(z);
    time.copyTo(A(cv::Range(0, rows), cv::Range(1, 2)));
    temp = time.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(2, 3)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(3, 4)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(4, 5)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(5, 6)));
    transpose(A, temp);
    b = temp * z;
    A = temp * A;
    if (!cv::solve(A, b, z, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "stateVec_interp(): matrix defficiency!\n");
        return -1;
    }

    //拟合vx

    A = Mat::ones(rows, 6, CV_64F);
    Mat vx; statevec(cv::Range(0, rows), cv::Range(4, 5)).copyTo(vx);
    time.copyTo(A(cv::Range(0, rows), cv::Range(1, 2)));
    temp = time.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(2, 3)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(3, 4)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(4, 5)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(5, 6)));
    transpose(A, temp);
    b = temp * vx;
    A = temp * A;
    if (!cv::solve(A, b, vx, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "stateVec_interp(): matrix defficiency!\n");
        return -1;
    }

    //拟合vy

    A = Mat::ones(rows, 6, CV_64F);
    Mat vy; statevec(cv::Range(0, rows), cv::Range(5, 6)).copyTo(vy);
    time.copyTo(A(cv::Range(0, rows), cv::Range(1, 2)));
    temp = time.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(2, 3)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(3, 4)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(4, 5)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(5, 6)));
    transpose(A, temp);
    b = temp * vy;
    A = temp * A;
    if (!cv::solve(A, b, vy, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "stateVec_interp(): matrix defficiency!\n");
        return -1;
    }

    //拟合vz

    A = Mat::ones(rows, 6, CV_64F);
    Mat vz; statevec(cv::Range(0, rows), cv::Range(6, 7)).copyTo(vz);
    time.copyTo(A(cv::Range(0, rows), cv::Range(1, 2)));
    temp = time.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(2, 3)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(3, 4)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(4, 5)));
    temp = temp.mul(time);
    temp.copyTo(A(cv::Range(0, rows), cv::Range(5, 6)));
    transpose(A, temp);
    b = temp * vz;
    A = temp * A;
    if (!cv::solve(A, b, vz, cv::DECOMP_NORMAL))
    {
        fprintf(stderr, "stateVec_interp(): matrix defficiency!\n");
        return -1;
    }

    //插值

    int count = 1;
    double t = 0;
    while (t <= time.at<double>(rows - 1, 0))
    {
        count++;
        t += time_interval;
    }
    stateVec_interp.create(count, 7, CV_64F);
    Mat tt = Mat::ones(count, 6, CV_64F);
    tt(cv::Range(0, count), cv::Range(0, 1)).copyTo(stateVec_interp(cv::Range(0, count), cv::Range(0, 1)));
    t = 0.0;
    for (int i = 0; i < count; i++)
    {
        tt.at<double>(i, 1) = t;
        tt.at<double>(i, 2) = t * t;
        tt.at<double>(i, 3) = t * t * t;
        tt.at<double>(i, 4) = t * t * t * t;
        tt.at<double>(i, 5) = t * t * t * t * t;
        t += time_interval;
    }
    x = tt * x;
    y = tt * y;
    z = tt * z;
    vx = tt * vx;
    vy = tt * vy;
    vz = tt * vz;
    x.copyTo(stateVec_interp(cv::Range(0, count), cv::Range(1, 2)));
    y.copyTo(stateVec_interp(cv::Range(0, count), cv::Range(2, 3)));
    z.copyTo(stateVec_interp(cv::Range(0, count), cv::Range(3, 4)));
    vx.copyTo(stateVec_interp(cv::Range(0, count), cv::Range(4, 5)));
    vy.copyTo(stateVec_interp(cv::Range(0, count), cv::Range(5, 6)));
    vz.copyTo(stateVec_interp(cv::Range(0, count), cv::Range(6, 7)));
    return 0;
}




int Utils::coord_conversion(Mat& coefficient, Mat& coord_in_1, Mat& coord_in_2, Mat& coord_out)
{
    if (coefficient.cols != 32 ||
        coefficient.rows != 1 ||
        coefficient.type() != CV_64F ||
        coord_in_1.empty() ||
        coord_in_2.empty() ||
        coord_in_1.type() != CV_64F ||
        coord_in_2.type() != CV_64F ||
        coord_in_1.rows != coord_in_2.rows ||
        coord_in_1.cols != coord_in_2.cols)
    {
        fprintf(stderr, "coord_conversion(): input check failed!\n");
        return -1;
    }
    double offset_out, scale_out, offset_in_1, offset_in_2, scale_in_1, scale_in_2;
    double a0, a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11,
        a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23, a24;
    offset_out = coefficient.at<double>(0, 0);
    scale_out = coefficient.at<double>(0, 1);
    offset_in_1 = coefficient.at<double>(0, 2);
    scale_in_1 = coefficient.at<double>(0, 3);
    offset_in_2 = coefficient.at<double>(0, 4);
    scale_in_2 = coefficient.at<double>(0, 5);

    a0 = coefficient.at<double>(0, 6);
    a1 = coefficient.at<double>(0, 7);
    a2 = coefficient.at<double>(0, 8);
    a3 = coefficient.at<double>(0, 9);
    a4 = coefficient.at<double>(0, 10);
    a5 = coefficient.at<double>(0, 11);
    a6 = coefficient.at<double>(0, 12);
    a7 = coefficient.at<double>(0, 13);
    a8 = coefficient.at<double>(0, 14);
    a9 = coefficient.at<double>(0, 15);
    a10 = coefficient.at<double>(0, 16);
    a11 = coefficient.at<double>(0, 17);
    a12 = coefficient.at<double>(0, 18);
    a13 = coefficient.at<double>(0, 19);
    a14 = coefficient.at<double>(0, 20);
    a15 = coefficient.at<double>(0, 21);
    a16 = coefficient.at<double>(0, 22);
    a17 = coefficient.at<double>(0, 23);
    a18 = coefficient.at<double>(0, 24);
    a19 = coefficient.at<double>(0, 25);
    a20 = coefficient.at<double>(0, 26);
    a21 = coefficient.at<double>(0, 27);
    a22 = coefficient.at<double>(0, 28);
    a23 = coefficient.at<double>(0, 29);
    a24 = coefficient.at<double>(0, 30);
    int rows = coord_in_1.rows; int cols = coord_in_1.cols;
    Mat coord_out1(rows, cols, CV_64F);
#pragma omp parallel for schedule(guided)
    for (int i = 0; i < rows; i++)
    {
        double in1, in2, out;
        for (int j = 0; j < cols; j++)
        {
            in1 = (coord_in_1.at<double>(i, j) - offset_in_1) / scale_in_1;
            in2 = (coord_in_2.at<double>(i, j) - offset_in_2) / scale_in_2;
            out = a0 + a1 * in1 + a2 * in1 * in1 + a3 * in1 * in1 * in1 + a4 * in1 * in1 * in1 * in1 +
                a5 * in2 + a6 * in2 * in1 + a7 * in2 * in1 * in1 + a8 * in2 * in1 * in1 * in1 + a9 * in2 * in1 * in1 * in1 * in1 +
                a10 * in2 * in2 + a11 * in2 * in2 * in1 + a12 * in2 * in2 * in1 * in1 + a13 * in2 * in2 * in1 * in1 * in1 + a14 * in2 * in2 * in1 * in1 * in1 * in1 +
                a15 * in2 * in2 * in2 + a16 * in2 * in2 * in2 * in1 + a17 * in2 * in2 * in2 * in1 * in1 + a18 * in2 * in2 * in2 * in1 * in1 * in1 + a19 * in2 * in2 * in2 * in1 * in1 * in1 * in1 +
                a20 * in2 * in2 * in2 * in2 + a21 * in2 * in2 * in2 * in2 * in1 + a22 * in2 * in2 * in2 * in2 * in1 * in1 + a23 * in2 * in2 * in2 * in2 * in1 * in1 * in1 + a24 * in2 * in2 * in2 * in2 * in1 * in1 * in1 * in1;
            out = out * scale_out + offset_out;
            coord_out1.at<double>(i, j) = out;
        }
    }
    coord_out1.copyTo(coord_out);
    return 0;
}


int Utils::homogeneous_test(const Mat& pixel1, const Mat& pixel2, int* homo_flag, double alpha, const char* method)
{
    if (pixel1.cols != 1 ||
        pixel1.rows < 5 ||
        pixel1.type() != CV_64F ||
        pixel1.rows != pixel2.rows ||
        pixel1.cols != pixel2.cols ||
        pixel2.type() != CV_64F ||
        homo_flag == NULL ||
        method == NULL
        )
    {
        fprintf(stderr, "homogeneous_test(): input check failed!\n");
        return -1;
    }

    //Kolmogorov-Smirnov检验
    /*
     alpha      0.20    0.15    0.10    0.05    0.025    0.01    0.005    0.001
     c(alpha)   1.073    1.138    1.224    1.358    1.48    1.628    1.731    1.949
    */
    if (strcmp(method, "KS") == 0)
    {
        Mat p1, p2, cdf1, cdf2;
        double thresh;
        pixel1.copyTo(p1); pixel2.copyTo(p2);
        int N = p1.rows;
        cv::sort(p1, p1, cv::SORT_EVERY_COLUMN + cv::SORT_ASCENDING);
        cv::sort(p2, p2, cv::SORT_EVERY_COLUMN + cv::SORT_ASCENDING);
        if (p1.at<double>(N - 2, 0) <= p2.at<double>(0, 0) || p2.at<double>(N - 2, 0) <= p1.at<double>(0, 0))
        {
            *homo_flag = -1;
            return 0;
        }
        //确定threshold
        if (fabs(alpha - 0.2) < 0.01)
        {
            thresh = sqrt(2 / (double)N) * 1.073;
        }
        else if (fabs(alpha - 0.15) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.138;
        }
        else if (fabs(alpha - 0.1) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.224;
        }
        else if (fabs(alpha - 0.05) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.358;
        }
        else if (fabs(alpha - 0.025) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.48;
        }
        else if (fabs(alpha - 0.01) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.628;
        }
        else if (fabs(alpha - 0.005) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.731;
        }
        else if (fabs(alpha - 0.001) < 0.0001)
        {
            thresh = sqrt(2 / (double)N) * 1.949;
        }
        else
        {
            thresh = sqrt(2 / (double)N) * 1.358;
        }
        //计算C.D.F最大间距
        double Dmax = 0.0, tmp;
        int front_1 = 0, front_2 = 0;
        if (p1.at<double>(0, 0) > p2.at<double>(0, 0))
        {
            for (int i = 0; i < N - 1; i++)
            {
                if (p2.at<double>(i, 0) <= p1.at<double>(0, 0) && p2.at<double>(i + 1, 0) >= p1.at<double>(0, 0))
                {
                    front_2 = i;
                    break;
                }
            }
        }
        else
        {
            for (int i = 0; i < N - 1; i++)
            {
                if (p1.at<double>(i, 0) <= p2.at<double>(0, 0) && p1.at<double>(i + 1, 0) >= p2.at<double>(0, 0))
                {
                    front_1 = i;
                    break;
                }
            }
        }
        while (front_1 < N && front_2 < N)
        {
            tmp = fabs((double)front_1 / (double)N - (double)front_2 / (double)N);
            Dmax = Dmax > tmp ? Dmax : tmp;
            if (front_1 >= N - 1 || front_2 >= N - 1)
            {
                break;
            }
            if (p1.at<double>(front_1 + 1, 0) < p2.at<double>(front_2 + 1, 0)) front_1++;
            else if (p1.at<double>(front_1 + 1, 0) > p2.at<double>(front_2 + 1, 0)) front_2++;
            else
            {
                front_1++; front_2++;
            }
        }
        if (Dmax > thresh) *homo_flag = -1;
        else *homo_flag = 0;
    }

    //Anderson-Darling 检验
    else if (strcmp(method, "AD") == 0)
    {
        /*
        alpha = 0.01, AD_inf = 3.857;
        alpha = 0.05, AD_inf = 2.492;
        alpha = 0.1,  AD_inf = 1.933;
        */
        Mat p1, p2, p;
        double thresh;
        pixel1.copyTo(p1); pixel2.copyTo(p2);
        cv::vconcat(p1, p2, p);
        int n = p1.rows; int N = 2 * n;
        cv::sort(p1, p1, cv::SORT_EVERY_COLUMN + cv::SORT_ASCENDING);
        cv::sort(p2, p2, cv::SORT_EVERY_COLUMN + cv::SORT_ASCENDING);
        cv::sort(p, p, cv::SORT_EVERY_COLUMN + cv::SORT_ASCENDING);
        if (fabs(alpha - 0.05) < 0.0001)
        {
            thresh = (2.492 - 1) * (1 - 1.55 / (double)N) + 1;
        }
        else if (fabs(alpha - 0.01) < 0.0001)
        {
            thresh = (3.857 - 1) * (1 - 1.55 / (double)N) + 1;
        }
        else
        {
            thresh = (1.933 - 1) * (1 - 1.55 / (double)N) + 1;
        }
        int c = 0; double sum = 0.0, sentinel = -1.0;
        for (int i = 1; i < N; i++)
        {
            while (c <= n - 1)
            {
                if (p.at<double>(i - 1, 0) <= p1.at<double>(c, 0)) break;
                c++;
            }
            sum += double((N * c - n * i) * (N * c - n * i)) / double(i * (N - i));
        }
        sum /= (double)(n * n);
        if (sum > thresh) *homo_flag = -1;
        else *homo_flag = 0;

    }

    return 0;
}


int Utils::coherence_matrix_estimation(const vector<ComplexMat>& slc_series, ComplexMat& coherence_matrix, int est_window_width, int est_window_height, int ref_row, int ref_col, bool b_homogeneous_test, bool b_normalize)
{
    if (slc_series.size() < 5 ||
        est_window_width < 3 ||
        est_window_height < 3 ||
        est_window_height % 2 != 1 ||
        est_window_width % 2 != 1 ||
        ref_col < 0 ||
        ref_row < 0
        )
    {
        fprintf(stderr, "coherence_estimation(): input check failed!\n");
        return -1;
    }
    if (slc_series[0].type() != CV_64F || slc_series[0].isempty() || ref_row > slc_series[0].GetRows() - 1 || ref_col > slc_series[0].GetCols() - 1
        )
    {
        fprintf(stderr, "coherence_estimation(): input check failed!\n");
        return -1;
    }
    int n_images = slc_series.size(), ret;
    int rows = slc_series[0].GetRows(); int cols = slc_series[0].GetCols();
    int radius_width = (est_window_width - 1) / 2;
    int radius_height = (est_window_height - 1) / 2;
    int left, right, bottom, top;
    left = (ref_col - radius_width) < 0 ? 0 : (ref_col - radius_width);
    right = (ref_col + radius_width) > cols - 1 ? cols - 1 : (ref_col + radius_width);
    bottom = (ref_row + radius_height) > rows - 1 ? rows - 1 : (ref_row + radius_height);
    top = (ref_row - radius_height) < 0 ? 0 : (ref_row - radius_height);
    rows = bottom - top + 1;
    cols = right - left + 1;
    if (b_homogeneous_test)
    {
        //统计同质检验
        ComplexMat pix1(n_images, 1); ComplexMat pix2(n_images, 1);
        Mat pix1_amp, pix2_amp;
        Mat mask = Mat::zeros(rows, cols, CV_32S);
        mask.at<int>(ref_row - top, ref_col - left) = 1;
        int b_homo, count = 1;
        for (int i = 0; i < n_images; i++)
        {
            pix1.re.at<double>(i, 0) = slc_series[i].re.at<double>(ref_row, ref_col);
            pix1.im.at<double>(i, 0) = slc_series[i].im.at<double>(ref_row, ref_col);
        }
        pix1_amp = pix1.GetMod();
        for (int i = 0; i < rows; i++)
        {

            for (int j = 0; j < cols; j++)
            {
                if (i == (ref_row - top) && j == (ref_col - left)) continue;
                for (int k = 0; k < n_images; k++)
                {
                    pix2.re.at<double>(k, 0) = slc_series[k].re.at<double>(i + top, j + left);
                    pix2.im.at<double>(k, 0) = slc_series[k].im.at<double>(i + top, j + left);
                }
                pix2_amp = pix2.GetMod();
                ret = homogeneous_test(pix1_amp, pix2_amp, &b_homo, 0.1);
                //ret = homogeneous_test(pix1_amp, pix2_amp, &b_homo, 0.1, "AD");
                if (return_check(ret, "homogeneous_test()", error_head)) return -1;
                if (b_homo == 0) { mask.at<int>(i, j) = 1; count++; }
            }
        }
        //Mat c; mask.convertTo(c, CV_64F);
        //cvmat2bin("E:\\working_dir\\projects\\software\\InSAR\\bin\\mask.bin", c);
        if (count < 2)
        {
            //fprintf(stderr, "coherence_matrix_estimation(): no homogenous pixels inside estimation window!\n");
            return -1;
        }
        //估计相关矩阵
        int count2 = count; count = 0;
        ComplexMat Covariance;
        Mat sum(n_images, 1, CV_64F), A(n_images, count2, CV_64F), B(n_images, count2, CV_64F), C, A_t, B_t;
        double s;
        for (int k = 0; k < n_images; k++)
        {
            s = 0.0;
            count = 0;
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    if (mask.at<int>(i, j) > 0)
                    {
                        A.at<double>(k, count) = slc_series[k].re.at<double>(i + top, j + left);
                        B.at<double>(k, count) = slc_series[k].im.at<double>(i + top, j + left);
                        count++;
                        s += A.at<double>(k, count - 1) * A.at<double>(k, count - 1)
                            + B.at<double>(k, count - 1) * B.at<double>(k, count - 1);
                    }
                }
            }
            sum.at<double>(k, 0) = s;
        }
        cv::transpose(A, A_t); cv::transpose(B, B_t);
        C = A * A_t + B * B_t;
        C.copyTo(Covariance.re);
        C = B * A_t - A * B_t;
        C.copyTo(Covariance.im);
        double denum;
        if (b_normalize)
        {

            for (int i = 0; i < n_images; i++)
            {
                for (int j = 0; j < n_images; j++)
                {
                    denum = sqrt(sum.at<double>(i, 0) * sum.at<double>(j, 0));
                    Covariance.re.at<double>(i, j) = Covariance.re.at<double>(i, j) / (denum + 1e-10);
                    Covariance.im.at<double>(i, j) = Covariance.im.at<double>(i, j) / (denum + 1e-10);
                }
            }


        }
        else
        {
            Covariance = Covariance * (1 / (double)count2);
        }
        coherence_matrix = Covariance;
    }
    else
    {

    }
    return 0;
}



int Utils::unwrap_region_growing(
    vector<tri_node>& nodes,
    const vector<tri_edge>& edges,
    size_t start_edge,
    double distance_thresh,
    double quality_thresh
)
{
    if (nodes.size() < 3 ||
        edges.size() < 3 ||
        start_edge < 1 ||
        start_edge > edges.size()
        )
    {
        fprintf(stderr, "unwrap_region_growing(): input check failed!\n\n");
        return -1;
    }
    if (distance_thresh < 1.0) distance_thresh = 1.0;
    if (quality_thresh > 0.9) quality_thresh = 0.9;
    //找到增量积分起始点
    int ix = 0;
    double MC = -1.0;
    size_t num_edges = edges.size();
    //for (int i = 0; i < num_edges; i++)
    //{
    //    if (edges[i].MC > MC)
    //    {
    //        ix = i;
    //        MC = edges[i].MC;
    //    }
    //}
    size_t start = edges[start_edge - 1].end1;

    //采用类似质量图法解缠的算法进行增量积分集成
    edge_index tmp;
    priority_queue<edge_index> que;
    //nodes[start - 1].set_vel(0.0);//起始点形变速率和高程误差设置为0，后续可根据参考点进行校正
    //nodes[start - 1].set_height(0.0);
    nodes[start - 1].set_status(true);
    long* ptr_neigh = NULL;
    int num_neigh, end2, number, row1, col1, row2, col2;
    double distance, phase, MC_total, phase_total, delta_phase;

    nodes[start - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
    for (int i = 0; i < num_neigh; i++)
    {
        end2 = edges[*(ptr_neigh + i) - 1].end1 == start ? edges[*(ptr_neigh + i) - 1].end2 : edges[*(ptr_neigh + i) - 1].end1;
        nodes[start - 1].get_distance(nodes[end2 - 1], &distance);
        if (!nodes[end2 - 1].get_status() &&
            distance <= distance_thresh &&
            edges[*(ptr_neigh + i) - 1].quality > quality_thresh
            )
        {
            tmp.num = *(ptr_neigh + i);
            tmp.quality = -edges[*(ptr_neigh + i) - 1].quality;
            que.push(tmp);
        }
    }

    while (que.size() != 0)
    {
        tmp = que.top();
        que.pop();
        if (nodes[edges[tmp.num - 1].end1 - 1].get_status())
        {
            number = edges[tmp.num - 1].end1;
            end2 = edges[tmp.num - 1].end2;
        }
        else
        {
            number = edges[tmp.num - 1].end2;
            end2 = edges[tmp.num - 1].end1;
        }
        MC_total = 1e-10;
        phase_total = 0.0;
        if (!nodes[end2 - 1].get_status())
        {
            nodes[end2 - 1].get_pos(&row2, &col2);
            nodes[end2 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
            for (int i = 0; i < num_neigh; i++)
            {
                number = edges[*(ptr_neigh + i) - 1].end1 == end2 ? edges[*(ptr_neigh + i) - 1].end2 : edges[*(ptr_neigh + i) - 1].end1;
                if (nodes[number - 1].get_status())
                {
                    nodes[number - 1].get_phase(&phase);
                    nodes[number - 1].get_pos(&row1, &col1);
                    if (row1 > row2)
                    {
                        delta_phase = -edges[*(ptr_neigh + i) - 1].phase_diff;
                    }
                    if (row1 < row2)
                    {
                        delta_phase = edges[*(ptr_neigh + i) - 1].phase_diff;
                    }
                    if (row1 == row2)
                    {
                        if (col1 > col2)
                        {
                            delta_phase = -edges[*(ptr_neigh + i) - 1].phase_diff;
                        }
                        else
                        {
                            delta_phase = edges[*(ptr_neigh + i) - 1].phase_diff;
                        }
                    }
                    phase_total += (delta_phase + phase) * edges[*(ptr_neigh + i) - 1].quality;
                    MC_total += edges[*(ptr_neigh + i) - 1].quality;
                }
            }
            nodes[end2 - 1].set_phase(phase_total / MC_total);
            nodes[end2 - 1].set_status(true);
            nodes[end2 - 1].get_neigh_ptr(&ptr_neigh, &num_neigh);
            number = end2;
            for (int i = 0; i < num_neigh; i++)
            {

                end2 = edges[*(ptr_neigh + i) - 1].end1 == number ? edges[*(ptr_neigh + i) - 1].end2 : edges[*(ptr_neigh + i) - 1].end1;
                nodes[number - 1].get_distance(nodes[end2 - 1], &distance);
                if (!nodes[end2 - 1].get_status() &&
                    distance <= distance_thresh &&
                    edges[*(ptr_neigh + i) - 1].quality > quality_thresh
                    )
                {
                    tmp.num = *(ptr_neigh + i);
                    tmp.quality = -edges[*(ptr_neigh + i) - 1].quality;
                    que.push(tmp);
                }
            }
        }
    }
    return 0;
}

//int Utils::unwrap_3D(
//    const Mat& mask,
//    const vector<Mat>& quality_map,
//    vector<Mat>& wrapped_phase_series,
//    vector<Mat>& unwrapped_phase_series,
//    const char* delaunay_exe_path,
//    const char* tmp_file_path,
//    double distance_thresh,
//    double quality_thresh
//)
//{
//    if (mask.rows < 2 ||
//        mask.cols < 2 ||
//        mask.type() != CV_32S ||
//        wrapped_phase_series.size() < 2 ||
//        delaunay_exe_path == NULL ||
//        tmp_file_path == NULL ||
//        quality_map.size() != wrapped_phase_series.size()
//        )
//    {
//        fprintf(stderr, "unwrap_3D(): input check failed!\n");
//        return -1;
//    }
//
//    quality_thresh = quality_thresh > 1.0 ? 0.9 : quality_thresh;
//    distance_thresh = distance_thresh < 1.0 ? 1.0 : distance_thresh;
//
//    /*----------------------------------*/
//    /*            时间维解缠            */
//    /*----------------------------------*/
//
//    /*
//    * 利用mask生成delaunay三角网络
//    */
//    size_t nr = mask.rows;
//    size_t nc = mask.cols;
//    long num_nodes = cv::countNonZero(mask);
//    if (num_nodes < 3)
//    {
//        fprintf(stderr, "unwrap_3D(): at least 3 nodes are needed!\n");
//        return -1;
//    }
//    int ret;
//    int n_images = wrapped_phase_series.size();
//    for (size_t i = 0; i < n_images; i++)
//    {
//        if (quality_map[i].rows != nr || quality_map[i].cols != nc)
//        {
//            fprintf(stderr, "unwrap_3D(): quality map size mismatch!\n");
//            return -1;
//        }
//        if (wrapped_phase_series[i].rows != nr || wrapped_phase_series[i].cols != nc)
//        {
//            fprintf(stderr, "unwrap_3D(): wrapped phase size mismatch!\n");
//            return -1;
//        }
//    }
//    unwrapped_phase_series.resize(n_images);
//    vector<tri_node> nodes; vector<vector<tri_node>> nodes_vec; nodes_vec.resize(n_images);
//    vector<tri_edge> edges; vector<vector<tri_edge>> edges_vec; edges_vec.resize(n_images);
//    vector<int> node_neighbour;
//    string tmp_folder(tmp_file_path);
//    string node_file = tmp_folder + "\\triangle.node";
//    ret = write_node_file(node_file.c_str(), mask);
//    if (return_check(ret, "write_node_file()", error_head)) return -1;
//    ret = gen_delaunay(node_file.c_str(), delaunay_exe_path);
//    if (return_check(ret, "gen_delaunay()", error_head)) return -1;
//    string edge_file = tmp_folder + "\\triangle.1.edge";
//    ret = read_edges(edge_file.c_str(), edges, node_neighbour, num_nodes);
//    if (return_check(ret, "read_edges()", error_head)) return -1;
//    for (int i = 0; i < n_images; i++)
//    {
//        ret = init_tri_node(nodes, wrapped_phase_series[i], mask, edges, node_neighbour, num_nodes);
//        if (return_check(ret, "init_tri_node()", error_head)) return -1;
//        ret = init_edge_phase_diff(edges, nodes);
//        if (return_check(ret, "init_edge_phase_diff()", error_head)) return -1;
//        ret = init_edges_quality(quality_map[i], edges, nodes);
//        if (return_check(ret, "init_edges_quality()", error_head)) return -1;
//        nodes_vec[i] = nodes;
//        edges_vec[i] = edges;
//    }
//
//    /*
//    * 一维高斯滤波
//    */
//
//    int Gaussian_radius = 2;
//    double sigma = 1.0;
//    Mat Gaussian_template = Mat::zeros(2 * Gaussian_radius + 1, 1, CV_64F);
//    for (int i = 0; i < 2 * Gaussian_radius + 1; i++)
//    {
//        Gaussian_template.at<double>(i, 0) = exp(-(double(i - Gaussian_radius)) * (double(i - Gaussian_radius)) / (2.0 * sigma * sigma))
//            / (sigma * sqrt(PI * 2.0));
//    }
//    size_t num_edges = edges.size();
//#pragma omp parallel for schedule(guided)
//    for (long long i = 0; i < num_edges; i++)
//    {
//        Mat time_series = Mat::zeros(n_images, 1, CV_64F); Mat temp;
//        for (size_t j = 0; j < n_images; j++)
//        {
//            time_series.at<double>(j, 0) = edges_vec[j][i].phase_diff;
//        }
//        cv::copyMakeBorder(time_series, time_series, Gaussian_radius, Gaussian_radius, 0, 0, cv::BORDER_REPLICATE);
//        for (size_t j = Gaussian_radius; j < n_images + Gaussian_radius; j++)
//        {
//            time_series.at<double>(j, 0) = cv::sum(time_series(cv::Range(j - Gaussian_radius, j + Gaussian_radius + 1),
//                cv::Range(0, 1)).mul(Gaussian_template))[0];
//        }
//        time_series(cv::Range(Gaussian_radius, n_images + Gaussian_radius), cv::Range(0, 1)).copyTo(temp);
//
//        /*
//        * 时间维解缠
//        */
//        Mat temp2; temp.copyTo(temp2);
//        for (size_t j = 1; j < n_images; j++)
//        {
//            double tmp = temp.at<double>(j, 0) - temp.at<double>(j - 1, 0);
//            tmp = atan2(sin(tmp), cos(tmp));
//            temp2.at<double>(j, 0) = temp2.at<double>(j - 1, 0) + tmp;
//        }
//
//        for (size_t j = 0; j < n_images; j++)
//        {
//            edges_vec[j][i].phase_diff = temp2.at<double>(j, 0);
//        }
//    }
//
//    /*----------------------------------*/
//    /*            空间维解缠            */
//    /*----------------------------------*/
//
//    /*
//    * 确定解缠起始点
//    */
//    double mean_quality, max_quality = -1.0;
//    size_t ix_max;
//    for (size_t i = 0; i < num_edges; i++)
//    {
//        mean_quality = 0.0;
//        for (size_t j = 0; j < n_images; j++)
//        {
//            mean_quality += edges_vec[j][i].quality;
//        }
//        mean_quality /= (double)n_images;
//        if (mean_quality > max_quality)
//        {
//            max_quality = mean_quality;
//            ix_max = i;
//        }
//    }
//    //#pragma omp parallel for schedule(guided)
//    for (size_t i = 0; i < n_images; i++)
//    {
//        //vector<tri_node> nodes; vector<tri_edge>edges;
//        int row, col; double phase;
//        Mat unwrapped_phase;
//        wrapped_phase_series[i].copyTo(unwrapped_phase);
//        nodes = nodes_vec[i];
//        edges = edges_vec[i];
//        ret = unwrap_region_growing(nodes, edges, ix_max, distance_thresh, quality_thresh);
//        if (return_check(ret, "unwrap_region_growing()", error_head)) return -1;
//
//
//
//        /*
//        * 从节点中取出解缠相位
//        */
//
//        for (size_t j = 0; j < num_nodes; j++)
//        {
//            if (nodes[j].get_status())
//            {
//                nodes[j].get_pos(&row, &col);
//                nodes[j].get_phase(&phase);
//                unwrapped_phase.at<double>(row, col) = phase;
//            }
//        }
//        unwrapped_phase.copyTo(unwrapped_phase_series[i]);
//    }
//
//    /*----------------------------------*/
//    /*          时间维再解缠            */
//    /*----------------------------------*/
//
//
////#pragma omp parallel for schedule(guided)
////    for (int i = 0; i < nr; i++)
////    {
////        double tmp, tmp_old;
////        for (size_t j = 0; j < nc; j++)
////        {
////            if (mask.at<int>(i, j) > 0)
////            {
////                for (size_t k = 1; k < n_images; k++)
////                {
////                    if (k == 1) tmp_old = unwrapped_phase_series[0].at<double>(i, j);
////                    tmp = unwrapped_phase_series[k].at<double>(i, j) - tmp_old;
////                    tmp = atan2(sin(tmp), cos(tmp));
////                    tmp_old = unwrapped_phase_series[k].at<double>(i, j);
////                    unwrapped_phase_series[k].at<double>(i, j) = unwrapped_phase_series[k - 1].at<double>(i, j) + tmp;
////                }
////            }
////            
////        }
////    }
//
//    return 0;
//}

//int Utils::unwrap_3D_mcf(
//    const Mat& mask,
//    const vector<Mat>& quality_map,
//    vector<Mat>& wrapped_phase_series,
//    vector<Mat>& unwrapped_phase_series,
//    const char* delaunay_exe_path,
//    const char* mcf_exe_path,
//    const char* tmp_file_path,
//    double distance_thresh
//)
//{
//    if (mask.rows < 2 ||
//        mask.cols < 2 ||
//        mask.type() != CV_32S ||
//        wrapped_phase_series.size() < 2 ||
//        delaunay_exe_path == NULL ||
//        tmp_file_path == NULL ||
//        mcf_exe_path == NULL ||
//        quality_map.size() != wrapped_phase_series.size()
//        )
//    {
//        fprintf(stderr, "unwrap_3D_mcf(): input check failed!\n");
//        return -1;
//    }
//
//    distance_thresh = distance_thresh < 1.0 ? 1.0 : distance_thresh;
//
//    /*----------------------------------*/
//    /*            时间维解缠            */
//    /*----------------------------------*/
//
//    /*
//    * 利用mask生成delaunay三角网络
//    */
//    size_t nr = mask.rows;
//    size_t nc = mask.cols;
//    long num_nodes = cv::countNonZero(mask);
//    if (num_nodes < 3)
//    {
//        fprintf(stderr, "unwrap_3D_mcf(): at least 3 nodes are needed!\n");
//        return -1;
//    }
//    int ret;
//    int n_images = wrapped_phase_series.size();
//    for (size_t i = 0; i < n_images; i++)
//    {
//        if (quality_map[i].rows != nr || quality_map[i].cols != nc)
//        {
//            fprintf(stderr, "unwrap_3D_mcf(): quality map size mismatch!\n");
//            return -1;
//        }
//        if (wrapped_phase_series[i].rows != nr || wrapped_phase_series[i].cols != nc)
//        {
//            fprintf(stderr, "unwrap_3D_mcf(): wrapped phase size mismatch!\n");
//            return -1;
//        }
//    }
//    Unwrap unwrap;
//    unwrapped_phase_series.resize(n_images);
//    vector<tri_node> nodes;
//    vector<tri_edge> edges;
//    vector<triangle> tri;
//    vector<int> node_neighbour;
//    Mat out_mask;
//    string tmp_folder(tmp_file_path);
//    string node_file = tmp_folder + "\\triangle.node";
//    string ele_file = tmp_folder + "\\triangle.1.ele";
//    string neigh_file = tmp_folder + "\\triangle.1.neigh";
//    string mcf_problem = tmp_folder + "\\mcf_delaunay.net";
//    string mcf_solution = tmp_folder + "\\mcf_delaunay.net.sol";
//    string edge_file = tmp_folder + "\\triangle.1.edge";
//    ret = write_node_file(node_file.c_str(), mask);
//    if (return_check(ret, "write_node_file()", error_head)) return -1;
//    ret = gen_delaunay(node_file.c_str(), delaunay_exe_path);
//    if (return_check(ret, "gen_delaunay()", error_head)) return -1;
//    ret = read_edges(edge_file.c_str(), edges, node_neighbour, num_nodes);
//    if (return_check(ret, "read_edges()", error_head)) return -1;
//    int num_triangle, positive = 0, negative = 0;
//    size_t num_edges = edges.size();
//    //先将起始点做时间维解缠
//    int pos_row, pos_col; double tmp;
//    ret = init_tri_node(nodes, wrapped_phase_series[0], mask, edges, node_neighbour, num_nodes);
//    if (return_check(ret, "init_tri_node()", error_head)) return -1;
//    nodes[199].get_pos(&pos_row, &pos_col);
//    Mat time_series(n_images, 1, CV_64F);
//    for (int i = 0; i < n_images; i++)
//    {
//        time_series.at<double>(i, 0) = wrapped_phase_series[i].at<double>(pos_row, pos_col);
//    }
//    for (int i = 1; i < n_images; i++)
//    {
//        tmp = time_series.at<double>(i, 0) - time_series.at<double>(i - 1, 0);
//        wrapped_phase_series[i].at<double>(pos_row, pos_col) = time_series.at<double>(i - 1, 0) +
//            atan2(sin(tmp), cos(tmp));
//    }
//
//    for (int i = 0; i < n_images; i++)
//    {
//        //将edges的gain清零
//        for (size_t ii = 0; ii < num_edges; ii++)
//        {
//            edges[ii].gain = 0.0;
//        }
//        positive = 0, negative = 0;
//        ret = init_tri_node(nodes, wrapped_phase_series[i], mask, edges, node_neighbour, num_nodes);
//        if (return_check(ret, "init_tri_node()", error_head)) return -1;
//        if (i == 0)
//        {
//            ret = read_triangle(ele_file.c_str(), neigh_file.c_str(), tri, nodes, edges);
//            if (return_check(ret, "read_triangle()", error_head)) return -1;
//            num_triangle = tri.size();
//        }
//        ret = residue(tri, nodes, edges, 10.0);
//        if (return_check(ret, "residue()", error_head)) return -1;
//        /*
//        * 检查残差点数，若无残差点则不使用mcf.exe求解
//        */
//        for (int ii = 0; ii < num_triangle; ii++)
//        {
//            if (tri[ii].residue > 0.7)
//            {
//                positive++;
//            }
//            if (tri[ii].residue < -0.7)
//            {
//                negative++;
//            }
//        }
//        if (positive == 0 && negative == 0)
//        {
//
//            ret = unwrap.MCF(wrapped_phase_series[i], unwrapped_phase_series[i], out_mask, mask, nodes, edges, 200, false, distance_thresh);
//            if (return_check(ret, "unwrap.MCF()", error_head)) return -1;
//        }
//        else
//        {
//            ret = write_DIMACS(mcf_problem.c_str(), tri, nodes, edges, quality_map[i]);
//            if (return_check(ret, "write_DIMACS()", error_head)) return -1;
//            ret = unwrap.mcf_delaunay(mcf_problem.c_str(), mcf_exe_path);
//            if (return_check(ret, "unwrap.mcf_delaunay()", error_head)) return -1;
//            ret = read_DIMACS(mcf_solution.c_str(), edges, nodes, tri);
//            if (return_check(ret, "read_DIMACS()", error_head)) return -1;
//            ret = unwrap.MCF(wrapped_phase_series[i], unwrapped_phase_series[i], out_mask, mask, nodes, edges, 200, false, distance_thresh);
//            if (return_check(ret, "unwrap.MCF()", error_head)) return -1;
//        }
//
//    }
//    return 0;
//}

//int Utils::unwrap_3D_adaptive_tiling(
//    const Mat& mask,
//    const vector<Mat>& quality_map,
//    vector<Mat>& wrapped_phase_series,
//    vector<Mat>& unwrapped_phase_series,
//    const char* delaunay_exe_path,
//    const char* mcf_exe_path,
//    const char* tmp_file_path,
//    double distance_thresh,
//    double quality_thresh
//)
//{
//    if (mask.rows < 2 ||
//        mask.cols < 2 ||
//        mask.type() != CV_32S ||
//        wrapped_phase_series.size() < 2 ||
//        delaunay_exe_path == NULL ||
//        tmp_file_path == NULL ||
//        mcf_exe_path == NULL ||
//        quality_map.size() != wrapped_phase_series.size() ||
//        distance_thresh < 1.0 ||
//        quality_thresh > 1.0
//        )
//    {
//        fprintf(stderr, "unwrap_3D_adaptive_tiling(): input check failed!\n");
//        return -1;
//    }
//
//    /*----------------------------------*/
//    /*           自适应分块             */
//    /*----------------------------------*/
//
//    /*
//    * 确定搜索范围半径
//    */
//    int r = (int)floor(distance_thresh), c, ret;
//    Mat c_mat = Mat::zeros(2 * r + 1, 1, CV_32S);
//    for (int i = 0; i < 2 * r + 1; i++)
//    {
//        c_mat.at<int>(i, 0) = (int)floor(sqrt(distance_thresh * distance_thresh - double(r - i) * double(r - i)));
//    }
//
//    Mat mask1, mask2; mask.copyTo(mask1);
//    int nr = mask.rows; int nc = mask.cols;
//    int n_images = wrapped_phase_series.size();
//    size_t num_nodes = cv::countNonZero(mask);
//    if (num_nodes < 3)
//    {
//        fprintf(stderr, "unwrap_3D_adaptive_tiling(): at least 3 nodes are needed!\n");
//        return -1;
//    }
//    size_t node_count = 0;
//    int block_count = 1, row, col, mask2_start_row = nr, mask2_end_row = -1, mask2_start_col = nc, mask2_end_col = -1;
//    bool b_break;
//    char str[4096];
//    node_index node_ix, node_ix2;
//    queue<node_index> que;
//    vector<Mat> wrapped_phase, unwrapped_phase, qualitymap;
//    wrapped_phase.resize(n_images);
//    qualitymap.resize(n_images);
//    unwrapped_phase_series.resize(n_images);
//    for (int i = 0; i < n_images; i++)
//    {
//        wrapped_phase_series[i].copyTo(unwrapped_phase_series[i]);
//    }
//    while (node_count != num_nodes)
//    {
//        mask2_start_row = nr, mask2_end_row = -1, mask2_start_col = nc, mask2_end_col = -1;
//        b_break = false;
//        for (int i = 0; i < nr; i++)
//        {
//            for (int j = 0; j < nc; j++)
//            {
//                if (mask1.at<int>(i, j) == 1)
//                {
//                    mask2_start_row = mask2_start_row > i ? i : mask2_start_row;
//                    mask2_end_row = mask2_end_row < i ? i : mask2_end_row;
//                    mask2_start_col = mask2_start_col > j ? j : mask2_start_col;
//                    mask2_end_col = mask2_end_col < j ? j : mask2_end_col;
//
//                    mask1.at<int>(i, j) += block_count; //标注已选点并将其加入到待处理队列中
//                    node_ix.row = i; node_ix.col = j;
//                    que.push(node_ix);
//                    b_break = true;
//                    break;
//                }
//            }
//            if (b_break) break;
//        }
//
//        /*
//        * 搜索满足条件的点，并加入队列
//        */
//
//        while (!que.empty())
//        {
//            node_ix = que.front();
//            que.pop();
//            node_count++;
//
//            for (int i = 0; i <= 2 * r; i++)
//            {
//                c = c_mat.at<int>(i, 0);
//                for (int j = 0; j <= 2 * c; j++)
//                {
//                    row = node_ix.row + (i - r);
//                    row = row < 0 ? 0 : row; row = row > nr - 1 ? nr - 1 : row;
//                    col = node_ix.col + (j - c);
//                    col = col < 0 ? 0 : col; col = col > nc - 1 ? nc - 1 : col;
//                    if (mask1.at<int>(row, col) == 1)
//                    {
//                        mask2_start_row = mask2_start_row > row ? row : mask2_start_row;
//                        mask2_end_row = mask2_end_row < row ? row : mask2_end_row;
//                        mask2_start_col = mask2_start_col > col ? col : mask2_start_col;
//                        mask2_end_col = mask2_end_col < col ? col : mask2_end_col;
//
//                        mask1.at<int>(row, col) += block_count; //标注已选点
//                        node_ix2.row = row; node_ix2.col = col;
//                        que.push(node_ix2);
//                    }
//                }
//            }
//        }
//
//        /*
//        * 确定新的掩膜矩阵mask2
//        */
//
//        mask2 = Mat::zeros(mask2_end_row - mask2_start_row + 1, mask2_end_col - mask2_start_col + 1, CV_32S);
//
//        for (int i = mask2_start_row; i <= mask2_end_row; i++)
//        {
//            for (int j = mask2_start_col; j <= mask2_end_col; j++)
//            {
//                if (mask1.at<int>(i, j) == block_count + 1)
//                {
//                    mask2.at<int>(i - mask2_start_row, j - mask2_start_col) = 1;
//                }
//            }
//        }
//        mask2.convertTo(mask2, CV_64F);
//        cvmat2bin("E:\\working_dir\\projects\\software\\InSAR\\bin\\mask2.bin", mask2);
//        mask2.convertTo(mask2, CV_32S);
//        if (3 > cv::countNonZero(mask2))
//        {
//            block_count++;
//            continue;
//        }
//
//        /*
//        * 3D相位解缠
//        */
//
//        for (int i = 0; i < n_images; i++)
//        {
//            wrapped_phase_series[i](cv::Range(mask2_start_row, mask2_end_row + 1), cv::Range(mask2_start_col, mask2_end_col + 1)).copyTo
//            (wrapped_phase[i]);
//            quality_map[i](cv::Range(mask2_start_row, mask2_end_row + 1), cv::Range(mask2_start_col, mask2_end_col + 1)).copyTo
//            (qualitymap[i]);
//        }
//
//        //ret = unwrap_3D(mask2, qualitymap, wrapped_phase, unwrapped_phase, delaunay_exe_path, tmp_file_path,
//        //    distance_thresh + 1.0, quality_thresh);
//        ret = unwrap_3D_mcf(mask2, qualitymap, wrapped_phase, unwrapped_phase, delaunay_exe_path,
//            mcf_exe_path, tmp_file_path, distance_thresh);
//        if (return_check(ret, "unwrap_3D_mcf()", error_head)) return -1;
//
//        for (int i = 0; i < n_images; i++)
//        {
//            unwrapped_phase[i].copyTo(unwrapped_phase_series[i](cv::Range(mask2_start_row, mask2_end_row + 1),
//                cv::Range(mask2_start_col, mask2_end_col + 1)));
//
//            memset(str, 0, 4096);
//            sprintf(str, "H:\\data\\experiment\\test\\Regis2\\unwrapped_phase_%d.jpg", i + 1);
//            //cvmat2bin(str, unwrapped_phase[i]);
//            savephase(str, "jet", unwrapped_phase[i]);
//        }
//        block_count++;
//
//    }
//    mask1.convertTo(mask1, CV_64F);
//    cvmat2bin("E:\\working_dir\\projects\\software\\InSAR\\bin\\mask1.bin", mask1);
//    return 0;
//}

















tri_node::tri_node()
{
    this->rows = 0;
    this->cols = 0;
    this->num_neigh_edges = 0;
    this->phase = 0;
    this->b_unwrapped = false;
    this->b_balanced = true;
    this->neigh_edges = NULL;
    this->b_residue = false;
    this->epsilon_height = 0.0;
    this->vel = 0.0;
    //std::cout << "constructor1" << "\n";
}

tri_node::tri_node(const tri_node& node)
{
    this->b_unwrapped = node.b_unwrapped;
    this->b_balanced = node.b_balanced;
    this->cols = node.cols;
    this->b_residue = node.b_residue;
    int num_node = node.num_neigh_edges <= 0 ? 1 : node.num_neigh_edges;
    if (node.neigh_edges == NULL)
    {
        this->neigh_edges = NULL;
    }
    else
    {
        this->neigh_edges = (long*)malloc(sizeof(long) * num_node);
        long* ptr = NULL;
        int num;
        node.get_neigh_ptr(&ptr, &num);
        if (this->neigh_edges != NULL || num > 0 || ptr != NULL)
        {
            std::memcpy(this->neigh_edges, ptr, sizeof(long) * num_node);
        }
    }

    this->num_neigh_edges = node.num_neigh_edges;
    this->phase = node.phase;
    this->rows = node.rows;
    this->epsilon_height = node.epsilon_height;
    this->vel = node.vel;
    //std::cout << "constructor2" << "\n";
}

tri_node::tri_node(int row, int col, int num_neigh_edge, double phi)
{
    this->rows = row;
    this->cols = col;
    this->num_neigh_edges = num_neigh_edge;
    this->phase = phi;
    this->b_unwrapped = false;
    this->b_residue = false;
    this->b_balanced = true;
    this->epsilon_height = 0.0;
    this->vel = 0.0;
    if (num_neigh_edge > 0)
    {
        this->neigh_edges = (long*)malloc(sizeof(long) * num_neigh_edge);
    }
    else
    {
        this->neigh_edges = NULL;
    }
    if (this->neigh_edges != NULL)
    {
        for (int i = 0; i < num_neigh_edge; i++)
        {
            *(this->neigh_edges + i) = -1;//初始化邻接边序号都为-1
        }
    }

    //std::cout << "constructor3" << "\n";
}

tri_node::~tri_node()
{
    if (this->neigh_edges != NULL)
    {
        free(this->neigh_edges);
        this->neigh_edges = NULL;
    }
    //std::cout << "destructor" << "\n";
}

tri_node tri_node::operator=(const tri_node& src)
{
    if (src.neigh_edges == this->neigh_edges && this->neigh_edges != NULL)//两者相等
    {
        return *this;
    }
    else
    {
        if (this->neigh_edges)
        {
            free(this->neigh_edges);
            this->neigh_edges = NULL;
        }
        if (src.num_neigh_edges > 0)
        {
            this->neigh_edges = (long*)malloc(src.num_neigh_edges * sizeof(long));
            if (this->neigh_edges != NULL && src.neigh_edges != NULL)
            {
                memcpy(this->neigh_edges, src.neigh_edges, src.num_neigh_edges * sizeof(long));
            }
        }
        this->b_balanced = src.b_balanced;
        this->b_residue = src.b_residue;
        this->b_unwrapped = src.b_unwrapped;
        this->cols = src.cols;
        this->rows = src.rows;
        this->num_neigh_edges = src.num_neigh_edges;
        this->phase = src.phase;
        this->epsilon_height = src.epsilon_height;
        this->vel = src.vel;
        return *this;
    }
}

int tri_node::get_phase(double* phi) const
{
    if (phi == NULL)
    {
        fprintf(stderr, "get_phase(): input check failed!\n\n");
        return -1;
    }
    *phi = this->phase;
    return 0;
}

int tri_node::get_pos(int* rows, int* cols) const
{
    if (rows == NULL ||
        cols == NULL)
    {
        fprintf(stderr, "tri_node::get_pos(): input check failed!\n\n");
        return -1;
    }
    *rows = this->rows;
    *cols = this->cols;
    return 0;
}

int tri_node::set_phase(double phi)
{
    this->phase = phi;
    return 0;
}

int tri_node::get_neigh_ptr(long** ptr2ptr, int* num) const
{
    if (ptr2ptr == NULL || num == NULL)
    {
        fprintf(stderr, "get_neigh_ptr(): input check failed!\n\n");
        return -1;
    }
    *ptr2ptr = this->neigh_edges;
    *num = this->num_neigh_edges;
    return 0;
}

int tri_node::set_status(bool b_unwrapped)
{
    this->b_unwrapped = b_unwrapped;
    return 0;
}

int tri_node::set_balance(bool b_balanced)
{
    this->b_balanced = b_balanced;
    return 0;
}

int tri_node::print_neighbour() const
{
    if (this->num_neigh_edges <= 0)
    {
        fprintf(stdout, "no neighbour edges!\n");
        return 0;
    }
    for (int i = 0; i < this->num_neigh_edges; i++)
    {
        fprintf(stdout, "%ld ", *(this->neigh_edges + i));
    }
    fprintf(stdout, "\n");
    return 0;
}

int tri_node::get_num_neigh(int* num_neigh) const
{
    *num_neigh = this->num_neigh_edges;
    return 0;
}

int tri_node::get_distance(tri_node node, double* distance) const
{
    *distance = sqrt(((double)node.rows - (double)this->rows) * ((double)node.rows - (double)this->rows) +
        ((double)node.cols - (double)this->cols) * ((double)node.cols - (double)this->cols));
    return 0;
}

bool tri_node::get_status() const
{
    return this->b_unwrapped;
}

bool tri_node::get_balance() const
{
    return this->b_balanced;
}

bool tri_node::is_residue_node() const
{
    return this->b_residue;
}

int tri_node::set_residue(bool b_res)
{
    this->b_residue = b_res;
    return 0;
}

double tri_node::get_vel() const
{
    return this->vel;
}

double tri_node::get_height() const
{
    return this->epsilon_height;
}

int tri_node::set_vel(double vel)
{
    this->vel = vel;
    return 0;
}

int tri_node::set_height(double height)
{
    this->epsilon_height = height;
    return 0;
}

6、需要使用的头文件

 6.1：com_htzs_insar_jni_Registration.h

/* DO NOT EDIT THIS FILE - it is machine generated */
#include <jni.h>
/* Header for class com_htzs_insar_jni_Registration */

#ifndef _Included_com_htzs_insar_jni_Registration
#define _Included_com_htzs_insar_jni_Registration
#ifdef __cplusplus
extern "C" {
#endif
/*
 * Class:     com_htzs_insar_jni_Registration
 * Method:    calcOffset
 * Signature: (I[S[S[S[S)Lcom/htzs/insar/jni/Point;
 */
JNIEXPORT jobject JNICALL Java_com_htzs_insar_jni_Registration_calcOffset
  (JNIEnv *, jclass, jint, jshortArray, jshortArray, jshortArray, jshortArray);

/*
 * Class:     com_htzs_insar_jni_Registration
 * Method:    fitting
 * Signature: ([Lcom/htzs/insar/jni/Point;[Lcom/htzs/insar/jni/Point;D)Lcom/htzs/insar/jni/Fit;
 */
JNIEXPORT jobject JNICALL Java_com_htzs_insar_jni_Registration_fitting
  (JNIEnv *, jclass, jobjectArray, jobjectArray, jdouble);

/*
 * Class:     com_htzs_insar_jni_Registration
 * Method:    registration
 * Signature: (I[S[SLcom/htzs/insar/jni/Point;Lcom/htzs/insar/jni/Fit;I)[S
 */
JNIEXPORT jshortArray JNICALL Java_com_htzs_insar_jni_Registration_registration
  (JNIEnv *, jclass, jint, jshortArray, jshortArray, jobject, jobject, jint);

#ifdef __cplusplus
}
#endif
#endif

 6.2：ComplexMat.h

#pragma once
#ifndef __COMPLEXMAT__H__
#define __COMPLEXMAT__H__
#include"opencv2/core/core.hpp"
#include"opencv2/highgui/highgui.hpp"
#include"opencv2/imgproc/imgproc.hpp"
#include"opencv2/opencv.hpp"
#include <omp.h>  /*多线程计算库*/

using cv::Mat;
namespace InSAR {
    class  ComplexMat
    {
    public:
        ComplexMat();
        ComplexMat(Mat& real, Mat& imagine);
        ComplexMat(int rows, int cols);
        /*拷贝构造函数*/
        ComplexMat(const ComplexMat& b);
        ~ComplexMat();
        void SetRe(Mat& re);
        void SetIm(Mat& im);
        Mat GetRe() const;
        Mat GetIm() const;
        Mat GetMod() const;
        /*计算复矩阵的相位*/
        Mat GetPhase();
        int type() const;
        int GetRows() const;
        int GetCols() const;
        /*计算复数（共轭）点乘*/
        int Mul(const ComplexMat& Src, ComplexMat& Dst, bool bConj) const;
        /*计算复数乘积(点乘,elementwise)*/
        ComplexMat operator*(const ComplexMat& b) const;
        /*复数矩阵与实数矩阵对应相乘*/
        ComplexMat operator*(const Mat& a) const;
        /*复数矩阵乘以常数*/
        ComplexMat operator*(const double& a) const;
        /*取出部分复数矩阵*/
        ComplexMat operator()(cv::Range _rowRange, cv::Range _colRange) const;
        /*将复数矩阵部分进行赋值*/
        int SetValue(cv::Range _rowRange, cv::Range _colRange, ComplexMat& src);
        /*复数矩阵加法*/
        ComplexMat operator+(const ComplexMat& b) const;
        /*深拷贝赋值*/
        ComplexMat operator=(const ComplexMat&);
        /*复数矩阵内求和
        * 参数1 求和方向（0为沿着每列求和，1为沿着每行求和）
        */
        ComplexMat sum(int dim = 0) const;
        /*求取复共轭*/
        ComplexMat conj() const;
        /*计算非零元素个数*/
        int countNonzero() const;
        /*数组是否为空*/
        bool isempty()const;
        /*转换类型*/
        void convertTo(ComplexMat& out, int type) const;
        Mat re;
        Mat im;
    private:

        Mat mod;
        Mat Phase;
    };
}


#endif // !__COMPLEXMAT__H__

 6.3：Registration.h

#pragma once
#ifndef __REGISTRATION__H__
#define __REGISTRATION__H__

#include"opencv2/core/core.hpp"
#include"opencv2/highgui/highgui.hpp"
#include"opencv2/imgproc/imgproc.hpp"
#include"opencv2/opencv.hpp"
#include <omp.h>  /*多线程计算库*/
#include<vector>

#include"ComplexMat.h"
#include"Utils.h"


using cv::Mat;
using InSAR::ComplexMat;
namespace InSAR
{
    
    class Registration
    {
    public:
        Registration();
        ~Registration();
        /*求取两幅辅图像的实相关函数
         参数1 主图像（复）
         参数2 辅图像（复）
         参数3 行偏移量（返回值）
         参数4 列偏移量（返回值）
        */
        int real_coherent(ComplexMat& Master, ComplexMat& Slave, int* offset_row, int* offset_col);
        /*2D FFTSHIFT(原地操作)*/
        int fftshift2(Mat& matrix);
        /*2D FFT
         参数1 输入矩阵
         参数2 输出矩阵
        */
        int fft2(Mat& Src, Mat& Dst);
        /*像元级配准（原地操作）
         参数1 主图像（复）（输入值/返回值）
         参数2 辅图像（复）（输入值/返回值）
         参数3 行偏移量（返回值）
         参数4 列偏移量（返回值）
        */
        int registration_pixel(ComplexMat& Master, ComplexMat& Slave, int* move_r = NULL, int* move_c = NULL);
        /*频域补零插值
         参数1 输入矩阵（复）
         参数2 输出矩阵（复）
         参数3 插值倍数（大于1， 且是2的n次幂）
        */
        int interp_paddingzero(ComplexMat& InputMatrix, ComplexMat& OutputMatrix, int interp_times);
        /*立方插值
         参数1 输入矩阵
         参数2 输出矩阵
         参数3 行偏移量
         参数4 列偏移量
        */
        int interp_cubic(ComplexMat& InputMatrix, ComplexMat& OutputMatrix, double offset_row, double offset_col);
        /*立方插值
        参数1 输入矩阵
        参数2 输出矩阵
        参数3 拟合系数
        */
        int interp_cubic(ComplexMat& InputMatrix, ComplexMat& OutputMatrix, Mat& Coefficient);
        /*计算每个像素的偏移量
         参数1 像素行号
         参数2 像素列号
         参数3 拟合系数
         参数4 行偏移量
         参数5 列偏移量
        */
        int every_subpixel_move(int i, int j, Mat& coefficient, double* offset_row, double* offset_col);
        /*计算卷积核权重*/
        double WeightCalculation(double offset);
        /*亚像素级配准
         参数1 主图像（复）
         参数2 辅图像（复）
         参数3 子块大小（大于1）
         参数4 插值倍数（大于1）
        */
        int registration_subpixel(ComplexMat& Master, ComplexMat& Slave, int blocksize, int interp_times);
        /** @brief 精配准

        @param master              主图像
        @param slave               辅图像
        @param blocksize           主图像参考块分块大小（blocksize×blocksize，blocksize为2的n次幂）
        @param interp_times        插值倍数(InSAR要求至少8倍插值)
        */
        int coregistration_subpixel(
            ComplexMat& master,
            ComplexMat& slave,
            int blocksize,
            int interp_times
        );
        /*拟合像素偏移量
         参数1 行序列号
         参数2 列序列号
         参数3 行偏移量
         参数4 列偏移量
         参数5 拟合系数（返回值）
        */
        int all_subpixel_move(Mat& Coordinate_x, Mat& Coordinate_y, Mat& offset_row, Mat& offset_col, Mat& para);
        /*根据粗配准偏移量筛选控制点
        * 参数1 原始图像行数
        * 参数2 原始图像列数
        * 参数3 行偏移
        * 参数4 列偏移
        * 参数5 控制点信息
        */
        int gcps_sift(int rows, int cols, int move_rows, int move_cols, Mat& gcps);


    private:
        char error_head[256];
        char parallel_error_head[256];

    };
}






#endif // !__REGISTRATION__H__

 6.4：Utils.h

#pragma once
#ifndef __UTILS__H__
#define __UTILS__H__
#include<fstream>
#include<iostream>
#include<vector>
#include<string>
#include"opencv2/core/core.hpp"
#include"opencv2/highgui/highgui.hpp"
#include"opencv2/imgproc/imgproc.hpp"
#include"opencv2/opencv.hpp"
#include <omp.h>  /*多线程计算库*/

#include"ComplexMat.h"


#define INPUTMAXSIZE 1024
#define PI 3.141592653589793238
#define VEL_C 299792458.0

using cv::Mat;
using std::vector;
using std::string;
using InSAR::ComplexMat;

namespace InSAR
{
    /*********************************************************/
    /*                Delaunay三角网 节点类                  */
    /*********************************************************/
    class  tri_node
    {
    public:
        /*默认构造函数*/
        tri_node();
        /*拷贝构造函数*/
        tri_node(const tri_node& node);
        /*构造函数
        * 参数1 节点行数
        * 参数2 节点列数
        * 参数3 节点邻接边数
        * 参数4 节点相位
        */
        tri_node(int, int, int, double);
        ~tri_node();
        /*赋值函数（深拷贝赋值）*/
        tri_node operator = (const tri_node& src);
        /*获取节点相位
        * 参数1 相位指针（返回值）
        */
        int get_phase(double* phi) const;
        /*获取节点行列坐标
        * 参数1 行序号
        * 参数2 列序号
        */
        int get_pos(int* rows, int* cols) const;
        /*节点相位赋值
        * 参数1 输入相位
        */
        int set_phase(double phi);
        /*获取邻接边指针
        * 参数1 指向邻接边指针的指针（返回值）
        * 参数2 邻接边个数指针（返回值）
        */
        int get_neigh_ptr(long** ptr2ptr, int* num) const;
        /*改变解缠状态
        * 参数1 是否已经解缠
        */
        int set_status(bool b_unwrapped);
        /*改变平衡状态
        * 参数1 是否属于残差平衡三角形
        */
        int set_balance(bool b_balanced);
        /*打印邻接边序号
        *
        */
        int print_neighbour() const;
        /*获取邻接边个数
        * 参数1 邻接边个数指针
        */
        int get_num_neigh(int* num_neigh) const;
        /*获取与另一节点的距离
        * 参数1 另一节点
        * 参数2 距离
        */
        int get_distance(tri_node node, double* distance) const;
        /*获取解缠状态
        * 返回值（是否已解缠）
        */
        bool get_status() const;
        /*获取平衡状态
        * 返回值（是否平衡，默认是）
        */
        bool get_balance() const;
        /*返回是否节点属于残差三角形
        */
        bool is_residue_node() const;
        /*设置节点是否属于残差节点
        */
        int set_residue(bool b_res);
        /*获取形变速率*/
        double get_vel() const;
        /*获取高程误差*/
        double get_height() const;
        /*设置形变速率*/
        int set_vel(double vel);
        /*设置高程误差*/
        int set_height(double height);

    private:

        /*****************InSAR处理变量*******************/

        /*是否已解缠(默认未解缠)*/
        bool b_unwrapped;
        /*是否属于残差节点*/
        bool b_residue;
        /*是否属于平衡三角形的顶点（默认为是），同时在PS-InSAR中充当是否节点被丢弃的标志(为true表示不被丢弃， 为false表示被丢弃)*/
        bool b_balanced;
        /*节点行数（起始值为0）*/
        int rows;
        /*节点列数（起始值为0）*/
        int cols;
        /*节点邻接边数*/
        int num_neigh_edges;
        /*节点相位*/
        double phase;
        /*节点邻接边序号*/
        long* neigh_edges;

        /*****************PS-InSAR处理变量*******************/

        /*形变速率*/
        double vel;
        /*高程误差*/
        double epsilon_height;
    };

    /*********************************************************/
    /*             Delaunay三角网 三角形结构体               */
    /*********************************************************/
    struct triangle
    {
        /*三角形序号*/
        int num;
        /*点1*/
        int p1;
        /*点2*/
        int p2;
        /*点3*/
        int p3;
        /*三角形残差值*/
        double residue;
        /*相邻三角形序号1*/
        int neigh1;
        /*相邻三角形序号2*/
        int neigh2;
        /*相邻三角形序号3*/
        int neigh3;
        /*边1（从1开始）*/
        int edge1;
        /*边2（从1开始）*/
        int edge2;
        /*边3（从1开始）*/
        int edge3;

        /*默认构造函数*/
        triangle()
        {
            num = p1 = p2 = p3 = neigh1 = neigh2 = neigh3 = edge1 = edge2 = edge3 = 0;
            residue = 0.0;
        }
        /*拷贝构造函数*/
        triangle(const triangle& cp)
        {
            this->edge1 = cp.edge1;
            this->edge2 = cp.edge2;
            this->edge3 = cp.edge3;
            this->neigh1 = cp.neigh1;
            this->neigh2 = cp.neigh2;
            this->neigh3 = cp.neigh3;
            this->num = cp.num;
            this->p1 = cp.p1; this->p2 = cp.p2; this->p3 = cp.p3;
            this->residue = cp.residue;
        }
        /*赋值(深拷贝)*/
        triangle operator= (const triangle& cp)
        {
            this->edge1 = cp.edge1;
            this->edge2 = cp.edge2;
            this->edge3 = cp.edge3;
            this->neigh1 = cp.neigh1;
            this->neigh2 = cp.neigh2;
            this->neigh3 = cp.neigh3;
            this->num = cp.num;
            this->p1 = cp.p1; this->p2 = cp.p2; this->p3 = cp.p3;
            this->residue = cp.residue;
            return *this;
        }
    };


    /*********************************************************/
    /*             Delaunay三角网 三角形边结构体             */
    /*********************************************************/
    struct tri_edge
    {
        /**********InSAR变量**********/

        /*积分增益（序号从小到大为正）*/
        double gain;
        /*相位质量（用于质量图法解缠）*/
        double quality;
        /*边序列号*/
        int num;
        /*端点1*/
        int end1;
        /*端点2*/
        int end2;
        /*残差边标志*/
        bool isResidueEdge;
        /*网络边界标志*/
        bool isBoundry;


        /**********PS_InSAR变量**********/

        /*线性形变速度差系数（4 * pi / lambda * Ti）*/
        //double coef_delta_vel;
        /*高程误差系数（4 * pi * bperp_i / lambda / R_i / sin_theta_i  ）*/
        //double coef_delta_height;
        /*线性形变速度差(定义为大坐标 - 小坐标)*/
        double delta_vel;
        /*高程误差(定义为大坐标 - 小坐标)*/
        double delta_height;
        /*模型相干系数*/
        double MC;
        /*端点相位差（相位差定义为：大序号端点减小序号端点）*/
        double phase_diff;

        /*默认构造函数*/
        tri_edge() {
            gain = 0.0;
            quality = 0.0;
            num = 0;
            end1 = 0; end2 = 0;
            isResidueEdge = false;
            isBoundry = false;
            delta_vel = 0.0;
            delta_height = 0.0; MC = 0.0; phase_diff = 0.0;
        }
        /*拷贝构造函数*/
        tri_edge(const tri_edge& cp)
        {
            gain = cp.gain;
            quality = cp.quality;
            num = cp.num;
            end1 = cp.end1; end2 = cp.end2;
            isResidueEdge = cp.isResidueEdge;
            isBoundry = cp.isBoundry;
            delta_vel = cp.delta_vel;
            delta_height = cp.delta_height; MC = cp.MC; phase_diff = cp.phase_diff;
        }
        /*赋值函数（深拷贝赋值）*/
        tri_edge operator = (const tri_edge& cp)
        {
            gain = cp.gain;
            quality = cp.quality;
            num = cp.num;
            end1 = cp.end1; end2 = cp.end2;
            isResidueEdge = cp.isResidueEdge;
            isBoundry = cp.isBoundry;
            delta_vel = cp.delta_vel;
            delta_height = cp.delta_height; MC = cp.MC; phase_diff = cp.phase_diff;
            return *this;
        }
    };

    /*********************************************************/
    /*          Delaunay三角网 三角形边序列号结构体          */
    /*********************************************************/

    struct edge_index
    {
        double quality;
        int num;
        edge_index() { num = 0; quality = 0.0; }
        friend bool operator < (struct edge_index a, struct edge_index b)
        {
            return a.quality > b.quality;
        }

    };

    /*-------------------------------------------------------*/
    /*                   规则网格节点结构体                  */
    /*-------------------------------------------------------*/

    struct node_index
    {
        /*节点行数（从0开始）*/
        int row;
        /*节点列数（从0开始）*/
        int col;
        /*默认构造函数*/
        node_index()
        {
            row = 0; col = 0;
        }
        /*拷贝构造函数*/
        node_index(const node_index& cp)
        {
            this->row = cp.row; this->col = cp.col;
        }
        /*赋值函数*/
        node_index operator = (const node_index& cp)
        {
            this->row = cp.row; this->col = cp.col;
            return *this;
        }
    };


    /*********************************************************/
    /*               干涉SAR处理基本函数类库                 */
    /*********************************************************/
    class  Utils
    {
    public:
        Utils();
        ~Utils();
        /** @brief 求int型矩阵的众数

        @param input                  输入矩阵（int型）
        @param out                    输出结果
        @return 成功返回0，否则返回-1
        */
        int get_mode_index(const Mat& input, int* out);
        /*计算矩阵梯度
         参数1 源矩阵
         参数2 行方向梯度（返回值）
         参数3 列方向梯度（返回值）
         参数4 是否补零使得梯度矩阵和源矩阵大小相同（默认补零）
        */
        int diff(Mat& Src, Mat& diff_1, Mat& diff_2, bool same = true);
        /*计算干涉相位
         参数1 主图像（复）
         参数2 辅图像（复）
         参数3 干涉相位（返回值）
        */
        int generate_phase(const ComplexMat& Master, const ComplexMat& Slave, Mat& phase);

        /** @brief 最大似然相干估算器

        @param master_image                       主图像（复）
        @param slave_image                        辅图像（复）
        @param coherence                          相干系数（返回值）
        @return 成功返回0，否则返回-1
        */
        int real_coherence(ComplexMat& master_image, ComplexMat& slave_image, Mat& coherence);
        /** @brief 最大似然相干估算器（带估计窗口尺寸接口）

        @param master_image                       主图像（复）
        @param slave_image                        辅图像（复）
        @param est_wndsize_rg                     估计窗口距离向尺寸（奇数）
        @param est_wndsize_az                     估计窗口方位向尺寸（奇数）
        @param coherence                          相干系数（返回值）
        */
        int real_coherence(
            const ComplexMat& master_image,
            const ComplexMat& slave_image,
            int est_wndsize_rg,
            int est_wndsize_az,
            Mat& coherence
        );
        /** @brief 频率无关相干估算器

         @param master_image                        主图像（复）
         @param slave_image                         辅图像（复）
         @param coherence                           相干系数（返回值）
         @return 成功返回0，否则返回-1
        */
        int complex_coherence(ComplexMat& master_image, ComplexMat& slave_image, Mat& coherence);
        /** @brief 频率无关相干估算器（带估计窗口尺寸接口）

        @param master_image                         主图像
        @param slave_image                          辅图像
        @param est_wndsize_rg                       估计窗口距离向尺寸（奇数）
        @param est_wndsize_az                       估计窗口方位向尺寸（奇数）
        @param coherence                            相关系数（返回值）
        @return 成功返回0，否则返回-1
        */
        int complex_coherence(
            const ComplexMat& master_image,
            const ComplexMat& slave_image,
            int est_wndsize_rg,
            int est_wndsize_az,
            Mat& coherence
        );
        /** @brief 根据干涉相位求相关系数
        @param phase                          输入相位
        @param coherence                      相关系数（返回值）
        @return 成功返回0，否则返回-1
        */
        int phase_coherence(Mat& phase, Mat& coherence);
        /** @brief 根据干涉相位求相关系数（带估计窗口尺寸接口）

        @param phase                          输入相位
        @param est_wndsize_rg                 估计窗口距离向尺寸（奇数）
        @param est_wndsize_az                 估计窗口方位向尺寸（奇数）
        @param coherence                      相关系数（返回值）
        @return 成功返回0，否则返回-1
        */
        int phase_coherence(
            const Mat& phase,
            int est_wndsize_rg,
            int est_wndsize_az,
            Mat& coherence
        );
        /*求解相位导数方差
        * 参数1 干涉相位
        * 参数2 相位导数方差（返回值）
        * 参数3 计算窗口大小（奇数）
        */
        int phase_derivatives_variance(Mat& phase, Mat& phase_derivatives_variance, int wndsize = 3);
        /*最大可积距离
        * 参数1 原始相位
        * 参数2 最大可积距离（返回值）
        * 参数3 保守值（最大积分距离不能超过该值）
        */
        int max_integrable_distance(Mat& phase, Mat& max_integrable_distance, double conservative_thresh = 20.0);
        /*FFTSHIFT
         参数1 待fftshift的矩阵（原地进行fftshift操作）
        */
        int fftshift(Mat& matrix);

        /*计算干涉相位图的残差值（点）
         参数1 干涉相位
         参数2 残差点矩阵（返回值）
        */
        int residue(Mat& phase, Mat& residue);
        /*计算Delaunay三角网络的残差值（并且标注残差边和残差节点,便于解缠时避开）
        * 参数1 Delaunay三角网三角形结构体数组
        * 参数2 Delaunay三角网三角形数量
        * 参数3 Delaunay三角网节点数组
        * 参数4 Delaunay三角网边结构体数组
        * 参数5 Delaunay三角网边数量
        */
        int residue(triangle* tri, int num_triangle, vector<tri_node>& nodes, tri_edge* edges, int num_edges);
        /** @brief 计算Delaunay三角网络的残差值（并且标注残差边和残差节点）

        @param triangle                              Delaunay三角网三角形结构体数组
        @param nodes                                 Delaunay三角网节点数组
        @param edges                                 Delaunay三角网边结构体数组
        @param distance_thresh                       边长度阈值（超过此阈值不参与残差点计算）
        @return 成功返回0，否则返回-1
        */
        int residue(
            vector<triangle>& triangle,
            vector<tri_node>& nodes,
            vector<tri_edge>& edges,
            double distance_thresh
        );
        /*计算mask（筛选高质量点）
        * 参数1 相关系数矩阵
        * 参数2 mask举矩阵（返回值）
        * 参数3 窗口半径
        * 参数4 阈值
        */
        int gen_mask(Mat& coherence, Mat& mask, int wnd_size, double thresh);
        /*计算mask（筛选高质量点）
        * 参数1 相关系数矩阵
        * 参数2 相位导数方差
        * 参数3 mask举矩阵（返回值）
        * 参数4 窗口半径
        * 参数5 相关系数阈值
        * 参数6 相位导数方差阈值
        */
        int gen_mask(
            Mat& coherence,
            Mat& phase_derivatives,
            Mat& mask, int wnd_size,
            double coh_thresh,
            double phase_derivative_thresh
        );
        /*根据设定阈值筛选残差点
        * 参数1 原始残差点矩阵
        * 参数2 筛选后残差点矩阵
        * 参数3 筛选阈值（大于0）
        * 参数4 残差点个数
        */
        int residue_sift(Mat& residue_src, Mat& residue_dst, double thresh, long* num_residue);
        /*缠绕相位至（-pi,pi）
         参数1 待缠绕相位
         参数2 缠绕后的相位（返回值）
        */
        int wrap(Mat& Src, Mat& Dst);

        /*按行或列累计积分
         参数1 待积分数据
         参数2 积分方向(dim = 1,按列计算 dim = 2,按行计算)
        */
        int cumsum(Mat& phase, int dim);
        /*叉乘运算（三维）
        * 参数1 向量一(n * 3)
        * 参数2 向量二(n * 3)
        * 参数3 输出
        */
        int cross(Mat& vec1, Mat& vec2, Mat& out);

        /*写入DIMACS文件（描述最小费用问题）
         参数1 目标文件名
         参数2 残差点矩阵
         参数3 相干系数矩阵
         参数4 残差点阈值(大于0)
        */
        int write_DIMACS(const char* DIMACS_file_problem, Mat& residue, Mat& coherence, double thresh);
        /*写入DIMACS文件（描述最小费用问题，不规则三角网络）
        * 参数1 目标文件名
        * 参数2 Delaunay三角形结构体数组
        * 参数3 Delaunay三角形数量
        * 参数4 Delaunay三角网节点数组
        * 参数5 Delaunay三角网边结构体数组
        * 参数6 Delaunay三角网边数量
        * 参数7 每个节点的费用
        */
        int write_DIMACS(
            const char* DIMACS_file_problem,
            triangle* tri,
            int num_triangle,
            vector<tri_node>& nodes,
            tri_edge* edges,
            long num_edges,
            Mat& cost
        );
        /** @brief 写入DIMACS文件（描述最小费用问题，Delaunay三角网络）

        @param DIMACS_file_problem                         目标DIMACS文件
        @param triange                                     Delaunay三角形结构体数组
        @param nodes                                       Delaunay三角网节点数组
        @param edges                                       Delaunay三角网边结构体数组
        @param cost                                        每个节点的费用
        @return 成功返回0，否则返回-1
        */
        int write_DIMACS(
            const char* DIMACS_file_problem,
            vector<triangle>& triangle,
            vector<tri_node>& nodes,
            vector<tri_edge>& edges,
            const Mat& cost
        );
        /*读取DIMACS文件（获取求解器求解结果）
         参数1 最小费用流问题解文件
         参数2 枝切路径1
         参数3 枝切路径2
         参数4 干涉相位图像行数
         参数5 干涉相位图像列数
        */
        int read_DIMACS(const char* DIMACS_file_solution, Mat& k1, Mat& k2, int rows, int cols);
        /*读取DIMACS文件（获取求解器求解结果）
        * 参数1 最小费用流问题解文件
        * 参数2 Delaunay三角网边结构体数组
        * 参数3 Delaunay三角网边数量
        * 参数4 Delaunay三角网节点数组
        * 参数5 Delaunay三角网三角形数组
        * 参数6 Delaunay三角网三角形数量
        */
        int read_DIMACS(
            const char* DIMACS_file_solution,
            tri_edge* edges,
            int num_edges,
            vector<tri_node>& nodes,
            triangle* tri,
            int num_triangle
        );
        /** @brief 读取DIMACS文件（获取求解器求解结果）

        @param DIMACS_file_solution                         最小费用流问题解文件
        @param edges                                        Delaunay三角网边结构体数组
        @param nodes                                        Delaunay三角网节点数组
        @param triangle                                     Delaunay三角网三角形数组
        @param return 成功返回0，否则返回-1
        */
        int read_DIMACS(
            const char* DIMACS_file_solution,
            vector<tri_edge>& edges,
            vector<tri_node>& nodes,
            vector<triangle>& triangle
        );
        /*将OpenCV Mat数据以二进制方式写入目标文件
        * 参数1 目标文件名
        * 参数2 待写入数据
        */
        int cvmat2bin(const char* Dst_file, Mat& Src);
        /*从二进制文件中读数据，并将数据转换成OpenCV Mat格式
        * 参数1 二进制文件
        * 参数2 目标矩阵
        */
        int bin2cvmat(const char* Src_file, Mat& Dst);
        /*InSAR多视处理（配准之后进行， 改变图像尺寸）
        * 参数1 主图像（SLC）
        * 参数2 辅图像（SLC）
        * 参数3 多视相位
        * 参数4 多视倍数（大于1）
        */
        int multilook(ComplexMat& Master, ComplexMat& Slave, Mat& phase, int multilook_times);
        /** @brief InSAR多视处理（不改变图像尺寸）

        @param master_slc                    主图像
        @param slave_slc                     辅图像
        @param multilook_rg                  距离向多视倍数
        @param multilook_az                  方位向多视倍数
        @param multilooked_phase             多视相位
        */
        int multilook(const ComplexMat& master, const ComplexMat& slave, int multilook_rg, int multilook_az, Mat& phase);
        /** @brief 将相位转换成cos和sin（实部和虚部）

        @param phase                     输入相位
        @param cos                       实部
        @param sin                       虚部
        @return 成功返回0，否则返回-1
        */
        int phase2cos(const Mat& phase, Mat& cos, Mat& sin);
        /*84坐标系转经纬高坐标系
        * 参数1 84坐标系坐标
        * 参数2 经纬高坐标系坐标（度/度/米）
        */
        int xyz2ell(Mat xyz, Mat& llh);
        /*经纬高坐标系转84坐标系
        * 参数1 经纬高坐标系坐标（纬度/经度/高度）
        * 参数2 84坐标系坐标
        */
        int ell2xyz(Mat llh, Mat& xyz);



        /*******************************************************/
        /*                     图像存储工具集                  */
        /*******************************************************/

        /*量化保存SLC功率图
        * 参数1 目标文件名
        * 参数2 功率量化参数（可视范围dB）
        * 参数3 单视复图像
        */
        int saveSLC(const char* filename, double db, ComplexMat& SLC);
        /*保存干涉相位图
        * 参数1 目标文件名
        * 参数2 颜色映射（jet/hsv/cool/parula等）
        * 参数3 待保存相位
        */
        int savephase(const char* filename, const char* colormap, Mat phase);
        /*图像重采样
        * 参数1 原图像
        * 参数2 目标图像
        * 参数3 目标图像高度
        * 参数4 目标图像宽度
        */
        int resampling(const char* Src_file, const char* Dst_file, int dst_height, int dst_width);
        /*量化SAR图像与干涉相位叠加
        * 参数1 量化SAR图像
        * 参数2 干涉相位图
        * 参数3 叠加图像
        * 参数4 SAR图像占比
        */
        int amplitude_phase_blend(
            const char* amplitude_file,
            const char* phase_file,
            const char* blended_file,
            double SAR_ratio = 0.9
        );



        /*******************************************************/
        /*                Delaunay三角网相关函数库             */
        /*******************************************************/

        /*从.edge文件读取Delaunay三角网的边信息
        * 参数1 .edge文件
        * 参数2 指向边结构体的指针（返回值，内存需要手动释放）
        * 参数3 指向边个数的指针（返回值）
        * 参数4 统计每个节点的邻接边数（返回值，内存需要手动释放）
        * 参数5 节点数
        */
        int read_edges(const char* filename, tri_edge** edges, long* num_edges, int** neighbours, long num_nodes);
        /** @brief 从.edge文件读取Delaunay三角网的边信息

        @param edge_file               .edge文件
        @param num_nodes               节点数
        @param edges                   Delaunay三角网边数组（返回值）
        @param node_neighbours         每个节点的邻接边数（返回值）
        @return  成功返回0， 否则返回-1
        */
        int read_edges(
            const char* edge_file,
            vector<tri_edge>& edges,
            vector<int>& node_neighbours,
            long num_nodes
        );
        /*初始化Delaunay三角网节点
        * 参数1 节点数组（返回值）
        * 参数2 相位(double型)
        * 参数3 相位mask（int 型）
        * 参数4 edges结构体数组
        * 参数5 edges个数
        * 参数6 每个节点的邻接边信息
        * 参数7 节点数
        */
        int init_tri_node(
            vector<tri_node>& node_array,
            Mat& phase,
            Mat& mask,
            tri_edge* edges,
            long num_edges,
            int* num_neighbour,
            int num_nodes
        );
        /** @brief 初始化Delaunay三角网节点

        @param node_array                 节点数组（返回值）
        @param phase                      相位值
        @param mask                       相位掩膜
        @param edges                      Delaunay三角网络边结构体数组
        @param node_neighbours            每个节点的邻边个数
        @param num_nodes                  节点数
        @return 成功返回0，否则返回-1
        */
        int init_tri_node(
            vector<tri_node>& node_array,
            const Mat& phase,
            const Mat& mask,
            const vector<tri_edge>& edges,
            const vector<int>& node_neighbours,
            int num_nodes
        );
        /** @brief 初始化Delaunay三角网络边相位差

        @param edges                  Delaunay三角网络边数组（已经使用read_edges函数初始化过的）
        @param node_array             Delaunay三角网络节点数组（已经使用init_tri_node函数初始化过的）
        @return 成功返回0，否则返回-1
        */
        int init_edge_phase_diff(
            vector<tri_edge>& edges,
            const vector<tri_node>& node_array
        );
        /*初始化Delaunay三角网边的相位质量
        * 参数1 相位质量图
        * 参数2 Delaunay三角网边结构体数组指针
        * 参数3 Delaunay三角网边结构体数组大小
        * 参数4 Delaunay三角网节点数组
        */
        int init_edges_quality(Mat& quality, tri_edge* edges, int num_edges, vector<tri_node>& nodes);
        /** @brief 初始化Delaunay三角网边的相位质量指数

        @param quality_index                  相位质量图指数（与相位质量相反）
        @param edges                          Delaunay三角网边结构体数组
        @param nodes                          Delaunay三角网节点数组
        @return 成功返回0， 否则返回-1
        */
        int init_edges_quality(
            const Mat& quality_index,
            vector<tri_edge>& edges,
            const vector<tri_node>& nodes
        );
        /*从.ele文件和.neigh文件读取Delaunay三角网的三角形信息
        * 参数1 .ele文件
        * 参数2 .neigh文件
        * 参数3 三角形结构体数组指针（返回值, 内存需要手动释放）
        * 参数4 三角形个数（返回值）
        * 参数5 Delaunay三角网节点数组
        * 参数6 Delaunay三角网边数组
        * 参数7 Delaunay三角网边数量
        */
        int read_triangle(
            const char* ele_file,
            const char* neigh_file,
            triangle** tri,
            int* num_triangle,
            vector<tri_node>& nodes,
            tri_edge* edges,
            int num_edgs
        );
        /** @brief 从.ele文件和.neigh文件读取Delaunay三角网的三角形信息

        @param ele_file                        .ele文件
        @param neigh_file                      .neigh文件
        @param triangle                        三角形结构体数组（返回值）
        @param nodes                           Delaunay三角网节点数组
        @param edges                           Delaunay三角网边数组
        @return 成功返回0，否则返回-1
        */
        int read_triangle(
            const char* ele_file,
            const char* neigh_file,
            vector<triangle>& triangle,
            vector<tri_node>& nodes,
            vector<tri_edge>& edges
        );
        /*写.node文件
        * 参数1 .node文件
        * 参数2 节点数组
        */
        int write_node_file(const char* filename, const Mat& mask);




        /*********************************************************/
        /*                PS-InSAR 常用函数                      */
        /*********************************************************/

        /*振幅离差指数法筛选PS点（D_A）
        * 参数1 SAR幅度矩阵组
        * 参数2 振幅离差阈值
        * 参数3 mask（满足条件的PS点位置mask为1，其他为0）
        */
        int PS_amp_dispersion(const vector<Mat>& amplitude, double thresh, Mat& mask);
        /*fifth-order butterworth filter（五阶巴特沃斯滤波器）
        * 参数1 grid_size
        * 参数2 n_win
        * 参数3 low_pass_wavelength
        * 参数4 滤波器系数（返回值）
        */
        int butter_lowpass(int grid_size, int n_win, double low_pass_wavelength, Mat& lowpass);
        /*circle_shift
        */
        int circshift(Mat& out, const cv::Point& delta);
        /*fftshift2
        */
        int fftshift2(Mat& out);
        /*ifftshift
        */
        int ifftshift(Mat& out);
        /*二维傅里叶变换
        * 参数1 输入矩阵
        * 参数2 输出结果
        */
        int fft2(Mat& Src, Mat& Dst);
        /*复数二维傅里叶变换
        * 参数1 输入矩阵
        * 参数2 输出结果
        */
        int fft2(ComplexMat& src, ComplexMat& dst);
        /*逆二维傅里叶变换
        * 参数1 输入矩阵
        * 参数2 输出结果
        */
        int ifft2(ComplexMat& src, ComplexMat& dst);
        /*求标准差
        * 参数1 输入矩阵
        * 参数2 标准差返回值
        */
        int std(const Mat& input, double* std);

        /*SAR图像干涉相位序列去平地
        * 参数1 干涉相位序列（原地操作）
        * 参数2 干涉组合
        * 参数3 卫星轨道参数（插值后， n_images×6）
        * 参数4 地面控制点信息（纬度/经度/高度/行/列, n_gcps × 5）
        * 参数5 SAR图像左上角在原SAR图像中的行数(可以直接设置为1)
        * 参数6 SAR图像左上角在原SAR图像中的列数(可以直接设置为1)
        * 参数7 收发方式（1单发单收， 2单发双收）
        * 参数8 波长
        */
        int PS_deflat(
            vector<Mat>& interf_phase,
            Mat& interf_combination,
            vector<Mat>& pos,
            vector<Mat>& gcps,
            Mat& start_row,
            Mat& start_col,
            int mode,
            double lambda
        );
        /*去平地(线性拟合法)
        * 参数1 干涉相位（原地操作）
        * 参数2 主星轨道参数（插值后， n_images×6）
        * 参数3 辅星轨道参数（插值后， n_images×6）
        * 参数4 地面控制点信息（纬度/经度/高度/行/列, n_gcps × 5）
        * 参数5 SAR图像左上角在原SAR图像中的行数(可以直接设置为1)
        * 参数6 SAR图像左上角在原SAR图像中的列数(可以直接设置为1)
        * 参数7 收发方式（1单发单收， 2单发双收）
        * 参数8 波长
        */
        int _PS_deflat(
            Mat& phase,
            Mat& pos1,
            Mat& pos2,
            Mat& gcps,
            int start_row,
            int start_col,
            int mode,
            double lambda
        );





        /*
        * 卫星轨道插值
        * 参数1：卫星轨道参数（未插值）
        * 参数2：插值时间间隔（s）
        * 参数3：插值结果
        */
        int stateVec_interp(Mat& stateVec, double time_interval, Mat& stateVec_interp);

        /** @brief 坐标转换工具函数

        @param coefficient       转换系数矩阵
        @param coord_in_1        原坐标矩阵1(1和2的顺序很重要，经度/行坐标在前)
        @param coord_in_2        原坐标矩阵2
        @param coord_out         转换结果矩阵
        */
        int coord_conversion(
            Mat& coefficient,
            Mat& coord_in_1,
            Mat& coord_in_2,
            Mat& coord_out
        );

        /** @brief 统计同质检验

        @param pixel1            待检验像元1幅度序列(size: n_images×1)
        @param pixel2            待检验像元2幅度序列(size: n_images×1)
        @param homo_flag         是否为同质像元(返回0则为同质像元，-1则为非同质像元)
        @param alpha             显著性水平（可以设定的值为 0.20,0.15,0.10,0.05,0.025,0.01,0.005,0.001。默认为0.05）
        @param method            检验方法（"KS":Kolmogorov-Smirnov检验，"AD":Anderson-Darling检验, 默认为KS检验）
        @return                  正常运行返回0，报错返回-1
        */
        int homogeneous_test(
            const Mat& pixel1,
            const Mat& pixel2,
            int* homo_flag,
            double alpha = 0.05,
            const char* method = "KS"
        );

        /** @brief 时序SAR图像复相关矩阵估计

        @param slc_series               slc数据堆栈
        @param coherence_matrix         相关矩阵（复数, 返回值）
        @param est_window_width         估计窗口宽度（奇数）
        @param est_window_height        估计窗口高度（奇数）
        @param ref_row                  （若进行统计同质检验）参考点行坐标，不进行同质检验则不需要此参数
        @param ref_col                  （若进行统计同质检验）参考点列坐标，不进行同质检验则不需要此参数
        @param b_homogeneous_test       是否进行统计同质检验（同质检验参考像素默认为中间点像素）
        @param b_normalize              估计相关矩阵时slc序列是否归一化处理
        @return                         成功返回0，否则返回-1
        */
        int coherence_matrix_estimation(
            const vector<ComplexMat>& slc_series,
            ComplexMat& coherence_matrix,
            int est_window_width,
            int est_window_height,
            int ref_row,
            int ref_col,
            bool b_homogeneous_test = true,
            bool b_normalize = true
        );

        /** @brief 区域生长法解缠（delaunay三角网）

        @param nodes                       Delaunay三角网络节点数组
        @param edges                       Delaunay三角网络边结构体数组
        @param start_edge                  积分起始边序号（从1开始）
        @param distance_thresh             边长阈值，超过此阈值不通过此边积分
        @param quality_thresh              质量阈值，低于此阈值不通过此边积分
        @return 成功返回0，否则返回-1
        */
        int unwrap_region_growing(
            vector<tri_node>& nodes,
            const vector<tri_edge>& edges,
            size_t start_edge,
            double distance_thresh,
            double quality_thresh
        );

    private:
        char error_head[256];
        char parallel_error_head[256];

    };

}

#endif

7、g++ `pkg-config --cflags opencv4` -c Registration.cpp com_htzs_insar_jni_Registration.cpp ComplexMat.cpp Utils.cpp -I /opt/module/jdk1.8.0_212/include/ -I /opt/module/jdk1.8.0_212/include/linux/ -fPIC -std=c++11（编译生成o文件）

8、g++ -shared com_htzs_insar_jni_Registration.o Utils.o ComplexMat.o Registration.o -o libRegistration.so `pkg-config --libs opencv4`（编译生成so文件）

注：当然也可以使用CMake来编译生成动态链接库

 9、查看是否依赖了opencv的lib库（ldd -r libRegistration.so）

 10:最后将Java程序生成jar在linux上运行

package com.htzs.insar.jni;

import com.htzs.insar.utils.ArrayUtil;

import java.io.*;
import java.io.FileOutputStream;
import java.nio.*;

/**
 * 测试类
 * @author ywb
 * @createdDate 2022/1/4 14:48
 * @updatedDate
 */
public class Main {
    static {
//        System.load("E:\\test\\inSAR_src\\x64\\Debug\\inSAR_src.dll");
//        System.loadLibrary("InSAR_src7");
        System.load("/usr/lib/libRegistration.so");
    }

    public static void main(String[] arg) throws FileNotFoundException {
        Point[] p = new Point[19*19];
        Point[] center_coord = new Point[19*19];
        int width = 512;
        int height = 512;
        int master_original_width = 10000;
        int slave_original_width = 10000;
        int offset_row, offset_col;
        int count = 0;
        short[] master_real;
        short[] master_imag;
        short[] slave_real;
        short[] slave_imag;
        String master_real_file = "/home/htzs/data/master_real.dat";
        String master_imag_file = "/home/htzs/data/master_imag.dat";
        String slave_real_file = "/home/htzs/data/slave_real.dat";
        String slave_imag_file = "/home/htzs/data/slave_imag.dat";
//        String master_real_file = "E:\\outputDat\\master_real.dat";
//        String master_imag_file = "E:\\outputDat\\master_imag.dat";
//        String slave_real_file = "E:\\outputDat\\slave_real.dat";
//        String slave_imag_file = "E:\\outputDat\\slave_imag.dat";
        try {
            for (int i = 0; i < 19; i++) {
                for (int j = 0; j < 19; j++) {
                    offset_row = i * height + 1;
                    offset_col = j * width + 1;
                    master_real = getSLice(master_real_file, offset_row, offset_col, master_original_width, height, width);
                    master_imag = getSLice(master_imag_file, offset_row, offset_col, master_original_width, height, width);
                    slave_real = getSLice(slave_real_file, offset_row, offset_col, slave_original_width, height, width);
                    slave_imag = getSLice(slave_imag_file, offset_row, offset_col, slave_original_width, height, width);
                    p[count] = Registration.calcOffset(width, master_real, master_imag, slave_real, slave_imag);
                    center_coord[count] = new Point();
                    center_coord[count].setRow(offset_row - 1 + 0.5 * height);
                    center_coord[count].setCol(offset_col - 1 + 0.5 * width);
                    System.out.println("(" + i + "," + j + "):" + p[count].getRow() + " , " + p[count].getCol() + " , " + p[count].getCoherence());
//                    System.out.println(p[count].getRow());
//                    System.out.println(p[count].getCol());
//                    System.out.println("相关系数：" + p[count].getCoherence());
//                    writeDoubleArray("D:\\image\\cut\\master_real\\" + i  +"_"+ j + "_master_real.dat", master_real);
//                    writeDoubleArray("D:\\image\\cut\\master_imag\\" + i  +"_"+ j  + "_master_imag.dat", master_imag);
//                    writeDoubleArray("D:\\image\\cut\\slave_real\\" + i  +"_"+ j  + "_slave_real.dat", slave_real);
//                    writeDoubleArray("D:\\image\\cut\\slave_imag\\" + i  +"_"+ j  + "_slave_imag.dat", slave_imag);
                    count++;
                }
            }

            Fit fit = Registration.fitting(p, center_coord, 0.5);
            System.out.println("Fit:" + fit.getA0() + "," + fit.getA1()+ "," + fit.getA2()+ ","+ fit.getA3()+ "," + fit.getA4()+ ","  + fit.getA5()+ ",B:,"+ fit.getB0() + "," + fit.getB1()+ "," + fit.getB2()+ ","+ fit.getB3()+ "," + fit.getB4()+ ","  + fit.getB5());
            Point cen = new Point();
            cen.setRow(1536);
            cen.setCol(1536);
            slave_real = getSLice(slave_real_file, 1, 1, slave_original_width, height * 3, width * 3);
            slave_imag = getSLice(slave_imag_file, 1, 1, slave_original_width, height * 3, width * 3);
            double[] buf = Registration.registration(width * 3, slave_real, slave_imag, cen, fit, 0);
            System.out.println(buf.length);
        } catch (InSAR_JNIException e) {
            System.out.println(e.getMessage());
            return;
        }

    }

    /**
     * 从图像文件中切片
     *
     * @param imageFile      图像文件
     * @param offset_row     切片左上角在图像中的行数
     * @param offset_col     切片左上角在图像中的列数
     * @param original_width 原始图像宽度
     * @param height         切片高度
     * @param width          切片宽度
     * @return ：切片数组
     **/
    public static short[] getSLice(
            String imageFile,
            int offset_row,
            int offset_col,
            int original_width,
            int height,
            int width
    ) {
        short[] slice = new short[height * width];
        int offset = ((offset_row - 1) * original_width + offset_col - 1) * Short.BYTES;
        byte[] byteBuf = new byte[width * Short.BYTES];
        short[] temp;
        int offset1 = 0;
        File file = new File(imageFile);
        try {
            RandomAccessFile fr = new RandomAccessFile(file, "rw");
            for (int i = 0; i < height; i++) {
                fr.seek(offset);
                fr.read(byteBuf, 0, width * Short.BYTES);
                temp = ArrayUtil.convertToShorts(byteBuf);
                System.arraycopy(temp, 0, slice, offset1, width);
                offset = offset + original_width * Short.BYTES;
                offset1 += width;
            }
            fr.close();
        } catch (IOException e) {
            e.printStackTrace();
        }
        return slice;
    }

    /**
     * 将byte数组转换为double数组
     *
     * @param b 输入字节数组
     * @return 返回double数组
     **/
    public static double[] getDouble(byte[] b) {
        double[] ret = new double[b.length / 8];
        for (int i = 0; i < b.length / 8; i++) {
            long m;
            m = b[8 * i];
            m &= 0xff;
            m |= ((long) b[1 + 8 * i] << 8);
            m &= 0xffff;
            m |= ((long) b[2 + 8 * i] << 16);
            m &= 0xffffff;
            m |= ((long) b[3 + 8 * i] << 24);
            m &= 0xffffffffL;
            m |= ((long) b[4 + 8 * i] << 32);
            m &= 0xffffffffffL;
            m |= ((long) b[5 + 8 * i] << 40);
            m &= 0xffffffffffffL;
            m |= ((long) b[6 + 8 * i] << 48);
            m &= 0xffffffffffffffL;
            m |= ((long) b[7 + 8 * i] << 56);
            ret[i] = Double.longBitsToDouble(m);
        }

        return ret;
    }

    public static ByteBuffer asByteBuffer(ShortBuffer input) {
        if (null == input) {
            return null;
        }
        ByteBuffer buffer = ByteBuffer.allocate(input.capacity() * (Short.BYTES));
        while (input.hasRemaining()) {
            buffer.putDouble(input.get());
        }
        return buffer;
    }

    public static byte[] asByteArray(short[] input) {
        if (null == input) {
            return null;
        }
        return asByteBuffer(ShortBuffer.wrap(input)).array();
    }

    public static void writeDoubleArray(
            String imageFile,
            short[] indata
    ) throws FileNotFoundException {

        try {
            DataOutputStream out = new DataOutputStream(new FileOutputStream(imageFile, true));
            byte[] bytearray = asByteArray(indata);
            out.write(bytearray, 0, bytearray.length);
            out.close();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }
}

计算偏移量运行结果：