Mat leastSquaresTransform(vector<R3> & src, vector<R3> & dst, vector<int> & indexes)
{
//M x src = dst
Mat ret;
Mat src_m = Mat::zeros(4, indexes.size(), CV_32FC1);
Mat dst_m = Mat::zeros(4, indexes.size(), CV_32FC1);
for(int i = 0; i < indexes.size(); i++)
{
src_m.at<float>(0, i) = src[i].x;
src_m.at<float>(1, i) = src[i].y;
src_m.at<float>(2, i) = src[i].z;
src_m.at<float>(3, i) = 1.0f;
int j = indexes[i];
dst_m.at<float>(0, i) = dst[j].x;
dst_m.at<float>(1, i) = dst[j].y;
dst_m.at<float>(2, i) = dst[j].z;
dst_m.at<float>(3, i) = 1.0f;
}
//src^T x M^T = dst^T
src_m = src_m.t();
dst_m = dst_m.t();
Mat Mt = least_squares(src_m, dst_m);
Mat M = Mt.t();
return M.clone();
}
QPair<Vector3D,Vector3D> calcBestAxisThroughPoints( const QVector<Vector3D>& points )
{
float a[3], b[3];
int n = points.count();
QVector<float> buf;
buf.resize(3*n);
for (int i = 0; i < n; i++) {
buf[i] = points[i].x;
buf[n + i] = points[i].y;
buf[2*n + i] = points[i].z;
}
least_squares(n, buf.data(), a, b);
least_squares(n, buf.data() + n, a + 1, b + 1);
least_squares(n, buf.data() + 2*n, a + 2, b + 2);
Vector3D pointA(b[0], b[1], b[2]);
Vector3D pointB(a[0]*(n-1) + b[0], a[1]*(n-1) + b[1], a[2]*(n-1) + b[2]);
return QPair<Vector3D,Vector3D>(pointA, pointB);
}
static int find_affine(int np, double *pts1, double *pts2, double *mat) {
const int np2 = np * 2;
double *a = (double *)aom_malloc(sizeof(*a) * (np2 * 7 + 42));
double *b = a + np2 * 6;
double *temp = b + np2;
int i;
double sx, sy, dx, dy;
double T1[9], T2[9];
normalize_homography(pts1, np, T1);
normalize_homography(pts2, np, T2);
for (i = 0; i < np; ++i) {
dx = *(pts2++);
dy = *(pts2++);
sx = *(pts1++);
sy = *(pts1++);
a[i * 2 * 6 + 0] = sx;
a[i * 2 * 6 + 1] = sy;
a[i * 2 * 6 + 2] = 0;
a[i * 2 * 6 + 3] = 0;
a[i * 2 * 6 + 4] = 1;
a[i * 2 * 6 + 5] = 0;
a[(i * 2 + 1) * 6 + 0] = 0;
a[(i * 2 + 1) * 6 + 1] = 0;
a[(i * 2 + 1) * 6 + 2] = sx;
a[(i * 2 + 1) * 6 + 3] = sy;
a[(i * 2 + 1) * 6 + 4] = 0;
a[(i * 2 + 1) * 6 + 5] = 1;
b[2 * i] = dx;
b[2 * i + 1] = dy;
}
if (!least_squares(6, a, np2, 6, b, temp, mat)) {
aom_free(a);
return 1;
}
denormalize_affine_reorder(mat, T1, T2);
aom_free(a);
return 0;
}
// Use bundle adjustment to compute global alignment from initial
// alignment.
int bundleAdjust(const vector<FeatureSet> &fs, MotionModel m, float f, int width, int height, const AlignMatrix &am, vector<CTransform3x3> &ms, int bundleAdjustIters) {
int n = ms.size();
int nEqs = 0;
double lastTotalError = 0;
double lambda = 1;
CVector3 p,q;
vector<CTransform3x3> msTemp(n);
// count the total number of inliers
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
nEqs += 3*am[i][j].inliers.size();
}
}
// compute the initial total error
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
int s = am[i][j].inliers.size();
for (int k=0; k<s; k++) {
int index1 = am[i][j].inliers[k];
int index2 = am[i][j].matches[index1].id - 1;
p[0] = fs[i][index1].x - (width/2.0);
p[1] = fs[i][index1].y - (height/2.0);
p[2] = f;
q[0] = fs[j][index2].x - (width/2.0);
q[1] = fs[j][index2].y - (height/2.0);
q[2] = f;
p = ms[i].Transpose() * p.Normalize();
q = ms[j].Transpose() * q.Normalize();
lastTotalError += pow(p[0]-q[0],2);
lastTotalError += pow(p[1]-q[1],2);
lastTotalError += pow(p[2]-q[2],2);
}
}
}
printf("initial error %f\n", f*sqrt(3*lastTotalError/nEqs));
// create the least squares matrices
matrix A(nEqs);
for (int i=0; i<nEqs; i++) {
A[i].resize(3*n);
for (int j=0; j<3*n; j++) {
A[i][j] = 0;
}
}
vector<double> b(nEqs);
for (int iter=0; iter<bundleAdjustIters; iter++) {
printf("performing iteration %d of bundle adjustment, ", iter);
int currentEq = 0;
double totalError = 0;
// clear the A matrix
for (int i=0; i<nEqs; i++)
for (int j=0; j<3*n; j++)
A[i][j] = 0;
// fill in the entries
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
int s = am[i][j].inliers.size();
for (int k=0; k<s; k++) {
int index1 = am[i][j].inliers[k];
int index2 = am[i][j].matches[index1].id - 1;
int var1 = 3*i;
int var2 = 3*j;
// BEGIN TODO
// fill in the three equations for this match
//
// that is, fill in the entries for the rows
// A[currentEq], A[currentEq+1], and A[currentEq+2]
//
// hint: only columns var1, var1+1, var1+2, and
// columns var2, var2+1, var2+2 will be nonzero
// END TODO
currentEq += 3;
}
}
}
// solve the system of equations
vector<double> x;
least_squares(A,b,x,lambda);
// update the rotation matrices
for (int i=0; i<n; i++) {
double wx = x[3*i+0];
double wy = x[3*i+1];
double wz = x[3*i+2];
// BEGIN TODO
// update the ith rotation matrix
// and store it in msTemp[i]
//
// you'll need to use the Rodriguez rule from the slides
// END TODO
}
// compute the new total error
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
int s = am[i][j].inliers.size();
for (int k=0; k<s; k++) {
int index1 = am[i][j].inliers[k];
int index2 = am[i][j].matches[index1].id - 1;
p[0] = fs[i][index1].x - (width/2.0);
p[1] = fs[i][index1].y - (height/2.0);
p[2] = f;
q[0] = fs[j][index2].x - (width/2.0);
q[1] = fs[j][index2].y - (height/2.0);
q[2] = f;
p = msTemp[i].Transpose() * p.Normalize();
q = msTemp[j].Transpose() * q.Normalize();
totalError += pow(p[0]-q[0],2);
totalError += pow(p[1]-q[1],2);
totalError += pow(p[2]-q[2],2);
}
}
}
if (totalError > lastTotalError) {
// error increased, don't use new transformations
lambda *= 10;
printf("error increased to %f, increasing lambda to %f\n", f*sqrt(3*totalError/nEqs), lambda);
}
else if (totalError < lastTotalError) {
// error decreased, use new transformations
lambda /= 10;
ms = msTemp;
lastTotalError = totalError;
printf("error decreased to %f, decreasing lambda to %f\n", f*sqrt(3*totalError/nEqs), lambda);
}
else {
// error converged, we can stop
printf("error converged at %f\n", f*sqrt(3*totalError/nEqs));
break;
}
}
return 0;
}
void calculate_cell_reconstruction_matrices(int n_variables, double *weight_exponent, int *maximum_order, struct FACE *face, int n_cells, struct CELL *cell, struct ZONE *zone)
{
int c, u, i, j, k, l;
int order, n_powers, n_stencil;
//find the overall maximum order
int maximum_maximum_order = 0;
for(u = 0; u < n_variables; u ++) if(maximum_order[u] > maximum_maximum_order) maximum_maximum_order = maximum_order[u];
//cell structure allocation
for(c = 0; c < n_cells; c ++) exit_if_false(cell_matrix_new(n_variables, &cell[c]),"allocating cell matrices");
//numerics values
double **matrix, *weight;
int n_constraints, *constraint;
exit_if_false(allocate_double_matrix(&matrix,ORDER_TO_POWERS(maximum_maximum_order),MAX_STENCIL),"allocating matrix");
exit_if_false(allocate_integer_vector(&constraint,MAX_STENCIL),"allocating constraints");
exit_if_false(allocate_double_vector(&weight,MAX_STENCIL),"allocating weights");
//stencil element properties
int s_id, s_index;
struct ZONE *s_zone;
char s_location, *s_condition;
double s_area, *s_centroid, s_weight;
//integration
double x[2];
int differential[2], d;
//CV polygon
int n_polygon;
double ***polygon;
exit_if_false(allocate_double_pointer_matrix(&polygon,MAX(MAX_CELL_FACES,4),2),"allocating polygon memory");
for(c = 0; c < n_cells; c ++)
{
for(u = 0; u < n_variables; u ++)
{
//problem size
order = cell[c].order[u];
n_powers = ORDER_TO_POWERS(order);
n_stencil = cell[c].n_stencil[u];
n_constraints = 0;
for(i = 0; i < n_stencil; i ++)
{
//stencil element properties
s_id = cell[c].stencil[u][i];
s_index = ID_TO_INDEX(s_id);
s_zone = &zone[ID_TO_ZONE(s_id)];
s_location = s_zone->location;
s_condition = s_zone->condition;
if(s_location == 'f') {
s_centroid = face[s_index].centroid;
s_area = face[s_index].area;
} else if(s_location == 'c') {
s_centroid = cell[s_index].centroid;
s_area = cell[s_index].area;
} else exit_if_false(0,"recognising zone location");
s_weight = (s_centroid[0] - cell[c].centroid[0])*(s_centroid[0] - cell[c].centroid[0]);
s_weight += (s_centroid[1] - cell[c].centroid[1])*(s_centroid[1] - cell[c].centroid[1]);
s_weight = 1.0/pow(s_weight,0.5*weight_exponent[u]);
if(s_location == 'c' && s_index == c) s_weight = 1.0;
weight[i] = s_weight;
//unknown and dirichlet conditions have zero differentiation
differential[0] = differential[1] = 0;
//other conditions have differential determined from numbers of x and y-s in the condition string
if(s_condition[0] != 'u' && s_condition[0] != 'd')
{
j = 0;
while(s_condition[j] != '\0')
{
differential[0] += (s_condition[j] == 'x');
differential[1] += (s_condition[j] == 'y');
j ++;
}
}
//index for the determined differential
d = differential_index[differential[0]][differential[1]];
//unknowns
if(s_condition[0] == 'u')
{
/*//fit unknowns to centroid points
x[0] = s_centroid[0] - cell[c].centroid[0];
x[1] = s_centroid[1] - cell[c].centroid[1];
for(j = 0; j < n_powers; j ++)
{
matrix[j][i] = polynomial_coefficient[d][j]*
integer_power(x[0],polynomial_power_x[d][j])*
integer_power(x[1],polynomial_power_y[d][j])*
s_weight;
}*/
//fit unknowns to CV average
n_polygon = generate_control_volume_polygon(polygon, s_index, s_location, face, cell);
for(j = 0; j < n_powers; j ++) matrix[j][i] = 0.0;
for(j = 0; j <= order; j ++)
{
for(k = 0; k < n_polygon; k ++)
{
x[0] = 0.5*polygon[k][0][0]*(1.0 - gauss_x[order][j]) +
0.5*polygon[k][1][0]*(1.0 + gauss_x[order][j]) -
cell[c].centroid[0];
x[1] = 0.5*polygon[k][0][1]*(1.0 - gauss_x[order][j]) +
0.5*polygon[k][1][1]*(1.0 + gauss_x[order][j]) -
cell[c].centroid[1];
for(l = 0; l < n_powers; l ++)
{
//[face integral of polynomial integrated wrt x] * [x normal] / [CV area]
matrix[l][i] += polynomial_coefficient[d][l] *
(1.0 / (polynomial_power_x[d][l] + 1.0)) *
integer_power(x[0],polynomial_power_x[d][l]+1) *
integer_power(x[1],polynomial_power_y[d][l]) *
s_weight * gauss_w[order][j] * 0.5 *
(polygon[k][1][1] - polygon[k][0][1]) / s_area;
}
}
}
}
//knowns
else
{
//known faces fit to face average
if(s_location == 'f')
{
for(j = 0; j < n_powers; j ++) matrix[j][i] = 0.0;
for(j = 0; j < order; j ++)
{
x[0] = 0.5*face[s_index].node[0]->x[0]*(1.0 - gauss_x[order-1][j]) +
0.5*face[s_index].node[1]->x[0]*(1.0 + gauss_x[order-1][j]) -
cell[c].centroid[0];
x[1] = 0.5*face[s_index].node[0]->x[1]*(1.0 - gauss_x[order-1][j]) +
0.5*face[s_index].node[1]->x[1]*(1.0 + gauss_x[order-1][j]) -
cell[c].centroid[1];
for(k = 0; k < n_powers; k ++)
{
matrix[k][i] += polynomial_coefficient[d][k] *
integer_power(x[0],polynomial_power_x[d][k]) *
integer_power(x[1],polynomial_power_y[d][k]) *
s_weight*gauss_w[order-1][j]*0.5;
}
}
}
//cells need implementing
//if(s_location == 'c')
//{
//}
}
//constraints are the centre cell and any dirichlet boundaries
if((s_location == 'c' && s_index == c) || s_condition[0] == 'd') constraint[n_constraints++] = i;
}
//solve
if(n_constraints > 0)
exit_if_false(constrained_least_squares(n_stencil,n_powers,matrix,n_constraints,constraint) == LS_SUCCESS, "doing CLS calculation");
else
exit_if_false(least_squares(n_stencil,n_powers,matrix) == LS_SUCCESS,"doing LS calculation");
//multiply by the weights
for(i = 0; i < n_powers; i ++) for(j = 0; j < n_stencil; j ++) matrix[i][j] *= weight[j];
//store in the cell structure
for(i = 0; i < n_powers; i ++) for(j = 0; j < n_stencil; j ++) cell[c].matrix[u][i][j] = matrix[i][j];
}
}
//clean up
free_matrix((void**)matrix);
free_vector(constraint);
free_vector(weight);
free_matrix((void**)polygon);
}
