void mtx_copy(float32_t eps) {
float32_t errf;
ambix_matrix_t *left=NULL, *right=NULL;
unsigned int i;
float32_t maxeps=eps;
STARTTEST("\n");
right=ambix_matrix_copy(left, NULL);
fail_if((NULL!=right), __LINE__, "copying from NULL matrix erroneously succeeded");
left=ambix_matrix_create();
fail_if((left !=ambix_matrix_init(4, 3, left )), __LINE__, "initializing left matrix failed");
ambix_matrix_fill_data(left, leftdata_4_3);
right=ambix_matrix_copy(left, NULL);
fail_if((NULL==right), __LINE__, "copying to NULL matrix failed");
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf>0.f, __LINE__, "diffing mtx with copy0 returned %g (>%g)", errf, 0.f);
right=ambix_matrix_copy(left, right);
fail_if((NULL==right), __LINE__, "copying to right matrix failed");
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf>0.f, __LINE__, "diffing mtx with copy returned %g (>%g)", errf, 0.f);
ambix_matrix_destroy(left);
ambix_matrix_destroy(right);
STOPTEST("\n");
}
void TwoFrameModel::WriteSparse(FILE *f)
{
// WriteCameraPose(f, m_camera0);
// WriteCameraPose(f, m_camera1);
/* Compute the camera pose of camera1 relative to camera0 */
double pos0[3], pos1[3];
// matrix_transpose_product(3, 3, 3, 1, m_camera0.R, m_camera0.t, pos0);
// matrix_transpose_product(3, 3, 3, 1, m_camera1.R, m_camera1.t, pos1);
memcpy(pos0, m_camera0.t, 3 * sizeof(double));
memcpy(pos1, m_camera1.t, 3 * sizeof(double));
// matrix_scale(3, 1, pos0, -1.0, pos0);
// matrix_scale(3, 1, pos1, -1.0, pos1);
double diff[3];
matrix_diff(3, 1, 3, 1, pos1, pos0, diff);
double R1[9], tr[3];
matrix_transpose_product2(3, 3, 3, 3, m_camera0.R, m_camera1.R, R1);
// matrix_transpose_product(3, 3, 3, 3, m_camera1.R, m_camera0.R, R1);
matrix_product(3, 3, 3, 1, m_camera0.R, diff, tr);
double norm = matrix_norm(3, 1, tr);
matrix_scale(3, 1, tr, 1.0 / norm, tr);
double viewdir[3] = { -R1[2], -R1[5], -R1[8] };
double twist_angle = GetTwist(R1);
/* Compute the distance to the scene */
double z_avg = 0.0;
for (int p = 0; p < m_num_points; p++) {
v3_t &pt = m_points[p];
double diff1[3], diff2[3];
matrix_diff(3, 1, 3, 1, pt.p, pos0, diff1);
matrix_diff(3, 1, 3, 1, pt.p, pos1, diff2);
double dist1 = matrix_norm(3, 1, diff1);
double dist2 = matrix_norm(3, 1, diff2);
z_avg += 0.5 * (dist1 + dist2) / norm;
}
z_avg /= m_num_points;
WriteVector(f, 9, R1);
/* Write the viewing direction */
// WriteVector(f, 3, viewdir);
/* Write the twist angle */
// fprintf(f, "%0.8f\n", twist_angle);
/* Write the translation */
WriteVector(f, 3, tr);
fprintf(f, "%0.6f\n", z_avg);
}
/* Returns true (and stores the endpoints in t and u) if the
* epipolar swath e1, e2 intersects this segment */
bool LineSegment2D::IntersectsEpipolarSwath(double e1[3], double e2[3],
double &t, double &u)
{
/* Find the line between the two endpoints */
double p[3] = { m_p1[0], m_p1[1], 1.0 };
double q[3] = { m_p2[0], m_p2[1], 1.0 };
double l[3];
matrix_cross(p, q, l);
/* Intersect the line with the two epipolar lines */
double i1[3], i2[3];
matrix_cross(l, e1, i1);
matrix_cross(l, e2, i2);
double inv_i12 = 1.0 / i1[2];
i1[0] *= inv_i12;
i1[1] *= inv_i12;
double inv_i22 = 1.0 / i2[2];
i2[0] *= inv_i22;
i2[1] *= inv_i22;
double qp[3], i1p[3], i2p[3];
matrix_diff(2, 1, 2, 1, q, p, qp);
matrix_diff(2, 1, 2, 1, i1, p, i1p);
matrix_diff(2, 1, 2, 1, i2, p, i2p);
double dot1, dot2;
matrix_product(1, 2, 2, 1, i1p, qp, &dot1);
matrix_product(1, 2, 2, 1, i2p, qp, &dot2);
int sign1 = SGN(dot1);
int sign2 = SGN(dot2);
double inv_norm = 1.0 / matrix_norm(2, 1, qp);
double mag1 = matrix_norm(2, 1, i1p) * inv_norm; // matrix_norm(2, 1, qp);
double mag2 = matrix_norm(2, 1, i2p) * inv_norm; // matrix_norm(2, 1, qp);
t = sign1 * mag1;
u = sign2 * mag2;
/* Intersection check */
if ((t < 0.0 && u < 0.0) || (t > 1.0 && u > 1.0))
return false;
else
return true;
}
void mtxmul_eye_tests(float32_t eps) {
float32_t errf;
ambix_matrix_t *left, *result, *eye;
STARTTEST("");
eye=ambix_matrix_init(4, 4, NULL);
fail_if((eye!=ambix_matrix_fill(eye, AMBIX_MATRIX_IDENTITY)), __LINE__, "filling unity matrix %p did not return original matrix %p", eye);
left=ambix_matrix_init(4, 2, NULL);
fail_if(AMBIX_ERR_SUCCESS!=ambix_matrix_fill_data(left, resultdata_4_2), __LINE__,
"filling left data failed");
result=ambix_matrix_init(4, 2, NULL);
fail_if(AMBIX_ERR_SUCCESS!=ambix_matrix_fill_data(result, resultdata_4_2), __LINE__,
"filling result data failed");
fail_if((result!=ambix_matrix_multiply(eye, left, result)), __LINE__, "multiplication into matrix did not return original matrix");
#if 0
matrix_print(eye);
matrix_print(result);
matrix_print(left);
#endif
errf=matrix_diff(__LINE__, left, result, eps);
fail_if((errf>eps), __LINE__, "diffing matrix M with E*M returned %f (>%f)", errf, eps);
ambix_matrix_destroy(left);
ambix_matrix_destroy(result);
ambix_matrix_destroy(eye);
}
void mtxmul_tests(float32_t eps) {
float32_t errf;
ambix_matrix_t *left=NULL, *right=NULL, *result, *testresult;
STARTTEST("\n");
/* fill in some test data */
left=ambix_matrix_init(4, 3, NULL);
ambix_matrix_fill_data(left, leftdata_4_3);
right=ambix_matrix_init(3, 2, NULL);
ambix_matrix_fill_data(right, rightdata_3_2);
testresult=ambix_matrix_init(4, 2, NULL);
ambix_matrix_fill_data(testresult, resultdata_4_2);
errf=matrix_diff(__LINE__, left, left, eps);
fail_if(!(errf<eps), __LINE__, "diffing matrix with itself returned %f (>%f)", errf, eps);
/* NULL multiplications */
result=ambix_matrix_multiply(NULL, NULL, NULL);
fail_if(NULL!=result, __LINE__, "multiplying NULL*NULL returned success");
result=ambix_matrix_multiply(left, NULL, result);
fail_if(NULL!=result, __LINE__, "multiplying left*NULL returned success");
result=ambix_matrix_multiply(NULL, left, result);
fail_if(NULL!=result, __LINE__, "multiplying NULL*left returned success");
/* do some matrix multiplication */
result=ambix_matrix_multiply(left, right, NULL);
fail_if((NULL==result), __LINE__, "multiply into NULL did not create matrix");
fail_if((result!=ambix_matrix_multiply(left, right, result)), __LINE__, "multiply into existing matrix returned new matrix");
#if 0
matrix_print(left);
matrix_print(right);
matrix_print(result);
printf("------------\n");
#endif
errf=matrix_diff(__LINE__, testresult, result, eps);
fail_if((errf>eps), __LINE__, "diffing two results of same multiplication returned %f (>%f)", errf, eps);
ambix_matrix_destroy(left);
ambix_matrix_destroy(right);
ambix_matrix_destroy(result);
ambix_matrix_destroy(testresult);
STOPTEST("\n");
}
double TwoFrameModel::AverageDistanceToPoints() const {
double dist_sum = 0.0;
const double *pos1 = m_camera0.t;
const double *pos2 = m_camera1.t;
for (int i = 0; i < m_num_points; i++) {
double diff1[3], diff2[3];
matrix_diff(3, 1, 3, 1, m_points[i].p, (double *) pos1, diff1);
matrix_diff(3, 1, 3, 1, m_points[i].p, (double *) pos2, diff2);
double norm1 = matrix_norm(3, 1, diff1);
double norm2 = matrix_norm(3, 1, diff2);
dist_sum += (norm1 + norm2);
}
return dist_sum / (2 * m_num_points);
}
static void mtxinverse_test(const ambix_matrix_t *mtx, const ambix_matrix_t *result, float32_t eps) {
ambix_matrix_t *pinv = 0;
ambix_matrix_t *mul=0;
ambix_matrix_t *eye=0;
float32_t errf;
int min_rowcol=(mtx->cols<mtx->rows)?mtx->cols:mtx->rows;
fail_if((NULL==mtx), __LINE__, "cannot invert NULL-matrix");
eye=ambix_matrix_init(min_rowcol, min_rowcol, eye);
eye=ambix_matrix_fill(eye, AMBIX_MATRIX_IDENTITY);
fail_if((NULL==eye), __LINE__, "cannot create eye-matrix for pinv-verification");
pinv=ambix_matrix_pinv(mtx, pinv);
if(NULL==pinv)matrix_print(mtx);
fail_if((NULL==pinv), __LINE__, "could not invert matrix");
if(mtx->cols < mtx->rows)
mul=ambix_matrix_multiply(pinv, mtx, 0);
else
mul=ambix_matrix_multiply(mtx, pinv, 0);
#if 0
matrix_print(mtx);
matrix_print(pinv);
matrix_print(mul);
if(result)matrix_print(result);
printf("------------\n");
#endif
if(result) {
errf=matrix_diff(__LINE__, pinv, result, eps);
fail_if((errf>eps), __LINE__, "diffing (pseudo)inverse returned %g (>%g)", errf, eps);
errf=matrix_diff(__LINE__, mul, eye, eps);
fail_if((errf>eps), __LINE__, "diffing mtx*pinv(mtx) returned %g (>%g)", errf, eps);
} else {
errf=matrix_diff(__LINE__, mul, eye, eps);
fail_if((!(isnan(errf) || isinf(errf) || (errf>eps))), __LINE__, "diffing invalid mtx*pinv(mtx) returned %g (!>%g)", errf, eps);
}
ambix_matrix_destroy(pinv);
ambix_matrix_destroy(mul);
ambix_matrix_destroy(eye);
}
/* 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);
}
bool PlaneData::CheckInside(double *point)
{
double side_vec[3],poly_vec[3],point_vec[3],cross1[3],cross2[3];
for(int i=0;i<4;i++)
{
int i1,i2,i3;
i1=i;
i2=(i1+1)%4;
i3=(i2+1)%4;
matrix_diff(3,1,3,1,m_corners+i3*3,m_corners+i2*3,side_vec);
matrix_diff(3,1,3,1,m_corners+i1*3,m_corners+i2*3,poly_vec);
matrix_diff(3,1,3,1,point,m_corners+i2*3,point_vec);
matrix_cross(side_vec,poly_vec,cross1);
matrix_cross(side_vec,point_vec,cross2);
double dp=0;
for(int j=0;j<3;dp+=cross1[j]*cross2[j],j++);
if(dp<0) return false;
}
return true;
}
/* Compute an RQ factorization of an m by n matrix A */
void dgerqf_driver(int m, int n, double *A, double *R, double *Q)
{
double *AT;
int lda = m;
double *tau;
int tau_dim = MIN(m, n);
double *work;
int block_size = 64; /* Just a guess... */
int lwork = n * block_size;
int info;
double *H;
double *v, *vvT;
double *Qtmp;
int i, j;
/* Transpose A */
AT = (double *) malloc(sizeof(double) * m * n);
matrix_transpose(m, n, A, AT);
/* Call the LAPACK routine */
tau = (double *) malloc(sizeof(double) * tau_dim);
work = (double *) malloc(sizeof(double) * lwork);
dgerqf_(&m, &n, AT, &lda, tau, work, &lwork, &info);
if (info < 0) {
printf("[dgeqrf_driver] An error occurred.\n");
free(AT);
free(work);
free(tau);
return;
}
/* Extract the R matrix */
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++) {
if (j < i)
R[i * n + j] = 0.0;
else
R[i * n + j] = AT[(n - m + j) * m + i];
}
}
/* Now extract the Q matrix */
H = (double *) malloc(sizeof(double) * n * n);
v = (double *) malloc(sizeof(double) * n);
vvT = (double *) malloc(sizeof(double) * n * n);
Qtmp = (double *) malloc(sizeof(double) * n * n);
for (i = 0; i < tau_dim; i++) {
matrix_ident(m, H);
for (j = 0; j < n; j++) {
if (j > n - tau_dim + i)
v[j] = 0.0;
else if (j == n - tau_dim + i)
v[j] = 1.0;
else
v[j] = AT[j * m + (m-tau_dim+i)];
}
matrix_transpose_product2(n, 1, n, 1, v, v, vvT);
matrix_scale(n, n, vvT, tau[i], vvT);
matrix_diff(n, n, n, n, H, vvT, H);
if (i == 0) {
memcpy(Q, H, sizeof(double) * n * n);
} else {
matrix_product(n, n, n, n, Q, H, Qtmp);
memcpy(Q, Qtmp, sizeof(double) * n * n);
}
}
matrix_product(m, n, n, n, R, Q, H);
free(H);
free(v);
free(vvT);
free(Qtmp);
free(tau);
free(work);
free(AT);
}
/* Align two sets of points with a 3D rotation */
double align_3D_rotation(int n, v3_t *r_pts, v3_t *l_pts, double *R)
{
double A[9];
double U[9], S[3], V[9], VT[9], RT[9];
int i;
double error;
#if 0
if (n > 3) {
printf("A:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(r_pts[i]), Vy(r_pts[i]), Vz(r_pts[i]));
}
printf("B:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(l_pts[i]), Vy(l_pts[i]), Vz(l_pts[i]));
}
}
#endif
for (i = 0; i < 9; i++)
A[i] = 0.0;
for (i = 0; i < n; i++) {
double *a = l_pts[i].p, *b = r_pts[i].p;
// matrix_product(3, 1, 1, 3, l_pts[i].p, r_pts[i].p, tensor);
A[0] += a[0] * b[0];
A[1] += a[0] * b[1];
A[2] += a[0] * b[2];
A[3] += a[1] * b[0];
A[4] += a[1] * b[1];
A[5] += a[1] * b[2];
A[6] += a[2] * b[0];
A[7] += a[2] * b[1];
A[8] += a[2] * b[2];
}
// dgesvd_driver(3, 3, A, U, S, VT);
// printf("svd:\n");
// matrix_print(3, 3, A);
svd(3, 3, 1, 1, 1.0e-12, 1.0e-12, A, S, U, V, VT);
// printf("U:\n");
// matrix_print(3, 3, U);
// printf("VT:\n");
// matrix_print(3, 3, VT);
// printf("S:\n");
// matrix_print(3, 3, S);
matrix_product33(U, VT, RT);
matrix_transpose(3, 3, RT, R);
// printf("R:\n");
// matrix_print(3, 3, R);
if (matrix_determinant3(R) < 0.0) {
/* We're dealing with a reflection */
double tmp[9];
double reflectZ[9] = { 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0 };
matrix_product33(U, reflectZ, tmp);
matrix_product33(tmp, VT, RT);
matrix_transpose(3, 3, RT, R);
}
/* Compute error */
error = 0.0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product331(R, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
// printf("d[%d] = %0.6f\n", i, dist);
error += dist;
}
return error / n;
}
/** call: ./main <matrix_dimension> <number_of_tests> <use_gpu>*/
int main(int argc, char* argv[])
{
cuda_identify();
if (argc != 4) {
printf("program must be called with arguments: matrix_dimension tests_number use_gpu(0/1)\n");
exit(1);
}
const int M = atoi(argv[1]);
printf("Using matrix dimension: %d\n", M);
const int tests = atoi(argv[2]);
const bool cpu = !atoi(argv[3]);
// always use the same seed to get the same matrices during tests
srand(0);
#ifdef DOUBLE
const fp_t min_diff = 0.00000001; //for double, fails with 8192 and floats on both cpu and gpu
#else
const fp_t min_diff = 0.000001;
#endif
const fp_t alpha = 0.9;
const int max_iter = 50;
fp_t* exec_times = malloc(tests * sizeof(fp_t));
fp_t* all_rmse = malloc(tests * sizeof(fp_t));
for (int k = 0; k < tests; k++) {
const DataSet dataset = generate_dataset(M);
Matrix* last_x = aligned_vector(M, true);
Matrix* x = aligned_vector(M, true);
for (int i = 0; i < M; i++) {
}
int iterations = 0;
// solve Ax = b
const fp_t start_time = omp_get_wtime();
fp_t sum = 0;
int j = 0;
int i = 0;
const Matrix* A = dataset.A;
const Matrix* b = dataset.b;
assert(x != last_x);
if (cpu) {
//#pragma omp parallel shared(last_x, x, iterations) private(i, j, sum)
while ((matrix_diff(x, last_x) > min_diff) && (max_iter < 0 || iterations < max_iter)) {
//fp_t st_time0 = omp_get_wtime();
//#pragma omp single
{
swap(last_x, x);
}
// A, M, alpha and b are constant, so they cannot be declared as shared
//#pragma omp for schedule(dynamic)
for (i = 0; i < M; i++) {
sum = 0;
//#pragma omp simd aligned(A, last_x: 16) reduction(+:sum) linear(j)
for (j = 0; j < M; j++) {
sum += A->elements[i * M + j] * last_x->elements[j];
}
sum -= A->elements[i * M + i] * last_x->elements[i]; // opt: outside the loop for sse optimizer
x->elements[i] = (1 - alpha) * last_x->elements[i] + alpha * (b->elements[i] - sum) / A->elements[i * M + i];
}
//#pragma omp single nowait
{
iterations++;
}
//printf("%dus spent\n", (int)((omp_get_wtime() - st_time0) * 1000000));
}
} else {
Matrix* d_A = device_matrix_from(A);
#ifndef DOUBLE
#ifdef TEXTURE
texbind(d_A->elements, d_A->size * sizeof(fp_t));
#endif
#endif
cudaMemcpy(d_A->elements, A->elements, A->size * sizeof(fp_t), cudaMemcpyHostToDevice);
Matrix* d_b = device_matrix_from(b);
cudaMemcpy(d_b->elements, b->elements, b->size * sizeof(fp_t), cudaMemcpyHostToDevice);
Matrix* d_last_x = device_matrix_from(last_x);
Matrix* d_c = device_matrix_from(b);
Matrix* d_x = device_matrix_from(x);
cudaMemcpy(d_x->elements, x->elements, x->size * sizeof(fp_t), cudaMemcpyHostToDevice);
cudaMemcpy(d_last_x->elements, last_x->elements, last_x->size * sizeof(fp_t), cudaMemcpyHostToDevice);
fp_t x_diff = 2 * min_diff;
fp_t* d_x_diff;
cudaMalloc((void**)&d_x_diff, sizeof(fp_t));
//fp_t stime;
while ((x_diff > min_diff) && (max_iter < 0 || iterations < max_iter)) {
//stime = omp_get_wtime();
cuda_multiply(*d_A, *d_last_x, *d_c);
//print_cuda_elapsed(stime);
//stime = omp_get_wtime();
cuda_reduce(*d_A, *d_b, *d_c, d_x, d_last_x, alpha); //performs swap
//print_cuda_elapsed(stime);
//stime = omp_get_wtime();
cuda_diff(*d_x, *d_last_x, d_x_diff);
//print_cuda_elapsed(stime);
iterations++;
//cudaMemcpyFromSymbol(&x_diff, "d_x_diff", sizeof(x_diff), 0, cudaMemcpyDeviceToHost);
//stime = omp_get_wtime();
cudaMemcpy(&x_diff, d_x_diff, sizeof(fp_t), cudaMemcpyDeviceToHost);
//print_cuda_elapsed(stime);
}
// copy last_x instead, as it was swapped
cudaMemcpy(x->elements, d_last_x->elements, x->size * sizeof(fp_t), cudaMemcpyDeviceToHost);
#ifndef DOUBLE
#ifdef TEXTURE
texunbind();
#endif
#endif
cudaFree(d_A->elements);
cudaFree(d_b->elements);
cudaFree(d_last_x->elements);
cudaFree(d_c->elements);
cudaFree(d_x->elements);
cudaFree(d_x_diff);
free(d_A);
free(d_b);
free(d_c);
free(d_last_x);
free(d_x);
}
const fp_t end_time = omp_get_wtime();
const fp_t seconds_spent = end_time - start_time;
exec_times[k] = seconds_spent;
if (verbose) {
printf("x: ");
print_matrix(x);
printf("expected_x: ");
print_matrix(dataset.x);
//print_matrix(dataset.A);
//print_matrix(dataset.b);
}
Matrix* bx = aligned_vector(M, false);
for (int i = 0; i < M; i++) {
for (int j = 0; j < M; j++) {
bx->elements[i] += A->elements[i * M + j] * x->elements[j];
}
}
if (verbose) {
printf("resulting b: ");
print_matrix(bx);
}
all_rmse[k] = rmse(bx, b);
printf("RMSE: %0.10f\n", all_rmse[k]);
printf("iterations: %d\nseconds: %0.10f\n", iterations, seconds_spent);
assert(x != last_x);
free(bx->elements);
free(x->elements);
free(last_x->elements);
free(dataset.x->elements);
free(dataset.A->elements);
free(dataset.b->elements);
free(bx);
free(x);
free(last_x);
free(dataset.x);
free(dataset.A);
free(dataset.b);
}
printf("Time: mean %0.10f std %0.10f\n", array_mean(exec_times, tests), array_std(exec_times, tests));
printf("RMSE: mean %0.10f std %0.10f\n", array_mean(all_rmse, tests), array_std(all_rmse, tests));
free(all_rmse);
free(exec_times);
return 0;
}
void factorize(double *v, int row, int col, int features, unsigned int count, MatrixFactor *mf)
{
// 重みの行列と特徴の行列をランダムな値で初期化.
double * h = new double[features * col];
double * hn = new double[features * col];
double * hd = new double[features * col];
double * hd_t = new double[features * features];
double * w = new double[row * features];
double * wn = new double[row * features];
double * wd = new double[row * features];
double * wd_t = new double[row * col];
// 重みと特徴の乗算結果用行列.
double * wh = new double[row * col];
matrix_init_zero((double*)wh, row, col);
matrix_init(h, features, col);
matrix_init_zero(hn, features, col);
matrix_init_zero(hd, features, col);
matrix_init_zero(hd_t, features, features);
matrix_init(w, row, features);
matrix_init_zero(wn, row, features);
matrix_init_zero(wd, row, features);
matrix_init_zero(wd_t, row, col);
//matrix_print((double*)h, features, col);
//matrix_print((double*)w, raw, features);
unsigned int i = 0;
for (i = 0; i < count; i++){
// 特徴の重みの行列の積から、元データとの差を計算する.
matrix_x(wh, w, h, row, features, col);
int cost = matrix_diff(v, wh, row, col);
// 差が完全にゼロになったらループを抜ける.
if (cost == 0){
break;
}
//-------------------------------------------------------------------------------------------
// 特徴の行列を更新する.
//-------------------------------------------------------------------------------------------
// hn 転置した重みの行列にデータ行列を掛け合わせたもの
matrix_tx_left(hn, w, v, features, row, col);
// hd 転置した重みの行列に重みの行列を掛け合わせたものに特徴の行列を掛け合わせたもの
matrix_tx_left(hd_t, w, w, features, row, features);
matrix_x(hd, hd_t, h, features, features, col);
matrix_x(h, h, hn, features, col);
matrix_divide(h, h, hd, features, col);
//-------------------------------------------------------------------------------------------
// 重みの行列を更新する.
//-------------------------------------------------------------------------------------------
// wn データ行列に転置した特長の行列を掛け合わせたもの.
matrix_tx_right(wn, v, h, row, col, features);
// wd 重みの行列に、特徴の行列を掛け合わせたものに転置した特長の行列を掛け合わせたもの.
matrix_x(wd_t, w, h, row, features, col);
matrix_tx_right(wd, wd_t, h, row, col, features);
matrix_x(w, w, wn, row, features);
matrix_divide(w, w, wd, row, features);
}
mf->h = h;
mf->w = w;
//matrix_x(wh, w, h, ROW, FEATURES, COL);
//matrix_print(wh, ROW, COL);
//matrix_print(w, ROW, FEATURES);
//matrix_print(h, FEATURES, COL);
delete[] hn;
delete[] hd;
delete[] hd_t;
delete[] wn;
delete[] wd;
delete[] wd_t;
delete[] wh;
}
/* Align two sets of points with a 2D similarity transform */
int align_2D_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 2
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Tbest[9], R[9], T[9], scale;
int i;
if (n < 3) {
printf("[align_2D_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_mean, l_mean, r0, l0;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9], T1tmp[9], T2tmp[9], tmp[9], Tout[9];
double a, b;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
r_mean = v3_scale(0.5, v3_add(r_pts_small[0], r_pts_small[1]));
l_mean = v3_scale(0.5, v3_add(l_pts_small[0], l_pts_small[1]));
r0 = v3_sub(r_pts_small[0], r_mean);
// v3_t r1 = v3_sub(r_pts_small[1], mean);
l0 = v3_sub(l_pts_small[0], l_mean);
// v3_t l1 = v3_sub(l_pts_small[1], mean);
a = (Vy(r0) + Vx(r0) * Vx(l0) / Vy(l0)) /
(Vy(l0) + Vx(l0) * Vx(l0) / Vy(l0));
b = (Vx(r0) - a * Vx(l0)) / Vy(l0);
Rtmp[0] = a; Rtmp[1] = b; Rtmp[2] = 0.0;
Rtmp[3] = -b; Rtmp[4] = a; Rtmp[5] = 0.0;
Rtmp[6] = 0; Rtmp[7] = 0; Rtmp[8] = 1.0;
matrix_ident(3, T1tmp);
T1tmp[2] = -Vx(l_mean);
T1tmp[5] = -Vy(l_mean);
matrix_ident(3, T2tmp);
T2tmp[2] = Vx(r_mean);
T2tmp[5] = Vy(r_mean);
matrix_product(3, 3, 3, 3, Rtmp, T1tmp, tmp);
matrix_product(3, 3, 3, 3, T2tmp, tmp, Tout);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tout, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 9);
// printf(" inliers_new: %d\n", num_inliers);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tbest, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
// align_horn(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
align_2D(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
#else
memcpy(Tret, Tbest, 9 * sizeof(double));
#endif
free(r_inliers);
free(l_inliers);
return num_inliers;
#undef MIN_SUPPORT
}
void LineSegment3D::Render(const CameraInfo &camera, double max_width,
int stroke_texture, ParameterBound stroke_bounds)
{
#ifndef __BUNDLER__
#ifndef __DEMO__
#if 1
/* Project the line into the camera */
double p1[4] = { m_p1[0], m_p1[1], m_p1[2], 1.0 };
double p2[4] = { m_p2[0], m_p2[1], m_p2[2], 1.0 };
double proj1[3], proj2[3];
matrix_product(3, 4, 4, 1, (double *) camera.m_Pmatrix, p1, proj1);
matrix_product(3, 4, 4, 1, (double *) camera.m_Pmatrix, p2, proj2);
if (proj1[2] >= 0.0 || proj2[2] >= 0.0)
return;
double width = CLAMP(5.0 / (-proj1[2] - proj2[2]), 0.5, max_width);
proj1[0] /= -proj1[2];
proj1[1] /= -proj1[2];
proj2[0] /= -proj2[2];
proj2[1] /= -proj2[2];
#endif
#if 0
bool in_front1 = camera.Project(m_p1, proj1);
bool in_front2 = camera.Project(m_p2, proj2);
if (!in_front1 || !in_front2)
return;
#endif
/* Draw a line segment whose width is proportional to the distance
* from the camera */
#if 0
double pos[3];
camera.GetPosition(pos);
double disp[3];
matrix_diff(3, 1, 3, 1, pos, m_p1, disp);
double dist = matrix_norm(3, 1, disp);
#endif
if (stroke_texture != -1) {
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, stroke_texture);
}
#if 0
#define LINE_WIDTH_SIGMA 1.0
double width =
max_width * exp(-dist * dist / (LINE_WIDTH_SIGMA * LINE_WIDTH_SIGMA));
#endif
/* Create a stroke */
Stroke s;
s.radius() = 0.5 * width;
s.cap() = false;
s.depth() = 1.0;
if (stroke_texture == -1) {
s.useTexture() = false;
s.color() = makeColor(0, 0, 0, 0.9f);
} else {
GLubyte b = 0x0;
s.useTexture() = true;
s.color() = makeColor(b, b, b, 0.9f);
}
s.addControlPoint(proj1[0], proj1[1]);
s.addControlPoint(proj2[0], proj2[1]);
s.render();
if (stroke_texture != -1) {
glDisable(GL_TEXTURE_2D);
}
#endif /* __DEMO__ */
#endif /* __BUNDLER__ */
}
/* Align two sets of points with a 2D similarity transform */
int align_horn_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 3
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Tbest[9], R[9], T[9], scale;
int i;
if (n < 3) {
printf("[align_horn_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9], Ttmp[9], Tout[9], scale_tmp;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
align_horn(MIN_SUPPORT, r_pts_small, l_pts_small, Rtmp, Ttmp,
Tout, &scale_tmp, NULL);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tout, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 9);
// printf(" inliers_new: %d\n", num_inliers);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tbest, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
// printf(" inliers: %d\n", num_inliers);
align_horn(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
#else
memcpy(Tret, Tbest, 9 * sizeof(double));
#endif
free(r_inliers);
free(l_inliers);
return num_inliers;
#undef MIN_SUPPORT
}
/* 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);
}
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;
}
/* Align two sets of points with a 3D similarity transform */
int align_horn_3D_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 3
v3_t *l_inliers, *r_inliers;
double *Vp, *TVp;
int num_inliers, max_inliers = 0;
double Tbest[16], TbestT[16];
int i;
double *ptr;
double ransac_threshsq = ransac_thresh * ransac_thresh;
if (n < MIN_SUPPORT) {
printf("[align_horn_3D_ransac] Error: need at least %d points!\n",
MIN_SUPPORT);
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
Vp = (double *) malloc(sizeof(double) * 4 * n);
TVp = (double *) malloc(sizeof(double) * 4 * n);
for (i = 0; i < n; i++) {
memcpy(Vp + 4 * i, l_pts[i].p, 3 * sizeof(double));
Vp[4 * i + 3] = 1.0;
}
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Tout[16], ToutT[16];
int nan = 0;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
align_horn_3D_2(MIN_SUPPORT, r_pts_small, l_pts_small, 1, Tout);
#if 1
for (i = 0; i < 16; i++) {
if (isnan(Tout[i]) || Tout[i] != Tout[i]) {
nan = 1;
break;
}
}
if (nan == 1)
continue;
#endif
/* Count inliers */
num_inliers = 0;
#if 0
for (i = 0; i < n; i++) {
double Tp[4];
double diff[3];
double dist;
double p[4] = { l_pts[i].p[0], l_pts[i].p[1], l_pts[i].p[2], 1.0 };
matrix_product(4, 4, 4, 1, Tout, p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
}
#else
matrix_transpose(4, 4, Tout, ToutT);
matrix_product_old(n, 4, 4, 4, Vp, ToutT, TVp);
ptr = TVp;
for (i = 0; i < n; i++) {
// double diff[3], dist;
double dx, dy, dz, dist;
// matrix_diff(3, 1, 3, 1, TVp + 4 * i, r_pts[i].p, diff);
dx = ptr[0] - r_pts[i].p[0];
dy = ptr[1] - r_pts[i].p[1];
dz = ptr[2] - r_pts[i].p[2];
dist = dx * dx + dy * dy + dz * dz; // matrix_normsq(3, 1, diff);
if (dist < ransac_threshsq)
num_inliers++;
ptr += 4;
}
#endif
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 16);
}
}
/* Reestimate using all inliers */
#if 0
matrix_transpose(4, 4, Tbest, TbestT);
matrix_product(n, 4, 4, 4, Vp, TbestT, TVp);
num_inliers = 0;
for (i = 0; i < n; i++) {
// double Tp[4];
double diff[3];
double dist;
matrix_diff(3, 1, 3, 1, TVp + 4 * i, r_pts[i].p, diff);
// double p[4] = { l_pts[i].p[0], l_pts[i].p[1], l_pts[i].p[2], 1.0 };
// matrix_product(4, 4, 4, 1, Tbest, p, Tp);
// matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_normsq(3, 1, diff);
if (dist < ransac_threshsq) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
align_horn_3D_2(num_inliers, r_inliers, l_inliers, 1, Tret);
// memcpy(Tret, Tbest, 16 * sizeof(double));
if (isnan(Tret[0]) || Tret[0] != Tret[0]) {
printf("[align_horn_3D_ransac] nan at end [num_inliers: %d], "
"restoring old matrix\n", num_inliers);
memcpy(Tret, Tbest, sizeof(double) * 16);
}
#else
memcpy(Tret, Tbest, sizeof(double) * 16);
#endif
free(r_inliers);
free(l_inliers);
free(Vp);
free(TVp);
return max_inliers;
#undef MIN_SUPPORT
}
void mtx_diff(float32_t eps) {
float32_t errf;
ambix_matrix_t *left=NULL, *right=NULL;
unsigned int i;
const unsigned int rows=4;
const unsigned int cols=3;
float32_t*leftdata=leftdata_4_3;
float32_t*rightdata=malloc(sizeof(leftdata_4_3));
float32_t maxeps=eps;
STARTTEST("\n");
left=ambix_matrix_create();
right=ambix_matrix_create();
/* comparisons:
- failing tests:
- different dimensions
- left/right matrix is NULL
- non-failing tests:
- all values diff==0
- all values diff<eps
- few values diff<eps
- many values diff<eps
*/
fail_if((left !=ambix_matrix_init(3, 4, left )), __LINE__, "initializing left matrix failed");
fail_if((right!=ambix_matrix_init(3, 4, right)), __LINE__, "initializing right matrix failed");
/* compare equal matrices */
STARTTEST("ident\n");
ambix_matrix_fill_data(left, leftdata);
errf=matrix_diff(__LINE__, left, left, eps);
fail_if(errf>0.f, __LINE__, "diffing mtx with itself returned %g (>%g)", errf, 0.f);
/* compare equal matrices */
STARTTEST("equal\n");
for(i=0; i<rows*cols; i++) {
rightdata[i]=leftdata[i];
}
ambix_matrix_fill_data(left , leftdata);
ambix_matrix_fill_data(right, rightdata);
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf>0.f, __LINE__, "diffing mtx with copy returned %g (>%g)", errf, 0.f);
/* compare matrices where all values differ, but <eps */
STARTTEST("all<eps\n");
for(i=0; i<rows*cols; i++) {
rightdata[i]=leftdata[i]+eps*0.5;
}
ambix_matrix_fill_data(left , leftdata);
ambix_matrix_fill_data(right, rightdata);
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf>eps, __LINE__, "diffing mtx with mtx+eps/2 returned %g (>%g)", errf, eps);
for(i=0; i<rows*cols; i++) {
rightdata[i]=leftdata[i]-eps*0.5;
}
ambix_matrix_fill_data(left , leftdata);
ambix_matrix_fill_data(right, rightdata);
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf>eps, __LINE__, "diffing mtx with mtx-eps/2 returned %g (>%g)", errf, eps);
/* compare matrices where many values differ with <eps; but one with >eps */
STARTTEST("most<eps;one>eps\n");
for(i=0; i<rows*cols; i++) {
rightdata[i]=leftdata[i];
}
for(i=0; i<rows; i++) {
rightdata[i]=leftdata[i]+eps*0.5;
}
rightdata[0]=leftdata[0]+eps*1.5;
ambix_matrix_fill_data(left , leftdata);
ambix_matrix_fill_data(right, rightdata);
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf>(eps*2.0), __LINE__, "diffing mtx with one value>eps returned %g (>%g)", errf, eps);
fail_if(errf<(eps*1.0), __LINE__, "diffing mtx with one value>eps returned %g (>%g)", errf, eps);
/* compare matrices where most values differ with >eps */
STARTTEST("most>eps\n");
for(i=0; i<rows*cols; i++) {
rightdata[i]=leftdata[i];
}
maxeps=eps*1.5;
for(i=0; i<(rows*cols)-1; i++) {
rightdata[i]=leftdata[i]-maxeps;
}
ambix_matrix_fill_data(left , leftdata);
ambix_matrix_fill_data(right, rightdata);
errf=matrix_diff(__LINE__, left, right, eps);
fail_if(errf<eps*1.0, __LINE__, "diffing mtx with one value>eps returned %g (<%g)", errf, eps*1.0);
fail_if(errf>eps*2.0, __LINE__, "diffing mtx with one value>eps returned %g (<%g)", errf, eps*2.0);
ambix_matrix_destroy(left);
ambix_matrix_destroy(right);
free(rightdata);
STOPTEST("\n");
}
void check_create_extended(const char*path, ambix_sampleformat_t format, uint32_t chunksize, float32_t eps) {
ambix_info_t info, rinfo, winfo;
ambix_t*ambix=NULL;
float32_t*orgambidata,*ambidata,*resultambidata;
float32_t*orgotherdata,*otherdata,*resultotherdata;
uint32_t framesize=441000;
uint32_t ambichannels=4;
uint32_t extrachannels=2;
float32_t periods=20000;
ambix_matrix_t eye={0,0,NULL};
const ambix_matrix_t*eye2=NULL;
int64_t err64, gotframes;
float32_t diff=0.;
STARTTEST("\n");
printf("test using '%s' [%d] with chunks of %d and eps=%f\n", path, (int)format, (int)chunksize, eps);
resultambidata=(float32_t*)calloc(ambichannels*framesize, sizeof(float32_t));
ambidata=(float32_t*)calloc(ambichannels*framesize, sizeof(float32_t));
resultotherdata=(float32_t*)calloc(extrachannels*framesize, sizeof(float32_t));
otherdata=(float32_t*)calloc(extrachannels*framesize, sizeof(float32_t));
ambix_matrix_init(ambichannels, ambichannels, &eye);
ambix_matrix_fill(&eye, AMBIX_MATRIX_IDENTITY);
memset(&winfo, 0, sizeof(winfo));
memset(&info, 0, sizeof(info));
info.fileformat=AMBIX_EXTENDED;
info.ambichannels=ambichannels;
info.extrachannels=extrachannels;
info.samplerate=44100;
info.sampleformat=format;
memcpy(&rinfo, &info, sizeof(info));
ambix=ambix_open(path, AMBIX_WRITE, &rinfo);
fail_if((NULL==ambix), __LINE__, "couldn't create ambix file '%s' for writing", path);
orgambidata=data_sine (FLOAT32, framesize, ambichannels, periods);
orgotherdata=data_ramp(FLOAT32, framesize, extrachannels);
//data_print(FLOAT32, orgdata, 100);
fail_if((NULL==orgambidata), __LINE__, "couldn't create ambidata %dx%d sine @ %f", (int)framesize, (int)ambichannels, (float)periods);
fail_if((NULL==orgotherdata), __LINE__, "couldn't create otherdata %dx%d sine @ %f", (int)framesize, (int)extrachannels, (float)periods);
memcpy(ambidata, orgambidata, framesize*ambichannels*sizeof(float32_t));
memcpy(otherdata, orgotherdata, framesize*extrachannels*sizeof(float32_t));
fail_if((AMBIX_ERR_SUCCESS!=ambix_set_adaptormatrix(ambix, &eye)),
__LINE__, "failed setting adaptor matrix");
if(chunksize>0) {
uint32_t subframe=chunksize;
uint32_t chunks = framesize/chunksize;
uint32_t framesleft=framesize;
uint32_t frame;
printf("writing %d chunks of %d frames\n", (int)chunks, (int)chunksize);
for(frame=0; frame<chunks; frame++) {
err64=ambix_writef_float32(ambix, ambidata+ambichannels*frame*chunksize, otherdata+extrachannels*frame*chunksize, chunksize);
fail_if((err64!=chunksize), __LINE__, "wrote only %d chunksize of %d", (int)err64, (int)chunksize);
framesleft-=chunksize;
}
subframe=framesleft;
printf("writing rest of %d frames\n", (int)subframe);
err64=ambix_writef_float32(ambix, ambidata+ambichannels*frame*chunksize, otherdata+extrachannels*frame*chunksize, subframe);
fail_if((err64!=subframe), __LINE__, "wrote only %d subframe of %d", (int)err64, (int)subframe);
} else {
err64=ambix_writef_float32(ambix, ambidata, otherdata, framesize);
fail_if((err64!=framesize), __LINE__, "wrote only %d frames of %d", (int)err64, (int)framesize);
}
diff=data_diff(__LINE__, FLOAT32, orgambidata, ambidata, framesize*ambichannels, eps);
fail_if((diff>eps), __LINE__, "ambidata diff %f > %f", diff, eps);
diff=data_diff(__LINE__, FLOAT32, orgotherdata, otherdata, framesize*extrachannels, eps);
fail_if((diff>eps), __LINE__, "otherdata diff %f > %f", diff, eps);
fail_if((AMBIX_ERR_SUCCESS!=ambix_close(ambix)), __LINE__, "closing ambix file %p", ambix);
ambix=NULL;
/* read data back */
ambix=ambix_open(path, AMBIX_READ, &rinfo);
fail_if((NULL==ambix), __LINE__, "couldn't create ambix file '%s' for reading", path);
fail_if((info.fileformat!=rinfo.fileformat), __LINE__, "fileformat mismatch %d!=%d", (int)info.fileformat, (int)rinfo.fileformat);
fail_if((info.samplerate!=rinfo.samplerate), __LINE__, "samplerate mismatch %g!=%g", (float)info.samplerate, (float)rinfo.samplerate);
fail_if((info.sampleformat!=rinfo.sampleformat), __LINE__, "sampleformat mismatch %d!=%d", (int)info.sampleformat, (int)rinfo.sampleformat);
fail_if((info.ambichannels!=rinfo.ambichannels), __LINE__, "ambichannels mismatch %d!=%d", (int)info.ambichannels, (int)rinfo.ambichannels);
fail_if((info.extrachannels!=rinfo.extrachannels), __LINE__, "extrachannels mismatch %d!=%d", (int)info.extrachannels, (int)rinfo.extrachannels);
eye2=ambix_get_adaptormatrix(ambix);
fail_if((NULL==eye2), __LINE__, "failed reading adaptor matrix");
diff=matrix_diff(__LINE__, &eye, eye2, eps);
fail_if((diff>eps), __LINE__, "adaptormatrix diff %f > %f", diff, eps);
gotframes=0;
do {
//err64=ambix_readf_float32(ambix, resultambidata, resultotherdata, framesize);
err64=ambix_readf_float32(ambix,
resultambidata +(gotframes*ambichannels ),
resultotherdata+(gotframes*extrachannels),
(framesize-gotframes));
fail_if((err64<0), __LINE__, "reading frames failed after %d/%d frames", (int)gotframes, (int)framesize);
gotframes+=err64;
} while(err64>0 && gotframes<framesize);
diff=data_diff(__LINE__, FLOAT32, orgambidata, resultambidata, framesize*ambichannels, eps);
fail_if((diff>eps), __LINE__, "ambidata diff %f > %f", diff, eps);
diff=data_diff(__LINE__, FLOAT32, orgotherdata, resultotherdata, framesize*extrachannels, eps);
fail_if((diff>eps), __LINE__, "otherdata diff %f > %f", diff, eps);
fail_if((AMBIX_ERR_SUCCESS!=ambix_close(ambix)), __LINE__, "closing ambix file %p", ambix);
ambix=NULL;
free(resultambidata);
free(ambidata);
free(resultotherdata);
free(otherdata);
free(orgambidata);
free(orgotherdata);
ambix_matrix_deinit(&eye);
ambixtest_rmfile(path);
}
/* Align two sets of points with a 3D rotation */
int align_3D_rotation_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *R)
{
int round;
double error = 0.0;
#define MIN_SUPPORT 3
// const int min_support = 3;
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Rbest[9];
int i;
if (n < 3) {
printf("[align_3D_rotation_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9];
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
align_3D_rotation(MIN_SUPPORT, r_pts_small, l_pts_small, Rtmp);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Rtmp, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Rbest, Rtmp, sizeof(double) * 9);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Rbest, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
error = align_3D_rotation(num_inliers, r_inliers, l_inliers, R);
printf("[align_3D_rotation] Error: %0.3f\n", error);
free(r_inliers);
free(l_inliers);
return num_inliers;
#else
memcpy(R, Rbest, 9 * sizeof(double));
free(r_inliers);
free(l_inliers);
return max_inliers;
#endif
#undef MIN_SUPPORT
}
