/* 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);
}
}
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);
}
}
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];
}
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);
}
/* 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);
}
/* 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;
}
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]);
}
/* 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;
}
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;
}
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];
}
/* 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);
}
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));
}
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));
}
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);
}
}
}
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)));
}
/* 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;
}
/* 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)));
}
/* 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);
}
/* 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;
}
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__ */
}
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;
}
v3* v3_copy(v3* v) {
v3* ret = v3_new(0,0,0);
memcpy(ret->v, v->v, 3 * sizeof(float));
return ret;
}
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));
}
/* 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;
}
v3_t v3_scale(double c, const v3_t v) {
return v3_new(c * Vx(v), c * Vy(v), c * Vz(v));
}
/* 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;
}
/* 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);
}
