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);
}
