ca
main.chtml
#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.horm
//
// 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
相關文章
- 1. ca
- 2. fabric CA
- 3. Windows2016 主從CA（一、企業根CA與獨立從CA）
- 4. CA證書
- 5. fabric-ca安裝
- 6. CA升級
- 7. windows2003+CA+S3300+802.1X
- 8. CA (證書)
- 9. ca 證書
- 10. CA總線（二）
- 更多相關文章...
- • PHP mysqli_ssl_set() 函數 - PHP參考手冊
- • CAP理論是什麼？ - NoSQL教程
- • 使用Rxjava計算圓周率
- • IntelliJ IDEA 代碼格式化配置和快捷鍵
相關標籤/搜索
每日一句
-
每一个你不满意的现在，都有一个你没有努力的曾经。
歡迎關注本站公眾號,獲取更多信息
相關文章
- 1. ca
- 2. fabric CA
- 3. Windows2016 主從CA（一、企業根CA與獨立從CA）
- 4. CA證書
- 5. fabric-ca安裝
- 6. CA升級
- 7. windows2003+CA+S3300+802.1X
- 8. CA (證書)
- 9. ca 證書
- 10. CA總線（二）