void axis_angle_to_matrix4(double *axis, double angle, double *R) {
double ident[9];
double n[9] = { 0.0, -axis[2], axis[1],
axis[2], 0.0, -axis[0],
-axis[1], axis[0], 0.0 };
double nsq[9], sn[9], cnsq[9], tmp[9];
double R3[9];
double c, s;
c = cos(angle);
s = sin(angle);
matrix_ident(3, ident);
matrix_product33(n, n, nsq);
matrix_scale(3, 3, n, s, sn);
matrix_scale(3, 3, nsq, (1 - c), cnsq);
matrix_sum(3, 3, 3, 3, ident, sn, tmp);
matrix_sum(3, 3, 3, 3, tmp, cnsq, R3);
matrix_ident(4, R);
memcpy(R, R3, 3 * sizeof(double));
memcpy(R + 4, R3 + 3, 3 * sizeof(double));
memcpy(R + 8, R3 + 6, 3 * sizeof(double));
}
Matrix periodic_function(Matrix* inputChannel,short numChannels){
Matrix ret=empty_matrix(1,1);
Matrix temp=empty_matrix(1,1);
int i=0;
temp=matrix_mul_double(&inputChannel[0], gs->Ch0Gain);
ret=matrix_sum(&ret,&temp);
if(numChannels>=2){
temp=matrix_mul_double(&inputChannel[1], gs->Ch1Gain);
ret=matrix_sum(&ret,&temp);
if(numChannels>=3){
temp=matrix_mul_double(&inputChannel[2], gs->Ch2Gain);
ret=matrix_sum(&ret,&temp);
for(i=3;i<numChannels;i++){
ret=matrix_sum(&ret,&inputChannel[i]);
}
}
}
ret=matrix_mul_double(&ret,gs->OutputGain);
return ret;
}
/* Function: matrix_test
Perform matrix manipulation.
Parameters:
N - Dimensions of the matrix.
C - memory for result matrix.
A - input matrix
B - operator matrix (not changed during operations)
Returns:
A CRC value that captures all results calculated in the function.
In particular, crc of the value calculated on the result matrix
after each step by <matrix_sum>.
Operation:
1 - Add a constant value to all elements of a matrix.
2 - Multiply a matrix by a constant.
3 - Multiply a matrix by a vector.
4 - Multiply a matrix by a matrix.
5 - Add a constant value to all elements of a matrix.
After the last step, matrix A is back to original contents.
*/
ee_s16 matrix_test(ee_u32 N, MATRES *C, MATDAT *A, MATDAT *B, MATDAT val) {
ee_u16 crc=0;
MATDAT clipval=matrix_big(val);
matrix_add_const(N,A,val); /* make sure data changes */
#if CORE_DEBUG
printmat(A,N,"matrix_add_const");
#endif
matrix_mul_const(N,C,A,val);
crc=crc16(matrix_sum(N,C,clipval),crc);
#if CORE_DEBUG
printmatC(C,N,"matrix_mul_const");
#endif
matrix_mul_vect(N,C,A,B);
crc=crc16(matrix_sum(N,C,clipval),crc);
#if CORE_DEBUG
printmatC(C,N,"matrix_mul_vect");
#endif
matrix_mul_matrix(N,C,A,B);
crc=crc16(matrix_sum(N,C,clipval),crc);
#if CORE_DEBUG
printmatC(C,N,"matrix_mul_matrix");
#endif
matrix_mul_matrix_bitextract(N,C,A,B);
crc=crc16(matrix_sum(N,C,clipval),crc);
#if CORE_DEBUG
printmatC(C,N,"matrix_mul_matrix_bitextract");
#endif
matrix_add_const(N,A,-val); /* return matrix to initial value */
return crc;
}
void EKF::_predictState(double *state, double *state_new) const
{
// copy old to new
matrix_scale(1, 3 * _num_feats_init_structure + _num_motion_states, state, 1.0, state_new);
double LinVel[3];
double AngVel[3];
LinVel[0] = state[3 * _num_feats_init_structure + 0];
LinVel[1] = state[3 * _num_feats_init_structure + 1];
LinVel[2] = state[3 * _num_feats_init_structure + 2];
AngVel[0] = state[3 * _num_feats_init_structure + 3];
AngVel[1] = state[3 * _num_feats_init_structure + 4];
AngVel[2] = state[3 * _num_feats_init_structure + 5];
double curr[3], new_pos[3];
double R[3 * 3];
matrix_scale(1, 3, LinVel, _dt, LinVel);
matrix_scale(1, 3, AngVel, _dt, AngVel);
RODR(AngVel, R);
for (int i = 0; i < _num_feats_init_structure; i++)
{
curr[0] = state[3 * i + 0];
curr[1] = state[3 * i + 1];
curr[2] = state[3 * i + 2];
matrix_product(3, 3, 3, 1, R, curr, new_pos);
matrix_sum(3, 1, 3, 1, new_pos, LinVel, new_pos);
//new_pos = LinVel*dt + ublas::prod(RODR(AngVel*dt),curr);
state_new[3 * i + 0] = new_pos[0];
state_new[3 * i + 1] = new_pos[1];
state_new[3 * i + 2] = new_pos[2];
}
}
int main(int argc, char *argv[]) {
int len;
char *mat_file1, *mat_file2;
double **mat1, **mat2, **result_mat;
FILE *fp1, *fp2;
mat_file1 = readLine();
mat_file2 = readLine();
fp1 = fopen(mat_file1, "r");
fp2 = fopen(mat_file2, "r");
len = mat_length(fp1);
mat1 = write_matrix(fp1, len);
norma(mat1, len);
mat2 = write_matrix(fp2, len);
norma(mat2, len);
result_mat = matrix_sum(mat1, mat2, len);
norma(result_mat, len);
fclose(fp1);
fclose(fp2);
free(mat_file1);
free(mat_file2);
freePointers(mat1, len);
freePointers(mat2, len);
freePointers(result_mat, len);
return 0;
}
int main (int argc, char *argv[])
{
int n = get_input();
int matrix[n][n];
init_matrix(n, matrix);
int sum = matrix_sum(n, matrix);
printf("%d", sum);
return 0;
}
/* Compute an updated rotation matrix given the initial rotation (R)
* and the correction (w) */
void rot_update(double *R, double *w, double *Rnew)
{
double theta, sinth, costh, n[3];
double nx[9], nxsq[9];
double term2[9], term3[9];
double tmp[9], dR[9];
double ident[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
theta = sqrt(w[0] * w[0] + w[1] * w[1] + w[2] * w[2]);
if (theta == 0.0)
{
memcpy(Rnew, R, sizeof(double) * 9);
return;
}
n[0] = w[0] / theta;
n[1] = w[1] / theta;
n[2] = w[2] / theta;
nx[0] = 0.0;
nx[1] = -n[2];
nx[2] = n[1];
nx[3] = n[2];
nx[4] = 0.0;
nx[5] = -n[0];
nx[6] = -n[1];
nx[7] = n[0];
nx[8] = 0.0;
matrix_product33(nx, nx, nxsq);
sinth = sin(theta);
costh = cos(theta);
matrix_scale(3, 3, nx, sinth, term2);
matrix_scale(3, 3, nxsq, 1.0 - costh, term3);
matrix_sum(3, 3, 3, 3, ident, term2, tmp);
matrix_sum(3, 3, 3, 3, tmp, term3, dR);
matrix_product33(dR, R, Rnew);
}
void axis_angle_to_matrix(double *axis, double angle, double *R) {
double ident[9];
double n[9] = { 0.0, -axis[2], axis[1],
axis[2], 0.0, -axis[0],
-axis[1], axis[0], 0.0 };
double nsq[9], sn[9], cnsq[9], tmp[9];
double c, s;
c = cos(angle);
s = sin(angle);
matrix_ident(3, ident);
matrix_product33(n, n, nsq);
matrix_scale(3, 3, n, s, sn);
matrix_scale(3, 3, nsq, (1 - c), cnsq);
matrix_sum(3, 3, 3, 3, ident, sn, tmp);
matrix_sum(3, 3, 3, 3, tmp, cnsq, R);
}
/* Compute the mean of a set of vectors */
void v3_svd(int n, const v3_t *v, double *U, double *S, double *VT) {
double A[9] = { 0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0 };
int i;
for (i = 0; i < n; i++) {
double tensor[9];
matrix_product(3, 1, 1, 3, v[i].p, v[i].p, tensor);
matrix_sum(3, 3, 3, 3, A, tensor, A);
}
dgesvd_driver(3, 3, A, U, S, VT);
}
/* Compute the covariance of a set of (zero-mean) vectors */
void v3_covariance_zm(int n, const v3_t *v, double *cov) {
int i;
for (i = 0; i < 9; i++)
cov[i] = 0.0;
for (i = 0; i < n; i++) {
double tmp[9];
matrix_product(3, 1, 1, 3, v[i].p, v[i].p, tmp);
matrix_sum(3, 3, 3, 3, cov, tmp, cov);
}
matrix_scale(3, 3, cov, 1.0 / n, cov);
}
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]);
}
static ERL_NIF_TERM
sum(ErlNifEnv *env, int32_t argc, const ERL_NIF_TERM *argv) {
ErlNifBinary matrix;
float sum;
float *matrix_data;
(void)(argc);
if (!enif_inspect_binary(env, argv[0], &matrix)) return enif_make_badarg(env);
matrix_data = (float *) matrix.data;
sum = matrix_sum(matrix_data);
return enif_make_double(env, sum);
}
/* Project a point onto the plane */
void PlaneData::Project(double *p, double *p_proj)
{
/* Subtract the distance vector */
double vec[3];
matrix_scale(3, 1, m_normal, m_dist, vec);
double p_norm[3];
matrix_diff(3, 1, 3, 1, p, vec, p_norm);
double dot;
matrix_product(1, 3, 3, 1, m_normal, p_norm, &dot);
double p_par[3];
matrix_scale(3, 1, m_normal, dot, p_par);
double p_perp[3];
matrix_diff(3, 1, 3, 1, p_norm, p_par, p_perp);
matrix_sum(3, 1, 3, 1, p_perp, vec, p_proj);
}
/*
Function: strassen_multiply
--------------------------
Internal function. Matrix multiplication through Strenssen algorithm.
Calculates C = AB. Dimensions of A and B must be power of 2.
Parameters:
a - matrix A
rA_s - row start index of A
rA_e - row end index of A
cA_s - column start index of A
cA_e - column end index of A
b - matrix B
rB_s - column start index 0f B
rB_e - column end index of B
cB_s - column start index of B
cB_e - column end index of B
c - result matrix
*/
void strassen_multiply(
double **a, int rA_s, int rA_e, int cA_s, int cA_e,
double **b, int rB_s, int rB_e, int cB_s, int cB_e, double **c) {
if (((cA_e - cA_s) < 1) || ((rA_e - rA_s) < 1) ||
((cB_e - cB_s) < 1) ||
((cA_e - cA_s + 1 < SMALL_DIM) &&
(rA_e - rA_s + 1 < SMALL_DIM) &&
(cB_e - cB_s + 1 < SMALL_DIM))) {
for (int i = 0; i <= (rA_e - rA_s); ++i) {
for (int j = 0; j <= (cB_e - cB_s); ++j) {
c[i][j] = 0;
for (int k = 0; k <= (cA_e - cA_s); ++k) {
c[i][j] += a[i + rA_s][k + cA_s] * b[k + rB_s][j + cB_s];
}
}
}
}
else {
// Intermediate matrix initialization
double ***m = (double***)malloc(7 * sizeof(double**));
double ***c_sub = (double***)malloc(4 * sizeof(double**));
int mR = rA_e - rA_s + 1;
int mC = cB_e - cB_s + 1;
for (int i = 0; i < 7; ++i) {
m[i] = (double**)malloc((mR / 2) * sizeof(double*));
for (int j = 0; j < mR / 2; ++j) {
m[i][j] = (double*)malloc((mC / 2) * sizeof(double));
}
}
// Gets results of 7 intermediate matrices
int rA_m = (rA_s + rA_e) / 2;
int cA_m = (cA_s + cA_e) / 2;
int rB_m = (rB_s + rB_e) / 2;
int cB_m = (cB_s + cB_e) / 2;
// Temporary pointers
double **temp1;
double **temp2;
// Matrix m1
temp1 = matrix_sum(a, rA_s, rA_m, cA_s, cA_m, rA_m + 1, rA_e,
cA_m + 1, cA_e);
temp2 = matrix_sum(b, rB_s, rB_m, cB_s, cB_m, rB_m + 1, rB_e,
cB_m + 1, cB_e);
strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, temp2,
0, rB_m - rB_s, 0, cB_m - cB_s, m[0]);
clear2D(&temp1, rA_m - rA_s + 1);
clear2D(&temp2, rB_m - rB_s + 1);
// Matrix m2
temp1 = matrix_sum(a, rA_m + 1, rA_e, cA_s, cA_m, rA_m + 1,
rA_e, cA_m + 1, cA_e);
strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s,
b, rB_s, rB_m, cB_s, cB_m, m[1]);
clear2D(&temp1, rA_e - rA_m);
// Matrix m3
temp1 = matrix_sub(b, rB_s, rB_m, cB_m + 1, cB_e, rB_m + 1,
rB_e, cB_m + 1, cB_e);
strassen_multiply(a, rA_s, rA_m, cA_s, cA_m, temp1, 0,
rB_m - rB_s, 0, cB_m - cB_s, m[2]);
clear2D(&temp1, rB_m - rB_s + 1);
// Matrix m4
temp1 = matrix_sub(b, rB_m + 1, rB_e, cB_s, cB_m, rB_s, rB_m,
cB_s, cB_m);
strassen_multiply(a, rA_m + 1, rA_e, cA_m + 1, cA_e, temp1,
0, rB_m - rB_s, 0, cB_m - cB_s, m[3]);
clear2D(&temp1, rB_e - rB_m);
// Matrix m5
temp1 = matrix_sum(a, rA_s, rA_m, cA_s, cA_m, rA_s, rA_m,
cA_m + 1, cA_e);
strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, b,
rB_m + 1, rB_e, cB_m + 1, cB_e, m[4]);
clear2D(&temp1, rA_m - rA_s + 1);
// Matrix m6
temp1 = matrix_sub(a, rA_m + 1, rA_e, cA_s, cA_m, rA_s, rA_m,
cA_s, cA_m);
temp2 = matrix_sum(b, rB_s, rB_m, cB_s, cB_m, rB_s, rB_m,
cB_m + 1, cB_e);
strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s,
temp2, 0, rB_m - rB_s, 0, cB_m - cB_s, m[5]);
clear2D(&temp1, rA_e - rA_m);
clear2D(&temp2, rB_m - rB_s + 1);
// Matrix m7
temp1 = matrix_sub(a, rA_s, rA_m, cA_m + 1, cA_e, rA_m + 1,
rA_e, cA_m + 1, cA_e);
temp2 = matrix_sum(b, rB_m + 1, rB_e, cB_s, cB_m, rB_m + 1,
rB_e, cB_m + 1, cB_e);
strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, temp2,
0, rB_m - rB_s, 0, cB_m - cB_s, m[6]);
clear2D(&temp1, rA_m - rA_s + 1);
clear2D(&temp2, rB_e - rB_m);
// Calculates all result sub-matrices
temp1 = sum(m[0], m[3], mR / 2, mC / 2);
temp2 = sub(temp1, m[4], mR / 2, mC / 2);
c_sub[0] = sum(temp2, m[6], mR / 2, mC / 2);
clear2D(&temp1, mR / 2);
clear2D(&temp2, mR / 2);
c_sub[1] = sum(m[2], m[4], mR / 2, mC / 2);
c_sub[2] = sum(m[1], m[3], mR / 2, mC / 2);
temp1 = sum(m[0], m[2], mR / 2, mC / 2);
temp2 = sum(temp1, m[5], mR / 2, mC / 2);
c_sub[3] = sub(temp2, m[1], mR / 2, mC / 2);
clear2D(&temp1, mR / 2);
clear2D(&temp2, mR / 2);
// Free intermediate matrices
for (int i = 0; i < 7; ++i) {
for (int j = 0; j < mR / 2; ++j) {
free(m[i][j]);
}
free(m[i]);
}
free(m);
// Combine sub-matrices
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < mR / 2; ++j) {
for (int k = 0; k < mC / 2; ++k) {
c[(i / 2) * mR / 2 + j][(i % 2) * mC / 2 + k] =
c_sub[i][j][k];
}
}
}
// Free sub mmatrices
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < mR / 2; ++j) {
free(c_sub[i][j]);
}
free(c_sub[i]);
}
free(c_sub);
}
}
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 infomax(gsl_matrix *x_white, gsl_matrix *weights, gsl_matrix *S, int verbose){
/*Computes ICA infomax in whitened data
Decomposes x_white as x_white=AS
*Input
x_white: whitened data (Use PCAwhiten)
*Output
A : mixing matrix
S : source matrix
*/
// int verbose = 1; //true
size_t NCOMP = x_white->size1;
size_t NVOX = x_white->size2;
//getting permutation vector
const gsl_rng_type * T;
gsl_rng * r;
gsl_permutation * p = gsl_permutation_alloc (NVOX);
// gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc (T);
gsl_permutation_init (p);
gsl_matrix *old_weights = gsl_matrix_alloc(NCOMP,NCOMP);
gsl_matrix *bias = gsl_matrix_calloc(NCOMP, 1);
gsl_matrix *d_weights = gsl_matrix_calloc(NCOMP,NCOMP);
gsl_matrix *temp_change = gsl_matrix_alloc(NCOMP,NCOMP);
gsl_matrix *old_d_weights = gsl_matrix_calloc(NCOMP,NCOMP);
gsl_matrix *shuffled_x_white = gsl_matrix_calloc(NCOMP,x_white->size2);
gsl_matrix_memcpy(shuffled_x_white, x_white);
gsl_matrix_set_identity(weights);
gsl_matrix_set_identity(old_weights);
double lrate = 0.005/log((double)NCOMP);
double change=1;
double angle_delta =0;
size_t step = 1;
int error = 0;
while( (step < MAX_STEP) && (change > W_STOP)){
error = w_update(weights, x_white, bias,
shuffled_x_white, p, r, lrate);
if (error==1 || error==2){
// It blowed up! RESTART!
step = 1;
// change = 1;
error = 0;
lrate *= ANNEAL;
gsl_matrix_set_identity(weights);
gsl_matrix_set_identity(old_weights);
gsl_matrix_set_zero(d_weights);
gsl_matrix_set_zero(old_d_weights);
gsl_matrix_set_zero(bias);
if (lrate > MIN_LRATE){
printf("\nLowering learning rate to %g and starting again.\n",lrate);
}
else{
printf("\nMatrix may not be invertible");
}
}
else if (error==0){
gsl_matrix_memcpy(d_weights, weights);
gsl_matrix_sub(d_weights, old_weights);
change = matrix_norm(d_weights);
if (step > 2){
// Compute angle delta
gsl_matrix_memcpy(temp_change, d_weights);
gsl_matrix_mul_elements(temp_change, old_d_weights);
angle_delta = acos(matrix_sum(temp_change) / sqrt(matrix_norm(d_weights)*(matrix_norm(old_d_weights))));
angle_delta *= (180.0 / M_PI);
}
gsl_matrix_memcpy(old_weights, weights);
if (angle_delta > 60){
lrate *= ANNEAL;
gsl_matrix_memcpy(old_d_weights, d_weights);
} else if (step==1) {
gsl_matrix_memcpy(old_d_weights, d_weights);
}
if ((verbose && (step % 10)== 0) || change < W_STOP){
printf("\nStep %zu: Lrate %.1e, Wchange %.1e, Angle %.2f",
step, lrate, change, angle_delta);
}
step ++;
}
}
matrix_mmul(weights, x_white, S);
gsl_matrix_free(old_d_weights);
gsl_matrix_free(old_weights);
gsl_matrix_free(bias);
gsl_matrix_free(d_weights);
gsl_matrix_free(shuffled_x_white);
gsl_rng_free (r);
gsl_permutation_free (p);
}
/* Setup various planar aspect */
void PlaneData::Setup(std::vector<PointData> &point_data,
double *origin, double *up)
{
/* Compute the mean of the points on the plane */
double mean[3] = { 0.0, 0.0, 0.0 };
int num_plane_points = (int) m_points.size();
for (int i = 0; i < num_plane_points; i++) {
int pt_idx = m_points[i];
mean[0] += point_data[pt_idx].m_pos[0];
mean[1] += point_data[pt_idx].m_pos[1];
mean[2] += point_data[pt_idx].m_pos[2];
}
matrix_scale(3, 1, mean, 1.0 / num_plane_points, mean);
matrix_diff(3, 1, 3, 1, mean, origin, mean);
/* Project the mean onto the plane */
Project(mean, m_origin);
matrix_sum(3, 1, 3, 1, m_origin, origin, m_origin);
memcpy(m_vaxis, up, 3 * sizeof(double));
matrix_cross(m_normal, m_vaxis, m_uaxis);
/* Compute the variance in each direction */
double u_variance = 0.0, v_variance = 0.0;
for (int i = 0; i < num_plane_points; i++) {
int pt_idx = m_points[i];
/* Subtract out the origin */
double diff[3];
matrix_diff(3, 1, 3, 1, point_data[pt_idx].m_pos, m_origin, diff);
/* Project onto u,v axes */
double u_proj[3], v_proj[3];
ProjectU(diff, u_proj);
ProjectV(diff, v_proj);
u_variance += matrix_normsq(3, 1, u_proj);
v_variance += matrix_normsq(3, 1, v_proj);
}
u_variance = sqrt(u_variance / num_plane_points);
v_variance = sqrt(v_variance / num_plane_points);
m_u_extent = 2.0 * u_variance;
m_v_extent = 2.0 * v_variance;
/* Find all the views that see this plane */
std::vector<int> views;
for (int i = 0; i < num_plane_points; i++) {
int pt_idx = m_points[i];
for (int j = 0; j < (int) point_data[pt_idx].m_views.size(); j++) {
int view = point_data[pt_idx].m_views[j].first;
bool found = false;
for (int k = 0; k < (int) views.size(); k++) {
if (views[k] == view) {
found = true;
break;
}
}
if (!found) {
views.push_back(view);
}
}
}
m_views = views;
}
/* Computes the closed-form least-squares solution to a rigid
* body alignment.
*
* n: the number of points
* right_pts: Target set of n points
* left_pts: Source set of n points */
double align_horn_3D(int n, v3_t *right_pts, v3_t *left_pts, int scale_xform,
double *Tout) {
int i;
v3_t right_centroid = v3_new(0.0, 0.0, 0.0);
v3_t left_centroid = v3_new(0.0, 0.0, 0.0);
double M[3][3] = { { 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, } };
double MT[3][3];
double MTM[3][3];
double eval[3], sqrteval_inv[3];
double evec[3][3], evec_tmp[3][3];
double Sinv[3][3], U[3][3];
double Tcenter[4][4] = { { 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0 } };
double Ttmp[4][4];
double T[16], R[16];
double sum_num, sum_den, scale, RMS_sum;
int perm[3];
/* Compute the centroid of both point sets */
right_centroid = v3_mean(n, right_pts);
left_centroid = v3_mean(n, left_pts);
/* Compute the scale */
sum_num = sum_den = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
sum_num += v3_magsq(r);
sum_den += v3_magsq(l);
}
scale = sqrt(sum_num / sum_den);
/* Fill in the matrix M */
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
M[0][0] += Vx(r) * Vx(l);
M[0][1] += Vx(r) * Vy(l);
M[0][2] += Vx(r) * Vz(l);
M[1][0] += Vy(r) * Vx(l);
M[1][1] += Vy(r) * Vy(l);
M[1][2] += Vy(r) * Vz(l);
M[2][0] += Vz(r) * Vx(l);
M[2][1] += Vz(r) * Vy(l);
M[2][2] += Vz(r) * Vz(l);
}
/* Compute MTM */
matrix_transpose(3, 3, (double *)M, (double *)MT);
matrix_product(3, 3, 3, 3, (double *)MT, (double *)M, (double *)MTM);
/* Calculate Sinv, the inverse of the square root of MTM */
dgeev_driver(3, (double *)MTM, (double *)evec, eval);
/* Sort the eigenvalues */
qsort_descending();
qsort_perm(3, eval, perm);
memcpy(evec_tmp[0], evec[perm[0]], sizeof(double) * 3);
memcpy(evec_tmp[1], evec[perm[1]], sizeof(double) * 3);
memcpy(evec_tmp[2], evec[perm[2]], sizeof(double) * 3);
memcpy(evec, evec_tmp, sizeof(double) * 9);
sqrteval_inv[0] = 1.0 / sqrt(eval[0]);
sqrteval_inv[1] = 1.0 / sqrt(eval[1]);
if (eval[2] < 1.0e-8 * eval[0]) {
sqrteval_inv[2] = 0.0;
} else {
sqrteval_inv[2] = 1.0 / sqrt(eval[2]);
}
Sinv[0][0] =
sqrteval_inv[0] * evec[0][0] * evec[0][0] +
sqrteval_inv[1] * evec[1][0] * evec[1][0] +
sqrteval_inv[2] * evec[2][0] * evec[2][0];
Sinv[0][1] =
sqrteval_inv[0] * evec[0][0] * evec[0][1] +
sqrteval_inv[1] * evec[1][0] * evec[1][1] +
sqrteval_inv[2] * evec[2][0] * evec[2][1];
Sinv[0][2] =
sqrteval_inv[0] * evec[0][0] * evec[0][2] +
sqrteval_inv[1] * evec[1][0] * evec[1][2] +
sqrteval_inv[2] * evec[2][0] * evec[2][2];
Sinv[1][0] =
sqrteval_inv[0] * evec[0][1] * evec[0][0] +
sqrteval_inv[1] * evec[1][1] * evec[1][0] +
sqrteval_inv[2] * evec[2][1] * evec[2][0];
Sinv[1][1] =
sqrteval_inv[0] * evec[0][1] * evec[0][1] +
sqrteval_inv[1] * evec[1][1] * evec[1][1] +
sqrteval_inv[2] * evec[2][1] * evec[2][1];
Sinv[1][2] =
sqrteval_inv[0] * evec[0][1] * evec[0][2] +
sqrteval_inv[1] * evec[1][1] * evec[1][2] +
sqrteval_inv[2] * evec[2][1] * evec[2][2];
Sinv[2][0] =
sqrteval_inv[0] * evec[0][2] * evec[0][0] +
sqrteval_inv[1] * evec[1][2] * evec[1][0] +
sqrteval_inv[2] * evec[2][2] * evec[2][0];
Sinv[2][1] =
sqrteval_inv[0] * evec[0][2] * evec[0][1] +
sqrteval_inv[1] * evec[1][2] * evec[1][1] +
sqrteval_inv[2] * evec[2][2] * evec[2][1];
Sinv[2][2] =
sqrteval_inv[0] * evec[0][2] * evec[0][2] +
sqrteval_inv[1] * evec[1][2] * evec[1][2] +
sqrteval_inv[2] * evec[2][2] * evec[2][2];
/* U = M * Sinv */
matrix_product(3, 3, 3, 3, (double *)M, (double *)Sinv, (double *)U);
if (eval[2] < 1.0e-8 * eval[0]) {
double u3u3[9], Utmp[9];
matrix_transpose_product2(3, 1, 3, 1, evec[2], evec[2], u3u3);
matrix_sum(3, 3, 3, 3, (double *) U, u3u3, Utmp);
if (matrix_determinant3(Utmp) < 0.0) {
printf("[align_horn_3D] Recomputing matrix...\n");
matrix_diff(3, 3, 3, 3, (double *) U, u3u3, Utmp);
}
memcpy(U, Utmp, 9 * sizeof(double));
}
/* Fill in the rotation matrix */
R[0] = U[0][0]; R[1] = U[0][1]; R[2] = U[0][2]; R[3] = 0.0;
R[4] = U[1][0]; R[5] = U[1][1]; R[6] = U[1][2]; R[7] = 0.0;
R[8] = U[2][0]; R[9] = U[2][1]; R[10] = U[2][2]; R[11] = 0.0;
R[12] = 0.0; R[13] = 0.0; R[14] = 0.0; R[15] = 1.0;
/* Fill in the translation matrix */
matrix_ident(4, T);
T[3] = Vx(right_centroid);
T[7] = Vy(right_centroid);
T[11] = Vz(right_centroid);
if (scale_xform == 0)
scale = 1.0;
Tcenter[0][0] = scale;
Tcenter[1][1] = scale;
Tcenter[2][2] = scale;
Tcenter[0][3] = -scale * Vx(left_centroid);
Tcenter[1][3] = -scale * Vy(left_centroid);
Tcenter[2][3] = -scale * Vz(left_centroid);
matrix_product(4, 4, 4, 4, T, R, (double *) Ttmp);
matrix_product(4, 4, 4, 4, (double *)Ttmp, (double *)Tcenter, Tout);
#if 0
T[2] = Vx(v3_sub(right_centroid, left_centroid));
T[5] = Vy(v3_sub(right_centroid, left_centroid));
T[8] = Vz(v3_sub(right_centroid, left_centroid));
#endif
/* Now compute the RMS error between the points */
RMS_sum = 0.0;
for (i = 0; i < n; i++) {
double left[4] = { Vx(left_pts[i]),
Vy(left_pts[i]),
Vz(left_pts[i]), 1.0 };
double left_prime[3];
double dx, dy, dz;
matrix_product(4, 4, 4, 1, Tout, left, left_prime);
dx = left_prime[0] - Vx(right_pts[i]);
dy = left_prime[1] - Vy(right_pts[i]);
dz = left_prime[2] - Vz(right_pts[i]);
RMS_sum += dx * dx + dy * dy + dz * dz;
#if 0
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
v3_t resid;
/* Rotate, scale l */
v3_t Rl, SRl;
Vx(Rl) = R[0] * Vx(l) + R[1] * Vy(l) + R[2] * Vz(l);
Vy(Rl) = R[3] * Vx(l) + R[4] * Vy(l) + R[5] * Vz(l);
Vz(Rl) = R[6] * Vx(l) + R[7] * Vy(l) + R[8] * Vz(l);
SRl = v3_scale(scale, Rl);
resid = v3_sub(r, SRl);
RMS_sum += v3_magsq(resid);
#endif
}
return sqrt(RMS_sum / n);
}