add m

xuepengqiang

2020-05-26 48bf06c1cadadcfb93faa8df63db71e59b534f55

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
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);
}