bool BundlerApp::EstimateRelativePose2(int i1, int i2,
camera_params_t &camera1,
camera_params_t &camera2)
{
// int num_images = GetNumImages();
MatchIndex list_idx;
if (i1 < i2)
list_idx = GetMatchIndex(i1, i2); // i1 * num_images + i2;
else
list_idx = GetMatchIndex(i2, i1); // i2 * num_images + i1;
std::vector<KeypointMatch> &matches = m_matches.GetMatchList(list_idx);
// int num_matches = (int) m_match_lists[list_idx].size();
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 K1[9], K2[9];
GetIntrinsics(camera1, K1);
GetIntrinsics(camera2, K2);
double R0[9], t0[3];
int num_inliers = 0;
if (!m_optimize_for_fisheye) {
num_inliers =
EstimatePose5Point(m_image_data[i1].m_keys,
m_image_data[i2].m_keys,
matches,
512, /* m_fmatrix_rounds, 8 * m_fmatrix_rounds */
0.25 * m_fmatrix_threshold, // 0.003, // 0.004 /*0.001,*/ // /*0.5 **/ m_fmatrix_threshold,
K1, K2, R0, t0);
} else {
std::vector<Keypoint> k1 = m_image_data[i1].UndistortKeysCopy();
std::vector<Keypoint> k2 = m_image_data[i2].UndistortKeysCopy();
num_inliers =
EstimatePose5Point(k1, k2, matches,
1024, /*512*/ /* m_fmatrix_rounds, 8 * m_fmatrix_rounds */
0.25 * m_fmatrix_threshold, // 0.004, /*0.001,*/ // /*0.5 **/ m_fmatrix_threshold,
K1, K2, R0, t0);
}
if (num_inliers == 0)
return false;
printf(" Found %d / %d inliers (%0.3f%%)\n", num_inliers, num_matches,
100.0 * num_inliers / num_matches);
bool initialized = false;
if (!initialized) {
memcpy(camera2.R, R0, sizeof(double) * 9);
matrix_transpose_product(3, 3, 3, 1, R0, t0, camera2.t);
matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
}
return true;
}
/* Make match lists symmetric */
void BaseApp::MakeMatchListsSymmetric()
{
unsigned int num_images = GetNumImages();
std::vector<MatchIndex> matches;
for (unsigned int i = 0; i < num_images; i++) {
MatchAdjList::const_iterator iter;
for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) {
// unsigned int i = iter->first;
unsigned int j = iter->m_index; // iter->second;
if (j <= i)
continue;
assert(ImagesMatch(i, j));
// MatchIndex idx = *iter;
MatchIndex idx = GetMatchIndex(i, j);
MatchIndex idx_rev = GetMatchIndex(j, i);
// int num_matches = (int) m_match_lists[idx].size();
const std::vector<KeypointMatch> &list = iter->m_match_list;
unsigned int num_matches = list.size();
// m_match_lists[idx_rev].clear();
m_matches.SetMatch(idx_rev);
m_matches.ClearMatch(idx_rev);
for (unsigned int k = 0; k < num_matches; k++) {
KeypointMatch m1, m2;
m1 = list[k];
m2.m_idx1 = m1.m_idx2;
m2.m_idx2 = m1.m_idx1;
// m_match_lists[idx_rev].push_back(m2);
m_matches.AddMatch(idx_rev, m2);
}
matches.push_back(idx);
}
}
unsigned int num_matches = matches.size();
for (unsigned int i = 0; i < num_matches; i++) {
unsigned int img1 = matches[i].first;
unsigned int img2 = matches[i].second;
SetMatch(img2, img1);
}
matches.clear();
}
/* Compute epipolar geometry between all matching images */
void BundlerApp::ComputeEpipolarGeometry(bool removeBadMatches,
int new_image_start)
{
unsigned int num_images = GetNumImages();
m_transforms.clear();
std::vector<MatchIndex> remove;
for (unsigned int i = 0; i < num_images; i++) {
MatchAdjList::iterator iter;
for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) {
unsigned int j = iter->m_index; // first;
assert(ImagesMatch(i, j));
MatchIndex idx = GetMatchIndex(i, j);
MatchIndex idx_rev = GetMatchIndex(j, i);
bool connect12 =
ComputeEpipolarGeometry(i, j, removeBadMatches);
if (!connect12) {
if (removeBadMatches) {
// RemoveMatch(i, j);
// RemoveMatch(j, i);
remove.push_back(idx);
remove.push_back(idx_rev);
// m_match_lists[idx].clear();
// m_match_lists.erase(idx);
m_transforms.erase(idx);
m_transforms.erase(idx_rev);
}
} else {
matrix_transpose(3, 3,
m_transforms[idx].m_fmatrix,
m_transforms[idx_rev].m_fmatrix);
}
}
}
int num_removed = (int) remove.size();
for (int i = 0; i < num_removed; i++) {
int img1 = remove[i].first;
int img2 = remove[i].second;
// RemoveMatch(img1, img2);
m_matches.RemoveMatch(GetMatchIndex(img1, img2));
}
}
/* Compute epipolar geometry between a given pair of images */
bool BundlerApp::ComputeEpipolarGeometry(int idx1, int idx2,
bool removeBadMatches)
{
assert(m_image_data[idx1].m_keys_loaded);
assert(m_image_data[idx2].m_keys_loaded);
MatchIndex offset = GetMatchIndex(idx1, idx2);
MatchIndex offset_rev = GetMatchIndex(idx2, idx1);
std::vector<KeypointMatch> &list = m_matches.GetMatchList(offset);
double F[9];
std::vector<int> inliers =
EstimateFMatrix(m_image_data[idx1].m_keys,
m_image_data[idx2].m_keys,
list,
m_fmatrix_rounds,
m_fmatrix_threshold /* 20.0 */ /* 9.0 */, F);
int num_inliers = (int) inliers.size();
printf("Inliers[%d,%d] = %d out of %d\n", idx1, idx2, num_inliers,
(int) list.size());
if (removeBadMatches) {
/* Refine the matches */
std::vector<KeypointMatch> new_match_list;
for (int i = 0; i < num_inliers; i++) {
new_match_list.push_back(list[inliers[i]]);
}
// m_match_lists[offset].clear();
// m_match_lists[offset] = new_match_list;
list.clear();
list = new_match_list;
}
#define MIN_INLIERS_EPIPOLAR 16
if (num_inliers >= m_min_num_feat_matches /*MIN_INLIERS_EPIPOLAR*/) {
// if (m_transforms[offset] == NULL)
m_transforms[offset] = TransformInfo();
m_transforms[offset_rev] = TransformInfo();
memcpy(m_transforms[offset].m_fmatrix, F, 9 * sizeof(double));
// m_transforms[offset]->m_scale = sqrt(M[0] * M[0] + M[1] * M[1]);
printf("Inliers[%d,%d] = %d out of %lu\n", idx1, idx2, num_inliers,
list.size());
return true;
} else {
return false;
}
}
void WriteModels(ModelMap &models, int num_images, char *out_file)
{
FILE *f = fopen(out_file, "w");
if (f == NULL) {
printf("[WriteModels] Error opening file %s for reading\n", out_file);
} else {
fprintf(f, "%d\n", num_images);
// FIXME LOOP
for (unsigned int i = 0; i < (unsigned int) num_images; i++) {
// for (int j = i+1; j < num_images; j++) {
ModelTable::iterator iter;
for (iter = models.Begin(i); iter != models.End(i); iter++) {
unsigned int j = iter->first; // iter->m_index;
if (i >= j)
continue;
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
fprintf(f, "%d %d\n", i, j);
models.GetModel(idx).Write(f);
}
}
}
fclose(f);
}
}
/* Prune points that match to multiple targets */
void BaseApp::PruneDoubleMatches()
{
unsigned int num_images = GetNumImages();
for (unsigned int i = 0; i < num_images; i++) {
MatchAdjList::iterator iter;
std::vector<unsigned int> remove;
for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) {
HashSetInt seen;
int num_pruned = 0;
// MatchIndex idx = *iter; // GetMatchIndex(i, j);
std::vector<KeypointMatch> &list = iter->m_match_list;
/* Unmark keys */
// int num_matches = (int) m_match_lists[idx].size();
int num_matches = (int) list.size();
for (int k = 0; k < num_matches; k++) {
int idx2 = list[k].m_idx2;
// if (GetKey(j,idx2).m_extra != -1) {
if (seen.find(idx2) != seen.end()) {
/* This is a repeat */
// printf("[%d] Pruning repeat %d\n", i, idx2);
list.erase(list.begin() + k);
num_matches--;
k--;
num_pruned++;
} else {
/* Mark this key as matched */
// GetKey(j,idx2).m_extra = k;
seen.insert(idx2);
}
}
// unsigned int i = iter->first;
// unsigned int j = iter->second;
unsigned int j = iter->m_index; // first;
printf("[PruneDoubleMatches] Pruned[%d,%d] = %d / %d\n",
i, j, num_pruned, num_matches + num_pruned);
if (num_matches < m_min_num_feat_matches) {
/* Get rid of... */
remove.push_back(iter->m_index); // first);
}
}
for (unsigned int j = 0; j < remove.size(); j++) {
int idx2 = remove[j];
m_matches.RemoveMatch(GetMatchIndex(i, idx2));
printf("[PruneDoubleMatches] Removing[%d,%d]\n", i, idx2);
}
}
}
/*
Load the match table from file. This table contains the set of
pairwise matches across two images. The matches are specified
by the key point indices. The table location/slot specifies which
pair of images are being queried.
The filename is passed as an argument. If it doesn't exist, then
the program will quit.
*/
void BaseApp::LoadMatchTable(const char *filename)
{
if (m_matches_loaded) // Are matches already loaded?
return;
#ifdef _DEBUG_
printf("[LoadMatchTable] Loading matches\n");
#endif
// Clear all matches / empty the table.
RemoveAllMatches();
// Open the file, make sure OK. If not able to open, exit program.
FILE *f = fopen(filename, "r");
if (f == NULL)
{
printf("[LoadMatchTable] Error opening file %s for reading\n", filename);
exit(1);
}
// Read the matching keypoint indices.
char buf[256];
while (fgets(buf, 256, f))
{
// Read images indices describing which pairwise matching being loaded.
int i1, i2;
sscanf(buf, "%d %d\n", &i1, &i2);
SetMatch(i1, i2);
// SetMatch(i2, i1);
// Read the number of matches
int nMatches;
fscanf(f, "%d\n", &nMatches);
// Read the matching indices and populate match table.
std::vector<KeypointMatch> matches;
for (int i = 0; i < nMatches; i++)
{
int k1, k2;
fscanf(f, "%d %d\n", &k1, &k2);
#ifdef KEY_LIMIT
if (k1 > KEY_LIMIT || k2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
KeypointMatch m;
m.m_idx1 = k1;
m.m_idx2 = k2;
matches.push_back(m);
}
MatchIndex idx = GetMatchIndex(i1, i2);
m_matches.GetMatchList(idx) = matches;
}
fclose(f); // Done loading table, close shop.
}
void BaseApp::LoadMatchTable(const char *filename) {
if (m_matches_loaded)
return; /* we already loaded the matches */
printf("[LoadMatchTable] Loading matches\n");
/* Set all matches to false */
// ClearMatches();
RemoveAllMatches();
FILE *f = fopen(filename, "r");
if (f == NULL) {
printf("[LoadMatchTable] Error opening file %s for reading\n",
filename);
exit(1);
}
char buf[256];
while (fgets(buf, 256, f)) {
/* Read the images */
int i1, i2;
sscanf(buf, "%d %d\n", &i1, &i2);
SetMatch(i1, i2);
// SetMatch(i2, i1);
// m_matches[i1 * num_images + i2] = true;
// m_matches[i2 * num_images + i1] = true;
/* Read the number of matches */
int nMatches;
fscanf(f, "%d\n", &nMatches);
/* Read the matches */
std::vector<KeypointMatch> matches;
for (int i = 0; i < nMatches; i++) {
int k1, k2;
fscanf(f, "%d %d\n", &k1, &k2);
#ifdef KEY_LIMIT
if (k1 > KEY_LIMIT || k2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
KeypointMatch m;
m.m_idx1 = k1;
m.m_idx2 = k2;
matches.push_back(m);
}
MatchIndex idx = GetMatchIndex(i1, i2);
// m_match_lists[idx] = matches;
m_matches.GetMatchList(idx) = matches;
}
fclose(f);
}
void BaseApp::SetMatchesFromTracks(int img1, int img2)
{
std::vector<int> &tracks1 = m_image_data[img1].m_visible_points;
std::vector<int> &tracks2 = m_image_data[img2].m_visible_points;
std::vector<int> isect = GetVectorIntersection(tracks1, tracks2);
int num_isect = (int) isect.size();
if (num_isect == 0)
return;
MatchIndex idx = GetMatchIndex(img1, img2);
std::vector<KeypointMatch> &matches = m_matches.GetMatchList(idx);
// m_match_lists[idx];
matches.clear();
matches.resize(num_isect);
for (int i = 0; i < num_isect; i++) {
int tr = isect[i];
#if 0
int num_views = (int) m_track_data[tr].m_views.size();
int k1 = -1, k2 = -1;
for (int j = 0; j < num_views; j++) {
if (m_track_data[tr].m_views[j].first == img1) {
k1 = m_track_data[tr].m_views[j].second;
}
if (m_track_data[tr].m_views[j].first == img2) {
k2 = m_track_data[tr].m_views[j].second;
}
}
assert(k1 != -1 && k2 != -1);
#endif
std::pair<std::vector<int>::const_iterator,
std::vector<int>::const_iterator> p;
const std::vector<int> &pt1 = m_image_data[img1].m_visible_points;
p = equal_range(pt1.begin(), pt1.end(), tr);
assert(p.first != p.second);
int offset = p.first - pt1.begin();
int k1 = m_image_data[img1].m_visible_keys[offset];
const std::vector<int> &pt2 = m_image_data[img2].m_visible_points;
p = equal_range(pt2.begin(), pt2.end(), tr);
assert(p.first != p.second);
offset = p.first - pt2.begin();
int k2 = m_image_data[img2].m_visible_keys[offset];
matches[i] = KeypointMatch(k1, k2);
}
}
/* Remove matches close to the edges of the two given images */
void BundlerApp::RemoveMatchesNearBorder(int i1, int i2, int border_width)
{
MatchIndex idx = GetMatchIndex(i1, i2);
// assert(m_match_lists.find(idx) != m_match_lists.end());
m_matches.Contains(idx);
assert(m_image_data[i1].m_keys_loaded);
assert(m_image_data[i2].m_keys_loaded);
std::vector<KeypointMatch> &list = m_matches.GetMatchList(idx);
// m_match_lists[idx];
int num_matches = (int) list.size();
int w1 = m_image_data[i1].GetWidth();
int h1 = m_image_data[i1].GetHeight();
int w2 = m_image_data[i2].GetWidth();
int h2 = m_image_data[i2].GetHeight();
double w1_min = -0.5 * w1 + border_width;
double w1_max = 0.5 * w1 - border_width;
double h1_min = -0.5 * h1 + border_width;
double h1_max = 0.5 * h1 - border_width;
double w2_min = -0.5 * w2 + border_width;
double w2_max = 0.5 * w2 - border_width;
double h2_min = -0.5 * h2 + border_width;
double h2_max = 0.5 * h2 - border_width;
int num_removed = 0;
for (int k = 0; k < num_matches; k++) {
KeypointMatch &m = list[k];
const Keypoint &k1 = m_image_data[i1].m_keys[m.m_idx1];
const Keypoint &k2 = m_image_data[i2].m_keys[m.m_idx2];
if (k1.m_x < w1_min || k1.m_x > w1_max ||
k1.m_y < h1_min || k1.m_y > h1_max ||
k2.m_x < w2_min || k2.m_x > w2_max ||
k2.m_y < h2_min || k2.m_y > h2_max) {
/* Erase this match */
list.erase(list.begin() + k);
k--;
num_matches--;
num_removed++;
}
}
printf("[RemoveMatchesNearBorder] Removed %d matches from pair (%d,%d)\n",
num_removed, i1, i2);
}
void BaseApp::ReadMatchFile(int i, int j)
{
/* Read matches from a file */
char buf[256];
sprintf(buf, "%s/match-%03d-%03d.txt", m_match_directory, i, j);
if (FileExists(buf)) {
FILE *f = fopen(buf, "r");
/* Read the number of matches */
int num_matches;
fscanf(f, "%d", &num_matches);
if (num_matches < MIN_MATCHES) {
// RemoveMatch(i, j);
// RemoveMatch(j, i);
fclose(f);
return;
}
SetMatch(i, j);
// SetMatch(j, i);
std::vector<KeypointMatch> matches;
for (int k = 0; k < num_matches; k++) {
int idx1, idx2;
fscanf(f, "%d %d", &idx1, &idx2);
#ifdef KEY_LIMIT
if (idx1 > KEY_LIMIT || idx2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
KeypointMatch m;
m.m_idx1 = idx1;
m.m_idx2 = idx2;
matches.push_back(m);
}
MatchIndex idx = GetMatchIndex(i, j);
// m_match_lists[idx] = matches;
m_matches.GetMatchList(idx) = matches;
fclose(f);
} else {
// RemoveMatch(i, j);
// RemoveMatch(j, i);
}
}
int BaseApp::GetNumMatches(int i1, int i2)
{
int i_min = MIN(i1, i2);
int i_max = MAX(i1, i2);
MatchIndex idx = GetMatchIndex(i_min, i_max);
// if (m_match_lists.find(idx) == m_match_lists.end())
// return 0;
// return m_match_lists[idx].size();
return m_matches.GetNumMatches(idx);
}
void BaseApp::WriteMatchTable(const char *append)
{
int num_images = GetNumImages();
char buf[256];
sprintf(buf, "nmatches%s.txt", append);
FILE *f0 = fopen(buf, "w");
sprintf(buf, "matches%s.txt", append);
FILE *f1 = fopen(buf, "w");
if (f0 == NULL || f1 == NULL) {
printf("[WriteMatchTable] "
"Error opening files for writing.\n");
return;
}
fprintf(f0, "%d\n", num_images);
for (int i = 0; i < num_images; i++) {
for (int j = 0; j < num_images; j++) {
if (i >= j) {
fprintf(f0, "0 ");
fprintf(f1, "\n");
} else {
if (ImagesMatch(i, j)) {
MatchIndex idx = GetMatchIndex(i, j);
std::vector<KeypointMatch> &list =
m_matches.GetMatchList(idx);
unsigned int num_matches = list.size();
fprintf(f0, "%d ", num_matches);
for (unsigned int k = 0; k < num_matches; k++) {
KeypointMatch m = list[k];
fprintf(f1, "%d %d ", m.m_idx1, m.m_idx2);
}
fprintf(f1, "\n");
} else {
fprintf(f0, "0 ");
}
}
}
fprintf(f0, "\n");
}
fclose(f0);
fclose(f1);
}
/*
Read from file containing key point matches. The matches are
added to the list of matches/MatchTable. The filename is
determined by the two images whose matches should be loaded.
If the file doesn't exist, then no matched were found between
the two images.
The file is presumed to be called match-XXX-YYY.txt and
should be found in the match directory.
*/
void BaseApp::ReadMatchFile(int i, int j)
{
char buf[256];
sprintf(buf, "%s/match-%03d-%03d.txt", m_match_directory, i, j);
if (FileExists(buf))
{
FILE *f = fopen(buf, "r");
// Read the number of matches.
int num_matches;
fscanf(f, "%d", &num_matches);
// If less than minimum allowed, don't bother, move on.
if (num_matches < MIN_MATCHES)
{
fclose(f);
return;
}
SetMatch(i, j);
std::vector<KeypointMatch> matches;
for (int k = 0; k < num_matches; k++)
{
int idx1, idx2;
fscanf(f, "%d %d", &idx1, &idx2);
#ifdef KEY_LIMIT
if (idx1 > KEY_LIMIT || idx2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
KeypointMatch m;
m.m_idx1 = idx1;
m.m_idx2 = idx2;
matches.push_back(m);
}
MatchIndex idx = GetMatchIndex(i, j);
m_matches.GetMatchList(idx) = matches;
fclose(f);
}
}
void BaseApp::SetMatchesFromTracks()
{
/* Clear all matches */
// ClearMatches();
RemoveAllMatches();
int num_tracks = (int) m_track_data.size();
int num_tracks_used = 0;
for (int i = 0; i < num_tracks; i++) {
TrackData &t = m_track_data[i];
int num_views = (int) t.m_views.size();
if (num_views < m_min_track_views)
continue; /* Not enough observations */
if (num_views > m_max_track_views)
continue; /* Too many observations */
for (int j = 0; j < num_views; j++) {
for (int k = 0; k < num_views; k++) {
if (j == k) continue;
int v1 = t.m_views[j].first;
int v2 = t.m_views[k].first;
int k1 = t.m_views[j].second;
int k2 = t.m_views[k].second;
MatchIndex idx = GetMatchIndex(v1, v2);
// m_matches[idx] = true;
SetMatch(v1, v2);
// m_match_lists[idx].push_back(KeypointMatch(k1,k2));
m_matches.GetMatchList(idx).push_back(KeypointMatch(k1, k2));
}
}
num_tracks_used++;
}
LOGI("[BaseApp::SetMatchesFromTracks] Used %d tracks\n",
num_tracks_used);
}
ModelMap ReadModels(FILE *f, int *num_images_out)
{
char buf[256];
int num_images;
fscanf(f, "%d\n", &num_images);
ModelMap models(num_images);
// assert(num_images == num_images);
while (fgets(buf, 256, f)) {
int i1, i2;
sscanf(buf, "%d %d", &i1, &i2);
TwoFrameModel m;
m.Read(f);
if (m.ComputeTrace(true) < 0.0 || m.ComputeTrace(false) < 0.0) {
printf("[ReadModels] Error! Trace(%d,%d) < 0!\n", i1, i2);
continue;
}
if (m.m_num_points < 28 /*33*/) {
// printf("[ReadModels] Error! Too few points [%d] for (%d,%d)\n",
// m.m_num_points, i1, i2);
continue;
}
if (isnan(m.m_angle) || isnan(m.m_error)) {
printf("[ReadModels] Error! NaNs in pair %d,%d!\n", i1, i2);
continue;
}
assert(i1 < i2);
models.AddModel(GetMatchIndex(i1, i2), m);
}
if (num_images_out != NULL)
*num_images_out = num_images;
return models;
}
/* Use the bundle-adjusted points to create a new set of matches */
void BaseApp::SetMatchesFromPoints(int threshold)
{
LOGI("[BaseApp::SetMatchesFromPoints] Setting up matches...\n");
/* Clear all matches */
// ClearMatches();
RemoveAllMatches();
int num_points = (int) m_point_data.size();
for (int i = 0; i < num_points; i++) {
int num_views = (int) m_point_data[i].m_views.size();
if (num_views < threshold)
continue;
for (int j = 0; j < num_views; j++) {
for (int k = 0; k < num_views; k++) {
if (j == k) continue;
ImageKey view1 = m_point_data[i].m_views[j];
ImageKey view2 = m_point_data[i].m_views[k];
int v1 = view1.first;
int v2 = view2.first;
int k1 = view1.second;
int k2 = view2.second;
KeypointMatch m;
m.m_idx1 = k1;
m.m_idx2 = k2;
// m_matches[v1 * num_images + v2] = true;
SetMatch(v1, v2);
MatchIndex idx = GetMatchIndex(v1, v2);
// m_match_lists[idx].push_back(m);
m_matches.AddMatch(idx, m);
}
}
}
LOGI("[BaseApp::SetMatchesFromPoints] Done!\n");
}
bool BaseApp::ImagesMatch(int i1, int i2) {
return m_matches.Contains(GetMatchIndex(i1, i2));
}
void BaseApp::RemoveMatch(int i1, int i2) {
m_matches.RemoveMatch(GetMatchIndex(i1, i2));
}
void ThresholdTwists(int num_images, ModelMap &models,
std::vector<ImageData> &image_data, bool panos_only)
{
int *num_large_twists = new int[num_images];
int *degree = new int[num_images];
for (int i = 0; i < num_images; i++) {
num_large_twists[i] = 0;
degree[i] = 0;
}
for (int i = 0; i < num_images; i++) {
ModelTable::iterator iter;
for (iter = models.Begin(i); iter != models.End(i); iter++) {
unsigned int j = iter->first; // iter->m_index;
if (i >= j)
continue;
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
TwoFrameModel &m = models.GetModel(idx);
/* Compute the twist */
double Rp_i[9], Rp_j[9];
matrix_transpose(3, 3, m.m_camera0.R, Rp_i);
matrix_transpose(3, 3, m.m_camera1.R, Rp_j);
double Rp_ij[9];
matrix_transpose_product(3, 3, 3, 3, Rp_i, Rp_j, Rp_ij);
double twist_angle = GetTwist(Rp_ij);
if (fabs(RAD2DEG(twist_angle)) >= 12.0) {
num_large_twists[i]++;
num_large_twists[j]++;
}
degree[i]++;
degree[j]++;
}
}
}
for (int i = 0; i < num_images; i++) {
if (degree[i] == 0)
continue;
double perc_large_twists = (double) num_large_twists[i] / degree[i];
int w = image_data[i].GetWidth();
int h = image_data[i].GetHeight();
double ratio = (double) w / h;
if ((panos_only || perc_large_twists < 0.4) &&
ratio > 0.4 && ratio < 2.5) {
continue;
}
printf("[ThresholdTwists] Removing image %d with score %0.3f, %0.3f\n",
i, perc_large_twists, ratio);
std::list<unsigned int> nbrs = models.GetNeighbors(i);
std::list<unsigned int>::iterator iter;
for (iter = nbrs.begin(); iter != nbrs.end(); iter++) {
unsigned int j = *iter; // iter->m_index;
if (i < j) {
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
models.RemoveModel(idx);
}
} else {
MatchIndex idx = GetMatchIndex(j, i);
if (models.Contains(idx)) {
models.RemoveModel(idx);
}
}
}
}
}
/*
Load the keypoint indices for the matched points. For each image,
there is a specific file with this information. Loop through all
images and load the data from the corresponding file. Populate
the match table.
TODO: Correct to that it is LoadMatchIndices.
*/
void BaseApp::LoadMatchIndexes(const char *index_dir)
{
int num_images = GetNumImages();
for (int i = 0; i < num_images; i++) // For each image, try to load file ...
{
char buf[256];
sprintf(buf, "%s/match-%03d.txt", index_dir, i);
FILE *f = fopen(buf, "r");
if (f == NULL)
continue;
#ifdef _DEBUG_
printf("[LoadMatchIndexes] Loading matches for image %d... ", i);
#endif
int num_matched_images = 0;
int index;
while(fgets(buf, 256, f) != NULL) // If file has data, then process it ...
{
sscanf(buf, "%d\n", &index);
int num_matches;
fscanf(f, "%d\n", &num_matches);
std::vector<KeypointMatch> matches;
for (int k = 0; k < (num_matches-1); k++) //-1 is a temp hack
{
int idx1, idx2;
fscanf(f, "%d %d\n", &idx1, &idx2);
#ifdef KEY_LIMIT
if (idx1 > KEY_LIMIT || idx2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
KeypointMatch m;
m.m_idx1 = idx1;
m.m_idx2 = idx2;
matches.push_back(m);
}
if (num_matches < MIN_MATCHES || index >= num_images) // Bum matches.
{
matches.clear(); // Clear out whatever was loaded.
if (index >= num_images) // Kick out a warning message.
printf("[LoadMatchIndexes] image index %d > num_images\n", index);
}
else // Good matches.
{
SetMatch(i, index); // Add to table.
MatchIndex idx = GetMatchIndex(i, index);
m_matches.GetMatchList(idx) = matches;
num_matched_images++; // Keep count.
}
}
#ifdef _DEBUG_
printf("%d match files loaded.\n", num_matched_images);
fflush(stdout);
#endif
fclose(f);
}
}
/* 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;
}
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;
}
void BaseApp::ReadMatchTable(const char *append)
{
int num_images = GetNumImages();
unsigned long int num_matches_total = 0;
char buf[256];
sprintf(buf, "nmatches%s.txt", append);
FILE *f0 = fopen(buf, "r");
sprintf(buf, "matches%s.txt", append);
FILE *f1 = fopen(buf, "r");
if (f0 == NULL || f1 == NULL) {
printf("[ReadMatchTable] "
"Error opening files for reading.\n");
return;
}
int num_images_check;
fscanf(f0, "%d\n", &num_images_check);
assert(num_images == num_images_check);
RemoveAllMatches();
for (int i = 0; i < num_images; i++) {
for (int j = 0; j < num_images; j++) {
MatchIndex idx = GetMatchIndex(i, j);
int num_matches;
fscanf(f0, "%d", &num_matches);
if (num_matches > 0) {
// m_match_lists[idx].clear();
SetMatch(i, j);
std::vector<KeypointMatch> &list = m_matches.GetMatchList(idx);
// m_matches.ClearMatch(idx);
for (int k = 0; k < num_matches; k++) {
KeypointMatch m;
int idx1, idx2;
fscanf(f1, "%d %d", &(idx1), &(idx2));
#ifdef KEY_LIMIT
if (idx1 > KEY_LIMIT || idx2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
m.m_idx1 = idx1;
m.m_idx2 = idx2;
// m_match_lists[idx].push_back(m);
list.push_back(m);
}
num_matches_total += num_matches;
}
}
}
printf("[ReadMatchTable] Read %lu matches in total\n",
num_matches_total);
fclose(f0);
fclose(f1);
}
/* Compute rigid transforms between all matching images */
void BundlerApp::ComputeTransforms(bool removeBadMatches, int new_image_start)
{
unsigned int num_images = GetNumImages();
m_transforms.clear();
for (unsigned int i = 0; i < num_images; i++) {
MatchAdjList::iterator iter;
for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) {
unsigned int j = iter->m_index;
assert(ImagesMatch(i, j));
MatchIndex idx = GetMatchIndex(i, j);
MatchIndex idx_rev = GetMatchIndex(j, i);
m_transforms[idx] = TransformInfo();
m_transforms[idx_rev] = TransformInfo();
bool connect12 = ComputeTransform(i, j, removeBadMatches);
if (!connect12) {
if (removeBadMatches) {
// RemoveMatch(i, j);
// RemoveMatch(j, i);
// m_match_lists[idx].clear();
m_matches.RemoveMatch(idx);
m_matches.RemoveMatch(idx_rev);
// m_match_lists.erase(idx);
m_transforms.erase(idx);
m_transforms.erase(idx_rev);
}
} else {
matrix_invert(3, m_transforms[idx].m_H,
m_transforms[idx_rev].m_H);
}
}
}
/* Print the inlier ratios */
FILE *f = fopen("pairwise_scores.txt", "w");
for (unsigned int i = 0; i < num_images; i++) {
MatchAdjList::iterator iter;
for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) {
unsigned int j = iter->m_index; // first;
assert(ImagesMatch(i, j));
// MatchIndex idx = *iter;
MatchIndex idx = GetMatchIndex(i, j);
fprintf(f, "%d %d %0.5f\n", i, j,
m_transforms[idx].m_inlier_ratio);
}
}
fclose(f);
}
void BaseApp::LoadMatchIndexes(const char *index_dir)
{
int num_images = GetNumImages();
for (int i = 0; i < num_images; i++) {
char buf[256];
sprintf(buf, "%s/match-%03d.txt", index_dir, i);
FILE *f = fopen(buf, "r");
if (f == NULL)
continue;
printf("[LoadMatchIndexes] Loading matches for image %d... ", i);
int num_matched_images = 0;
int index;
while(fgets(buf, 256, f) != NULL) {
sscanf(buf, "%d\n", &index);
int num_matches;
fscanf(f, "%d\n", &num_matches);
std::vector<KeypointMatch> matches;
for (int k = 0; k < num_matches-1; k++) {//-1 is a temp hack
int idx1, idx2;
fscanf(f, "%d %d\n", &idx1, &idx2);
#ifdef KEY_LIMIT
if (idx1 > KEY_LIMIT || idx2 > KEY_LIMIT)
continue;
#endif /* KEY_LIMIT */
KeypointMatch m;
m.m_idx1 = idx1;
m.m_idx2 = idx2;
matches.push_back(m);
}
if (num_matches < MIN_MATCHES || index >= num_images) {
if (index >= num_images)
printf("[LoadMatchIndexes] image index %d > num_images\n",
index);
// RemoveMatch(i, index);
matches.clear();
} else {
SetMatch(i, index);
MatchIndex idx = GetMatchIndex(i, index);
// m_match_lists[idx] = matches;
m_matches.GetMatchList(idx) = matches;
num_matched_images++;
}
}
printf("%d match files loaded.\n", num_matched_images);
fflush(stdout);
fclose(f);
}
}
void BaseApp::SetMatch(int i1, int i2) {
m_matches.SetMatch(GetMatchIndex(i1, i2));
}
/* Compute a transform between a given pair of images */
bool BundlerApp::ComputeTransform(int idx1, int idx2, bool removeBadMatches)
{
assert(m_image_data[idx1].m_keys_loaded);
assert(m_image_data[idx2].m_keys_loaded);
MatchIndex offset = GetMatchIndex(idx1, idx2);
double M[9];
if (idx1 == idx2) {
printf("[ComputeTransform] Error: computing tranform "
"for identical images\n");
return false;
}
std::vector<KeypointMatch> &list = m_matches.GetMatchList(offset);
std::vector<int> inliers =
EstimateTransform(m_image_data[idx1].m_keys,
m_image_data[idx2].m_keys,
list, MotionHomography,
m_homography_rounds,
m_homography_threshold,
/* 15.0 */ /* 6.0 */ /* 4.0 */ M);
int num_inliers = (int) inliers.size();
printf("Inliers[%d,%d] = %d out of %d\n", idx1, idx2, num_inliers,
(int) list.size());
if (removeBadMatches) {
/* Refine the matches */
std::vector<KeypointMatch> new_match_list;
for (int i = 0; i < num_inliers; i++) {
new_match_list.push_back(list[inliers[i]]);
}
// m_match_lists[offset].clear();
// m_match_lists[offset] = new_match_list;
list.clear();
list = new_match_list;
}
#define MIN_INLIERS 10
if (num_inliers >= MIN_INLIERS) {
m_transforms[offset].m_num_inliers = num_inliers;
m_transforms[offset].m_inlier_ratio =
((double) num_inliers) / ((double) list.size());
memcpy(m_transforms[offset].m_H, M, 9 * sizeof(double));
// m_transforms[offset]->m_scale = sqrt(M[0] * M[0] + M[1] * M[1]);
#if 1
printf("Inliers[%d,%d] = %d out of %d\n", idx1, idx2, num_inliers,
(int) m_matches.GetNumMatches(offset));
// (int) m_match_lists[offset].size());
printf("Ratio[%d,%d] = %0.3e\n",
idx1, idx2, m_transforms[offset].m_inlier_ratio);
matrix_print(3, 3, m_transforms[offset].m_H);
#endif
return true;
} else {
return false;
}
}
