Beispiel #1
0
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));
}
Beispiel #2
0
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;
}
Beispiel #3
0
/* 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;
}
Beispiel #4
0
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;
}
Beispiel #7
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);
}
Beispiel #8
0
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);
}
Beispiel #9
0
/* 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);
}
Beispiel #10
0
/* 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);
}
Beispiel #11
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]);
}
Beispiel #12
0
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);
}
Beispiel #13
0
/* 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);
	}
}
Beispiel #15
0
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;
}
Beispiel #16
0
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);
}
Beispiel #17
0
/* 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;

}
Beispiel #18
0
/* 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);
}