double Delaunay::interpolateAttr(const Point2f &pt, int tri_id){
MatrixXd A(3,3);
Vector3d yv_mapped(-1,-1,-1);
VectorXd coeff;
triangle tri=triangulation[tri_id];
//create matrix
for (size_t i=0;i<3;++i){
A(i,0)=tri.vtx[i].pt.x;
A(i,1)=tri.vtx[i].pt.y;
A(i,2)=tri.vtx[i].attr;
}
//solve for linear least squares fit
ColPivHouseholderQR<MatrixXd> qr_decomp(A);
auto rank_A=qr_decomp.rank();
MatrixXd B(A.rows(),yv_mapped.cols()+A.cols());
B<<A,yv_mapped;
qr_decomp.compute(B);
auto rank_B=qr_decomp.rank();
double result;
if(rank_A==rank_B && rank_A==A.cols()){
coeff=A.householderQr().solve(yv_mapped);
result=(-1.0-coeff[0]*pt.x-coeff[1]*pt.y)/coeff[2];
}
else if(A(0,2)==A(1,2) && A(1,2)==A(2,2))
result=A(0,2);
else
exitwithErrors("Error occured while predicting the disparity!");
return result;
}
void orthoit(double* a, int lda, int n, double* x, int k, double eps){
double *q_hat, *y, *lambda, *test;
double norm;
q_hat = (double*) malloc(n*k*sizeof(double));
y = (double*) malloc(n*k*sizeof(double));
lambda = (double*) malloc(k*k*sizeof(double));
test = (double*) malloc(n*k*sizeof(double)); /* Matrix der Abbruchbedingung */
/* QR=X und Q extrahieren */
qr_decomp(x, n, k, n);
get_q(x, n, n, k, q_hat, k);
/* Y=AQ rechnen */
m_mult(a, n, n, 0, q_hat, n, k, 0, y);
/* Lambda=Q*Y rechnen */
m_mult(q_hat, n, k, 1, y, n, k, 0, lambda);
while (1)
{
// Abbruchbedingung berechnen und checken
m_mult(q_hat, n, k, 0, lambda, k, k, 0, test);
m_sub(test, test, y, n, n, k);
norm = norm_frob(test, n, n, k);
if (norm < eps) break;
// QR=Y und Q extrahieren
qr_decomp(y, n, k, n);
get_q(y, n, n, k, q_hat, k);
m_mult(a, n, n, 0, q_hat, n, k, 0, y);
m_mult(q_hat, n, k, 1, y, n, k, 0, lambda);
}
memcpy(x, q_hat, n*k*sizeof(double));
free(q_hat);
free(y);
free(lambda);
free(test);
}
int newton(double *x, VectorFunction f, MatrixFunction df, double eps, int maxSteps, int n)
{
int Steps;
int i;
double *fx;
double *dfx;
double *d;
double *x_old;
d = malloc(n*sizeof(double));
fx = malloc(n*sizeof(double));
dfx = malloc(n*n*sizeof(double));
Steps = 0;
f(x, fx, n);
while (norm2(fx, n) > eps) {
f(x, fx, n);
df(x, dfx, n);
printf("fx\n");
print_matrix(fx, n, 1);
printf("Dfx\n");
print_matrix(dfx, n, n);
qr_decomp(dfx, n, n, n);
for (i=0; i<n; i++) {
fx[i] = -1 * fx[i];
}
solve_qr_decomp(dfx, n, n, n, fx, d);
for (i=0; i<n; i++) {
x[i] = x[i] + d[i];
}
Steps++;
if (Steps == maxSteps) {
break;
}
f(x, fx, n);
df(x, dfx, n);
}
return Steps;
}
int main(void){
int i, j, rows, cols;
double *a, *qr;
time_t t;
time(&t);
srand(t);
/* Matrixdimension festlegen (rows = Zeilen, cols = Spalten) */
rows = 3;
cols = 3;
/* Speicher anfordern */
a = (double*) malloc(rows*cols*sizeof(double));
qr = (double*) malloc(rows*cols*sizeof(double));
/* Matrix a erzeugen mit (double) Eintraegen zwischen 0 und 10 */
for(i=0;i<rows;i++)
for(j=0;j<cols;j++){
set_entry(a,rows,i,j,(rand() % 200)/20.0);
set_entry(qr,rows,i,j,get_entry(a,rows,i,j));
}
/* rows und cols ausgeben */
printf("\nrows = %d, cols = %d\n",rows,cols);
/* Minimum von rows und cols ausgeben */
printf("Minimum = %d\n",min_int(rows,cols));
/* Matrix a ausgeben */
printf("\na:\n");
print_matrix(qr,rows,cols);
/* qr-Zerlegung */
qr_decomp(qr,rows,cols,rows);
/* qr-Zerlegung ausgeben */
printf("\nqr:\n");
print_matrix(qr,rows,cols);
/* Speicher freigeben */
free(a);
free(qr);
return 0;
}
