1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
// Copyright (c) 2012 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
#include <cstdio>
#include "libmv/logging/logging.h"
#include "libmv/simple_pipeline/modal_solver.h"
#include "libmv/simple_pipeline/rigid_registration.h"
#ifdef _MSC_VER
# define snprintf _snprintf
#endif
namespace libmv {
static void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) {
X(0) = marker.x;
X(1) = marker.y;
X(2) = 1.0;
X *= 5.0 / X.norm();
}
static void ModalSolverLogProress(ProgressUpdateCallback *update_callback,
double progress)
{
if (update_callback) {
char message[256];
snprintf(message, sizeof(message), "Solving progress %d%%", (int)(progress * 100));
update_callback->invoke(progress, message);
}
}
void ModalSolver(Tracks &tracks,
EuclideanReconstruction *reconstruction,
ProgressUpdateCallback *update_callback) {
int max_image = tracks.MaxImage();
int max_track = tracks.MaxTrack();
LG << "Max image: " << max_image;
LG << "Max track: " << max_track;
Mat3 R = Mat3::Identity();
for (int image = 0; image <= max_image; ++image) {
vector<Marker> all_markers = tracks.MarkersInImage(image);
ModalSolverLogProress(update_callback, (float) image / max_image);
// Skip empty frames without doing anything
if (all_markers.size() == 0) {
LG << "Skipping frame: " << image;
continue;
}
vector<Vec3> points, reference_points;
// Cnstruct pairs of markers from current and previous image,
// to reproject them and find rigid transformation between
// previous and current image
for (int track = 0; track <= max_track; ++track) {
EuclideanPoint *point = reconstruction->PointForTrack(track);
if (point) {
Marker marker = tracks.MarkerInImageForTrack(image, track);
if (marker.image == image) {
Vec3 X;
LG << "Use track " << track << " for rigid registration between image " <<
image - 1 << " and " << image;
ProjectMarkerOnSphere(marker, X);
points.push_back(point->X);
reference_points.push_back(X);
}
}
}
if (points.size()) {
// Find rigid delta transformation to current image
RigidRegistration(reference_points, points, R);
}
reconstruction->InsertCamera(image, R, Vec3::Zero());
// Review if there's new tracks for which position might be reconstructed
for (int track = 0; track <= max_track; ++track) {
if (!reconstruction->PointForTrack(track)) {
Marker marker = tracks.MarkerInImageForTrack(image, track);
if (marker.image == image) {
// New track appeared on this image, project it's position onto sphere
LG << "Projecting track " << track << " at image " << image;
Vec3 X;
ProjectMarkerOnSphere(marker, X);
reconstruction->InsertPoint(track, R.inverse() * X);
}
}
}
}
}
} // namespace libmv
|
