Ejemplo n.º 1
0
/* Scale the vector so that the 3rd coordinate is 1 */
v3_t v3_homogenize(const v3_t v) {
    if (Vz(v) == 0.0) {
	return v3_new(DBL_MAX, DBL_MAX, 1.0);
    } else {
	return v3_new(Vx(v) / Vz(v), Vy(v) / Vz(v), 1.0);
    }
}
Ejemplo n.º 2
0
void BaseApp::ReadPointConstraints()
{
    FILE *f = fopen(m_point_constraint_file, "r");
    
    if (f == NULL) {
	printf("[ReadPointConstraints] Error opening file %s "
	       "for reading\n", m_point_constraint_file);
	return;
    }

    int num_points = (int) m_point_data.size();
    m_point_constraints = new v3_t[num_points];

    for (int i = 0; i < num_points; i++) {
	m_point_constraints[i] = v3_new(0.0, 0.0, 0.0);
    }

    char buf[256];
    while (fgets(buf, 256, f) != NULL) {
	double x0, y0, z0;
	double x, y, z;
	sscanf(buf, "%lf %lf %lf %lf %lf %lf", &x0, &y0, &z0, &x, &y, &z);

	int pt_idx = -1;
	double min_dist = DBL_MAX;
	for (int i = 0; i < num_points; i++) {
	    double dx = m_point_data[i].m_pos[0] - x0;
	    double dy = m_point_data[i].m_pos[1] - y0;
	    double dz = m_point_data[i].m_pos[2] - z0;

	    double dsq = dx * dx + dy * dy + dz * dz;
	    
	    if (dsq < min_dist) {
		pt_idx = i;
		min_dist = dsq;
	    }
	}

	m_point_constraints[pt_idx] = v3_new(x, y, -z);

	printf("[ReadPointConstraints] Constraining %d: "
	       "%0.3f %0.3f %0.3f (%0.3f %0.3f %0.3f) => %0.3f %0.3f %0.3f\n",
	       pt_idx, 
	       m_point_data[pt_idx].m_pos[0], 
	       m_point_data[pt_idx].m_pos[1], 
	       m_point_data[pt_idx].m_pos[2], 
	       x0, y0, z0, x, y, z);
    }
}
Ejemplo n.º 3
0
v3_t FindRobustMean(const std::vector<v3_t> &points) 
{
    int num_points = (int) points.size();
    double best_sum = DBL_MAX;
    int best_idx = -1;

    for (int i = 0; i < num_points; i++) {
	double sum = 0.0;

	for (int j = 0; j < num_points; j++) {
	    v3_t diff = v3_sub(points[i], points[j]);
	    
	    sum += fabs(Vx(diff)) + fabs(Vy(diff)) + fabs(Vz(diff));
	}

	if (sum < best_sum) {
	    best_sum = sum;
	    best_idx = i;
	}
    }

    if (best_idx == -1)
	return v3_new(0.0, 0.0, 0.0);
    
    return points[best_idx];
}
Ejemplo n.º 4
0
Archivo: sim.c Proyecto: piotrm0/progs
GLvoid init_scene(GLvoid) {
  u_int i;
  // u_int d;

  grav_vector = v3_new(0,-g,0);
  gr = v3_copy(grav_vector);

  glutTimerFunc(100, update_world, 0);

  view_main = view_new(0,
		       VIEW_NONE,
		       GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT,
		       0, 0,
		       1, 1);
  view_main->draw_func = draw_scene;
  for (i = 0; i < PARTICLES; i++) {
            ps[i].c = v3_new(2 * B * frandom() - B,
			     2 * B * frandom() - B,
			     2 * B * frandom() - B);
	    
	            ps[i].v = v3_new((frandom() * 2 - 1),
	        		     (frandom() * 2 - 1),
	        		     (frandom() * 2 - 1));
	    //  ps[i].v = v3_new(0,0,0);
	    
  // ps[i].c = v3_new(0,//frandom() * 0.001 - 0.0005,
  //	   B - RADIUS - i * RADIUS * 2 * 1.01,
  //	   0);//frandom() * 0.001 - 0.0005);
  //	    
  }
  /*
    ps[0].v = v3_new(3,0,0);
    ps[1].v = v3_new(-3,0,0);
    ps[0].c = v3_new(-B + RADIUS, -B * 0 + RADIUS, 0);
    ps[1].c = v3_new(B - RADIUS, -B * 0 + RADIUS, 0);
  */

  timer_frames = timer_new();
  timer_bounce = timer_new();

  mutex_frames = 0;

  timer_start(timer_frames);
  timer_start(timer_bounce);
}
Ejemplo n.º 5
0
/* Compute the mean of a set of vectors */
v3_t v3_mean(int n, const v3_t *v) {
    int i;
    v3_t mean = v3_new(0.0, 0.0, 0.0);
    
    for (i = 0; i < n; i++) {
	mean = v3_add(mean, v[i]);
    }

    return v3_scale(1.0 / n, mean);
}
Ejemplo n.º 6
0
/* Returns true if the image becomes disconnected under the given
 * homography */
