ca
main.c
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>//
#include "bmp_loader.h"//
#include "calib.h"//
#define M_PI 3.141592653589793
//
// 模拟数据生成(参照、观测和模拟图像生成)dq
//
void gen_pts(
char *file,
double ax, double ay, double az,
double tx, double ty, double tz,
double alpha, double beta, double gamma,
double u0, double v0,
double k1, double k2,
int w, int h,
data_t &data)
{
int x, y;
point_t wpt, cpt;//二维点坐标,double
unsigned char *img;
img = new unsigned char[3 * w * h];//存储图像
memset(img, 0, 3 * w * h);
gen_homography_2D(
ax, ay, az,//旋转
tx, ty, tz,//平移
alpha, beta, gamma,
u0, v0, //内参
data.ref);//单应矩阵
data.pts_num = 0;
//
for (x = -2 * w; x <= 2 * w; x += 50)//竖线
{
for (y = -2 * h; y <= 2 * h; y += 1)
{
wpt.x = x;//二维点坐标,实际是模拟的三维点
wpt.y = y;
//世界坐标系到照相机坐标系转换
w2c_pt(wpt, cpt, data.ref, k1, k2, u0, v0);
if (cpt.x > 0 && cpt.y > 0 && cpt.x < w && cpt.y < h)
{
int dx, dy;
dx = cpt.x;
dy = cpt.y;
img[3 * (dx * w + dy) + 1] = 128;
};
};//y
};//x
for (x = -2 * w; x <= 2 * w; x += 1)//横线
{
for (y = -2 * h; y <= 2 * h; y += 50)
{
wpt.x = x;
wpt.y = y;
// 世界坐标系到照相机坐标系转换
w2c_pt(wpt, cpt, data.ref, k1, k2, u0, v0);
if (cpt.x > 0 && cpt.y > 0 && cpt.x < w && cpt.y < h)
{
int dx, dy;
dx = cpt.x;
dy = cpt.y;//得到横纵坐标
img[3 * (dx * w + dy) + 1] = 128;
};
};//y
};//x
//
for (x = -80; x <= 80; x += 40)//5(离散点)
{
for (y = -80; y <= 80; y += 40)//5,共25点
{
wpt.x = x;
wpt.y = y;
// 世界坐标系到照相机坐标系转换
w2c_pt(wpt, cpt, data.ref, k1, k2, u0, v0);
cpt.x += 0.0 * (rand() % 100) / 100.0;//加上噪声
cpt.y += 0.0 * (rand() % 100) / 100.0;
data.wpts[data.pts_num] = wpt;//三维点
data.cpts[data.pts_num] = cpt;//二维点,相互匹配
data.pts_num++;//计数
if (cpt.x > 0 && cpt.y > 0 && cpt.x < w && cpt.y < h)
{
int dx, dy;
dx = cpt.x;
dy = cpt.y;
img[3 * (dx * w + dy) + 0] = 255;
img[3 * (dx * w + dy) + 1] = 255;
img[3 * (dx * w + dy) + 2] = 255;
};
};//y
};//x
// 画像生成
write_bmp_rgb(file, img, w, h);
delete img;
};
//
// 模拟数据的制作。用它内部设计推测。
//
void test()
{
int w, h;
int i, j;
double k1, k2, u0, v0;
double alpha, beta, gamma;
int img_n;
char file[256];
data_t data[10];
img_n = 3; // 图像数目
w = h = 256; // 图像长宽
u0 = 128.0; // 图像光轴中心
v0 = 128.0;
k1 = -0.2; // 镜头失真系数
k2 = 0.2; //
// 内部参数
alpha = 512;
beta = 512;
gamma = 1.09083;
printf("[***GEN SIMULATION DATA***]\nalpha=%f, beta=%f, gamma=%f\n", alpha, beta, gamma);
printf("u0=%f, v0=%f\n", u0, v0);
printf("k1=%f, k2=%f\n", k1, k2);
//各图像生成
for (i = 0; i < img_n; i++)
{
sprintf(file, "./img/test%d.bmp", i + 1);//屌屌的
gen_pts(file,
M_PI * (rand() % 20 - 10) / 180.0, M_PI * (rand() % 20 - 10) / 180.0, M_PI * (rand() % 40 - 20) / 180.0,
rand() % 50 - 25, rand() % 50 - 25, 812 + rand() % 50,
alpha, beta, gamma,
u0, v0, k1, k2, w, h, data[i]);
//
sprintf(file, "./pt%d.txt", i + 1);
FILE *fp = fopen(file, "w");
for (int j = 0; j < data[i].pts_num; j++)
{
fprintf(fp, "%f %f\n", data[i].cpts[j].x, data[i].cpts[j].y);
};
fclose(fp);
fp = fopen("./model.txt", "w");
for (int j = 0; j < data[i].pts_num; j++)
{
fprintf(fp, "%f %f\n", data[i].wpts[j].x, data[i].wpts[j].y);
};
fclose(fp);
};//图像生成结束
// 线性方法,计算各图像的ホモグラフィ。镜头失真是不包含在内的。
for (i = 0; i < img_n; i++)
{
estimate_homography(data[i].wpts,//世界坐标点
data[i].cpts, //相机坐标点
data[i].pts_num, //点数目
data[i].H);
};
// 复数图像内部参数推测。镜头失真推测等。
estimate_intrinsic(data, img_n, alpha, beta, gamma, u0, v0, k1, k2);
for (i = 0; i < img_n; i++)
{
unsigned char *img;
unsigned char *out;
sprintf(file, "./img/test%d.bmp", i + 1);
load_bmp_rgb(file, &img, &w, &h);
int x, y;
double ix, iy, rx, ry;
out = new unsigned char[3 * w * h];
for (y = 0; y < h; y++)
{
for (x = 0; x < w; x++)
{
ix = (x - u0) / 512.0;
iy = (y - v0) / 512.0;
rx = x + (x - u0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));
ry = y + (y - v0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));
if (rx > 0 && rx < w && ry > 0 && ry < h)
{
int dx, dy;
dx = rx; dy = ry;
out[3 * (x + y * w) + 0] = img[3 * (dx + dy * w) + 0];
out[3 * (x + y * w) + 1] = img[3 * (dx + dy * w) + 1];
out[3 * (x + y * w) + 2] = img[3 * (dx + dy * w) + 2];
}
else {
out[3 * (x + y * w) + 0] = 0;
out[3 * (x + y * w) + 1] = 0;
out[3 * (x + y * w) + 2] = 0;
};
};//x
};//y
sprintf(file, "./img/out%d.bmp", i + 1);
write_bmp_rgb(file, out, w, h);
delete out;
delete img;
};
};
int main(int argc, char *argv[])
{
if (argc == 1)
{
test();
return 0;
};
double k1, k2, u0, v0;
double alpha, beta, gamma;
int img_n;
char file[256];
data_t data[10];
img_n = argc - 2;
int i;
for (i = 0; i < img_n; i++)
{
char file[256];
char txt[1024];
sprintf(file, "%s", argv[i + 2]);
FILE *fp = fopen(file, "r");
int n = 0, j;
while (fgets(txt, 1024, fp))
{
sscanf(txt, "%lf %lf", &data[i].cpts[n].x, &data[i].cpts[n].y);
n++;
};
fclose(fp);
fp = fopen(argv[1], "r");
n = 0;
while (fgets(txt, 1024, fp))
{
sscanf(txt, "%lf %lf", &data[i].wpts[n].x, &data[i].wpts[n].y);
n++;
};
fclose(fp);
// 正規化
if (0)
{
point_t mu1, mu2;
point_t sig1, sig2;
mu1.x = mu1.y = mu2.x = mu2.y = 0.0;
sig1.x = sig1.y = sig2.x = sig2.y = 0.0;
for (j = 0; j < n; j++)
{
mu1.x += data[i].cpts[j].x;
mu1.y += data[i].cpts[j].y;
mu2.x += data[i].wpts[j].x;
mu2.y += data[i].wpts[j].y;
};
mu1.x /= (double)n;
mu1.y /= (double)n;
mu2.x /= (double)n;
mu2.y /= (double)n;
for (j = 0; j < n; j++)
{
sig1.x += (data[i].cpts[j].x - mu1.x)*(data[i].cpts[j].x - mu1.x);
sig1.y += (data[i].cpts[j].y - mu1.y)*(data[i].cpts[j].y - mu1.y);
sig2.x += (data[i].wpts[j].x - mu2.x)*(data[i].wpts[j].x - mu2.x);
sig2.y += (data[i].wpts[j].y - mu2.y)*(data[i].wpts[j].y - mu2.y);
};
sig1.x = sqrt(sig1.x / (double)n);
sig1.y = sqrt(sig1.y / (double)n);
sig2.x = sqrt(sig2.x / (double)n);
sig2.y = sqrt(sig2.y / (double)n);
for (j = 0; j < n; j++)
{
data[i].cpts[j].x = (data[i].cpts[j].x - mu1.x) / sig1.x;
data[i].cpts[j].y = (data[i].cpts[j].y - mu1.y) / sig1.y;
data[i].wpts[j].x = (data[i].wpts[j].x - mu2.x) / sig2.x;
data[i].wpts[j].y = (data[i].wpts[j].y - mu2.y) / sig2.y;
};
};
data[i].pts_num = n;
};
// 复数图像内部参数推测。镜头失真推测等。
alpha = beta = gamma = u0 = v0 = k1 = k2 = 0.0;
// 线性方法,计算各图像的ホモグラフィ。镜头失真是不包含在内的。。
for (i = 0; i < img_n; i++)
{
estimate_homography(data[i].wpts, data[i].cpts, data[i].pts_num, data[i].H);
};
estimate_intrinsic(data, img_n, alpha, beta, gamma, u0, v0, k1, k2);
return 0;
};
calib.h
//
// Zhangのキャリブレーション
//
#include "minpack.h"
#include "mat.h"
#include "gen_geometric_mat.h"
// LAPACK使用
extern "C"{
#include "f2c.h"
#include "clapack.h"
}
// 分群结构体
struct point_t
{
double x, y;
};
// 对应点的结构体
struct data_t
{
point_t wpts[100];//三维点
point_t cpts[100];//二维点
int pts_num;//点数目
double H[9];
double ref[9];//单应矩阵
double Rt[9];
double R[9];
double t[3];
};
// 为了minpack global变数
point_t fcn_wpts[1000];
point_t fcn_cpts[1000];
double tmp_Rt[1000];
int fcn_pts_num;
int fcn_pts_list[1000];
int fcn_list_num;
double dRt[10][9];
// 世界坐标系到照相机坐标系转换,径向失真矫正
void w2c_pt(point_t &in, point_t &out, double *H, double k1, double k2, double u0, double v0)
{
double ix, iy, rx, ry, rz;//三维点[x,y,0,1]到[u,v,1]
rz = (in.x * H[3 * 2 + 0] + in.y * H[3 * 2 + 1] + H[3 * 2 + 2]);//H*[x,y,1]T
rx = (in.x * H[3 * 0 + 0] + in.y * H[3 * 0 + 1] + H[3 * 0 + 2]) / (rz);
ry = (in.x * H[3 * 1 + 0] + in.y * H[3 * 1 + 1] + H[3 * 1 + 2]) / (rz);
ix = (rx - u0) / 512.0;
iy = (ry - v0) / 512.0;
out.x = rx + (rx - u0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));//(11)
out.y = ry + (ry - v0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));//(12)
};
// 投影误差计算
double calc_projection_err(doublereal * vvec)
{
int i, j, n;
double dx, dy;
double x, y, z, mx, my, tx, ty, ix, iy;
double H[10][9], Rt[9], dR[9];
double A[9];
double u0, v0, k1, k2;
gen_intrinsic_mat(vvec[0], vvec[1], vvec[2], vvec[3], vvec[4], A);
u0 = vvec[3];
v0 = vvec[4];
k1 = vvec[5];
k2 = vvec[6];
double err = 0.0;
/*
for(i = 0;i < fcn_list_num;i++)
{
for(j = 0;j < 9;j++)
{
Rt[j] = vvec[7 + i * 9 + j];
};
mat_mul(A, Rt, H[i], 3);
};// i
*/
for (i = 0; i < fcn_list_num; i++)
{
gen_rot_mat(vvec[7 + i * 6], vvec[7 + i * 6 + 1], vvec[7 + i * 6 + 2], dR);
mat_mul(dRt[i], dR, Rt, 3);
Rt[3 * 0 + 2] = vvec[7 + i * 6 + 3];
Rt[3 * 1 + 2] = vvec[7 + i * 6 + 4];
Rt[3 * 2 + 2] = vvec[7 + i * 6 + 5];
mat_mul(A, Rt, H[i], 3);
};// i
for (i = 0; i < fcn_pts_num; i++)
{
n = fcn_pts_list[i];
x = H[n][0] * fcn_wpts[i].x + H[n][1] * fcn_wpts[i].y + H[n][2];
y = H[n][3] * fcn_wpts[i].x + H[n][4] * fcn_wpts[i].y + H[n][5];
z = H[n][6] * fcn_wpts[i].x + H[n][7] * fcn_wpts[i].y + H[n][8];
mx = x / z;
my = y / z;
ix = (mx - u0) / 512.0;
iy = (my - v0) / 512.0;
tx = mx + (mx - u0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));
ty = my + (my - v0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));
err += sqrt((tx - fcn_cpts[i].x)*(tx - fcn_cpts[i].x) + (ty - fcn_cpts[i].y)*(ty - fcn_cpts[i].y));
};// i
return err / (double)fcn_pts_num;
};
//*************************************************************************
// minpack用优化过程
//
void fcn_homography(integer * fnum, integer * vnum,
doublereal * vvec, doublereal * fvec, integer * iflag)
{
int i;
double dx, dy;
double x, y, z, mx, my;
for (i = 0; i < fcn_pts_num; i++)
{
x = vvec[0] * fcn_wpts[i].x + vvec[1] * fcn_wpts[i].y + vvec[2];
y = vvec[3] * fcn_wpts[i].x + vvec[4] * fcn_wpts[i].y + vvec[5];
z = vvec[6] * fcn_wpts[i].x + vvec[7] * fcn_wpts[i].y + vvec[8];
mx = x / z;
my = y / z;
fvec[i] = mx - fcn_cpts[i].x;
fvec[i + fcn_pts_num] = my - fcn_cpts[i].y;
};// i
};
void fcn_homography_distortion(integer * fnum, integer * vnum,
doublereal * vvec, doublereal * fvec, integer * iflag)
{
int i, j, n;
double dx, dy;
double x, y, z, mx, my, tx, ty, ix, iy;
double H[10][9], R[9], dR[9], Rt[9];
double A[9];
double u0, v0, k1, k2;
gen_intrinsic_mat(vvec[0], vvec[1], vvec[2], vvec[3], vvec[4], A);
u0 = vvec[3];
v0 = vvec[4];
k1 = vvec[5];
k2 = vvec[6];
for (i = 0; i < fcn_list_num; i++)
{
gen_rot_mat(vvec[7 + i * 6], vvec[7 + i * 6 + 1], vvec[7 + i * 6 + 2], dR);
mat_mul(dRt[i], dR, Rt, 3);
/*
for(j = 0;j < 9;j++)
{
Rt[j] = vvec[7 + i * 9 + j];
};
*/
Rt[3 * 0 + 2] = vvec[7 + i * 6 + 3];
Rt[3 * 1 + 2] = vvec[7 + i * 6 + 4];
Rt[3 * 2 + 2] = vvec[7 + i * 6 + 5];
mat_mul(A, Rt, H[i], 3);
};// i
for (i = 0; i < fcn_pts_num; i++)
{
n = fcn_pts_list[i];
x = H[n][0] * fcn_wpts[i].x + H[n][1] * fcn_wpts[i].y + H[n][2];
y = H[n][3] * fcn_wpts[i].x + H[n][4] * fcn_wpts[i].y + H[n][5];
z = H[n][6] * fcn_wpts[i].x + H[n][7] * fcn_wpts[i].y + H[n][8];
mx = x / z;
my = y / z;
ix = (mx - u0) / 512.0;
iy = (my - v0) / 512.0;
tx = mx + (mx - u0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));
ty = my + (my - v0) * (k1 * (ix*ix + iy*iy) + k2 * (ix*ix + iy*iy) * (ix*ix + iy*iy));
fvec[i] = tx - fcn_cpts[i].x;
fvec[i + fcn_pts_num] = ty - fcn_cpts[i].y;
};// i
};
// minpack用优化过程
//*************************************************************************
//********************************************************************************
// 应对分开始内部パラメタ和镜头失真,パラメタ非线性优化调整。data初期解决。
//********************************************************************************
void optimize_homography_distortion(
double & alpha, double & beta, double & gamma,
double & u0, double & v0, double & k1, double & k2,
data_t * data, int N
)
{
int i, j;
integer m, n;
doublereal vvec[1000] = { 0 }, fvec[1000] = { 0 }, *wa, tol;
integer info = 0, lwa = 0;
integer *iwa;
n = 0; // 应优化矢量的角度
m = 0; // 目的函数的项数
for (i = 0; i < N; i++)
{
m += 2 * data[i].pts_num;
};
vvec[n++] = alpha;
vvec[n++] = beta;
vvec[n++] = gamma;
vvec[n++] = u0;
vvec[n++] = v0;
vvec[n++] = k1;
vvec[n++] = k2;
for (i = 0; i < N; i++)
{
/*
for(j = 0;j < 9;j++)
{
vvec[n++] = 0.0;//data[i].Rt[j];
};// j
*/
vvec[n++] = 0.0;
vvec[n++] = 0.0;
vvec[n++] = 0.0;
vvec[n++] = data[i].t[0];
vvec[n++] = data[i].t[1];
vvec[n++] = data[i].t[2];
memcpy(dRt[i], data[i].R, sizeof(double)* 9);
};// i
fcn_pts_num = 0;
fcn_list_num = N;
for (i = 0; i < N; i++)
{
for (j = 0; j < data[i].pts_num; j++)
{
fcn_wpts[fcn_pts_num] = data[i].wpts[j];
fcn_cpts[fcn_pts_num] = data[i].cpts[j];
fcn_pts_list[fcn_pts_num] = i;
fcn_pts_num++;
}; // j
};// i
lwa = m*n + 5 * n + m;
wa = new doublereal[lwa];
iwa = new integer[lwa];
tol = 10e-10;
double e1, e2;
e1 = calc_projection_err(vvec);
printf("optimizing....");
lmdif1_((U_fp)(fcn_homography_distortion), &m, &n, vvec, fvec, &tol, &info, iwa, wa, &lwa);
printf("ok\n");
e2 = calc_projection_err(vvec);
alpha = vvec[0];
beta = vvec[1];
gamma = vvec[2];
u0 = vvec[3];
v0 = vvec[4];
k1 = vvec[5];
k2 = vvec[6];
// print log
printf("[***RESULT***]\nerr=%f[pix](initial) ---> %f[pix](after non-linear optimize)\n", e1, e2);
printf("[***ESTIMATED PARAMETER***]\nalpha=%f, beta=%f, gamma=%f\n", alpha, beta, gamma);
printf("u0=%f, v0=%f\n", u0, v0);
printf("k1=%f, k2=%f\n", k1, k2);
delete wa;
delete iwa;
}
//********************************************************************************
// 从非线性ホモグラフィ对应点,调整优化。h是初期解决。
//********************************************************************************
void optimize_homography(double *h, point_t *wpts, point_t *cpts, int N)
{
int i;
integer m, n;
doublereal vvec[10000] = { 0 }, fvec[10000] = { 0 }, wa[10000] = { 0 }, tol;
integer info = 0, lwa = 0;
integer iwa[10000] = { 0 };
n = 9; // 应优化矢量的角度
m = 2 * N; // 目的函数的项数
lwa = m*n + 5 * n + m;
tol = 10e-10;
for (i = 0; i < 9; i++)
{
vvec[i] = h[i];
};
fcn_pts_num = N;
for (i = 0; i < N; i++)
{
fcn_wpts[i] = wpts[i];
fcn_cpts[i] = cpts[i];
};
/* the purpose of lmdif1 is to minimize the sum of the squares of */
/* m nonlinear functions in n variables by a modification of the */
/* levenberg-marquardt algorithm.*/
lmdif1_((U_fp)(fcn_homography), &m, &n, vvec, fvec, &tol, &info, iwa, wa, &lwa);
for (i = 0; i < 9; i++)
{
h[i] = vvec[i];
};
};
//********************************************************************************
// 参照和观测分cpts wpts分ホモグラフィ队伍从eH推测
//********************************************************************************
void estimate_homography(point_t *wpts,
point_t *cpts,
int pts_num,//点数
double *eH)
{
double B[10000];
double BB[81];
int i, j, k, n;
// 论文的公式:附录A
for (i = 0; i < pts_num; i++)
{
n = 2 * i;
B[n * 9 + 0] = wpts[i].x;
B[n * 9 + 1] = wpts[i].y;
B[n * 9 + 2] = 1.0;
B[n * 9 + 3] = 0.0;
B[n * 9 + 4] = 0.0;
B[n * 9 + 5] = 0.0;
B[n * 9 + 6] = -cpts[i].x * wpts[i].x;
B[n * 9 + 7] = -cpts[i].x * wpts[i].y;
B[n * 9 + 8] = -cpts[i].x;
n = 2 * i + 1;
B[n * 9 + 0] = 0.0;
B[n * 9 + 1] = 0.0;
B[n * 9 + 2] = 0.0;
B[n * 9 + 3] = wpts[i].x;
B[n * 9 + 4] = wpts[i].y;
B[n * 9 + 5] = 1.0;
B[n * 9 + 6] = -cpts[i].y * wpts[i].x;
B[n * 9 + 7] = -cpts[i].y * wpts[i].y;
B[n * 9 + 8] = -cpts[i].y;
};// i
// B^T B
for (i = 0; i < 9; i++)
{
for (j = 0; j < 9; j++)
{
double sum = 0.0;
for (k = 0; k < 2 * pts_num; k++)
{
sum += B[k * 9 + i] * B[k * 9 + j];
};// k
BB[i * 9 + j] = sum;
};//j
};// i
long N = 9;
double wr[1000], wi[2], work[1000], vl[1000], vr[1000];
long lda = N, ldvl = N, ldvr = N, lwork = 3 * N, info, ldvt = 5;
// 线性解法:固有方程解决
// A 为BB
//computes all eigenvalues and, optionally, eigenvectors of a real symmetric matrix A
// 'V': Compute eigenvalues and eigenvectors;
// 'U': Upper triangle of A is stored;
// The order of the matrix A
// lda: The leading dimension of the array A
// lwork: The length of the array WORK
dsyev_("V", "U", &N, BB, &lda, wr, work, &lwork, &info);
double h[9];
for (i = 0; i < 9; i++)
{
h[i] = BB[i];
};
// 非线性解法:马尔卡丁车法调整
optimize_homography(h, wpts, cpts, pts_num);
// 结果的表格
eH[3 * 0 + 0] = h[0];
eH[3 * 0 + 1] = h[1];
eH[3 * 0 + 2] = h[2];
eH[3 * 1 + 0] = h[3];
eH[3 * 1 + 1] = h[4];
eH[3 * 1 + 2] = h[5];
eH[3 * 2 + 0] = h[6];
eH[3 * 2 + 1] = h[7];
eH[3 * 2 + 2] = h[8];
};
//********************************************************************************
// 复数图像的观测分的对应,内部パラメタ推测
//********************************************************************************
void estimate_intrinsic(
data_t * data, // 对应点数据
int N, // 画像枚数
double & alpha, double & beta, double & gamma,
double & u0, double & v0, double & k1, double & k2
)
{
int n, i, j, k;
double V[1000], VV[1000], v[2][2][6];
// 行列V作成:Zhang論文の式(9)
for (n = 0; n < N; n++)
{
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
v[i][j][0] = data[n].H[i + 3 * 0] * data[n].H[j + 3 * 0];
v[i][j][1] = data[n].H[i + 3 * 0] * data[n].H[j + 3 * 1] + data[n].H[i + 3 * 1] * data[n].H[j + 3 * 0];
v[i][j][2] = data[n].H[i + 3 * 1] * data[n].H[j + 3 * 1];
v[i][j][3] = data[n].H[i + 3 * 2] * data[n].H[j + 3 * 0] + data[n].H[i + 3 * 0] * data[n].H[j + 3 * 2];
v[i][j][4] = data[n].H[i + 3 * 2] * data[n].H[j + 3 * 1] + data[n].H[i + 3 * 1] * data[n].H[j + 3 * 2];
v[i][j][5] = data[n].H[i + 3 * 2] * data[n].H[j + 3 * 2];
};//j
};//i
for (j = 0; j < 6; j++)
{
V[(2 * n) * 6 + j] = v[0][1][j];
V[(2 * n + 1) * 6 + j] = v[0][0][j] - v[1][1][j];
};
};// n
// V^T V
for (i = 0; i < 6; i++)
{
for (j = 0; j < 6; j++)
{
double sum = 0.0;
for (k = 0; k < 2 * N; k++)
{
sum += V[k * 6 + i] * V[k * 6 + j];
};// k
VV[i * 6 + j] = sum;
};//j
};// i
long P = 6;
double wr[1000], wi[2], work[1000], vl[1000], vr[1000];
long lda = P, ldvl = P, ldvr = P, lwork = 3 * P, info, ldvt = 6;
// 固有方程式解开
dsyev_("V", "U", &P, VV, &lda, wr, work, &lwork, &info);
// 求めた固有ベクトルから行列Ｂを構成：Zhang論文の式(6)
double B[9];
B[3 * 0 + 0] = VV[0];
B[3 * 0 + 1] = B[3 * 1 + 0] = VV[1];
B[3 * 1 + 1] = VV[2];
B[3 * 0 + 2] = B[3 * 2 + 0] = VV[3];
B[3 * 1 + 2] = B[3 * 2 + 1] = VV[4];
B[3 * 2 + 2] = VV[5];
// 内部参数推测:漳论文的附录B
double lambda;
v0 = (B[3 * 0 + 1] * B[3 * 0 + 2] - B[3 * 0 + 0] * B[3 * 1 + 2]) / (B[3 * 0 + 0] * B[3 * 1 + 1] - B[3 * 0 + 1] * B[3 * 0 + 1]);
lambda = B[3 * 2 + 2] - (B[3 * 0 + 2] * B[3 * 0 + 2] + v0 * (B[3 * 0 + 1] * B[3 * 0 + 2] - B[3 * 0 + 0] * B[3 * 1 + 2])) / B[3 * 0 + 0];
alpha = sqrt(lambda / B[3 * 0 + 0]);
beta = sqrt(lambda * B[3 * 0 + 0] / (B[3 * 0 + 0] * B[3 * 1 + 1] - B[3 * 0 + 1] * B[3 * 0 + 1]));
gamma = -B[3 * 0 + 1] * alpha * alpha * beta / lambda;
u0 = gamma * v0 / beta - B[3 * 0 + 2] * alpha * alpha / lambda;
// 内部行列の逆
double invA[9];
invA[0] = 1.0 / alpha;
invA[1] = -gamma / (alpha * beta);
invA[2] = (gamma * v0 - u0 * beta) / (alpha * beta);
invA[3] = 0.0; invA[4] = 1.0 / beta; invA[5] = -v0 / beta;
invA[6] = 0.0; invA[7] = 0.0; invA[8] = 1.0;
// invAとホモグラフィから各画像でのRt行列を求める
for (i = 0; i < N; i++)
{
gen_Rt_from_AH(invA, data[i].H, data[i].R, data[i].t);
};
k1 = k2 = 0.0;
// 非线性解法:马尔卡丁车法调整
optimize_homography_distortion(alpha, beta, gamma, u0, v0, k1, k2, data, N);
};
mat.h
//
// 娙堈峴楍墘嶼
//
#ifndef _MAT_
#define _MAT_
// 峴楍偺妡偗嶼丗 C = A * B
void mat_mul(double *A, double *B, double *C, int N)
{
int i, j, k;
double sum;
for(i = 0;i < N;i++)
{
for(j = 0;j < N;j++)
{
sum = 0.0;
for(k = 0;k < N;k++)
{
sum += A[i * N + k] * B[k * N + j];
};
C[i * N + j] = sum;
};//j
};//i
};
// 峴楍偲儀僋僩儖偺妡偗嶼丗b = A . v
void mat_vec(double *A, double *v, double *b, int N)
{
int i, k;
double sum;
for(i = 0;i < N;i++)
{
sum = 0.0;
for(k = 0;k < N;k++)
{
sum += A[i * N + k] * v[k];
};
b[i] = sum;
};//i
};
// 撪愊丗<a, b>
double dot(double *a, double *b, int N)
{
double sum = 0.0;
int i;
for(i = 0;i < N;i++)
sum += a[i] * b[i];
return sum;
};
// 奜愊丗a 亊 b
void cross3(double *a, double *b, double *c)
{
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = -(a[0] * b[2] - a[2] * b[0]);
c[2] = a[0] * b[1] - a[1] * b[0];
};
void normalize(double *a, int N)
{
int i;
double d = sqrt(dot(a, a, N));
for(i = 0;i < N;i++)
a[i] /= d;
};
#endif
gen_mat.h
//
// 几何变换用的队伍生成
//
#ifndef _GEN_GEOMETRIC_MAT_
#define _GEN_GEOMETRIC_MAT_
#include "mat.h"
// 内部行列 A 生成
void gen_intrinsic_mat(double alpha, double beta, double gamma, double u0, double v0, double *A)
{
A[0 * 3 + 0] = alpha; //搞定
A[0 * 3 + 1] = gamma;
A[0 * 3 + 2] = u0;
A[1 * 3 + 0] = 0.0;
A[1 * 3 + 1] = beta;
A[1 * 3 + 2] = v0;
A[2 * 3 + 0] = 0.0;
A[2 * 3 + 1] = 0.0;
A[2 * 3 + 2] = 1.0;
};
// 旋转队伍R生成(3个延展而成角开始生成)
void gen_rot_mat(double x, double y, double z, double *R)
{
double Rx[9], Ry[9], Rz[9], tmp[9];
Rz[0 * 3 + 0] = cos(z); Rz[0 * 3 + 1] = -sin(z); Rz[0 * 3 + 2] = 0.0;
Rz[1 * 3 + 0] = sin(z); Rz[1 * 3 + 1] = cos(z); Rz[1 * 3 + 2] = 0.0;
Rz[2 * 3 + 0] = 0.0; Rz[2 * 3 + 1] = 0.0; Rz[2 * 3 + 2] = 1.0;
Ry[0 * 3 + 0] = cos(y); Ry[0 * 3 + 1] = 0.0; Ry[0 * 3 + 2] = sin(y);
Ry[1 * 3 + 0] = 0.0; Ry[1 * 3 + 1] = 1.0; Ry[1 * 3 + 2] = 0.0;
Ry[2 * 3 + 0] = -sin(y); Ry[2 * 3 + 1] = 0.0; Ry[2 * 3 + 2] = cos(y);
Rx[0 * 3 + 0] = 1.0; Rx[0 * 3 + 1] = 0.0; Rx[0 * 3 + 2] = 0.0;
Rx[1 * 3 + 0] = 0.0; Rx[1 * 3 + 1] = cos(x); Rx[1 * 3 + 2] = -sin(x);
Rx[2 * 3 + 0] = 0.0; Rx[2 * 3 + 1] = sin(x); Rx[2 * 3 + 2] = cos(x);
mat_mul(Ry, Rx, tmp, 3);//矩阵乘法N*N
mat_mul(Rz, tmp, R, 3);
};
// ホモグラフィ（斜影変換）＋平行移動 行列 H 生成（２Ｄ用）
void gen_homography_2D(double x, double y, double z, double tx, double ty, double tz,
double alpha, double beta, double gamma, double u0, double v0, double *H
)
{
double R[9];//旋转矩阵
double A[9];//内参
double T[9];//
gen_rot_mat(x, y, z, R);//生成旋转矩阵
gen_intrinsic_mat(alpha, beta, gamma, u0, v0, A);//生成内参矩阵
//[r1,r2,t]
T[3 * 0 + 0] = R[3 * 0 + 0]; T[3 * 0 + 1] = R[3 * 0 + 1]; T[3 * 0 + 2] = tx;
T[3 * 1 + 0] = R[3 * 1 + 0]; T[3 * 1 + 1] = R[3 * 1 + 1]; T[3 * 1 + 2] = ty;
T[3 * 2 + 0] = R[3 * 2 + 0]; T[3 * 2 + 1] = R[3 * 2 + 1]; T[3 * 2 + 2] = tz;
mat_mul(A, T, H, 3);//H单应矩阵
};
// A ^ - 1和H开始生成旋转队伍
void gen_Rt_from_AH(double *invA, double *H, double * R, double * t)
{
double lambda;
double r1[3], r2[3], r3[3], r4[3];
double h1[3], h2[3], h3[3];
double Rt[9];
int i;
h1[0] = H[0];
h1[1] = H[3];
h1[2] = H[6];
h2[0] = H[1];
h2[1] = H[4];
h2[2] = H[7];
h3[0] = H[2];
h3[1] = H[5];
h3[2] = H[8];
mat_vec(invA, h1, r1, 3);
mat_vec(invA, h2, r2, 3);
mat_vec(invA, h3, t, 3);
lambda = 1.0 / sqrt(dot(r1, r1, 3));
for (i = 0; i < 3; i++)
{
r1[i] *= lambda;
r2[i] *= lambda;
t[i] *= lambda;
};
normalize(r1, 3);
normalize(r2, 3);
cross3(r1, r2, r3);
cross3(r1, r3, r4);
if (r4[0] * r2[0] < 0)
{
r2[0] = -r4[0];
r2[1] = -r4[1];
r2[2] = -r4[2];
}
else {
r2[0] = r4[0];
r2[1] = r4[1];
r2[2] = r4[2];
};
double d1 = dot(r1, r1, 3);
double d2 = dot(r2, r2, 3);
double d12 = dot(r1, r2, 3);
for (i = 0; i < 3; i++)
{
R[3 * i + 0] = r1[i];
R[3 * i + 1] = r2[i];
R[3 * i + 2] = r3[i];
};
for (i = 0; i < 3; i++)
{
Rt[3 * i + 0] = r1[i];
Rt[3 * i + 1] = r2[i];
Rt[3 * i + 2] = t[i];
};
};
#endif
bmp_loader.h
//
// bit图读入 g.koutaki 2012
//
#ifndef _BMP_LOADER_
#define _BMP_LOADER_
typedef unsigned long DWORD;
typedef unsigned short WORD;
typedef long LONG;
typedef struct tagBITMAPCOREHEADER{
DWORD bcSize;
WORD bcWidth;
WORD bcHeight;
WORD bcPlanes;
WORD bcBitCount;
} BITMAPCOREHEADER;//未使用
typedef struct tagBITMAPINFOHEADER{
DWORD biSize;
LONG biWidth;
LONG biHeight;
WORD biPlanes;
WORD biBitCount;
DWORD biCompression;
DWORD biSizeImage;
LONG biXPelsPerMeter;
LONG biYPelsPerMeter;
DWORD biClrUsed;
DWORD biClrImportant;
} BITMAPINFOHEADER;//位图信息头
typedef struct tagBITMAPFILEHEADER {
WORD bfType;
DWORD bfSize;
WORD bfReserved1;
WORD bfReserved2;
DWORD bfOffBits;
} BITMAPFILEHEADER;//位图文件头
typedef struct tagBITMAPINFO {
BITMAPINFOHEADER bmiHeader;
DWORD bmiColors[1];
} BITMAPINFO;//彩色表/调色板
void load_bmp_rgb(char *filename, unsigned char **img, int *w, int *h)
{
FILE *fp = fopen(filename, "rb");
BITMAPINFOHEADER BmpInfoH;//位图信息头
BITMAPFILEHEADER BmpFileH;//位图文件头
int x, y, pitch;
unsigned char line[3 * 9096];
int s = sizeof(BITMAPINFOHEADER);
if (fp)//打开成功
{
fread(&BmpFileH, 1, 14, fp);//文件头
fread(&BmpInfoH, 1, 40, fp);//信息头
//位图的幅度要求
*w = BmpInfoH.biWidth;
*h = BmpInfoH.biHeight;
printf("load %s:%dx%d\n", filename, *w, *h);//打印信息
(*img) = new unsigned char[*w * *h * 3];//C++的
pitch = (*w * 3 + 3) & ~3;//对齐？
//ビットマップデータ为止文件点移动
fseek(fp, 54, SEEK_SET);//偏移到数据
for (y = *h - 1; y >= 0; y--)//从最后一行读？？
{
fread(line, pitch, 1, fp);
for (x = 0; x < *w; x++)
{
(*img)[(x + y * *w) * 3 + 0] = line[x * 3 + 2]; // R
(*img)[(x + y * *w) * 3 + 1] = line[x * 3 + 1]; // G
(*img)[(x + y * *w) * 3 + 2] = line[x * 3 + 0]; // B
};
};
fclose(fp);
};
};
void load_bmp_gray(char *filename, unsigned char **img, int *w, int *h){
FILE *fp = fopen(filename, "rb");
BITMAPINFOHEADER BmpInfoH;
BITMAPFILEHEADER BmpFileH;
int x, y, pitch;
unsigned char line[3 * 9096];
int s = sizeof(BITMAPINFOHEADER);
if (fp){
fread(&BmpFileH, 1, 14, fp);
fread(&BmpInfoH, 1, 40, fp);
//位图的幅度要求
*w = BmpInfoH.biWidth;
*h = BmpInfoH.biHeight;
printf("load %s:%dx%d\n", filename, *w, *h);
(*img) = new unsigned char[*w * *h * 3];
pitch = (*w * 3 + 3) & ~3;
//ビットマップデータ为止文件点移动
fseek(fp, 54, SEEK_SET);
for (y = *h - 1; y >= 0; y--)
{
fread(line, pitch, 1, fp);
for (x = 0; x < *w; x++)
{
(*img)[(x + y * *w)] = line[x * 3 + 0]; // R
};
};
fclose(fp);
};
};
void write_bmp_rgb( char *filename, //文件名
unsigned char *img, //图像数据
int w, //宽
int h) //高
{
FILE *fp = fopen(filename, "wb");
BITMAPINFOHEADER BmpInfoH;//长宽大小等信息
BITMAPINFO BmpInfo;
BITMAPFILEHEADER BmpFileH;
int x, y, pitch;
unsigned char line[3 * 9096];
int s = sizeof(BITMAPFILEHEADER);
if (fp){
//位图的幅度要求
pitch = (w * 3 + 3) & ~3;
memset(&BmpFileH, 0, 14);
memset(&BmpInfo, 0, 40);
memset(&BmpInfoH, 0, 40);
//位图信息头
BmpInfo.bmiHeader.biHeight = h;
BmpInfo.bmiHeader.biWidth = w;
BmpInfo.bmiHeader.biSize = 40;
BmpInfo.bmiHeader.biSizeImage = pitch * h;
BmpInfo.bmiHeader.biPlanes = 1;
BmpInfo.bmiHeader.biBitCount = 24;
BmpInfo.bmiHeader.biCompression = 0;
BmpInfo.bmiHeader.biClrUsed = 0;
BmpInfo.bmiHeader.biXPelsPerMeter = 0;
BmpInfo.bmiHeader.biYPelsPerMeter = 0;
BmpInfo.bmiHeader.biClrImportant = 0;
BmpInfoH = BmpInfo.bmiHeader;
//位图文件头
BmpFileH.bfOffBits = 3435921408;
BmpFileH.bfSize = pitch * h + BmpFileH.bfOffBits;
BmpFileH.bfType = 19778;
BmpFileH.bfReserved1 = 0;
BmpFileH.bfReserved2 = 54;
printf("write %s:%dx%d\n", filename, w, h);
fwrite(&BmpFileH, 14, 1, fp);
fwrite(&BmpInfoH, 40, 1, fp);
for (y = h - 1; y >= 0; y--){
for (x = 0; x < w; x++){
line[x * 3 + 2] = img[(x + y * w) * 3 + 0];
line[x * 3 + 1] = img[(x + y * w) * 3 + 1];
line[x * 3 + 0] = img[(x + y * w) * 3 + 2];
};
fwrite(line, pitch, 1, fp);
};
fclose(fp);
};
};
void write_bmp_gray(char *filename, unsigned char *img, int w, int h)
{
FILE *fp = fopen(filename, "wb");
BITMAPINFOHEADER BmpInfoH;
BITMAPINFO BmpInfo;
BITMAPFILEHEADER BmpFileH;
int x, y, pitch;
unsigned char line[3 * 9096];
int s = sizeof(BITMAPFILEHEADER);
if (fp)
{
// 位图的幅度要求
pitch = (w * 3 + 3) & ~3;
memset(&BmpFileH, 0, 14);
memset(&BmpInfo, 0, 40);
memset(&BmpInfoH, 0, 40);
BmpInfo.bmiHeader.biHeight = h;
BmpInfo.bmiHeader.biWidth = w;
BmpInfo.bmiHeader.biSize = 40;
BmpInfo.bmiHeader.biSizeImage = pitch * h;
BmpInfo.bmiHeader.biPlanes = 1;
BmpInfo.bmiHeader.biBitCount = 24;
BmpInfo.bmiHeader.biCompression = 0;
BmpInfo.bmiHeader.biClrUsed = 0;
BmpInfo.bmiHeader.biXPelsPerMeter = 0;
BmpInfo.bmiHeader.biYPelsPerMeter = 0;
BmpInfo.bmiHeader.biClrImportant = 0;
BmpInfoH = BmpInfo.bmiHeader;
BmpFileH.bfOffBits = 3435921408;
BmpFileH.bfSize = pitch * h + BmpFileH.bfOffBits;
BmpFileH.bfType = 19778;
BmpFileH.bfReserved1 = 0;
BmpFileH.bfReserved2 = 54;
printf("write %s:%dx%d\n", filename, w, h);
fwrite(&BmpFileH, 14, 1, fp);
fwrite(&BmpInfoH, 40, 1, fp);
for (y = h - 1; y >= 0; y--)
{
for (x = 0; x < w; x++)
{
line[x * 3 + 2] = img[(x + y * w)];
line[x * 3 + 1] = img[(x + y * w)];
line[x * 3 + 0] = img[(x + y * w)];
};
fwrite(line, pitch, 1, fp);
};
fclose(fp);
};
};
#endif
