add m

xuepengqiang

2020-05-26 48bf06c1cadadcfb93faa8df63db71e59b534f55

add m

 CMakeLists.txt

New file
@@ -0,0 +1,24 @@
cmake_minimum_required(VERSION 3.15)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -std=c++11 -W")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -std=c++11 -W")  #
project(sw)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build)

set(CMAKE_CXX_STANDARD 11)

add_compile_options(-fPIC -g -Wall -Wshadow -Wno-sign-compare -w)



file(GLOB_RECURSE current_headers
        ./src/*.h
        )
file(GLOB sources ./*.cpp ./src/*.cpp)

source_group("Include" FILES ${current_headers})
source_group("Source" FILES ${sources})

add_executable(${PROJECT_NAME} main.cpp ${sources})

target_link_libraries(${PROJECT_NAME}
        )

 main.cpp

New file
@@ -0,0 +1,152 @@
#include <iostream>
#include "./src/coord_interface.h"
#include "src/con_target.h"
#include "./src/math_util.h"

int main() {
    std::cout << "Hello, World!" << std::endl;
//    float Ax = 30.0f, Ay = 110.0f, Bx = 326.36f, By = 64.45f;
//    const char* config_str = "{\"leftCam\":{ \"A1x\": 30.0, \"A1y\":110.0 ,\"A2x\": 120.0,\"A2y\":200.0, ,\"A3x\": 80.0,\"A3y\":400.0, ,\"A4x\": 20.0,\"A4y\":287.0},"
//                                "rightCam\":{ \"B1x\": 30.0, \"B1y\":110.0 ,\"B2x\": 120.0,\"B2y\":200.0, ,\"B3x\": 80.0,\"B3y\":400.0, ,\"B4x\": 20.0,\"B4y\":287.0}"
//                                "}";

//    CoordManager CM(config_str);

    // ---- 初始化赋值 ---
    CamVec *CV_Reg = (CamVec*)malloc(sizeof(CamVec));
    CV_Reg->count = 3; //表示有3个摄像机相互关联
    int reg_point_size = 4; // 关联区域的坐标点数
    CV_Reg->regInfo = (RegInfo*)malloc(sizeof(RegInfo)*CV_Reg->count);

    SPoint2f *regA_point = (SPoint2f*)malloc(sizeof(SPoint2f)*reg_point_size);
    regA_point[0] = SPoint2f(100.0f, 100.0f);
    regA_point[1] = SPoint2f(120.0f, 200.0f);
    regA_point[2] = SPoint2f(80.0f, 400.0f);
    regA_point[3] = SPoint2f(20.0f, 287.0f);
    CV_Reg->regInfo[0].count = reg_point_size;
    CV_Reg->regInfo[0].point = regA_point;

    SPoint2f* regB_point = (SPoint2f*)malloc(sizeof(SPoint2f)*reg_point_size);
    regB_point[0] = SPoint2f(326.36f, 64.45f);
    regB_point[1] = SPoint2f(339.16f, 128.24f);
    regB_point[2] = SPoint2f(313.73f, 255.86f);
    regB_point[3] = SPoint2f(275.41f, 183.80f);
    CV_Reg->regInfo[1].count = reg_point_size;
    CV_Reg->regInfo[1].point = regB_point;

    SPoint2f* regC_point = (SPoint2f*)malloc(sizeof(SPoint2f)*reg_point_size);
    regC_point[0] = SPoint2f(326.36f, 64.45f);
    regC_point[1] = SPoint2f(339.16f, 128.24f);
    regC_point[2] = SPoint2f(313.73f, 255.86f);
    regC_point[3] = SPoint2f(275.41f, 183.80f);
    CV_Reg->regInfo[2].count = reg_point_size;
    CV_Reg->regInfo[2].point = regC_point;

    CoordManager CM(CV_Reg); //初始化区域信息


    // 获取待匹配的点信息
    MulCam* mulCam = (MulCam*)malloc(sizeof(MulCam));
    mulCam->cam_count = CV_Reg->count;
    mulCam->matchHuman = (MatchHuman*)malloc(sizeof(MatchHuman)*mulCam->cam_count);


//    MatchHuman * A_Human = (MatchHuman*)malloc(sizeof(MatchHuman));
    int A_hcount = 3;
    mulCam->matchHuman[0].humanInfo = (HumanInfo*)malloc(sizeof(HumanInfo)*A_hcount);
    mulCam->matchHuman[0].humanInfo[0].human_id = 10000;
    mulCam->matchHuman[0].humanInfo[0].point = SPoint2f(100.0f, 100.0f);
    mulCam->matchHuman[0].humanInfo[1].human_id = 1002;
    mulCam->matchHuman[0].humanInfo[1].point = SPoint2f(30.0f, 110.0f);
    mulCam->matchHuman[0].humanInfo[2].human_id = 1003;
    mulCam->matchHuman[0].humanInfo[2].point = SPoint2f(35.0f, 100.0f);
    mulCam->matchHuman[0].count = A_hcount;
//    mulCam->matchHuman[0].humanInfo = A_Human;

//    MatchHuman * B_Human = (MatchHuman*)malloc(sizeof(MatchHuman));
    int B_hcount = 2;
    mulCam->matchHuman[1].humanInfo = (HumanInfo*)malloc(sizeof(HumanInfo)*B_hcount);
    mulCam->matchHuman[1].humanInfo[0].human_id = 2001;
    mulCam->matchHuman[1].humanInfo[0].point = SPoint2f(326.36f, 64.45f);
    mulCam->matchHuman[1].humanInfo[1].human_id = 2003;
    mulCam->matchHuman[1].humanInfo[1].point = SPoint2f(280.9f, 60.49f);
    mulCam->matchHuman[1].count = B_hcount;
//    mulCam->matchHuman[1] = B_Human;

//    MatchHuman * C_Human = (MatchHuman*)malloc(sizeof(MatchHuman));
    int C_hcount = 1;
    mulCam->matchHuman[2].humanInfo = (HumanInfo*)malloc(sizeof(HumanInfo)*B_hcount);
    mulCam->matchHuman[2].humanInfo[0].human_id = 30001;
    mulCam->matchHuman[2].humanInfo[0].point = SPoint2f(284.9f, 64.49f);
    mulCam->matchHuman[2].count = C_hcount;
//    mulCam->matchHuman[2] = C_Human;




    MulCam* lastmulCam = (MulCam*)malloc(sizeof(MulCam));
    lastmulCam->cam_count = CV_Reg->count;
    lastmulCam->matchHuman = (MatchHuman*)malloc(sizeof(MatchHuman)*mulCam->cam_count);


//    MatchHuman * A_Human = (MatchHuman*)malloc(sizeof(MatchHuman));
    int lA_hcount = 3;
    lastmulCam->matchHuman[0].humanInfo = (HumanInfo*)malloc(sizeof(HumanInfo)*A_hcount);
    lastmulCam->matchHuman[0].humanInfo[0].human_id = 1;
    lastmulCam->matchHuman[0].humanInfo[0].point = SPoint2f(100.0f, 100.0f);
    lastmulCam->matchHuman[0].humanInfo[1].human_id = 2;
    lastmulCam->matchHuman[0].humanInfo[1].point = SPoint2f(30.0f, 110.0f);
    lastmulCam->matchHuman[0].humanInfo[2].human_id = 3;
    lastmulCam->matchHuman[0].humanInfo[2].point = SPoint2f(35.0f, 100.0f);
    lastmulCam->matchHuman[0].count = lA_hcount;
//    mulCam->matchHuman[0].humanInfo = A_Human;

//    MatchHuman * B_Human = (MatchHuman*)malloc(sizeof(MatchHuman));
    int lB_hcount = 2;
    lastmulCam->matchHuman[1].humanInfo = (HumanInfo*)malloc(sizeof(HumanInfo)*B_hcount);
    lastmulCam->matchHuman[1].humanInfo[0].human_id = 3;
    lastmulCam->matchHuman[1].humanInfo[0].point = SPoint2f(326.36f, 64.45f);
    lastmulCam->matchHuman[1].humanInfo[1].human_id = 5;
    lastmulCam->matchHuman[1].humanInfo[1].point = SPoint2f(284.9f, 64.49f);
    lastmulCam->matchHuman[1].count = lB_hcount;
//    mulCam->matchHuman[1] = B_Human;

//    MatchHuman * C_Human = (MatchHuman*)malloc(sizeof(MatchHuman));
    int lC_hcount = 1;
    lastmulCam->matchHuman[2].humanInfo = (HumanInfo*)malloc(sizeof(HumanInfo)*B_hcount);
    lastmulCam->matchHuman[2].humanInfo[1].human_id = 5;
    lastmulCam->matchHuman[2].humanInfo[1].point = SPoint2f(284.9f, 64.49f);
    lastmulCam->matchHuman[2].count = lC_hcount;



    char * result="";


    result = CM.get_result(mulCam, lastmulCam);// 批量匹配区域中的人

    std::cout<< " ----------   " <<  std::endl;
    std::cout<< result <<  std::endl;
//    //遍历结果
//    for (int i = 0; i < mulCam->cam_count-1; ++i) {
//        map<string,float>::iterator iter;
//        for (iter=target->camResult[i].confidence_map.begin() ;iter!=target->camResult[i].confidence_map.end();iter++) {
//            std::cout<< iter->first << "  confidence   " << iter->second << std::endl;
//        }
//    }
//
//    std::cout<< " ----------   " <<  std::endl;
//    map<int,int>::iterator direction_iter;
//    for (direction_iter=target->direction_map.begin();direction_iter!=target->direction_map.end();direction_iter++) {
//        std::cout<< direction_iter->first << "  direction   " << direction_iter->second << std::endl;
//    }

//    map<string,float>::iterator iter;
//    for (iter=confidence_map.begin() ;iter!=confidence_map.end();iter++) {
//        std::cout<< iter->first << "  confidence   " << iter->second << std::endl;
//    }


    std::cout << "--over" << std::endl;
    return 0;
}

 src/con_target.h

New file
@@ -0,0 +1,78 @@
//
// Created by Scheaven on 2020/5/20.
//

#ifndef INC_01_COORDINATE_TRANSFORMATION_CON_TARGET_H
#define INC_01_COORDINATE_TRANSFORMATION_CON_TARGET_H

#include <vector>
#include <map>
#include <string>

using namespace std;

#include <stdint.h>
#include <math.h>
#include <stdint.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <algorithm>
#include "stdio.h"
#include "stdlib.h"

#include "math_util.h"

// 初始化时区域结构
typedef struct _PInfo
{
    SPoint2f* point;
    int count;
}RegInfo;

typedef struct _CamVec
{
    RegInfo *regInfo;
    int count;
}CamVec;


// 传输的人体站立的点位信息
typedef struct _HumanInfo
{
    int human_id;
    SPoint2f point;
//    float * feature;
}HumanInfo;

typedef struct _MatchHuman
{
    HumanInfo* humanInfo;
    int count;      //一共多少个人的坐标点
}MatchHuman;

typedef struct _mulCam
{
    MatchHuman* matchHuman;
    int cam_count;
}MulCam;


// 获取最终的结果信息
typedef struct _CamResult
{
    map<string,float> confidence_map;
    string cam_pair;  //是哪两个摄像头相关
}CamResult;


typedef struct _STarget
{
    CamResult *camResult;
    map<int, int> direction_map; // 相互关联的摄像头
}STarget;

//typedef std::vector<Target> FIRST_VEC;


#endif //INC_01_COORDINATE_TRANSFORMATION_CON_TARGET_H

 src/coord_interface.cpp

New file
@@ -0,0 +1,135 @@
//
// Created by Scheaven on 2020/5/20.
//

#include "coord_interface.h"
#include "switcher_util.h"
#include "json_util.h"

CoordSwitcher switcher;
CoordManager::CoordManager(const char *json_document)
{

}

CoordManager::CoordManager(CamVec *CV_Reg)
{
    this->CV_Reg = CV_Reg;
    switcher.initHomography(CV_Reg);
//    this->init_flag = false;
//    CoordSwitcher switcher(CV_Reg);

}

CoordManager::~CoordManager() {}

float confidence =0.0;


char* CoordManager::get_result(MulCam* curr_Human, MulCam* last_Human)
{
    // 创建获取结果的结构体
    STarget* target = new STarget();
//    stemtarget->temcamResult = (TemCamResult*)malloc(sizeof(TemCamResult)*(mulCam->cam_count-1));
    target->camResult = new CamResult[curr_Human->cam_count-1];
    group_compare(curr_Human, last_Human , target);// 批量匹配区域中的人


//    //    //遍历结果
//    for (int i = 0; i < curr_Human->cam_count-1; ++i) {
//        map<string,float>::iterator iter;
//        for (iter=target->camResult[i].confidence_map.begin() ;iter!=target->camResult[i].confidence_map.end();iter++) {
//            std::cout<< iter->first << "  confidence   " << iter->second << std::endl;
//        }
//    }
//
//    std::cout<< " ----------   " <<  std::endl;
//    map<int,int>::iterator direction_iter;
//    for (direction_iter=target->direction_map.begin();direction_iter!=target->direction_map.end();direction_iter++) {
//        std::cout<< direction_iter->first << "  direction   " << direction_iter->second << std::endl;
//    }


    string result_json = "{\"confidence\":\n\t{";
    for (int i = 0; i < curr_Human->cam_count-1; ++i) {
        string c_json = map_json(target->camResult[i].confidence_map);
        result_json += "\t\"" + target->camResult[i].cam_pair + "\":[" + c_json.substr(0,c_json.length()-1) + "],\n";
    }
    result_json = result_json.substr(0,result_json.length()-2)+"\n\t},\n";
    result_json += "\"direction\":[";
    string d_json = map_json(target->direction_map);
    result_json += d_json.substr(0,d_json.length()-1) + "]\n}\n";


//    string json = to_json_string(target);
    char *result = (char*)result_json.c_str();
//    cout << result << endl;
    return result;



}


void CoordManager::group_compare(MulCam* curr_Human, MulCam* last_Human, STarget* target)
{

    map<int,float> global_min_points_map; 

    for (int k = 1; k < curr_Human->cam_count; ++k) {
        for (int i = 0; i < curr_Human->matchHuman[0].count; ++i) {
            int curr_direction = -1;
            float local_min_distance = 100000.0;

            target->camResult[k-1].cam_pair = to_string(1)+":"+to_string(k+1);

            for (int j = 0; j < curr_Human->matchHuman[k].count; ++j) {
                confidence = switcher.compare(curr_Human->matchHuman[0].humanInfo[i].point, curr_Human->matchHuman[k].humanInfo[j].point, k - 1);
                if(local_min_distance>confidence){
                    local_min_distance = confidence;
                }
                target->camResult[k-1].confidence_map.insert(pair<string,float >(to_string(curr_Human->matchHuman[0].humanInfo[i].human_id)+ ":" +to_string( curr_Human->matchHuman[k].humanInfo[j].human_id), confidence));
            }

            if(global_min_points_map.count(curr_Human->matchHuman[0].humanInfo[i].human_id)>0)
            {
                if (global_min_points_map[curr_Human->matchHuman[0].humanInfo[i].human_id]>local_min_distance)
                {
                    global_min_points_map[curr_Human->matchHuman[0].humanInfo[i].human_id] = local_min_distance; //更新方向
                    curr_direction = k+1;
                }
            }else
            {
                curr_direction = k+1;
                global_min_points_map.insert(pair<int, float>(curr_Human->matchHuman[0].humanInfo[i].human_id, local_min_distance)); // 首次添加方向
            }


            if(target->direction_map.count(curr_Human->matchHuman[0].humanInfo[i].human_id)>0)
            {
                if (curr_direction!=-1)
                    target->direction_map[curr_Human->matchHuman[0].humanInfo[i].human_id] =curr_direction; //更新方向
            }else
            {
                target->direction_map.insert(pair<int, int>(curr_Human->matchHuman[0].humanInfo[i].human_id,  curr_direction)); // 首次添加方向
            }

        }
    }


//    map<string,float> confidence_map;
//    for (int i = 0; i < A_Human->count; ++i) {
//        for (int j = 0; j < B_Human->count; ++j) {
//            confidence = switcher.compare(A_Human->humanInfo[i].point, B_Human->humanInfo[j].point);
//            confidence_map.insert(pair<string,float >(to_string(A_Human->humanInfo[i].human_id)+ ":" +to_string(B_Human->humanInfo[j].human_id), confidence));
//        }
//    }
//    return confidence_map;
}

void CoordManager::init_Target(STarget *target)
{
    target->direction_map[0] = 10000;
    target->camResult = nullptr;
}

 src/coord_interface.h

New file
@@ -0,0 +1,26 @@
//
// Created by Scheaven on 2020/5/20.
//

#ifndef INC_01_COORDINATE_TRANSFORMATION_COORD_INTERFACE_H
#define INC_01_COORDINATE_TRANSFORMATION_COORD_INTERFACE_H

#include "con_target.h"

class CoordManager
{
private:
    CamVec *CV_Reg;
    bool init_flag;

public:
    CoordManager(const char* json_document);
    CoordManager(CamVec *CV_Reg);
    ~CoordManager();
    void init_Target(STarget *target);
    void group_compare(MulCam* curr_Human, MulCam* last_Human, STarget* target);
    char * get_result(MulCam* curr_Human, MulCam* last_Human);
};


#endif //INC_01_COORDINATE_TRANSFORMATION_COORD_INTERFACE_H

 src/homography_util.cpp

New file
@@ -0,0 +1,92 @@
//
// Created by Scheaven on 2020/5/22.
//

#include "homography_util.h"

static void gaussian_elimination(float *input, int n)
{
    float * A = input;
    int i = 0;
    int j = 0;
    //m = 8 rows, n = 9 cols
    int m = n-1;
    while (i < m && j < n)
    {
        int maxi = i;
        for(int k = i+1; k < m; k++)
        {
            if(fabs(A[k * n + j]) > fabs(A[maxi * n + j]))
            {
                maxi = k;
            }
        }
        if (A[maxi * n + j] != 0)
        {
            if(i != maxi)
                for(int k = 0; k < n; k++)
                {
                    float aux = A[i * n + k];
                    A[i * n + k] = A[maxi * n + k];
                    A[maxi * n + k] = aux;
                }
            float A_ij = A[i * n + j];
            for(int k = 0; k < n; k++)
            {
                A[i * n + k] /= A_ij;
            }
            for(int u = i+1; u< m; u++)
            {
                float A_uj = A[u * n + j];
                for(int k = 0; k <n; k++)
                {
                    A[u * n + k] -= A_uj * A[i * n + k];
                }
            }
            i++;
        }
        j++;
    }

    for(int i = m-2; i >= 0; i--)
    {
        for(int j = i+1; j < n-1; j++)
        {
            A[i * n + m] -= A[i * n + j] * A[j * n + m];
        }
    }

}

// 创建单应性矩阵
void creatHomography(vector<SPoint2f> src, vector<SPoint2f> dst,SHomogtaphy *SH_pointer,int j_index)
{
       float P[8][9]=
            {
                    {-src[0].x, -src[0].y, -1,   0,   0,  0, src[0].x * dst[0].x, src[0].y * dst[0].x, -dst[0].x }, // h11
                    {  0,   0,  0, -src[0].x, -src[0].y, -1, src[0].x * dst[0].y, src[0].y * dst[0].y, -dst[0].y }, // h12

                    {-src[1].x, -src[1].y, -1,   0,   0,  0, src[1].x * dst[1].x, src[1].y * dst[1].x, -dst[1].x }, // h13
                    {  0,   0,  0, -src[1].x, -src[1].y, -1, src[1].x * dst[1].y, src[1].y * dst[1].y, -dst[1].y }, // h21

                    {-src[2].x, -src[2].y, -1,   0,   0,  0, src[2].x * dst[2].x, src[2].y * dst[2].x, -dst[2].x }, // h22
                    {  0,   0,  0, -src[2].x, -src[2].y, -1, src[2].x * dst[2].y, src[2].y * dst[2].y, -dst[2].y }, // h23

                    {-src[3].x, -src[3].y, -1,   0,   0,  0, src[3].x * dst[3].x, src[3].y * dst[3].x, -dst[3].x }, // h31
                    {  0,   0,  0, -src[3].x, -src[3].y, -1, src[3].x * dst[3].y, src[3].y * dst[3].y, -dst[3].y }, // h32
            };

    gaussian_elimination(&P[0][0], 9);
    float aux_H[]={ P[0][8], P[1][8], P[2][8],    // h11  h21 0 h31
                    P[3][8], P[4][8], P[5][8],    // h12  h22 0 h32
                    P[6][8], P[7][8], 1};        // h13  h23 0 h33

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
//            homography[i * 3 + j] = aux_H[i * 3 + j];
            SH_pointer->matrix[j_index].homography[i][j] = aux_H[i * 3 + j];
        }
    }
}

 src/homography_util.h

New file
@@ -0,0 +1,28 @@
//
// Created by Scheaven on 2020/5/22.
//

#ifndef INC_02_COORDINATE_TRANSFORMATION_HOMOGRAPHY_UTIL_H
#define INC_02_COORDINATE_TRANSFORMATION_HOMOGRAPHY_UTIL_H

#include <vector>
#include "math_util.h"

typedef struct _Matrix
{
    float homography[3][3];
}SMatrix;

typedef struct _Homography
{
    SMatrix* matrix;
    int count;
}SHomogtaphy;

static void gaussian_elimination(float *input, int n);
void creatHomography();
//void creatHomography(vector<SPoint2f> src, vector<SPoint2f> dst,SMatrix* matrix);
void creatHomography(vector<SPoint2f> src, vector<SPoint2f> dst,SHomogtaphy *SH_pointer ,int j_index);


#endif //INC_02_COORDINATE_TRANSFORMATION_HOMOGRAPHY_UTIL_H

 src/json_util.cpp

New file
@@ -0,0 +1,25 @@
//
// Created by Scheaven on 2020/5/26.
//

#include "json_util.h"

string map_json(map<string,float> s_map)
{
    string json = "";
    map<string,float>::iterator iter;
    for (iter= s_map.begin(); iter!= s_map.end();iter++ ) {
        json += "{\"" + iter->first + "\":" + "" +to_string(iter->second) + "},";
    }
    return json;
}

string map_json(map<int,int> s_map)
{
    string json = "";
    map<int,int>::iterator iter;
    for (iter= s_map.begin(); iter!= s_map.end();iter++ ) {
        json += "{\""+ to_string(iter->first) + "\":" + to_string(iter->second) + "},";
    }
    return json;
}

 src/json_util.h

New file
@@ -0,0 +1,13 @@
//
// Created by Scheaven on 2020/5/26.
//

#ifndef INC_01_COORDINATE_TRANSFORMATION_JSON_UTIL_H
#define INC_01_COORDINATE_TRANSFORMATION_JSON_UTIL_H

#include "./con_target.h"

string map_json(map<string,float> s_map);
string map_json(map<int,int> s_map);

#endif //INC_01_COORDINATE_TRANSFORMATION_JSON_UTIL_H

 src/math_util.cpp

New file
@@ -0,0 +1,37 @@
//
// Created by Scheaven on 2020/5/21.
//

#include "math_util.h"

SPoint2f::_Point(float x, float y)
{
    this->x = x;
    this->y = y;
}

SPoint2f::_Point()
{

}

float calDistance(SPoint2f A, SPoint2f B)
{
    float x_d , y_d;
    x_d = A.x - B.x;
    y_d = A.y - B.y;
    float lea = sqrt(x_d*x_d + y_d*y_d);
    return lea;
}


//int main()
//{
//    SPoint2f A(100.0f, 100.0f);
//    SPoint2f B(10.0f, 10.0f) ;
//    float confidence = calDistance(A, B);
//    std::cout << confidence <<"-=-=-=-" << std::endl;
//    SPoint2f *regA_point = (SPoint2f*)malloc(sizeof(SPoint2f)*4);
//    std::cout << confidence <<"-=-=-=-" << std::endl;
//
//}

 src/math_util.h

New file
@@ -0,0 +1,30 @@
//
// Created by Scheaven on 2020/5/21.
//

#ifndef INC_01_COORDINATE_TRANSFORMATION_MATH_UTIL_H
#define INC_01_COORDINATE_TRANSFORMATION_MATH_UTIL_H
#include <math.h>
#include <stdint.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <algorithm>
#include "stdio.h"
#include "stdlib.h"

using namespace std;

typedef struct _Point
{
public:
    float x;
    float y;
public:
    _Point(float x, float y);
    _Point();
}SPoint2f;

float calDistance(SPoint2f A, SPoint2f B);

#endif //INC_01_COORDINATE_TRANSFORMATION_MATH_UTIL_H

 src/switcher_util.cpp

New file
@@ -0,0 +1,92 @@
//
// Created by Scheaven on 2020/5/20.
//

#include "switcher_util.h"


CoordSwitcher* CoordSwitcher::instance = NULL;
SHomogtaphy* CoordSwitcher::SHomogtaphy_pointer = (SHomogtaphy*)malloc(sizeof(SHomogtaphy));

CoordSwitcher* CoordSwitcher::getInstance(CamVec *CV_Reg)
{
    if(instance==NULL)
    {
        instance = new CoordSwitcher();
//        std::cout << "....---.loading Coord....." << std::endl;
    }
    return instance;
}

CoordSwitcher::CoordSwitcher()
{

//    initHomography(CV_Reg);
}


CoordSwitcher::~CoordSwitcher()
{

}

void CoordSwitcher::initHomography(CamVec *CV_Reg)
{
    vector<SPoint2f> reg_points;
    for (int k = 0; k < CV_Reg->count; ++k) {
        for (int i = 0; i < CV_Reg->regInfo[k].count; ++i) {
            reg_points.push_back(CV_Reg->regInfo[k].point[i]);
        }
        reg_points_vec.push_back(reg_points);
        reg_points.clear();
    }

    float reprojThresh = 4.0;

    SHomogtaphy_pointer->count = CV_Reg->count-1;
    SHomogtaphy_pointer->matrix = (SMatrix*)malloc(sizeof(SMatrix)*SHomogtaphy_pointer->count);

    for (int j_index = 1; j_index < CV_Reg->count; ++j_index) {
        creatHomography(reg_points_vec[0], reg_points_vec[j_index], SHomogtaphy_pointer, j_index-1);
    }

//    for (int l = 0; l < 3; ++l) {
//        for (int m = 0; m < 3; ++m) {
//            std::cout <<   SHomogtaphy_pointer->matrix[0].homography[l][m] <<  "---" << std::endl;
//            this->s_homography[l][m] = SHomogtaphy_pointer->matrix[0].homography[l][m];
//        }
//    }
//

}

float CoordSwitcher::compare(SPoint2f A, SPoint2f B, int h_index)
{
    SPoint2f rightPoint = A;
    SPoint2f leftPoint;
    leftPoint = getTransPoint(rightPoint, SHomogtaphy_pointer->matrix[h_index]);

    float distance = calDistance(leftPoint, B);

    return distance;
}

SPoint2f CoordSwitcher::getTransPoint(const SPoint2f rightPoint, SMatrix& matrix)
{
    float rightP[3][1], leftP[3][1];
    rightP[0][0] = rightPoint.x;
    rightP[1][0] = rightPoint.y;
    rightP[2][0] = 1.0;
    for(int i=0;i<3;++i){
        for(int j=0;j<1;++j){
            leftP[i][j]=0;
            for(int k=0;k<3;++k){
                leftP[i][j]+=matrix.homography[i][k]*rightP[k][j];
            }
        }
    }

    float x = leftP[0][0] / leftP[2][0];
    float y = leftP[1][0] / leftP[2][0];
    return SPoint2f(x, y);
}

 src/switcher_util.h

New file
@@ -0,0 +1,48 @@
//
// Created by Scheaven on 2020/5/20.
//

#ifndef INC_01_COORDINATE_TRANSFORMATION_SWITCHER_UTIL_H
#define INC_01_COORDINATE_TRANSFORMATION_SWITCHER_UTIL_H

#include "opencv2/opencv.hpp"
#pragma comment(lib, "opencv_core249.lib")

#include <iostream>
#include <vector>
#include <fstream>
#include <algorithm>
#include <math.h>
#include "stdio.h"
#include "stdlib.h"
#include "con_target.h"
#include "homography_util.h"

using namespace std;

class CoordSwitcher
{
private:
    vector<uchar> inliers;
    vector<vector<SPoint2f>> reg_points_vec;
    float s_homography[3][3];
//    vector<float[3][3]> homography_vec;
    static SHomogtaphy* SHomogtaphy_pointer;
    static CoordSwitcher* instance;
public:
    void initHomography(CamVec *CV_Reg); 
    static CoordSwitcher* getInstance(CamVec *CV_Reg);
    CoordSwitcher(); 
    CoordSwitcher(CamVec *CV_Reg); 
    ~CoordSwitcher();

    // 转换坐标
    SPoint2f getTransPoint(const SPoint2f rightPoint, SMatrix& matrix);

    float compare(SPoint2f A, SPoint2f B, int h_index);
    float compare(float Ax, float Ay, float Bx, float By);
};



#endif //INC_01_COORDINATE_TRANSFORMATION_SWITCHER_UTIL_H