int img_disconnected_under_homography(img_t *img, double *H) {
    /* Check if any point in the img maps to infinity under H.  This
     * is true for p if Hp = [x y 0], so if w is the third row of H, 
     * w^T * p = 0.  We check if this line intersects the image */

    v2_t origin = img->origin;
    int w = img->w, h = img->h;

    v3_t line = v3_new(H[6], H[7], H[8]);

    /* The bottom line is where y = Vy(origin) */
    v3_t bottom_line = v3_cross(v3_new(0.0, Vy(origin), 1.0), 
				v3_new(1.0, Vy(origin), 1.0));
    v3_t top_line = v3_cross(v3_new(0.0, Vy(origin) + h - 1, 1.0),
			     v3_new(1.0, Vy(origin) + h - 1, 1.0));
    v3_t left_line = v3_cross(v3_new(Vx(origin), 0.0, 1.0),
			      v3_new(Vx(origin), 1.0, 1.0));
    v3_t right_line = v3_cross(v3_new(Vx(origin) + w - 1, 0.0, 1.0),
			       v3_new(Vx(origin) + w - 1, 1.0, 1.0));

    v3_t isect;
    
    /* Do four intersection tests */
    isect = v3_homogenize(v3_cross(line, bottom_line));
    if (Vx(isect) > 0 && Vx(isect) < w - 1)
	return 1;
    
    isect = v3_homogenize(v3_cross(line, top_line));
    if (Vx(isect) > 0 && Vx(isect) < w - 1)
	return 1;

    isect = v3_homogenize(v3_cross(line, left_line));
    if (Vy(isect) > 0 && Vy(isect) < h - 1)
	return 1;

    isect = v3_homogenize(v3_cross(line, right_line));
    if (Vy(isect) > 0 && Vy(isect) < h - 1)
	return 1;

    return 0;
}
Ejemplo n.º 7
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]);
}
Ejemplo n.º 8
0
/* Compute the "median" of a set of vectors */
v3_t v3_median(int n, const v3_t *v) {
    int i;
    v3_t median = v3_new(0.0, 0.0, 0.0);
    double min_dist = DBL_MAX;

    for (i = 0; i < n; i++) {
        double dist = 0.0;
        int j;

        for (j = 0; j < n; j++) {
            dist += v3_mag(v3_sub(v[j], v[i]));
        }
        
        if (dist < min_dist) {
            min_dist = dist;
            median = v[i];
        }
    }

    return median;
}
Ejemplo n.º 9
0
static v3_t *
condition_points(int num_points, v3_t *pts, double *T)
{
  v3_t *pts_new = (v3_t *) malloc(sizeof(v3_t) * num_points);

  v3_t mean = v3_mean(num_points, pts);
  double total_dist = 0.0;
  double avg_dist;
  double factor;
  int i;

  for (i = 0; i < num_points; i++)
    {
      double dx = Vx(pts[i]) - Vx(mean);
      double dy = Vy(pts[i]) - Vy(mean);
      total_dist += sqrt(dx * dx + dy * dy);
    }

  avg_dist = total_dist / num_points;
  factor = sqrt(2.0) / avg_dist;

  for (i = 0; i < num_points; i++)
    {
      double x = factor * (Vx(pts[i]) - Vx(mean));
      double y = factor * (Vy(pts[i]) - Vy(mean));
      pts_new[i] = v3_new(x, y, 1.0);
    }

  T[0] = factor;
  T[1] = 0.0;
  T[2] = -factor * Vx(mean);
  T[3] = 0.0;
  T[4] = factor;
  T[5] = -factor * Vy(mean);
  T[6] = 0.0;
  T[7] = 0.0;
  T[8] = 1.0;

  return pts_new;
}
Ejemplo n.º 10
0
v3_t v3_extremum2(int n, const v3_t *a, const v3_t u, v3_t v) {
    int i;
    int max_idx = -1;
    double max_dist = 0.0;

    for (i = 0; i < n; i++) {
	v3_t diff1 = v3_sub(a[i], u);
	v3_t diff2 = v3_sub(a[i], v);
	double distsq1 = v3_magsq(diff1);
	double distsq2 = v3_magsq(diff2);
	if (MIN(distsq1, distsq2) > max_dist) {
	    max_idx = i;
	    max_dist = MIN(distsq1, distsq2);
	}
    }

    if (max_idx == -1) {
	printf("[v3_extremum2] Couldn't find extremum!\n");
	return v3_new(0.0, 0.0, 0.0);
    }

    return a[max_idx];
}
Ejemplo n.º 11
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_2(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 Tcenter[16] = { 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], R3x3[9];

    double sum_num, sum_den, scale, RMS_sum;

    v3_t *left_pts_zm  = malloc(sizeof(v3_t) * n);
    v3_t *right_pts_zm = malloc(sizeof(v3_t) * n);

    double error = 0.0;

    /* 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);

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

        right_pts_zm[i] = r;
        left_pts_zm[i]  = v3_scale(scale, l);
    }

    /* Compute the rotation */
    error = align_3D_rotation(n, right_pts_zm, left_pts_zm, R3x3);
    // printf("error[%d]: %0.3f\n", n, error);
    // matrix_print(3, 3, R3x3);

    /* Fill in the rotation matrix */
    R[0]  = R3x3[0]; R[1]  = R3x3[1]; R[2]  = R3x3[2]; R[3]  = 0.0;
    R[4]  = R3x3[3]; R[5]  = R3x3[4]; R[6]  = R3x3[5]; R[7]  = 0.0;
    R[8]  = R3x3[6]; R[9]  = R3x3[7]; R[10] = R3x3[8]; 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[0] = 1.0;  T[1] = 0.0;  T[2] = 0.0;  T[3]  = Vx(right_centroid);
    T[4] = 0.0;  T[5] = 1.0;  T[6] = 0.0;  T[7]  = Vy(right_centroid);
    T[8] = 0.0;  T[9] = 0.0;  T[10] = 1.0; T[11] = Vz(right_centroid);
    T[12] = 0.0; T[13] = 0.0; T[14] = 0.0; T[15] = 1.0;

    if (scale_xform == 0)
	scale = 1.0;

    Tcenter[0] = scale;
    Tcenter[5] = scale;
    Tcenter[10] = scale;
    
    Tcenter[3] = -scale * Vx(left_centroid);
    Tcenter[7] = -scale * Vy(left_centroid);
    Tcenter[11] = -scale * Vz(left_centroid);

    matrix_product44(T, R, (double *) Ttmp);
    matrix_product44((double *)Ttmp, (double *)Tcenter, Tout);

    /* 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_product441(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;
    }

    free(left_pts_zm);
    free(right_pts_zm);
    
    return sqrt(RMS_sum / n);
}
Ejemplo n.º 12
0
v3_t v3_add(const v3_t u, const v3_t v) {
    return v3_new(Vx(u) + Vx(v), Vy(u) + Vy(v), Vz(u) + Vz(v));
}
Ejemplo n.º 13
0
v3_t v3_cross(const v3_t u, const v3_t v) {
    return v3_new(Vy(u) * Vz(v) - Vz(u) * Vy(v),
                  Vz(u) * Vx(v) - Vx(u) * Vz(v),
                  Vx(u) * Vy(v) - Vy(u) * Vx(v));
}
Ejemplo n.º 14
0
Archivo: sim.c Proyecto: piotrm0/progs
GLvoid sample_world() {
  //  int timing;
  u_int i,j,d;
  //  float t0,t1,t2;

  v3* temp;
  v3* disp;
  //  v3* disp2;
  float dot;
  float dm;

  char* temp_s1;
  char* temp_s2;

  for (i = 0; i < PARTICLES; i++) {
    //    printf("adding g %d,%f\n", i,ps[i].v[1]);
    v3_add(ps[i].v, gr);
    //    ps[i].v->v[1] -= g;

    v3_mult(ps[i].v, LOSS_V);
    for (d = 0; d < D; d++) {
      v3_mult_add(ps[i].c, ps[i].v, 1 / (float) SPS); //  ps[i].c[d] += ps[i].v[d] / (float) SPS;
      if (ps[i].c->v[d] < -B + RADIUS) {

	disp = v3_new(0,0,0);
	disp->v[d] = ps[i].c->v[d] + B;
	dm = 1 / (1 - (RADIUS - v3_mag(disp)) / RADIUS);
	v3_unit(disp);

	v3_mult_add(ps[i].v, disp, dm * LOSS_WALL * dt);

	//	ps[i].c->v[d] = -B + RADIUS;
	//	ps[i].v->v[d] *= -1 * LOSS_WALL;

      }
      if (ps[i].c->v[d] > B - RADIUS) {

	disp = v3_new(0,0,0);
	disp->v[d] = ps[i].c->v[d] - B;
	//	dm = (RADIUS - v3_mag(disp)) / RADIUS;
	dm = 1 / (1 - (RADIUS - v3_mag(disp)) / RADIUS);
	v3_unit(disp);

	v3_mult_add(ps[i].v, disp, dm * LOSS_WALL * dt);

	//	ps[i].c->v[d] = B - RADIUS;
	//	ps[i].v->v[d] *= -1 * LOSS_WALL;
      }
    }
  }
  for (i = 0; i < PARTICLES; i++) {
    for (j = 0; j < PARTICLES; j++) {
      if (i == j) continue;
      if (v3_dist(ps[i].c, ps[j].c) >= RADIUS * 2) continue;

      /*      temp_s1 = v3_str(ps[i].c);
      temp_s2 = v3_str(ps[j].c);
      printf("collision pos: %s %s\n", temp_s1, temp_s2);
      free(temp_s1);
      free(temp_s2);*/

      /*
      temp_s1 = v3_str(ps[i].v);
      temp_s2 = v3_str(ps[j].v);
      printf("collision vel: %s %s\n", temp_s1, temp_s2);
      free(temp_s1);
      free(temp_s2);
      */

      temp = v3_copy(ps[i].v);
      v3_sub(temp, ps[j].v);

      /*
      temp_s1 = v3_str(temp);
      printf("temp = %s\n", temp_s1);
      free(temp_s1);
      */

      disp = v3_copy(ps[i].c);
      v3_sub(disp, ps[j].c);

      //      disp2 = v3_copy(disp);

      dm = RADIUS - v3_mag(disp) / 2;

      /*
      temp_s1 = v3_str(disp);
      printf("disp = %s\n", temp_s1);
      free(temp_s1);
      */

      v3_unit(disp);

      /*
      temp_s1 = v3_str(disp);
      printf("disp unit = %s\n", temp_s1);
      free(temp_s1);
      */

      dot = v3_dot(temp, disp);

      /*
      printf("dot = %f\n", dot);
      */

      v3_mult_add(ps[i].v, disp, dm*LOSS_COL*dt);
      v3_mult_sub(ps[j].v, disp, dm*LOSS_COL*dt);

      //      v3_mult_add(ps[i].c, disp, dm);
      //      v3_mult_sub(ps[j].c, disp, dm);

      v3_del(disp);
      v3_del(temp);
    }
  }
}
Ejemplo n.º 15
0
v3_t v3_max(const v3_t u, const v3_t v) {
    return v3_new(MAX(Vx(u), Vx(v)),
		  MAX(Vy(u), Vy(v)),
		  MAX(Vz(u), Vz(v)));
}
Ejemplo n.º 16
0
/* Fit a plane to the points at the given indices */
std::vector<int> FitPlaneToPoints(const std::vector<PointData> &points,
                                  const std::vector<int> &indices,
                                  double *plane, 
                                  int ransac_rounds,
                                  double ransac_threshold,
                                  bool par_to_up, bool perp_to_up,
                                  double *up)
{
    if (par_to_up && perp_to_up) {
	printf("[FitPlaneToPoints] Error: cannot be both "
	       "parallel and perpendicular to the up vector!\n");
	perp_to_up = false;
    }

    std::vector<int> inliers;
    if (!par_to_up) {
        /* Marshall the points */
        int num_points = (int) indices.size();
        v3_t *pts = new v3_t[num_points];
        for (int i = 0; i < num_points; i++) {
            int pt_idx = indices[i];
            const PointData &pt = points[pt_idx];
            pts[i] = v3_new(pt.m_pos[0], pt.m_pos[1], pt.m_pos[2]);
        }

        /* Fit the plane */
        int num_inliers = 0;
        double error = fit_3D_plane_ortreg_ransac(num_points, pts, 
                                                  ransac_rounds, 
                                                  ransac_threshold,
                                                  &num_inliers, plane);

        printf("error = %0.3f\n", error);

        /* Gather the inliers */
        for (int i = 0; i < num_points; i++) {
            double dist = plane_point_distance(plane, pts[i]);
            if (dist < ransac_threshold) {
                inliers.push_back(indices[i]);
            }
        }

        if (perp_to_up) {
            /* Compute the mean of the inliers */
            int num_inliers = (int) inliers.size();
            v3_t *pts_inlier = new v3_t[num_inliers];

            for (int i = 0; i < num_inliers; i++) {
                int pt_idx = inliers[i];
                const PointData &pt = points[pt_idx];
                pts_inlier[i] = v3_new(pt.m_pos[0], pt.m_pos[1], pt.m_pos[2]);
            }
            
            v3_t mean = v3_mean(num_inliers, pts_inlier);

            double dot;
            matrix_product(1, 3, 3, 1, up, mean.p, &dot);

            plane[0] = up[0];
            plane[1] = up[1];
            plane[2] = up[2];
            plane[3] = -dot;

            delete [] pts_inlier;
        }

        delete [] pts;
    } else {
        assert(fabs(up[1] - 1.0) < 1.0e-5);

        /* Marshall the points */
        int num_points = (int) indices.size(); // points.size();
        v2_t *pts = new v2_t[num_points];
        for (int i = 0; i < num_points; i++) {
            int pt_idx = indices[i];
            const PointData &pt = points[pt_idx];

            pts[i] = v2_new(pt.m_pos[0], pt.m_pos[2]);
        }

        /* Fit the plane */
        double line[3];
        int num_inliers = 0;
        double error = fit_2D_line_ortreg_ransac(num_points, pts, 
                                                 ransac_rounds, 
                                                 ransac_threshold,
                                                 &num_inliers, line);

        plane[0] = line[0];
        plane[1] = 0.0;
        plane[2] = line[1];
        plane[3] = line[2];

        printf("error = %0.3f\n", error);
        printf("num_inliers = %d\n", num_inliers);

        /* Gather the inliers */
        for (int i = 0; i < num_points; i++) {
            int pt_idx = indices[i];
            const PointData &p = points[pt_idx];
            v3_t pt = v3_new(p.m_pos[0], p.m_pos[1], p.m_pos[2]);
            double dist = plane_point_distance(plane, pt);

            if (dist < ransac_threshold) {
                inliers.push_back(indices[i]);
            }
        }
    }

    return inliers;
}
Ejemplo n.º 17
0
/* Compute coordinate-wise min, max of two vectors */
v3_t v3_min(const v3_t u, const v3_t v) {
    return v3_new(MIN(Vx(u), Vx(v)),
		  MIN(Vy(u), Vy(v)),
		  MIN(Vz(u), Vz(v)));
}
Ejemplo n.º 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(int n, v3_t *right_pts, v3_t *left_pts, 
		  double *R, double *T, 
		  double *Tout, double *scale, double *weight) {
    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[2][2] = { { 0.0, 0.0 }, 
                       { 0.0, 0.0 } };
    double MT[2][2];
    double MTM[2][2];
    double eval[2], sqrteval[2];
    double evec[2][2];
    double S[2][2], Sinv[2][2], U[2][2];
    double Tcenter[3][3] = { { 1.0, 0.0, 0.0 },
			     { 0.0, 1.0, 0.0 },
			     { 0.0, 0.0, 1.0 } };

    double Ttmp[3][3];

    double sum_num, sum_den, RMS_sum;

#if 1
    double weight_sum = 0.0;

    if (weight == NULL) {
        weight_sum = n;

        for (i = 0; i < n; i++) {
            right_centroid = 
                v3_add(right_centroid, right_pts[i]);
            left_centroid = 
                v3_add(left_centroid, left_pts[i]);
        }
        
        right_centroid = v3_scale(1.0 / weight_sum, right_centroid);
        left_centroid = v3_scale(1.0 / weight_sum, left_centroid);        
    } else {
        /* Compute the weighted centroid of both point sets */
        for (i = 0; i < n; i++) {
            right_centroid = 
                v3_add(right_centroid, v3_scale(weight[i], right_pts[i]));
            left_centroid = 
                v3_add(left_centroid, v3_scale(weight[i], left_pts[i]));
            weight_sum += weight[i];
        }

        right_centroid = v3_scale(1.0 / weight_sum, right_centroid);
        left_centroid = v3_scale(1.0 / weight_sum, left_centroid);
    }
#else
    /* Calculate the centroid of both sets of points */
    for (i = 0; i < n; i++) {
        right_centroid = v3_add(right_centroid, right_pts[i]);
        left_centroid = v3_add(left_centroid, left_pts[i]);
    }

    right_centroid = v3_scale(1.0 / n, right_centroid);
    left_centroid = v3_scale(1.0 / n, left_centroid);
#endif

    /* 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]);

	if (weight != NULL) {
	    M[0][0] += Vx(r) * Vx(l);
	    M[0][1] += Vx(r) * Vy(l);
	    M[1][0] += Vy(r) * Vx(l);
	    M[1][1] += Vy(r) * Vy(l);
	} else {
	    M[0][0] += Vx(r) * Vx(l);
	    M[0][1] += Vx(r) * Vy(l);
	    M[1][0] += Vy(r) * Vx(l);
	    M[1][1] += Vy(r) * Vy(l);
	}
    }

    /* Compute MTM */
    matrix_transpose(2, 2, (double *)M, (double *)MT);
    matrix_product(2, 2, 2, 2, (double *)MT, (double *)M, (double *)MTM);

    /* Calculate Sinv, the inverse of the square root of MTM */
    dgeev_driver(2, (double *)MTM, (double *)evec, eval);
    
    /* MTM = eval[0] * evec[0]T * evec[0] + eval[1] * evec[1]T * evec[1] */
    /* S = sqrt(eval[0]) * evec[0]T * evec[0] + sqrt(eval[1]) * evec[1]T * evec[1] */
    sqrteval[0] = sqrt(eval[0]);
    sqrteval[1] = sqrt(eval[1]);

    S[0][0] = 
        (sqrteval[0]) * evec[0][0] * evec[0][0] +
        (sqrteval[1]) * evec[1][0] * evec[1][0];
    S[0][1] = 
        (sqrteval[0]) * evec[0][0] * evec[0][1] +
        (sqrteval[1]) * evec[1][0] * evec[1][1];
    S[1][0] = 
        (sqrteval[0]) * evec[0][1] * evec[0][0] +
        (sqrteval[1]) * evec[1][1] * evec[1][0];
    S[1][1] = 
        (sqrteval[0]) * evec[0][1] * evec[0][1] +
        (sqrteval[1]) * evec[1][1] * evec[1][1];
    
    Sinv[0][0] = 
        (1.0 / sqrteval[0]) * evec[0][0] * evec[0][0] +
        (1.0 / sqrteval[1]) * evec[1][0] * evec[1][0];
    Sinv[0][1] = 
        (1.0 / sqrteval[0]) * evec[0][0] * evec[0][1] +
        (1.0 / sqrteval[1]) * evec[1][0] * evec[1][1];
    Sinv[1][0] = 
        (1.0 / sqrteval[0]) * evec[0][1] * evec[0][0] +
        (1.0 / sqrteval[1]) * evec[1][1] * evec[1][0];
    Sinv[1][1] = 
        (1.0 / sqrteval[0]) * evec[0][1] * evec[0][1] +
        (1.0 / sqrteval[1]) * evec[1][1] * evec[1][1];
    
    // matrix_product(2, 2, 2, 2, (double *)S, (double *)Sinv, (double *)U);

    /* U = M * Sinv */
    matrix_product(2, 2, 2, 2, (double *)M, (double *)Sinv, (double *)U);

    /* Fill in the rotation matrix */
    R[0] = U[0][0]; R[1] = U[0][1]; R[2] = 0.0;
    R[3] = U[1][0], R[4] = U[1][1]; R[5] = 0.0;
    R[6] = 0.0;     R[7] = 0.0;     R[8] = 1.0;

    // memcpy(R, U, sizeof(double) * 4);

    /* Fill in the translation matrix */
    T[0] = T[4] = T[8] = 1.0;
    T[1] = T[3] = T[6] = T[7] = 0.0;
    T[2] = Vx(right_centroid);
    T[5] = Vy(right_centroid);

    Tcenter[0][0] = *scale;
    Tcenter[1][1] = *scale;
    Tcenter[0][2] = -*scale * Vx(left_centroid);
    Tcenter[1][2] = -*scale * Vy(left_centroid);

    matrix_product(3, 3, 3, 3, T, R, (double *)Ttmp);

#if 0
#if 0
    /* Do the scaling */
    Ttmp[0][0] *= *scale;
    Ttmp[0][1] *= *scale;
    Ttmp[0][2] *= *scale;
    Ttmp[1][0] *= *scale;
    Ttmp[1][1] *= *scale;
    Ttmp[1][2] *= *scale;
#else
    Tcenter[0][0] *= *scale;
    Tcenter[0][2] *= *scale;
    Tcenter[1][1] *= *scale;
    Tcenter[1][2] *= *scale;
#endif
#endif

    matrix_product(3, 3, 3, 3, (double *)Ttmp, (double *)Tcenter, Tout);

    T[2] = Vx(v3_sub(right_centroid, left_centroid));
    T[5] = Vy(v3_sub(right_centroid, left_centroid));


    /* Now compute the RMS error between the points */
    RMS_sum = 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]);
	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);
    }
    
    return sqrt(RMS_sum / n);
}
Ejemplo n.º 19
0
/* Add new points to the bundle adjustment */
int BundlerApp::BundleAdjustAddNewPoints(int camera_idx, 
                                         int num_points, int num_cameras,
                                         int *added_order,
                                         camera_params_t *cameras,
                                         v3_t *points, v3_t *colors,
                                         double reference_baseline,
                                         std::vector<ImageKeyVector> &pt_views)
{
    int pt_count = num_points;

    int image_idx = added_order[camera_idx];

    /* Recompute the locations of the new points given the initial
     * pose estimate */
    for (int i = 0; i < num_cameras; i++) {
	int other = added_order[i];

	if (other == image_idx)
	    continue;

	int first = MIN(image_idx, other);
	int second = MAX(image_idx, other);

	MatchIndex idx = GetMatchIndex(first, second);

        SetMatchesFromTracks(first, second);

	printf("  Matches[%d,%d] = %d\n", image_idx, other,
               (int) m_matches.GetNumMatches(idx));
	       // (int) m_match_lists[idx].size());

	double disti = GetCameraDistance(cameras + i, cameras + camera_idx);

	printf("  dist0, disti = %0.3e, %0.3e\n", reference_baseline, disti);

	if (disti < m_min_camera_distance_ratio * reference_baseline) {
	    printf("  Distance too low (possible panorama?)\n");
            // m_match_lists[idx].clear();
            m_matches.ClearMatch(idx);
	    continue;
	}

        std::vector<KeypointMatch> &list = m_matches.GetMatchList(idx);
	for (int j = 0; j < (int) list.size(); j++) {
	    int idx1 = list[j].m_idx1;
	    int idx2 = list[j].m_idx2;

	    int this_idx, other_idx;
	    if (image_idx == first) {
		this_idx = idx1;
		other_idx = idx2;
	    } else {
		other_idx = idx1;
		this_idx = idx2;
	    }
		
	    if (GetKey(other,other_idx).m_extra == -2) {
		/* The other key was already marked as an outlier */
		continue;
	    } else if (GetKey(image_idx,this_idx).m_extra == -2) {
		/* This key was already marked as an outlier */
		continue;
	    }

	    if (GetKey(other,other_idx).m_extra == -1 &&
		GetKey(image_idx,this_idx).m_extra >= 0) {  

		/**** Connecting an existing point *** */


		/* Connect up the other point to this one */
		int pt_idx = GetKey(image_idx,this_idx).m_extra;

		/* Check reprojection error */	    
		v2_t pr = sfm_project_final(cameras + i, points[pt_idx], 
					    true, m_estimate_distortion);

		double dx = GetKey(other,other_idx).m_x - Vx(pr);
		double dy = GetKey(other,other_idx).m_y - Vy(pr);
		    
		double proj_error = sqrt(dx * dx + dy * dy);

		if (proj_error >= 32.0) {
		    printf("  Would have connected existing match "
			   "%d ==> %d [%d] (cam: %d), \n"
			   "    but reprojection error (%0.3f) "
			   "is too high.\n", 
			   this_idx, other_idx, pt_idx, other, proj_error);
		} else {
		    printf("  Connecting existing match "
			   "%d ==> %d [%d] (cam: %d) [%0.3f]\n",
			   this_idx, other_idx, pt_idx, other, proj_error);
		    
		    GetKey(other,other_idx).m_extra = pt_idx;
		    pt_views[pt_idx].push_back(ImageKey(i, other_idx));
		}
	    } else if (GetKey(other,other_idx).m_extra == -1) {

		if (GetKey(image_idx,this_idx).m_extra != -1) {
		    printf("Error!  Key (%d,%d) shouldn't be seen yet!\n",
			   image_idx, this_idx);
		    printf("Point index is %d\n", 
			   GetKey(image_idx,this_idx).m_extra);
		}

		/* This is a new point */
		GetKey(other,other_idx).m_extra = pt_count;
		GetKey(image_idx,this_idx).m_extra = pt_count;

		/* Set up the 3D point */
		v2_t p = v2_new(GetKey(other,other_idx).m_x,
				GetKey(other,other_idx).m_y);
		    
		v2_t q = v2_new(GetKey(image_idx,this_idx).m_x,
				GetKey(image_idx,this_idx).m_y);

                if (m_optimize_for_fisheye) {
                    double p_x = Vx(p), p_y = Vy(p);
                    double q_x = Vx(q), q_y = Vy(q);
                    
                    m_image_data[other].
                        UndistortPoint(p_x, p_y, Vx(p), Vy(p));
                    m_image_data[image_idx].
                        UndistortPoint(q_x, q_y, Vx(q), Vy(q));
                }

		double proj_error = 0.0;
		bool in_front = false;
		double angle = 0.0;

		points[pt_count] = 
		    Triangulate(p, q, cameras[i], cameras[camera_idx], 
				proj_error, in_front, angle, true);


		/* Check that the angle between the rays is large
		 * enough */
		if (RAD2DEG(angle) < m_ray_angle_threshold) {
		    printf(" Ray angle %d => %d is too small (%0.3f)\n", 
			   this_idx, other_idx, RAD2DEG(angle));

		    /* Remove point */
		    GetKey(other,other_idx).m_extra = -1;
		    GetKey(image_idx,this_idx).m_extra = -1;

		    continue;
		}

		/* Check the reprojection error */
		if (proj_error >= ADD_REPROJECTION_ERROR) {
		    printf("  Projection error for %d => %d is %0.3e, "
			   "removing\n",
			   this_idx, other_idx, proj_error);

		    /* Remove point */
		    GetKey(other,other_idx).m_extra = -2;
		    GetKey(image_idx,this_idx).m_extra = -2;

		    continue;
		}

		/* Check cheirality */
		if (!in_front) {
		    printf("  Cheirality violated!\n");

		    /* Remove point */
		    GetKey(other,other_idx).m_extra = -2;
		    GetKey(image_idx,this_idx).m_extra = -2;

		    continue;
		}

		printf("  Adding match %d ==> %d [%d] (cam: %d ==> %d) "
		       "[%0.3f, %0.3f]\n", 
		       other_idx, this_idx, pt_count, image_idx, other, 
		       RAD2DEG(angle), proj_error);

		/* Finally, add the point */
		unsigned char r = GetKey(other,other_idx).m_r;
		unsigned char g = GetKey(other,other_idx).m_g;
		unsigned char b = GetKey(other,other_idx).m_b;

		colors[pt_count] = v3_new((double) r, 
					  (double) g,
					  (double) b);
    
		ImageKeyVector views;
		views.push_back(ImageKey(i, other_idx));
		views.push_back(ImageKey(camera_idx, this_idx));
		pt_views.push_back(views);

		pt_count++;

	    } else if (GetKey(other,other_idx).m_extra >= 0 && 
		       GetKey(image_idx,this_idx).m_extra == -1) {

		/* We didn't connect this point originally --
		 * check if it's now a good idea to add it in */

		/* Connect up the other point to this one */
		int pt_idx = GetKey(other,other_idx).m_extra;

		/* Check reprojection error */
		v2_t pr = sfm_project_final(cameras + camera_idx, 
					    points[pt_idx],
					    true, m_estimate_distortion);

		double dx = GetKey(image_idx,this_idx).m_x - Vx(pr);
		double dy = GetKey(image_idx,this_idx).m_y - Vy(pr);
		    
		double proj_error = sqrt(dx * dx + dy * dy);

		if (proj_error <= INIT_REPROJECTION_ERROR) {
		    printf("  Reconnecting point [%d] (%d) (error: %0.3f)\n", 
			   pt_idx, this_idx, proj_error);
		    GetKey(image_idx,this_idx).m_extra = pt_idx;
		    pt_views[pt_idx].push_back(ImageKey(camera_idx,this_idx));
		} else {
		    /* Throw out this point as an outlier */
		    GetKey(image_idx,this_idx).m_extra = -2;
		}
	    }
	}

        // m_match_lists[idx].clear();
        m_matches.ClearMatch(idx);
    }

    return pt_count;
}
Ejemplo n.º 20
0
void BaseApp::ReloadBundleFile(const char *filename)
{
#ifndef __DEMO__
    /* Count the old number of cameras */
    int num_images = GetNumImages();

    int old_num_cameras = 0;
    for (int i = 0; i < num_images; i++) {
        if (m_image_data[i].m_camera.m_adjusted)
            old_num_cameras++;
    }


    /* Save the previous model */
    std::vector<PointData> old_points = m_point_data;
    std::vector<ImageData> old_images = m_image_data;

    /* Load the new model */
    ClearModel();
    ReadBundleFile(filename);

    if (m_bundle_version < 0.3)
        FixReflectionBug();

    /* Count the new number of cameras */
    int num_cameras = 0;
    for (int i = 0; i < num_images; i++) {
        if (m_image_data[i].m_camera.m_adjusted)
            num_cameras++;
    }

    int old_num_points = old_points.size();
    int new_num_points = m_point_data.size();

    std::vector<v3_t> points_old_csp, points_new_csp;
    /* Find point correspondences */
    for (int i = 0; i < old_num_points; i++) {
        for (int j = i - 5; j < i + 5; j++) {
            if (j < 0 || j >= new_num_points) continue;

            float *old_col = old_points[i].m_color;
            float *col = m_point_data[j].m_color;

            if (old_col[0] == col[0] && 
                old_col[1] == col[1] && 
                old_col[2] == col[2]) {

                    double *old_pos = old_points[i].m_pos;
                    double *pos = m_point_data[i].m_pos;

                    points_old_csp.push_back(v3_new(old_pos[0], 
                        old_pos[1], 
                        old_pos[2]));

                    points_new_csp.push_back(v3_new(pos[0], pos[1], pos[2]));

                    goto Next;
            }
        }
Next: ;
    }

    int num_csp_points = points_old_csp.size();

    int num_points = old_num_cameras + num_csp_points;
    v3_t *left_points = new v3_t[num_points];
    v3_t *right_points = new v3_t[num_points];

    int count = 0;
    for (int i = 0; i < num_images; i++) {
        if (old_images[i].m_camera.m_adjusted) {
            double left_pos[3], right_pos[3];

            m_image_data[i].m_camera.GetPosition(left_pos);
            old_images[i].m_camera.GetPosition(right_pos);

            left_points[count] = 
                v3_new(left_pos[0], left_pos[1], left_pos[2]);
            right_points[count] = 
                v3_new(right_pos[0], right_pos[1], right_pos[2]);

            count++;
        }
    }

    for (int i = 0; i < num_csp_points; i++) {
        left_points[count] = points_new_csp[i];
        right_points[count] = points_old_csp[i];
        count++;	
    }

    /* Do the registration */
    double T[16];
    align_horn_3D(num_points, right_points, left_points, 1, T);

    /* Transform the world */
    memcpy(m_xform, T, 16 * sizeof(double));
    TransformWorldReal();
    // TransformWorld();

    delete [] left_points;
    delete [] right_points;
#endif /* __DEMO__ */
}
Ejemplo n.º 21
0
bool BundlerApp::EstimateRelativePose(int i1, int i2, 
                                      camera_params_t &camera1, 
                                      camera_params_t &camera2)
{
    MatchIndex list_idx;

    if (i1 < i2)
        list_idx = GetMatchIndex(i1, i2);
    else
        list_idx = GetMatchIndex(i2, i1);

    std::vector<KeypointMatch> &matches = m_matches.GetMatchList(list_idx);
    int num_matches = (int) matches.size();

    double f1 = m_image_data[i1].m_init_focal;
    double f2 = m_image_data[i2].m_init_focal;

    double E[9], F[9];
    std::vector<int> inliers;

    if (!m_optimize_for_fisheye) {
        inliers = 
            EstimateEMatrix(m_image_data[i1].m_keys, m_image_data[i2].m_keys, 
                            matches,
                            4 * m_fmatrix_rounds, // 8 * m_fmatrix_rounds,
                            m_fmatrix_threshold * m_fmatrix_threshold, 
                            f1, f2, E, F);
    } else {
        /* FIXME */
        inliers = 
            EstimateEMatrix(m_image_data[i1].m_keys, m_image_data[i2].m_keys, 
                            matches,
                            4 * m_fmatrix_rounds, // 8 * m_fmatrix_rounds,
                            m_fmatrix_threshold * m_fmatrix_threshold, 
                            f1, f2, E, F);        
    }

    if ((int) inliers.size() == 0)
        return false;

    int num_inliers = (int) inliers.size();
    printf("  Found %d / %d inliers (%0.3f%%)\n", num_inliers, num_matches,
           100.0 * num_inliers / num_matches);


    /* Estimate a homography with the inliers */
    std::vector<KeypointMatch> match_inliers;
    for (int i = 0; i < num_inliers; i++) {
        match_inliers.push_back(matches[inliers[i]]);
    }

    int num_match_inliers = (int) match_inliers.size();

    double H[9];
    std::vector<int> Hinliers = 
        EstimateTransform(m_image_data[i1].m_keys, m_image_data[i2].m_keys,
                          match_inliers, MotionHomography,
                          128 /*m_homography_rounds*/, 
                          6.0 /*m_homography_threshold*/, H);

    printf("  Found %d / %d homography inliers (%0.3f%%)\n",
           (int) Hinliers.size(), num_inliers, 
           100.0 * Hinliers.size() / num_inliers);

    bool initialized = false;
    if ((int) Hinliers.size() > 0) {
        matrix_print(3, 3, H);
        printf("\n");
    
        if ((double) Hinliers.size() / num_inliers >= 0.75 /*0.85*/) {
            KeypointMatch &match0 = matches[Hinliers[0]];
            v2_t p10 = v2_new(m_image_data[i1].m_keys[match0.m_idx1].m_x,
                              m_image_data[i1].m_keys[match0.m_idx1].m_y);
            v2_t p20 = v2_new(m_image_data[i2].m_keys[match0.m_idx2].m_x,
                              m_image_data[i2].m_keys[match0.m_idx2].m_y);

            double R1[9], t1[3], R2[9], t2[3];
            bool success = 
                DecomposeHomography(H, f1, f2, R1, t1, R2, t2, p10, p20);
        
            if (success) {
                printf("[BundleTwoFrame] Using homography "
                       "for initialization\n");

                /* Decide which solution to use */
                double F1h[9], F2h[9];
                ComputeFundamentalMatrix(f1, f2, R1, t1, F1h);
                ComputeFundamentalMatrix(f1, f2, R2, t2, F2h);
                
                double F1hT[9], F2hT[9];
                matrix_transpose(3, 3, F1h, F1hT);
                matrix_transpose(3, 3, F2h, F2hT);

                int num_inliers1 = 0, num_inliers2 = 0;

                for (int i = 0; i < num_match_inliers; i++) {
                    const KeypointMatch &match = match_inliers[i];
                    const Keypoint &k1 = m_image_data[i1].m_keys[match.m_idx1];
                    const Keypoint &k2 = m_image_data[i2].m_keys[match.m_idx2];

                    v3_t rt = v3_new(k1.m_x, k1.m_y, 1.0);
                    v3_t lft = v3_new(k2.m_x, k2.m_y, 1.0);
                    double r1a = fmatrix_compute_residual(F1h, lft, rt);
                    double r1b = fmatrix_compute_residual(F1hT, rt, lft);

                    double r2a = fmatrix_compute_residual(F2h, lft, rt);
                    double r2b = fmatrix_compute_residual(F2hT, rt, lft);

                    if (r1a < m_fmatrix_threshold && r1b < m_fmatrix_threshold)
                        num_inliers1++;

                    if (r2a < m_fmatrix_threshold && r2b < m_fmatrix_threshold)
                        num_inliers2++;
                }

                initialized = true;

                double *R, *t;
                printf("  H1: %d inliers, H2: %d inliers\n", 
                       num_inliers1, num_inliers2);
                if (num_inliers1 > num_inliers2) {
                    R = R1;
                    t = t1;
                } else {
                    R = R2;
                    t = t2;
                }

                memcpy(camera2.R, R, sizeof(double) * 9);
                matrix_transpose_product(3, 3, 3, 1, R, t, camera2.t);
                matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
            }
        }
    }

    if (!initialized) {
        KeypointMatch &match = matches[inliers[0]];
        v2_t p1 = v2_new(m_image_data[i1].m_keys[match.m_idx1].m_x / f1,
                         m_image_data[i1].m_keys[match.m_idx1].m_y / f1);
        v2_t p2 = v2_new(m_image_data[i2].m_keys[match.m_idx2].m_x / f2,
                         m_image_data[i2].m_keys[match.m_idx2].m_y / f2);
    
        double R[9], t[3];
        int success = find_extrinsics_essential(E, p1, p2, R, t);
    
        if (!success) {
            return false;
        }

        memcpy(camera2.R, R, sizeof(double) * 9);

        matrix_transpose_product(3, 3, 3, 1, R, t, camera2.t);
        matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
    }

    return true;
}
Ejemplo n.º 22
0
v3* v3_copy(v3* v) {
  v3* ret = v3_new(0,0,0);
  memcpy(ret->v, v->v, 3 * sizeof(float));

  return ret;
}
Ejemplo n.º 23
0
v3_t v3_sub(const v3_t u, const v3_t v) {
    return v3_new(Vx(u) - Vx(v), Vy(u) - Vy(v), Vz(u) - Vz(v));
}
Ejemplo n.º 24
0
/* Add new points to the bundle adjustment */
int 
BundlerApp::BundleAdjustAddAllNewPoints(int num_points, int num_cameras,
                                        int *added_order,
                                        camera_params_t *cameras,
                                        v3_t *points, v3_t *colors,
                                        double reference_baseline,
                                        std::vector<ImageKeyVector> &pt_views,
                                        double max_reprojection_error,
                                        int min_views)
{
    std::vector<int> track_idxs;
    std::vector<ImageKeyVector> new_tracks;

    int num_tracks_total = (int) m_track_data.size();
    int *tracks_seen = new int[num_tracks_total];
    for (int i = 0; i < num_tracks_total; i++) {
	tracks_seen[i] = -1;
    }

    /* Gather up the projections of all the new tracks */
    for (int i = 0; i < num_cameras; i++) {
	int image_idx1 = added_order[i];

	int num_keys = GetNumKeys(image_idx1);
	
	for (int j = 0; j < num_keys; j++) {
	    Keypoint &key = GetKey(image_idx1, j);

	    if (key.m_track == -1)
		continue;  /* Key belongs to no track */

	    if (key.m_extra != -1)
		continue;  /* Key is outlier or has already been added */

	    int track_idx = key.m_track;
	    
	    /* Check if this track is already associated with a point */
	    if (m_track_data[track_idx].m_extra != -1)
		continue;

	    /* Check if we've seen this track */
	    int seen = tracks_seen[track_idx];

	    if (seen == -1) {
		/* We haven't yet seen this track, create a new track */
		tracks_seen[track_idx] = (int) new_tracks.size();

		ImageKeyVector track;
		track.push_back(ImageKey(i, j));
		new_tracks.push_back(track);
		track_idxs.push_back(track_idx);
	    } else {
		new_tracks[seen].push_back(ImageKey(i, j));
	    }
	}
    }
    
    delete [] tracks_seen;

    /* Now for each (sub) track, triangulate to see if the track is
     * consistent */
    int pt_count = num_points;

    int num_ill_conditioned = 0;
    int num_high_reprojection = 0;
    int num_cheirality_failed = 0;
    int num_added = 0;

    int num_tracks = (int) new_tracks.size();
    for (int i = 0; i < num_tracks; i++) {
	int num_views = (int) new_tracks[i].size();
	
	if (num_views < min_views) continue;  /* Not enough views */

#if 0
	printf("Triangulating track ");
	PrintTrack(new_tracks[i]);
	printf("\n");
#endif

	/* Check if at least two cameras fix the position of the point */
	bool conditioned = false;
	bool good_distance = false;
	double max_angle = 0.0;
	for (int j = 0; j < num_views; j++) {
	    for (int k = j+1; k < num_views; k++) {
		int camera_idx1 = new_tracks[i][j].first;
		int image_idx1 = added_order[camera_idx1];
		int key_idx1 = new_tracks[i][j].second;

		int camera_idx2 = new_tracks[i][k].first;
		int image_idx2 = added_order[camera_idx2];
		int key_idx2 = new_tracks[i][k].second;

		Keypoint &key1 = GetKey(image_idx1, key_idx1);
		Keypoint &key2 = GetKey(image_idx2, key_idx2);

		v2_t p = v2_new(key1.m_x, key1.m_y);
		v2_t q = v2_new(key2.m_x, key2.m_y);

                if (m_optimize_for_fisheye) {
                    double p_x = Vx(p), p_y = Vy(p);
                    double q_x = Vx(q), q_y = Vy(q);
                    
                    m_image_data[image_idx1].
                        UndistortPoint(p_x, p_y, Vx(p), Vy(p));
                    m_image_data[image_idx2].
                        UndistortPoint(q_x, q_y, Vx(q), Vy(q));
                }

		double angle = ComputeRayAngle(p, q, 
					       cameras[camera_idx1], 
					       cameras[camera_idx2]);

		if (angle > max_angle)
		    max_angle = angle;

		/* Check that the angle between the rays is large
		 * enough */
		if (RAD2DEG(angle) >= m_ray_angle_threshold) {
		    conditioned = true;
		}

#if 0
		double dist_jk = 
		    GetCameraDistance(cameras + j, cameras + k, 
				      m_explicit_camera_centers);

		if (dist_jk > m_min_camera_distance_ratio * reference_baseline)
		    good_distance = true;
#else
                good_distance = true;
#endif
	    }
	}
	
	if (!conditioned || !good_distance) {
	    num_ill_conditioned++;

#if 0
	    printf(">> Track is ill-conditioned [max_angle = %0.3f]\n", 
		   RAD2DEG(max_angle));
	    fflush(stdout);
#endif
	    continue;
	}
	
	double error;
	v3_t pt;

        if (!m_panorama_mode) {
            pt = TriangulateNViews(new_tracks[i], added_order, cameras, 
                                   error, true);
        } else {
            pt = GeneratePointAtInfinity(new_tracks[i], added_order, cameras, 
                                         error, true);
        }
       
		// Changed by Wan, Yi
	if (::isnan(error) || error > max_reprojection_error) {
	    num_high_reprojection++;
#if 0
	    printf(">> Reprojection error [%0.3f] is too large\n", error);
	    fflush(stdout);
#endif
	    continue;	    
	}

	bool all_in_front = true;
	for (int j = 0; j < num_views; j++) {
	    int camera_idx = new_tracks[i][j].first;
	    bool in_front = CheckCheirality(pt, cameras[camera_idx]);
	 
	    if (!in_front) {
		all_in_front = false;
		break;
	    }
	}

	if (!all_in_front) {
	    num_cheirality_failed++;

#if 0
	    printf(">> Cheirality check failed\n");
	    fflush(stdout);
#endif
	    continue;
	}
	
	/* All tests succeeded, so let's add the point */
#if 0
	printf("Triangulating track ");
	PrintTrack(new_tracks[i]);
	printf("\n");
	printf(">> All tests succeeded [%0.3f, %0.3f] for point [%d]\n", 
	       RAD2DEG(max_angle), error, pt_count);
#endif

	fflush(stdout);

	points[pt_count] = pt;

	int camera_idx = new_tracks[i][0].first;
	int image_idx = added_order[camera_idx];
	int key_idx = new_tracks[i][0].second;

	unsigned char r = GetKey(image_idx, key_idx).m_r;
	unsigned char g = GetKey(image_idx, key_idx).m_g;
	unsigned char b = GetKey(image_idx, key_idx).m_b;
	colors[pt_count] = v3_new((double) r, (double) g, (double) b);
    
	pt_views.push_back(new_tracks[i]);

	/* Set the point index on the keys */
	for (int j = 0; j < num_views; j++) {
	    int camera_idx = new_tracks[i][j].first;
	    int image_idx = added_order[camera_idx];
	    int key_idx = new_tracks[i][j].second;
	    GetKey(image_idx, key_idx).m_extra = pt_count;
	}

	int track_idx = track_idxs[i];
	m_track_data[track_idx].m_extra = pt_count;
	
	pt_count++;
        num_added++;
    }

    printf("[AddAllNewPoints] Added %d new points\n", num_added);
    printf("[AddAllNewPoints] Ill-conditioned tracks: %d\n", 
           num_ill_conditioned);
    printf("[AddAllNewPoints] Bad reprojections: %d\n", num_high_reprojection);
    printf("[AddAllNewPoints] Failed cheirality checks: %d\n", 
           num_cheirality_failed);

    return pt_count;
}
Ejemplo n.º 25
0
v3_t v3_scale(double c, const v3_t v) {
    return v3_new(c * Vx(v), c * Vy(v), c * Vz(v));
}
Ejemplo n.º 26
0
/* Estimate an F-matrix from a given set of point matches */
std::vector<int> EstimateFMatrix(const std::vector<Keypoint> &k1, 
				 const std::vector<Keypoint> &k2, 
				 std::vector<KeypointMatch> matches, 
				 int num_trials, double threshold, 
				 double *F, bool essential)
{
    int num_pts = (int) matches.size();

    /* num_pts should be greater than a threshold */
    if (num_pts < 20) {
        std::vector<int> inliers;
        return inliers;
    }

    v3_t *k1_pts = new v3_t[num_pts];
    v3_t *k2_pts = new v3_t[num_pts];

    v3_t *k1_pts_in = new v3_t[num_pts];
    v3_t *k2_pts_in = new v3_t[num_pts];

    for (int i = 0; i < num_pts; i++) {
        int idx1 = matches[i].m_idx1;
        int idx2 = matches[i].m_idx2;

		if(idx1 >= k1.size()) {
			fprintf(stderr, "Error: %d >= %d\n", idx1, k1.size());
			fprintf(stderr, "idx1/idx2: %d %d\n", idx1, idx2);
			fprintf(stderr, "Fix me 1\n");
			continue;
		}

		if(idx2 >= k2.size()) {
			fprintf(stderr, "Error: %d >= %d\n", idx2, k2.size());
			fprintf(stderr, "idx1/idx2: %d %d\n", idx1, idx2);
			fprintf(stderr, "Fix me 2\n");
			continue;
		}

        assert(idx1 < (int) k1.size());
        assert(idx2 < (int) k2.size());

        k1_pts[i] = v3_new(k1[idx1].m_x, k1[idx1].m_y, 1.0);
        k2_pts[i] = v3_new(k2[idx2].m_x, k2[idx2].m_y, 1.0);
    }

    estimate_fmatrix_ransac_matches(num_pts, k2_pts, k1_pts, 
        num_trials, threshold, 0.99,
        (essential ? 1 : 0), F);

    /* Find the inliers */
    std::vector<int> inliers;

    for (int i = 0; i < num_pts; i++) {
        double dist = fmatrix_compute_residual(F, k2_pts[i], k1_pts[i]);
        if (dist < threshold) {
            inliers.push_back(i);
        }
    }

    /* Re-estimate using inliers */
    int num_inliers = (int) inliers.size();

    for (int i = 0; i < num_inliers; i++) {
        k1_pts_in[i] = k1_pts[inliers[i]]; // v3_new(k1[idx1]->m_x, k1[idx1]->m_y, 1.0);
        k2_pts_in[i] = k2_pts[inliers[i]]; // v3_new(k2[idx2]->m_x, k2[idx2]->m_y, 1.0);
    }

    // printf("[1] num_inliers = %d\n", num_inliers);

#if 0
    double F0[9];
    double e1[3], e2[3];
    estimate_fmatrix_linear(num_inliers, k2_pts_in, k1_pts_in, F0, e1, e2);

    inliers.clear();
    for (int i = 0; i < num_pts; i++) {
        double dist = fmatrix_compute_residual(F0, k2_pts[i], k1_pts[i]);
        if (dist < threshold) {
            inliers.push_back(i);
        }
    }
    num_inliers = inliers.size();
    // printf("[2] num_inliers = %d\n", num_inliers);

    // matrix_print(3, 3, F0);
#else
    double F0[9];
    memcpy(F0, F, sizeof(double) * 9);
#endif

    if (!essential) {
        /* Refine using NLLS */
        for (int i = 0; i < num_inliers; i++) {
            k1_pts_in[i] = k1_pts[inliers[i]];
            k2_pts_in[i] = k2_pts[inliers[i]];
        }

        refine_fmatrix_nonlinear_matches(num_inliers, k2_pts_in, k1_pts_in, 
            F0, F);
    } else {
        memcpy(F, F0, sizeof(double) * 9);
    }

#if 0
    if (essential) {
        /* Compute the SVD of F */
        double U[9], S[3], VT[9];
        dgesvd_driver(3, 3, F, U, S, VT);
        double E0[9] = { 1.0, 0.0, 0.0,
            0.0, 1.0, 0.0,
            0.0, 0.0, 0.0 };

        double tmp[9];
        matrix_product(3, 3, 3, 3, U, E0, tmp);
        matrix_product(3, 3, 3, 3, tmp, VT, F);
    }
#endif

    inliers.clear();
    for (int i = 0; i < num_pts; i++) {
        double dist = fmatrix_compute_residual(F, k2_pts[i], k1_pts[i]);
        if (dist < threshold) {
            inliers.push_back(i);
        }
    }
    num_inliers = (int) inliers.size();

    delete [] k1_pts;
    delete [] k2_pts;
    delete [] k1_pts_in;
    delete [] k2_pts_in;

    return inliers;
}
Ejemplo n.º 27
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);
}