#include "stdafx.h"
#include "ls_match.h"
#include "gdal_priv.h"
#include "gdal_alg.h"
bool LSM::Slovepara()
{
GDALAllRegister(); //注册驱动
float *col1 = new float[path.size()];
float *row1 = new float[path.size()];
Keypoint pt;
for (int i = 0; i < path.size(); i++)
{
GDALDataset *poDataset = (GDALDataset *)GDALOpen(path[i], GA_ReadOnly);
if (poDataset->GetRasterXSize() > 20000)
{
col1[i] = ZY3_NAD_LAT;
row1[i] = ZY3_NAD_LON;
}
else
{
col1[i] = ZY3_DLC_LAT;
row1[i] = ZY3_DLC_LON;
}
GDALClose(poDataset);
pt = pre_match[i];
pt.col = 0; pt.row = 0;
match.push_back(pt);
}
float pa0, pb0;
a0.clear(); a1.clear(); a2.clear();
b0.clear(); b1.clear(); b2.clear();
h0.clear(); h1.clear();
for (int i = 0; i < pre_match.size(); i++)
{
int coli = pre_match[i].col;
int rowi = pre_match[i].row;
pt.col = pre_match[i].col - coli + (col*col - 1) / 2;
pt.row = pre_match[i].row - rowi + (row*row - 1) / 2;
re_match.push_back(pt);
}
for (int i = 1; i < pre_match.size(); i++)
{
a1.push_back(col1[0] / col1[i]);
b1.push_back(0.0f);
a2.push_back(0.0f);
b2.push_back(row1[0] / row1[i]);
h0.push_back(0.0f);
h1.push_back(1.0f);
pa0 = re_match[i].col - a1[i - 1] * re_match[0].col;
pb0 = re_match[i].row - b2[i - 1] * re_match[0].row;
a0.push_back(pa0);
b0.push_back(pb0);
}
delete[]col1;
delete[]row1;
return true;
}
float LSM::GetCorrCoef(Mat left, Mat right)
{
float CorrCoef;
float p1 = 0; float p2 = 0; float p3 = 0;
float suml = 0; float sumr = 0;
float N = row * col;
//将两窗口灰度阵列中心化
for(int i=1;i<row+1;i++)
for (int j = 1; j < col + 1; j++)
{
suml += left.ptr<float>(i)[j];
sumr += right.ptr<float>(i)[j];
}
suml = suml / N;
sumr = sumr / N;
//p^2 = (∑g1*g2)^2 / (∑g1^2 * ∑g2^2)
for (int i = 1; i<row+1; i++)
for (int j = 1; j < col + 1; j++)
{
p1 += (left.ptr<float>(i)[j] - suml)*(right.ptr<float>(i)[j] - sumr);
p2 += (left.ptr<float>(i)[j] - suml) *(left.ptr<float>(i)[j] - suml);
p3 += (right.ptr<float>(i)[j] - sumr) *(right.ptr<float>(i)[j] - sumr);
}
CorrCoef = (p1*p1) / (p2 * p3);
CorrCoef = sqrtf(CorrCoef);
/*for (int i = 1; i<row+1; i++)
for (int j = 1; j < col + 1; j++)
{
p1 += left.ptr<float>(i)[j] * right.ptr<float>(i)[j];
p2 += left.ptr<float>(i)[j] * left.ptr<float>(i)[j];
p3 += right.ptr<float>(i)[j] * right.ptr<float>(i)[j];
}*/
//CorrCoef = (p1 - suml * sumr / N) / sqrt((p2 - suml * suml / N)*(p3 - sumr * sumr / N));
return CorrCoef;
}
Mat LSM::LSsolve_Single_Point(Mat g0, vector<Mat> gi)
{
Mat C = Mat(row*col*gi.size(), 8 * gi.size(), CV_32F);
Mat L = Mat(row*col*gi.size(), 1, CV_32F);
Mat X = Mat(8 * gi.size(), 1, CV_32F);
//系数阵归零
for (int i = 0; i < C.rows; i++)
for (int j = 0; j < C.cols; j++)
C.ptr<float>(i)[j] = 0;
for (int t = 0; t < gi.size(); t++)
for (int i = 1; i < row + 1; i++)
for (int j = 1; j < col + 1; j++)
{
L.ptr<float>(row*col * t + (i - 1)*col + j - 1)[0] = g0.ptr<float>(i)[j] - gi[t].ptr<float>(i)[j];
//L.ptr<float>(row*col * t + (i - 1)*col + j - 1)[0] = gi[t].ptr<float>(i)[j] - g0.ptr<float>(i)[j];
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t] = 1;
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 1] = gi[t].ptr<float>(i)[j];
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 2] = (gi[t].ptr<float>(i)[j + 1] - gi[t].ptr<float>(i)[j - 1]) / 2; //gx
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 3] = (cordrx[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i)[j + 1] - gi[t].ptr<float>(i)[j - 1]) / 2; //x*gx
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 4] = (cordry[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i)[j + 1] - gi[t].ptr<float>(i)[j - 1]) / 2; //y*gx
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 5] = (gi[t].ptr<float>(i + 1)[j] - gi[t].ptr<float>(i - 1)[j]) / 2; //gy
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 6] = (cordrx[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i + 1)[j] - gi[t].ptr<float>(i - 1)[j]) / 2; //x*gy
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[8 * t + 7] = (cordry[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i + 1)[j] - gi[t].ptr<float>(i - 1)[j]) / 2; //y*gy
}
Mat N = C.t()*C;
Mat invN = N.inv();
double condition = invert(N, invN, DECOMP_SVD);
condition = 1 / condition;
Mat CtL = C.t()*L;
X = invN * CtL;
//X = (C.t()*C).inv()*C.t()*L;
return X;
}
Mat LSM::LSsolve_Geometric(Mat g0, vector<Mat> gi)
{
Mat C = Mat(row*col*gi.size(), 6 * gi.size(), CV_32F);
Mat L = Mat(row*col*gi.size(), 1, CV_32F);
Mat X = Mat(6 * gi.size(), 1, CV_32F);
//float na0, na1, na2, nb0, nb1, nb2, nh0, nh1;
//系数阵归零
for (int i = 0; i < C.rows; i++)
for (int j = 0; j < C.cols; j++)
C.ptr<float>(i)[j] = 0;
for (int t = 0; t < gi.size(); t++)
for (int i = 1; i < row + 1; i++)
for (int j = 1; j < col + 1; j++)
{
L.ptr<float>(row*col * t + (i - 1)*col + j - 1)[0] = g0.ptr<float>(i)[j] - gi[t].ptr<float>(i)[j];
//L.ptr<float>(row*col * t + (i - 1)*col + j - 1)[0] = gi[t].ptr<float>(i)[j] - g0.ptr<float>(i)[j];
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[6 * t] = (gi[t].ptr<float>(i)[j + 1] - gi[t].ptr<float>(i)[j - 1]) / 2; //gx
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[6 * t + 1] = (cordrx[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i)[j + 1] - gi[t].ptr<float>(i)[j - 1]) / 2; //x*gx
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[6 * t + 2] = (cordry[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i)[j + 1] - gi[t].ptr<float>(i)[j - 1]) / 2; //y*gx
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[6 * t + 3] = (gi[t].ptr<float>(i + 1)[j] - gi[t].ptr<float>(i - 1)[j]) / 2; //gy
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[6 * t + 4] = (cordrx[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i + 1)[j] - gi[t].ptr<float>(i - 1)[j]) / 2; //x*gy
C.ptr<float>(row*col * t + (i - 1)*col + j - 1)[6 * t + 5] = (cordry[t].ptr<float>(i)[j])*(gi[t].ptr<float>(i + 1)[j] - gi[t].ptr<float>(i - 1)[j]) / 2; //y*gy
}
Mat N = C.t()*C;
Mat invN = N.inv();
double condition = invert(N, invN, DECOMP_SVD);
condition = 1 / condition;
Mat CtL = C.t()*L;
X = invN * CtL;
//X = (C.t()*C).inv()*C.t()*L;
return X;
}
void LSM::LSMatch_Single_Point()
{
bool flag = false;
vector<float> nh0 = h0; vector<float> nh1 = h1;
vector<float> na0 = a0; vector<float> na1 = a1; vector<float> na2 = a2;
vector<float> nb0 = b0; vector<float> nb1 = b1; vector<float> nb2 = b2;
Mat g0 = Mat(row + 2, col + 2, CV_32F); //目标影像在搜索窗口的灰度值阵列
int rowi = (row + 1) / 2;
int coli = (col + 1) / 2;
for (int i = -rowi; i <= rowi; i++)
for (int j = -coli; j <= coli; j++)
{
float bi = BinaryInterpolation(path[0], Point2f(pre_match[0].col + j, pre_match[0].row + i));
if (bi == -1.0f)
{
match = pre_match;
return;
}
g0.ptr<float>(i + rowi)[j + coli] = bi;
}
vector<Mat> g; //待匹配影像的相对影像
Mat g_ = Mat(row * row, col * col, CV_32F); //待匹配影像的相对影像
for (int t = 0; t < a0.size(); t++)
{
int g_x = pre_match[t + 1].col;
int g_y = pre_match[t + 1].row;
for (int i = -(g_.rows - 1) / 2; i <= (g_.rows - 1) / 2; i++)
for (int j = -(g_.cols - 1) / 2; j <= (g_.cols - 1) / 2; j++)
{
float bi = BinaryInterpolation(path[t + 1], Point2f(g_x + j, g_y + i));
if (bi == -1.0f)
{
match = pre_match;
return;
}
g_.ptr<float>(i + (g_.rows - 1) / 2)[j + (g_.cols - 1) / 2] = bi;
}
g.push_back(g_);
}
vector<Mat> gi; //待匹配影像在搜索窗口的灰度值阵列
Mat g1 = Mat(row + 2, col + 2, CV_32F); //搜索窗口的灰度值阵列
Mat cordx = Mat(row + 2, col + 2, CV_32F); //搜索窗口的坐标序列x
Mat cordy = Mat(row + 2, col + 2, CV_32F); //搜索窗口
评论0