/* 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;
}
/* Use RANSAC to estimate a homography */
void align_homography_ransac_matches(int num_pts, v3_t *a_pts, v3_t *b_pts,
int num_trials, double threshold,
double success_ratio,
int essential, double *H)
{
#define MIN_SAMPLES 4
int i, j, k, idx;
v3_t l_pts_best[MIN_SAMPLES], r_pts_best[MIN_SAMPLES];
double Hbest[MIN_SAMPLES];
double *resid;
double error_min;
int inliers_max;
double *a_matrix, *b_matrix;
// double threshold = 1.0e-10;
// srand(time(0));
/* Make an array of all good correspondences */
if (num_pts < MIN_SAMPLES)
{
printf("[align_homography_ransac] "
"Could not find 8 good correspondences, "
"homography estimation failed\n");
return;
}
a_matrix = malloc(sizeof(double) * 3 * num_pts);
b_matrix = malloc(sizeof(double) * 3 * num_pts);
for (i = 0; i < num_pts; i++)
{
a_matrix[i] = Vx(a_pts[i]);
a_matrix[i+num_pts] = Vy(a_pts[i]);
a_matrix[i+2*num_pts] = Vz(a_pts[i]);
b_matrix[i] = Vx(b_pts[i]);
b_matrix[i+num_pts] = Vy(b_pts[i]);
b_matrix[i+2*num_pts] = Vz(b_pts[i]);
}
error_min = DBL_MAX;
inliers_max = 0;
resid = (double *) malloc(sizeof(double) * num_pts);
/* Estimate the homography using RANSAC */
for (i = 0; i < num_trials; i++)
{
int idxs[MIN_SAMPLES];
v3_t l_pts[MIN_SAMPLES], r_pts[MIN_SAMPLES];
double Htmp[9];
// double error;
int num_inliers = 0;
int success, nan = 0;
int round = 0;
/* Sample 4 random correspondences */
for (j = 0; j < MIN_SAMPLES; j++)
{
int reselect = 0;
if (round == 1000)
return;
idx = rand() % num_pts;
/* Make sure we didn't sample this index yet */
for (k = 0; k < j; k++)
{
if (idx == idxs[k] ||
(Vx(a_pts[idx]) == Vx(a_pts[idxs[k]]) &&
Vy(a_pts[idx]) == Vy(a_pts[idxs[k]]) &&
Vz(a_pts[idx]) == Vz(a_pts[idxs[k]])) ||
(Vx(b_pts[idx]) == Vx(b_pts[idxs[k]]) &&
Vy(b_pts[idx]) == Vy(b_pts[idxs[k]]) &&
Vz(b_pts[idx]) == Vz(b_pts[idxs[k]])))
{
reselect = 1;
break;
}
}
if (reselect)
{
round++;
j--;
continue;
}
idxs[j] = idx;
}
/* Fill in the left and right points */
for (j = 0; j < 8; j++)
{
l_pts[j] = b_pts[idxs[j]];
r_pts[j] = a_pts[idxs[j]];
}
/* Estimate the F-matrix */
success = estimate_fmatrix_linear(8, r_pts, l_pts, essential,
Ftmp, e1_tmp, e2_tmp);
if (success == 0)
nan = 1;
for (j = 0; j < 9; j++)
{
if (Ftmp[j] != Ftmp[j] /* isnan(Ftmp[j]) */)
{
printf("[estimate_fmatrix_ransac_matches] nan encountered\n");
nan = 1;
break;
}
}
/* Check for nan entries */
if (isnan(Ftmp[0]) || isnan(Ftmp[1]) || isnan(Ftmp[2]) ||
isnan(Ftmp[3]) || isnan(Ftmp[4]) || isnan(Ftmp[5]) ||
isnan(Ftmp[6]) || isnan(Ftmp[7]) || isnan(Ftmp[8]))
{
printf("[estimate_fmatrix_ransac_matches] "
"nan matrix encountered\n");
nan = 1;
}
if (nan)
{
// error = DBL_MAX;
num_inliers = 0;
}
else
{
// printf("%0.3f\n", Ftmp[0]);
/* Compute residuals */
#if 1
for (j = 0; j < num_pts; j++)
{
resid[j] = fmatrix_compute_residual(Ftmp, a_pts[j], b_pts[j]);
if (resid[j] < threshold)
num_inliers++;
}
#else
fmatrix_compute_residuals(num_pts, Ftmp, a_matrix, b_matrix,
resid);
for (j = 0; j < num_pts; j++)
{
if (resid[j] < threshold)
num_inliers++;
}
#endif
#if 0
/* Find the median */
error = median(num_pts, resid);
if (error < error_min)
{
error_min = error;
memcpy(Fbest, Ftmp, sizeof(double) * 9);
memcpy(l_pts_best, l_pts, sizeof(v3_t) * 8);
memcpy(r_pts_best, r_pts, sizeof(v3_t) * 8);
}
#else
if (num_inliers > inliers_max)
{
inliers_max = num_inliers;
memcpy(Fbest, Ftmp, sizeof(double) * 9);
memcpy(l_pts_best, l_pts, sizeof(v3_t) * 8);
memcpy(r_pts_best, r_pts, sizeof(v3_t) * 8);
}
#endif
}
#if 0
if (error < threshold)
break;
#endif
if ((double) num_inliers / num_pts > success_ratio)
break;
}
// printf("Minimum error: %0.5e\n", error_min);
// printf("Maximum inliers: %d\n", inliers_max);
// matrix_print(3, 3, Fbest);
free(resid);
/* Copy out the F-matrix */
memcpy(F, Fbest, sizeof(double) * 9);
free(a_matrix);
free(b_matrix);
}
