# 视图重建

## 两视图重建

$\left\{\begin{array}{ll} x=P*X\\ x^{'}=P^{'}*X \end{array}\right.$

$PX=[P^{1T}X,P^{2T}X,P^{3T}X]$

$\begin{bmatrix} i&&j&&k\\ x&&y&&1\\ P^{1T}X&&P^{2T}X&&P^{3T}X \end{bmatrix}$

$\left\{\begin{array}{ll} y(P^{3T}X)-(P^{2T}X)=0\\ x(P^{3T}X)-(P^{1T}X)=0\\ x(P^{2T}X)-y(P^{1T}X)=0\\ \end{array} \right.$

$A=\begin{bmatrix} xP^{3T}-P^{1T}\\ yP^{3T}-P^{2T}\\ x^{'}P^{'3T}-P^{'1T}\\ y^{'}P^{'3T}-P^{'2T} \end{bmatrix}$

$AX=0$

## 多点反投影（重建）

struct camera_params_t
{
double R[9];     /* Rotation */
double T[3];     /* Translation */
double t[3];     /*-RT*/
};

v3_t triangulate_Pre( vector<v2_t> &p, vector<camera_params_t>& params, double &error_out)

$C*p=C*\begin{bmatrix}p.x\\p.y\\1\end{bmatrix}=[R|t]*X=R*X+t$

$\left\{\begin{array}{ll} p.x=\frac{R^{1T}*X+t[0]}{R^{3T}*X+t[2]}\\ p.y=\frac{R^{2T}*X+t[1]}{R^{3T}*X+t[2]} \end{array} \right.$

v3_t triangulate_Pre( vector<v2_t> &p, vector<camera_params_t>& params, double &error_out)
{//T
int num_points = p.size();
int num_eqs = 2 * num_points;
int num_vars = 3;

double *A = (double *) malloc(sizeof(double) * num_eqs * num_vars);
double *b = (double *) malloc(sizeof(double) * num_eqs);
double *x = (double *) malloc(sizeof(double) * num_vars);

int i;
double error;
v3_t r;

for (i = 0; i < num_points; i++) {
int row = 6 * i;
int brow = 2 * i;

A[row + 0] = params[i].R[0] - Vx(p[i]) * params[i].R[6];
A[row + 1] = params[i].R[1] - Vx(p[i]) * params[i].R[7];
A[row + 2] = params[i].R[2] - Vx(p[i]) * params[i].R[8];

A[row + 3] = params[i].R[3] - Vy(p[i]) * params[i].R[6];
A[row + 4] = params[i].R[4] - Vy(p[i]) * params[i].R[7];
A[row + 5] = params[i].R[5] - Vy(p[i]) * params[i].R[8];

b[brow + 0] = params[i].T[2] * Vx(p[i]) - params[i].T[0];
b[brow + 1] = params[i].T[2] * Vy(p[i]) - params[i].T[1];
}

/* Find the least squares result */
dgelsy_driver(A, b, x, num_eqs, num_vars, 1);
error = 0.0;
for (i = 0; i < num_points; i++) {
double dx, dy;
double pp[3];

/* Compute projection error */
matrix_product331(params[i].R , x, pp);
pp[0] += params[i].T[0];
pp[1] += params[i].T[1];
pp[2] += params[i].T[2];

dx = pp[0] / pp[2] - Vx(p[i]);
dy = pp[1] / pp[2] - Vy(p[i]);

error += dx * dx + dy * dy;
}
error = sqrt(error / num_points);
printf("1:triangulate error is %f \n", error);
/* Run a non-linear optimization to refine the result */
global_num_points = num_points;
global_p = p;
global_params = params;
lmdif_driver(triangulate_n_residual, num_eqs, num_vars, x, 1.0e-5);

error = 0.0;
for (i = 0; i < num_points; i++) {
double dx, dy;
double pp[3];

/* Compute projection error */
matrix_product331(params[i].R , x, pp);
pp[0] += params[i].T[0];
pp[1] += params[i].T[1];
pp[2] += params[i].T[2];

dx = pp[0] / pp[2] - Vx(p[i]);
dy = pp[1] / pp[2] - Vy(p[i]);

error += dx * dx + dy * dy;
}
error = sqrt(error / num_points);
printf("2:triangulate error is %f \n", error);

if (error_out != NULL) {
error_out = error;
}
r = v3_new(x[0], x[1], x[2]);
free(A);
free(b);
free(x);
return r;
}

dgelsy_driver函数: Driver for the lapack function dgelss, which finds x to minimize norm(b - A * x)

lmdif_driver函数:Driver for the minpack function lmdif, which uses Levenberg-Marquardt for non-linear least squares minimization (可参见:https://www.math.utah.edu/software/minpack/minpack/lmdif.html)

void triangulate_n_residual(const int *m, const int *n,
double *x, double *fvec, double *iflag) {
int i;
for (i = 0; i < global_num_points; i++) {
/* Project the point into the view */
v2_t p = project(i, x);
fvec[2 * i + 0] = Vx(global_p[i]) - Vx(p);
fvec[2 * i + 1] = Vy(global_p[i]) - Vy(p);
}
}
//
v2_t project(int index, double *P) {
/* Rigid transform */
double tmp[3], tmp2[3];
matrix_product331(global_params[index].R, P, tmp);
matrix_sum(3, 1, 3, 1, tmp, global_params[index].T, tmp2);

/* Perspective division */
v2_t result;
Vx(result) = tmp2[0] / tmp2[2];
Vy(result) = tmp2[1] / tmp2[2];

return result;
}
posted @ 2017-02-05 17:56  侯凯  阅读(731)  评论(0编辑  收藏  举报