test_loop_speed_inversetransform(void) { struct weston_matrix m; struct inverse_matrix inv; struct weston_vector v = { { 0.5, 0.5, 0.5, 1.0 } }; unsigned long count = 0; double t; printf("\nRunning 3 s test on inverse_transform()...\n"); weston_matrix_init(&m); matrix_invert(inv.LU, inv.perm, &m); running = 1; alarm(3); reset_timer(); while (running) { inverse_transform(inv.LU, inv.perm, v.f); count++; } t = read_timer(); printf("%lu iterations in %f seconds, avg. %.1f ns/iter.\n", count, t, 1e9 * t / count); }
bool matrix_unmap(const Transform *matrix, const double *pts, double *opts, int count) { // FIXME: do this more efficiently? Transform imx; if (!matrix_invert(&imx, matrix)) return FALSE; matrix_map(&imx, pts, opts, count); return TRUE; }
static int _ApplyLeastSquares(float X[], const float *pJ, const float R[], int m, int n) { float (*J)[m] = (float(*)[m])pJ; int i, j, k; // Compute N=(JtRt)t, mxn*nx1 get mx1 float N[m]; for(i = 0; i<m; i++) { N[i] = 0; for(j = 0; j<n; j++) N[i] += J[j][i]*R[j]; } // Compute S, S = JtJ, multiply mxn matrix with nxm and get mxm float S[m][m]; for(i = 0; i<m; i++) for(j = 0; j<m; j++) { S[i][j] = 0; for(k = 0; k<n; k++) S[i][j] += J[k][i]*J[k][j]; } // Invert S if(matrix_invert(S, m)) return 1; // Compute X+=(S^-1*Nt)t, multiply mxm matrix with mx1 and get mx1 for(i = 0; i<m; i++) for(j = 0; j<m; j++) X[i] += S[i][j]*N[j]; return 0; }
void PlaneData::Transform(const double *M) { double p[4] = { m_normal[0], m_normal[1], m_normal[2], m_dist }; double Minv[16]; matrix_invert(4, (double *)M, Minv); double pNew[4]; matrix_transpose_product(4, 4, 4, 1, Minv, p, pNew); double len = matrix_norm(3, 1, pNew); m_normal[0] = pNew[0] / len; m_normal[1] = pNew[1] / len; m_normal[2] = pNew[2] / len; m_dist = pNew[3] / len; #if 0 double origin[4] = { m_origin[0], m_origin[1], m_origin[2], 1.0 }; double Morigin[4]; matrix_product(4, 4, 4, 1, (double *) M, origin, Morigin); double dot0, dot1; matrix_product(1, 4, 4, 1, p, origin, &dot0); matrix_product(1, 4, 4, 1, pNew, Morigin, &dot1); printf("dot0 = %0.3f\n", dot0); printf("dot1 = %0.3f\n", dot1); #endif }
/* Take a matrix, compute inverse, multiply together * and subtract the identity matrix to get the error matrix. * Return the largest absolute value from the error matrix. */ static double test_inverse(struct weston_matrix *m) { unsigned i; struct inverse_matrix q; double errsup = 0.0; if (matrix_invert(q.LU, q.perm, m) != 0) return INFINITY; for (i = 0; i < 4; ++i) inverse_transform(q.LU, q.perm, &m->d[i * 4]); m->d[0] -= 1.0f; m->d[5] -= 1.0f; m->d[10] -= 1.0f; m->d[15] -= 1.0f; for (i = 0; i < 16; ++i) { double err = fabs(m->d[i]); if (err > errsup) errsup = err; } return errsup; }
double multi_regression(FILE *fp, int nrow, double *y, int ncol, double **xx, double *a0) { int row, niter, i, j; double ax, chi2, **a, **at, **ata, *atx; a = alloc_matrix(nrow, ncol); at = alloc_matrix(ncol, nrow); ata = alloc_matrix(ncol, ncol); for (i = 0; (i < nrow); i++) { for (j = 0; (j < ncol); j++) { at[j][i] = a[i][j] = xx[j][i]; } } matrix_multiply(fp, nrow, ncol, a, at, ata); if ((row = matrix_invert(fp, ncol, ata)) != 0) { gmx_fatal(FARGS, "Matrix inversion failed. Incorrect row = %d.\nThis probably indicates that you do not have sufficient data points, or that some parameters are linearly dependent.", row); } snew(atx, ncol); for (i = 0; (i < ncol); i++) { atx[i] = 0; for (j = 0; (j < nrow); j++) { atx[i] += at[i][j]*y[j]; } } for (i = 0; (i < ncol); i++) { a0[i] = 0; for (j = 0; (j < ncol); j++) { a0[i] += ata[i][j]*atx[j]; } } chi2 = 0; for (j = 0; (j < nrow); j++) { ax = 0; for (i = 0; (i < ncol); i++) { ax += a0[i]*a[j][i]; } chi2 += sqr(y[j]-ax); } sfree(atx); free_matrix(a); free_matrix(at); free_matrix(ata); return chi2; }
int matrix_invert__inline(double **initial, int rows, int cols) { double **result; int retval; new_matrix(&result, rows, rows); retval = matrix_invert(initial, rows, rows, result); copy_matrix(result, rows, rows, initial); free_matrix(&result, rows, rows); return retval; }
/** * A transformation from window coordinates to paint coordinates. */ VGboolean vg_get_paint_matrix(struct vg_context *ctx, const struct matrix *paint_to_user, const struct matrix *user_to_surface, struct matrix *mat) { struct matrix tmp; /* get user-to-paint matrix */ memcpy(mat, paint_to_user, sizeof(*paint_to_user)); if (!matrix_invert(mat)) return VG_FALSE; /* get surface-to-user matrix */ memcpy(&tmp, user_to_surface, sizeof(*user_to_surface)); if (!matrix_invert(&tmp)) return VG_FALSE; matrix_mult(mat, &tmp); return VG_TRUE; }
int main() { double A[16] = { -1, 0, 1, 8, 1, 0, 1, 4, -1, 0, 1, 2, 1, 1, 1, 1}; double R[16]; matrix_invert(4, A, (double *)R); print_matrix(4, 4, R); return 0; }
int main(void) { struct sigaction ding; struct weston_matrix M; struct inverse_matrix Q; int ret; double errsup; double det; ding.sa_handler = stopme; sigemptyset(&ding.sa_mask); ding.sa_flags = 0; sigaction(SIGALRM, &ding, NULL); srandom(13); M.d[0] = 3.0; M.d[4] = 17.0; M.d[8] = 10.0; M.d[12] = 0.0; M.d[1] = 2.0; M.d[5] = 4.0; M.d[9] = -2.0; M.d[13] = 0.0; M.d[2] = 6.0; M.d[6] = 18.0; M.d[10] = -12; M.d[14] = 0.0; M.d[3] = 0.0; M.d[7] = 0.0; M.d[11] = 0.0; M.d[15] = 1.0; ret = matrix_invert(Q.LU, Q.perm, &M); printf("ret = %d\n", ret); printf("det = %g\n\n", determinant(&M)); if (ret != 0) return 1; print_inverse_data_matrix(&Q); printf("P * A = L * U\n"); print_permutation_matrix(&Q); print_LU_decomposition(&Q); printf("a random matrix:\n"); randomize_matrix(&M); det = determinant(&M); print_matrix(&M); errsup = test_inverse(&M); printf("\nThe matrix multiplied by its inverse, error:\n"); print_matrix(&M); printf("max abs error: %g, original determinant %g\n", errsup, det); test_loop_precision(); test_loop_speed_matrixvector(); test_loop_speed_inversetransform(); test_loop_speed_invert(); test_loop_speed_invert_explicit(); return 0; }
v3_t BundlerApp::GeneratePointAtInfinity(const ImageKeyVector &views, int *added_order, camera_params_t *cameras, double &error, bool explicit_camera_centers) { camera_params_t *cam = NULL; int camera_idx = views[0].first; int image_idx = added_order[camera_idx]; int key_idx = views[0].second; Keypoint &key = GetKey(image_idx, key_idx); cam = cameras + camera_idx; double p3[3] = { key.m_x, key.m_y, 1.0 }; if (m_optimize_for_fisheye) { /* Undistort the point */ double x = p3[0], y = p3[1]; m_image_data[image_idx].UndistortPoint(x, y, p3[0], p3[1]); } double K[9], Kinv[9]; GetIntrinsics(cameras[camera_idx], K); matrix_invert(3, K, Kinv); double ray[3]; matrix_product(3, 3, 3, 1, Kinv, p3, ray); /* We now have a ray, put it at infinity */ double ray_world[3]; matrix_transpose_product(3, 3, 3, 1, cam->R, ray, ray_world); double pos[3] = { 0.0, 0.0, 0.0 }; double pt_inf[3] = { 0.0, 0.0, 0.0 }; if (!explicit_camera_centers) { } else { memcpy(pos, cam->t, 3 * sizeof(double)); double ray_extend[3]; matrix_scale(3, 1, ray, 100.0, ray_extend); matrix_sum(3, 1, 3, 1, pos, ray, pt_inf); } return v3_new(pt_inf[0], pt_inf[1], pt_inf[2]); }
/** * Analyze and update a matrix. * * \param mat matrix. * * If the matrix type is dirty then calls either analyse_from_scratch() or * analyse_from_flags() to determine its type, according to whether the flags * are dirty or not, respectively. If the matrix has an inverse and it's dirty * then calls matrix_invert(). Finally clears the dirty flags. */ void _math_matrix_analyse( GLmatrix *mat ) { if (mat->flags & MAT_DIRTY_TYPE) { if (mat->flags & MAT_DIRTY_FLAGS) analyse_from_scratch( mat ); else analyse_from_flags( mat ); } if (mat->inv && (mat->flags & MAT_DIRTY_INVERSE)) { matrix_invert( mat ); mat->flags &= ~MAT_DIRTY_INVERSE; } mat->flags &= ~(MAT_DIRTY_FLAGS | MAT_DIRTY_TYPE); }
static CMATRIX *_divo(CMATRIX *a, void *b, bool invert) { bool complex = COMPLEX(a); CMATRIX *m; gsl_complex c; if (!GB.Is(b, CLASS_Complex)) return NULL; c = ((CCOMPLEX *)b)->number; if (invert) { void *inv = matrix_invert(MAT(a), complex); if (!inv) { GB.Error(GB_ERR_ZERO); return NULL; } m = MATRIX_create_from(inv, complex); } else { if (GSL_REAL(c) == 0 && GSL_IMAG(c) == 0) { GB.Error(GB_ERR_ZERO); return NULL; } c = gsl_complex_inverse(c); m = MATRIX_make(a); } MATRIX_ensure_complex(m); gsl_matrix_complex_scale(CMAT(m), c); return m; }
static CMATRIX *_powf(CMATRIX *a, double f, bool invert) { if (invert || f != (double)(int)f) return NULL; CMATRIX *m; int n = (int)f; if (n == 0) { m = MATRIX_make(a); if (COMPLEX(m)) gsl_matrix_complex_set_identity(CMAT(m)); else gsl_matrix_set_identity(MAT(m)); } else if (n == 1) { m = a; } else if (n > 1) { m = _powi(MATRIX_copy(a), n); } else if (n < 0) { void *inv = matrix_invert(a->matrix, COMPLEX(a)); if (inv == NULL) { GB.Error(GB_ERR_ZERO); return NULL; } m = _powi(MATRIX_create_from(inv, COMPLEX(a)), (-n)); } return m; }
static void get_brick_vertices( Brick *br, VRState *state, Coord cpt[8], VRVolumeData *vd, Matrix RTTMat ) { int i; Matrix BTRMat; /* Locate brick cube at the proper location and rotate it */ matrix_copy(IdentityMatrix, 4, BTRMat); matrix_translate(1.0, 1.0, 1.0, BTRMat); matrix_scale(0.5, 0.5, 0.5, BTRMat); matrix_translate(br->xOff, br->yOff, br->zOff, BTRMat); matrix_scale(1.0 / (float) vd->nxBricks, 1.0 / (float) vd->nyBricks, 1.0 / (float) vd->nzBricks, BTRMat); matrix_scale(2.0, 2.0, 2.0, BTRMat); matrix_translate(-1.0, -1.0, -1.0, BTRMat); matrix_mult_safe(BTRMat, vd->VTRMat, BTRMat); /* Transform unit cube */ for (i = 0; i < 8; i++) { coord_transform(PlusMinusCube[i], BTRMat, cpt[i]); } /* Store invers transformation so texture coords are in range [0.0 1.0] */ matrix_invert(BTRMat, RTTMat); matrix_translate(1.0, 1.0, 1.0, RTTMat); matrix_scale(0.5, 0.5, 0.5, RTTMat); matrix_translate(br->txOff, br->tyOff, br->tzOff, RTTMat); matrix_scale(br->txScl, br->tyScl, br->tzScl, RTTMat); }
static CMATRIX *_divf(CMATRIX *a, double f, bool invert) { bool complex = COMPLEX(a); CMATRIX *m; if (invert) { void *inv = matrix_invert(MAT(a), complex); if (!inv) { GB.Error(GB_ERR_ZERO); return NULL; } m = MATRIX_create_from(inv, complex); } else { if (f == 0.0) { GB.Error(GB_ERR_ZERO); return NULL; } f = 1 / f; m = MATRIX_make(a); } if (complex) gsl_matrix_complex_scale(CMAT(m), gsl_complex_rect(f, 0)); else gsl_matrix_scale(MAT(m), f); return m; }
double EKF::_step(const std::vector<vision::FeaturePtr> &y) { double mean_innovation; if (y.size() != _num_feats_init_structure) { ROS_ERROR( "[EKF::_step] The size of the measurement y differs from the expected number of tracked and matched Features.\nSize of y: %d Expected size (number of matched Features): %d", y.size(), _num_feats_init_structure); throw std::string( "FATAL ERROR [EKF::_step]: The size of the measurement y differs from the expected number of tracked and matched Features."); } // Prediction step _getA(_updated_state, _A_ekf); _predictState(_updated_state, _predicted_state); // predCovar=A*upCovar*A'+WQW; matrix_product(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, _A_ekf, _updated_covar, _A_ekf_covar); matrix_transpose_product2(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, _A_ekf_covar, _A_ekf, _predicted_covar); matrix_sum(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, _predicted_covar, _W_Q_W_ekf_trans, _predicted_covar); //update step _computeH(_predicted_state, _H_ekf); // K=predCovar*H'/(H*predCovar*H'+R); // H' matrix_transpose(_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states, _H_ekf, _H_ekf_trans); // predCovar*H' matrix_product(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _predicted_covar, _H_ekf_trans, _covar_H_ekf); // H*predCovar matrix_product(_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, _H_ekf, _predicted_covar, _H_ekf_covar); // H*predCovar*H' matrix_product(_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _H_ekf_covar, _H_ekf_trans, _H_covar_H_ekf); // (H*predCovar*H'+R) matrix_sum(_num_feats_init_structure * 2, _num_feats_init_structure * 2, _num_feats_init_structure * 2, _num_feats_init_structure * 2, _H_covar_H_ekf, _R_ekf, _H_covar_H_ekf); // inv(H*predCovar*H'+R) matrix_invert(_num_feats_init_structure * 2, _H_covar_H_ekf, _H_Q_H_ekf); //K=predCovar*H'*inv(H*predCovar*H'+R); matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _num_feats_init_structure * 2, _num_feats_init_structure * 2, _covar_H_ekf, _H_Q_H_ekf, _K_ekf); // predict _predictObservation(_predicted_state, _estimated_z); // measurement for (int i = 0; i < _num_feats_init_structure; i++) { _z[i * 2 + 0] = y[i]->getX(); _z[i * 2 + 1] = y[i]->getY(); } //innovation = z-z_estimate; matrix_diff(1, _num_feats_init_structure * 2, 1, _num_feats_init_structure * 2, _z, _estimated_z, _innovation); // upState=predState+K*(innovation); matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _num_feats_init_structure * 2, 1, _K_ekf, _innovation, _updated_state); matrix_sum(1, 3 * _num_feats_init_structure + _num_motion_states, 1, 3 * _num_feats_init_structure + _num_motion_states, _updated_state, _predicted_state, _updated_state); // upCovar=(eye(3*numFeatures+6)-K*H)*predCovar; matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states, _K_ekf, _H_ekf, _K_H_ekf); //I_KH = (I - KH); matrix_diff(_num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states, _I_ekf, _K_H_ekf, _I_K_H_ekf); matrix_product(_num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states, _I_K_H_ekf, _predicted_covar, _updated_covar); // calculate error after correction _predictObservation(_updated_state, _estimated_z); //innovation = z-z_estimate; matrix_diff(1, _num_feats_init_structure * 2, 1, _num_feats_init_structure * 2, _z, _estimated_z, _innovation); double dist = 0; for (int i = 0; i < _num_feats_init_structure; i++) { dist += sqrt(pow(_innovation[i * 2 + 0], 2) + pow(_innovation[i * 2 + 1], 2)); } mean_innovation = dist / ((double)_num_feats_init_structure); return mean_innovation; }
void MPI_matrix_multiply(Matrix *result, Matrix *a, Matrix *b, int n, int root, MPI_Comm comm) { int rank, total_procs; MPI_Comm_rank(comm, &rank); MPI_Comm_size(comm, &total_procs); int rows_per_proc = n / total_procs; int total_procs_used = total_procs; MPI_Comm gather_comm = comm; int gather_root = root; if (rows_per_proc == 0) { if (rank == root) printf("Warning: multilpying two %dx%d matricies takes %d or fewer" " processes. %d were given.\n", n, n, n, total_procs); int color = rank + root - 1; int key = color % total_procs + 1; color /= total_procs; MPI_Comm_split(comm, color, key, &gather_comm); if (rank == root) MPI_Comm_rank(gather_comm, &gather_root); MPI_Bcast(&gather_root, 1, MPI_INT, root, comm); if (color > 0) return; rows_per_proc = 1; total_procs_used = n; } if (n > total_procs && n % total_procs != 0) { if (rank == root) printf("Number of rows is not evenly divisible by number of processes.\n"); return; } int size = rows_per_proc * n; // ################################################## // Step 0: Distribute A and invert b // ################################################## Matrix A = matrix_malloc(rows_per_proc, n); MPI_Scatter(rank == root ? a->data : NULL, size, MPI_FLOAT, A.data, size, MPI_FLOAT, root, comm); if (rank == root && b->is_inverted != 1) matrix_invert(b); int i; int sender = (rank - 1) % total_procs; if (sender < 0) sender += total_procs; int reciever = (rank + 1) % total_procs; int last_proc_rank = (root + total_procs_used - 1) % total_procs; Matrix B = matrix_malloc(n, rows_per_proc); B.is_inverted = 1; Matrix C = matrix_malloc(rows_per_proc, n); C.is_inverted = 1; float *c_data = C.data; MPI_Status status; double start_time = MPI_Wtime(); for (i = 0; i < total_procs_used; i++) { // ################################################## // Step 1: Recieve Data // ################################################## if (rank == root) matrix_get_submatrix(&B, *b, 0, i * rows_per_proc); else MPI_Recv(B.data, rows_per_proc * n, MPI_FLOAT, sender, MPI_ANY_TAG, comm, &status); // ################################################## // Step 2: Calculate Dot Product // ################################################## matrix_multiply(&C, A, B); C.data += rows_per_proc; // ################################################## // Step 3: Send Data // ################################################## if (rank != last_proc_rank) MPI_Send(B.data, rows_per_proc * n, MPI_FLOAT, reciever, 0, comm); } C.data = c_data; // ################################################## // Step 4: Get time and Gather on root // ################################################## double end_time = MPI_Wtime(); if (rank == last_proc_rank) MPI_Send(&end_time, 1, MPI_DOUBLE, 0, 0, comm); if (rank == root) { MPI_Recv(&end_time, 1, MPI_DOUBLE, last_proc_rank, MPI_ANY_TAG, comm, &status); printf("Multiplication took %f seconds.\n", end_time - start_time); } int send_count = C.width * C.height; MPI_Gather(C.data, send_count, MPI_FLOAT, rank == root ? result->data : NULL, send_count, MPI_FLOAT, gather_root, gather_comm); free(C.data); free(B.data); free(A.data); }
trans2D_t *transform_invert(trans2D_t *T) { trans2D_t *Tinv = new_zero_transform(); matrix_invert(3, (double *)T->T, (double *)Tinv->T); return Tinv; }
void clsLinearModelEM::CalculateSlopeInterceptEstimates() { MATRIX *wx = matrix_mult(mobj_Weights, mobj_X) ; if (wx == NULL) { ///PrintMatrix(wx, "c:\\test1.csv") ; } MATRIX *xprime_wx = matrix_mult(mobj_XTranspose, wx) ; if (xprime_wx == NULL) { //PrintMatrix(xprime_wx, "c:\\test1.csv") ; } MATRIX *inv_xprime_wx = matrix_invert(xprime_wx) ; if (inv_xprime_wx == NULL) { //PrintMatrix(inv_xprime_wx, "c:\\test1.csv") ; } MATRIX *wy = matrix_mult(mobj_Weights, mobj_Y) ; if (wy == NULL) { //PrintMatrix(wy, "c:\\test1.csv") ; } MATRIX *xprime_wy = matrix_mult(mobj_XTranspose, wy) ; if (xprime_wy == NULL) { //PrintMatrix(xprime_wy, "c:\\test1.csv") ; } MATRIX *beta = matrix_mult(inv_xprime_wx, xprime_wy) ; if (beta == NULL) { //PrintMatrix(beta, "c:\\test1.csv") ; } double **betaPtr = (double **) beta->ptr ; mdbl_slope = betaPtr[0][0] ; mdbl_intercept = betaPtr[1][0] ; int numPoints = mobj_X->rows ; double maxDiff = -1*DBL_MAX ; double minDiff = DBL_MAX ; double **ptrMatrixX = (double **) mobj_X->ptr ; double **ptrMatrixY = (double **) mobj_Y->ptr ; double maxY = -1 * DBL_MAX ; for (int index = 0 ; index < numPoints ; index++) { double diff = ptrMatrixY[index][0] - (mdbl_slope * ptrMatrixX[index][0] + mdbl_intercept) ; if (diff > maxDiff) maxDiff = diff ; if (diff < minDiff) minDiff = diff ; if (ptrMatrixY[index][0] > maxY) maxY = ptrMatrixY[index][0] ; } mdbl_unif_u = 1.0 / (maxDiff - minDiff) ; mdbl_unif_u = 1.0 / maxY ; matrix_free(wx) ; matrix_free(xprime_wx) ; matrix_free(inv_xprime_wx) ; matrix_free(wy) ; matrix_free(xprime_wy) ; matrix_free(beta) ; }
/* Triangulate a subtrack */ v3_t BundlerApp::TriangulateNViews(const ImageKeyVector &views, int *added_order, camera_params_t *cameras, double &error, bool explicit_camera_centers) { int num_views = (int) views.size(); v2_t *pv = new v2_t[num_views]; double *Rs = new double[9 * num_views]; double *ts = new double[3 * num_views]; for (int i = 0; i < num_views; i++) { camera_params_t *cam = NULL; int camera_idx = views[i].first; int image_idx = added_order[camera_idx]; int key_idx = views[i].second; Keypoint &key = GetKey(image_idx, key_idx); double p3[3] = { key.m_x, key.m_y, 1.0 }; if (m_optimize_for_fisheye) { /* Undistort the point */ double x = p3[0], y = p3[1]; m_image_data[image_idx].UndistortPoint(x, y, p3[0], p3[1]); } double K[9], Kinv[9]; GetIntrinsics(cameras[camera_idx], K); matrix_invert(3, K, Kinv); double p_n[3]; matrix_product(3, 3, 3, 1, Kinv, p3, p_n); // EDIT!!! pv[i] = v2_new(-p_n[0], -p_n[1]); pv[i] = UndistortNormalizedPoint(pv[i], cameras[camera_idx]); cam = cameras + camera_idx; memcpy(Rs + 9 * i, cam->R, 9 * sizeof(double)); if (!explicit_camera_centers) { memcpy(ts + 3 * i, cam->t, 3 * sizeof(double)); } else { matrix_product(3, 3, 3, 1, cam->R, cam->t, ts + 3 * i); matrix_scale(3, 1, ts + 3 * i, -1.0, ts + 3 * i); } } v3_t pt = triangulate_n(num_views, pv, Rs, ts, &error); error = 0.0; for (int i = 0; i < num_views; i++) { int camera_idx = views[i].first; int image_idx = added_order[camera_idx]; int key_idx = views[i].second; Keypoint &key = GetKey(image_idx, key_idx); v2_t pr = sfm_project_final(cameras + camera_idx, pt, explicit_camera_centers ? 1 : 0, m_estimate_distortion ? 1 : 0); if (m_optimize_for_fisheye) { double x = Vx(pr), y = Vy(pr); m_image_data[image_idx].DistortPoint(x, y, Vx(pr), Vy(pr)); } double dx = Vx(pr) - key.m_x; double dy = Vy(pr) - key.m_y; error += dx * dx + dy * dy; } error = sqrt(error / num_views); delete [] pv; delete [] Rs; delete [] ts; return pt; }
/* Triangulate two points */ v3_t Triangulate(v2_t p, v2_t q, camera_params_t c1, camera_params_t c2, double &proj_error, bool &in_front, double &angle, bool explicit_camera_centers) { double K1[9], K2[9]; double K1inv[9], K2inv[9]; GetIntrinsics(c1, K1); GetIntrinsics(c2, K2); matrix_invert(3, K1, K1inv); matrix_invert(3, K2, K2inv); /* Set up the 3D point */ // EDIT!!! double proj1[3] = { Vx(p), Vy(p), -1.0 }; double proj2[3] = { Vx(q), Vy(q), -1.0 }; double proj1_norm[3], proj2_norm[3]; matrix_product(3, 3, 3, 1, K1inv, proj1, proj1_norm); matrix_product(3, 3, 3, 1, K2inv, proj2, proj2_norm); v2_t p_norm = v2_new(proj1_norm[0] / proj1_norm[2], proj1_norm[1] / proj1_norm[2]); v2_t q_norm = v2_new(proj2_norm[0] / proj2_norm[2], proj2_norm[1] / proj2_norm[2]); /* Undo radial distortion */ p_norm = UndistortNormalizedPoint(p_norm, c1); q_norm = UndistortNormalizedPoint(q_norm, c2); /* Compute the angle between the rays */ angle = ComputeRayAngle(p, q, c1, c2); /* Triangulate the point */ v3_t pt; if (!explicit_camera_centers) { pt = triangulate(p_norm, q_norm, c1.R, c1.t, c2.R, c2.t, &proj_error); } else { double t1[3]; double t2[3]; /* Put the translation in standard form */ matrix_product(3, 3, 3, 1, c1.R, c1.t, t1); matrix_scale(3, 1, t1, -1.0, t1); matrix_product(3, 3, 3, 1, c2.R, c2.t, t2); matrix_scale(3, 1, t2, -1.0, t2); pt = triangulate(p_norm, q_norm, c1.R, t1, c2.R, t2, &proj_error); } proj_error = (c1.f + c2.f) * 0.5 * sqrt(proj_error * 0.5); /* Check cheirality */ bool cc1 = CheckCheirality(pt, c1); bool cc2 = CheckCheirality(pt, c2); in_front = (cc1 && cc2); return pt; }
int matrix_invert__alloc(double **initial, int rows, int cols, double ***result) { new_matrix(result, rows, rows); return matrix_invert(initial, rows, cols, *result); }
/* Compute rigid transforms between all matching images */ void BundlerApp::ComputeTransforms(bool removeBadMatches, int new_image_start) { unsigned int num_images = GetNumImages(); m_transforms.clear(); for (unsigned int i = 0; i < num_images; i++) { MatchAdjList::iterator iter; for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) { unsigned int j = iter->m_index; assert(ImagesMatch(i, j)); MatchIndex idx = GetMatchIndex(i, j); MatchIndex idx_rev = GetMatchIndex(j, i); m_transforms[idx] = TransformInfo(); m_transforms[idx_rev] = TransformInfo(); bool connect12 = ComputeTransform(i, j, removeBadMatches); if (!connect12) { if (removeBadMatches) { // RemoveMatch(i, j); // RemoveMatch(j, i); // m_match_lists[idx].clear(); m_matches.RemoveMatch(idx); m_matches.RemoveMatch(idx_rev); // m_match_lists.erase(idx); m_transforms.erase(idx); m_transforms.erase(idx_rev); } } else { matrix_invert(3, m_transforms[idx].m_H, m_transforms[idx_rev].m_H); } } } /* Print the inlier ratios */ FILE *f = fopen("pairwise_scores.txt", "w"); for (unsigned int i = 0; i < num_images; i++) { MatchAdjList::iterator iter; for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) { unsigned int j = iter->m_index; // first; assert(ImagesMatch(i, j)); // MatchIndex idx = *iter; MatchIndex idx = GetMatchIndex(i, j); fprintf(f, "%d %d %0.5f\n", i, j, m_transforms[idx].m_inlier_ratio); } } fclose(f); }
/* Computes the homography that, when applied to the points in l_pts, * minimizes the least-squares error between the result and the * corresponding points in r_pts. * * n -- number of points * r_pts -- matches * l_pts -- initial points * Tout -- on return, contains the 3x3 transformation matrix */ void align_homography(int num_pts, v3_t *r_pts, v3_t *l_pts, double *Tout, int refine) { int m = num_pts * 2; int n = 8; int nrhs = 1; int i, base; double *A = malloc(sizeof(double) * m * n); /* Left-hand matrix */ double *B = malloc(sizeof(double) * m * nrhs); /* Right-hand matrix */ double Ttmp[9]; double T1[9], T2[9]; #define _CONDITION_ #ifdef _CONDITION_ /* Normalize the points */ v3_t *r_pts_norm = condition_points(num_pts, r_pts, T1); v3_t *l_pts_norm = condition_points(num_pts, l_pts, T2); double T1inv[9]; #else v3_t *r_pts_norm = r_pts; v3_t *l_pts_norm = l_pts; #endif for (i = 0; i < num_pts; i++) { base = 2 * i * n; A[base + 0] = Vx(l_pts_norm[i]); A[base + 1] = Vy(l_pts_norm[i]); A[base + 2] = 1.0; A[base + 3] = A[base + 4] = A[base + 5] = 0.0; A[base + 6] = -Vx(l_pts_norm[i]) * Vx(r_pts_norm[i]); A[base + 7] = -Vy(l_pts_norm[i]) * Vx(r_pts_norm[i]); base = (2 * i + 1) * n; A[base + 0] = A[base + 1] = A[base + 2] = 0.0; A[base + 3] = Vx(l_pts_norm[i]); A[base + 4] = Vy(l_pts_norm[i]); A[base + 5] = 1.0; A[base + 6] = -Vx(l_pts_norm[i]) * Vy(r_pts_norm[i]); A[base + 7] = -Vy(l_pts_norm[i]) * Vy(r_pts_norm[i]); B[2 * i + 0] = Vx(r_pts_norm[i]); B[2 * i + 1] = Vy(r_pts_norm[i]); } /* Make the call to dgelsy */ dgelsy_driver(A, B, Tout, m, n, nrhs); Tout[8] = 1.0; #ifdef _CONDITION_ /* Undo normalization */ matrix_invert(3, T1, T1inv); matrix_product(3, 3, 3, 3, T1inv, Tout, Ttmp); matrix_product(3, 3, 3, 3, Ttmp, T2, Tout); matrix_scale(3, 3, Tout, 1.0 / Tout[8], Tout); #endif if (refine) { memcpy(Ttmp, Tout, sizeof(double) * 9); align_homography_non_linear(num_pts, r_pts, l_pts, Ttmp, Tout); } free(A); free(B); #ifdef _CONDITION_ free(r_pts_norm); free(l_pts_norm); #endif }
/* Write point files to a ply file */ void BaseApp::DumpPointsToPly(const char *output_directory, const char *filename, int num_points, int num_cameras, v3_t *points, v3_t *colors, camera_params_t *cameras /*bool reflect*/) { int num_good_pts = 0; for (int i = 0; i < num_points; i++) { if (Vx(colors[i]) == 0x0 && Vy(colors[i]) == 0x0 && Vz(colors[i]) == 0xff) continue; num_good_pts++; } char ply_out[256]; sprintf(ply_out, "%s/%s", output_directory, filename); FILE *f = fopen(ply_out, "w"); if (f == NULL) { printf("Error opening file %s for writing\n", ply_out); return; } /* Print the ply header */ fprintf(f, ply_header, num_good_pts + 2 * num_cameras); /* Now triangulate all the correspondences */ for (int i = 0; i < num_points; i++) { if (Vx(colors[i]) == 0x0 && Vy(colors[i]) == 0x0 && Vz(colors[i]) == 0xff) continue; /* Output the vertex */ fprintf(f, "%0.6e %0.6e %0.6e %d %d %d\n", Vx(points[i]), Vy(points[i]), Vz(points[i]), // Vx(points[idx]), Vy(points[idx]), Vz(points[idx]), // (reflect ? -1 : 1) * Vz(points[i]), iround(Vx(colors[i])), iround(Vy(colors[i])), iround(Vz(colors[i]))); } for (int i = 0; i < num_cameras; i++) { double c[3]; double Rinv[9]; matrix_invert(3, cameras[i].R, Rinv); memcpy(c, cameras[i].t, 3 * sizeof(double)); if ((i % 2) == 0) fprintf(f, "%0.6e %0.6e %0.6e 0 255 0\n", c[0], c[1], c[2]); // (reflect ? -1 : 1) * c[2]); else fprintf(f, "%0.6e %0.6e %0.6e 255 0 0\n", c[0], c[1], c[2]); // (reflect ? -1 : 1) * c[2]); double p_cam[3] = { 0.0, 0.0, -0.05 }; double p[3]; // if (!reflect) // p_cam[2] *= -1.0; matrix_product(3, 3, 3, 1, Rinv, p_cam, p); p[0] += c[0]; p[1] += c[1]; p[2] += c[2]; fprintf(f, "%0.6e %0.6e %0.6e 255 255 0\n", p[0], p[1], p[2]); // (reflect ? -1 : 1) * p[2]); } fclose(f); }
trans3D_t *invert_transform3D(trans3D_t *T) { trans3D_t *Tinv = new_zero_transform3D(); matrix_invert(4, (double *)T->T, (double *)Tinv->T); return Tinv; }
/* Write point files to a ply file */ void BaseApp::DumpPointsToPly(char *output_directory, char *filename, int num_points, int num_cameras, v3_t *points, v3_t *colors, camera_params_t *cameras /*bool reflect*/, int *added_order, std::vector<ImageKeyVector> &pt_views) { int num_good_pts = 0; for (int i = 0; i < num_points; i++) { if (Vx(colors[i]) == 0x0 && Vy(colors[i]) == 0x0 && Vz(colors[i]) == 0xff) continue; num_good_pts++; } char ply_out[256]; sprintf(ply_out, "%s/%s", output_directory, filename); FILE *f = fopen(ply_out, "w"); char xml_out[256]; sprintf(xml_out, "%s/output.xml", output_directory, filename); FILE *fxml = fopen(xml_out, "w"); if (f == NULL) { printf("Error opening file %s for writing\n", ply_out); return; } // JSON char json_out[256]; sprintf(json_out, "%s/output.json", output_directory, filename); FILE *fjson = fopen(json_out, "w"); jsontool::Object json; std::vector<jsontool::Object> allPoints; std::vector<jsontool::Object> allCameras; /* Print the ply header */ fprintf(f, ply_header, num_good_pts + 2 * num_cameras); fprintf(fxml, "<?xml version=\"1.0\" encoding=\"UTF-8\" ?>\n"); fprintf(fxml, "<bundle>\n"); fprintf(fxml, "<points>\n"); /* Now triangulate all the correspondences */ for (int i = 0; i < num_points; i++) { if (Vx(colors[i]) == 0x0 && Vy(colors[i]) == 0x0 && Vz(colors[i]) == 0xff) continue; /* Output the vertex */ fprintf(f, "%0.6e %0.6e %0.6e %d %d %d\n", Vx(points[i]), Vy(points[i]), Vz(points[i]), // Vx(points[idx]), Vy(points[idx]), Vz(points[idx]), // (reflect ? -1 : 1) * Vz(points[i]), iround(Vx(colors[i])), iround(Vy(colors[i])), iround(Vz(colors[i]))); fprintf(fxml, "<point>\n"); fprintf(fxml, "\t<position> <x>%0.6e</x> <y>%0.6e</y> <z>%0.6e</z> </position>\n\t<color> <r>%d</r> <g>%d</g> <b>%d</b> </color>\n", Vx(points[i]), Vy(points[i]), Vz(points[i]), iround(Vx(colors[i])), iround(Vy(colors[i])), iround(Vz(colors[i]))); jsontool::Object curPoint; std::vector<jsontool::Object> visArray; /* Pos on image */ int num_visible = (int) pt_views[i].size(); fprintf(fxml, "\t<visible>\n"); for (int j = 0; j < num_visible; j++) { int img = added_order[pt_views[i][j].first]; int key = pt_views[i][j].second; double x = m_image_data[img].m_keys[key].m_x; double y = m_image_data[img].m_keys[key].m_y; //fprintf(fxml, " %d %d %0.4f %0.4f", img, key, x, y); fprintf(fxml, "\t\t<imgID> %d </imgID>\n", img); fprintf(fxml, "\t\t<key> %d </key>\n", key); fprintf(fxml, "\t\t<coord> <x> %0.4f </x> <y> %0.4f </y> </coord>\n", x, y); // JSON jsontool::Object curVis; curVis["img"] = img; curVis["key"] = key; curVis["x"] = x; curVis["y"] = y; visArray.push_back( curVis ); } fprintf(fxml, "\t</visible>\n"); fprintf(fxml, "</point>\n"); // JSON curPoint["x"] = Vx(points[i]); curPoint["y"] = Vy(points[i]); curPoint["z"] = Vz(points[i]); curPoint["r"] = iround(Vx(colors[i])); curPoint["g"] = iround(Vy(colors[i])); curPoint["b"] = iround(Vz(colors[i])); curPoint["visible"] = visArray; allPoints.push_back( curPoint ); } fprintf(fxml, "</points>\n"); fprintf(fxml, "<cameras>\n"); for (int i = 0; i < num_cameras; i++) { double c[3]; double Rinv[9]; matrix_invert(3, cameras[i].R, Rinv); memcpy(c, cameras[i].t, 3 * sizeof(double)); fprintf(fxml, "<camera>\n\t<p1> <x>%0.6e</x> <y>%0.6e</y> <z>%0.6e</z> </p1>\n\t", c[0], c[1], c[2]); if ((i % 2) == 0) fprintf(f, "%0.6e %0.6e %0.6e 0 255 0\n", c[0], c[1], c[2]); // (reflect ? -1 : 1) * c[2]); else fprintf(f, "%0.6e %0.6e %0.6e 255 0 0\n", c[0], c[1], c[2]); // (reflect ? -1 : 1) * c[2]); double p_cam[3] = { 0.0, 0.0, -0.05 }; double p[3]; // if (!reflect) // p_cam[2] *= -1.0; matrix_product(3, 3, 3, 1, Rinv, p_cam, p); p[0] += c[0]; p[1] += c[1]; p[2] += c[2]; fprintf(f, "%0.6e %0.6e %0.6e 255 255 0\n", p[0], p[1], p[2]); // (reflect ? -1 : 1) * p[2]); fprintf(fxml, "<p2> <x>%0.6e</x> <y>%0.6e</y> <z>%0.6e</z> </p2>\n", p[0], p[1], p[2]); fprintf(fxml, "\t<filename>%s</filename>\n", m_image_data[i].m_name); fprintf(fxml, "</camera>\n"); //JSON jsontool::Object curCamera; jsontool::Object p1, p2; p1["x"] = c[0]; p1["y"] = c[1]; p1["z"] = c[2]; p2["x"] = p[0]; p2["y"] = p[1]; p2["z"] = p[2]; curCamera["c"] = p1; curCamera["p"] = p2; curCamera["filename"] = m_image_data[i].m_name; allCameras.push_back( curCamera ); } fprintf(fxml, "</cameras>\n"); fprintf(fxml, "</bundle>\n"); fclose(fxml); // JSON json["points"] = allPoints; json["cameras"] = allCameras; fprintf(fjson, "%s", stringify(json).c_str()); fclose(fjson); fclose(f); }