Fix T38844: Crash if weight track = 0
[blender.git] / extern / libmv / libmv / simple_pipeline / intersect.cc
1 // Copyright (c) 2011 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/simple_pipeline/intersect.h"
22
23 #include "libmv/base/vector.h"
24 #include "libmv/logging/logging.h"
25 #include "libmv/multiview/projection.h"
26 #include "libmv/multiview/triangulation.h"
27 #include "libmv/multiview/nviewtriangulation.h"
28 #include "libmv/numeric/numeric.h"
29 #include "libmv/numeric/levenberg_marquardt.h"
30 #include "libmv/simple_pipeline/reconstruction.h"
31 #include "libmv/simple_pipeline/tracks.h"
32
33 #include "ceres/ceres.h"
34
35 namespace libmv {
36
37 namespace {
38
39 class EuclideanIntersectCostFunctor {
40  public:
41   EuclideanIntersectCostFunctor(const Marker &marker,
42                                 const EuclideanCamera &camera)
43       : marker_(marker), camera_(camera) {}
44
45   template<typename T>
46   bool operator()(const T *X, T *residuals) const {
47     typedef Eigen::Matrix<T, 3, 3> Mat3;
48     typedef Eigen::Matrix<T, 3, 1> Vec3;
49
50     Vec3 x(X);
51     Mat3 R(camera_.R.cast<T>());
52     Vec3 t(camera_.t.cast<T>());
53
54     Vec3 projected = R * x + t;
55     projected /= projected(2);
56
57     residuals[0] = (projected(0) - T(marker_.x)) * marker_.weight;
58     residuals[1] = (projected(1) - T(marker_.y)) * marker_.weight;
59
60     return true;
61   }
62
63   const Marker &marker_;
64   const EuclideanCamera &camera_;
65 };
66
67 }  // namespace
68
69 bool EuclideanIntersect(const vector<Marker> &markers,
70                         EuclideanReconstruction *reconstruction) {
71   if (markers.size() < 2) {
72     return false;
73   }
74
75   // Compute projective camera matrices for the cameras the intersection is
76   // going to use.
77   Mat3 K = Mat3::Identity();
78   vector<Mat34> cameras;
79   Mat34 P;
80   for (int i = 0; i < markers.size(); ++i) {
81     EuclideanCamera *camera = reconstruction->CameraForImage(markers[i].image);
82     P_From_KRt(K, camera->R, camera->t, &P);
83     cameras.push_back(P);
84   }
85
86   // Stack the 2D coordinates together as required by NViewTriangulate.
87   Mat2X points(2, markers.size());
88   for (int i = 0; i < markers.size(); ++i) {
89     points(0, i) = markers[i].x;
90     points(1, i) = markers[i].y;
91   }
92
93   Vec4 Xp;
94   LG << "Intersecting with " << markers.size() << " markers.";
95   NViewTriangulateAlgebraic(points, cameras, &Xp);
96
97   // Get euclidean version of the homogeneous point.
98   Xp /= Xp(3);
99   Vec3 X = Xp.head<3>();
100
101   ceres::Problem problem;
102
103   // Add residual blocks to the problem.
104   int num_residuals = 0;
105   for (int i = 0; i < markers.size(); ++i) {
106     const Marker &marker = markers[i];
107     if (marker.weight != 0.0) {
108       const EuclideanCamera &camera =
109           *reconstruction->CameraForImage(marker.image);
110
111       problem.AddResidualBlock(
112           new ceres::AutoDiffCostFunction<
113               EuclideanIntersectCostFunctor,
114               2, /* num_residuals */
115               3>(new EuclideanIntersectCostFunctor(marker, camera)),
116           NULL,
117           &X(0));
118           num_residuals++;
119     }
120   }
121
122   // TODO(sergey): Once we'll update Ceres to the next version
123   // we wouldn't need this check anymore -- Ceres will deal with
124   // zero-sized problems nicely.
125   LG << "Number of residuals: " << num_residuals;
126   if (!num_residuals) {
127     LG << "Skipping running minimizer with zero residuals";
128
129         // We still add 3D point for the track regardless it was
130         // optimized or not. If track is a constant zero it'll use
131         // algebraic intersection result as a 3D coordinate.
132
133     Vec3 point = X.head<3>();
134     reconstruction->InsertPoint(markers[0].track, point);
135
136     return true;
137   }
138
139   // Configure the solve.
140   ceres::Solver::Options solver_options;
141   solver_options.linear_solver_type = ceres::DENSE_QR;
142   solver_options.max_num_iterations = 50;
143   solver_options.update_state_every_iteration = true;
144   solver_options.parameter_tolerance = 1e-16;
145   solver_options.function_tolerance = 1e-16;
146
147   // Run the solve.
148   ceres::Solver::Summary summary;
149   ceres::Solve(solver_options, &problem, &summary);
150
151   VLOG(1) << "Summary:\n" << summary.FullReport();
152
153   // Try projecting the point; make sure it's in front of everyone.
154   for (int i = 0; i < cameras.size(); ++i) {
155     const EuclideanCamera &camera =
156         *reconstruction->CameraForImage(markers[i].image);
157     Vec3 x = camera.R * X + camera.t;
158     if (x(2) < 0) {
159       LOG(ERROR) << "POINT BEHIND CAMERA " << markers[i].image
160                  << ": " << x.transpose();
161       return false;
162     }
163   }
164
165   Vec3 point = X.head<3>();
166   reconstruction->InsertPoint(markers[0].track, point);
167
168   // TODO(keir): Add proper error checking.
169   return true;
170 }
171
172 namespace {
173
174 struct ProjectiveIntersectCostFunction {
175  public:
176   typedef Vec  FMatrixType;
177   typedef Vec4 XMatrixType;
178
179   ProjectiveIntersectCostFunction(
180       const vector<Marker> &markers,
181       const ProjectiveReconstruction &reconstruction)
182     : markers(markers), reconstruction(reconstruction) {}
183
184   Vec operator()(const Vec4 &X) const {
185     Vec residuals(2 * markers.size());
186     residuals.setZero();
187     for (int i = 0; i < markers.size(); ++i) {
188       const ProjectiveCamera &camera =
189           *reconstruction.CameraForImage(markers[i].image);
190       Vec3 projected = camera.P * X;
191       projected /= projected(2);
192       residuals[2*i + 0] = projected(0) - markers[i].x;
193       residuals[2*i + 1] = projected(1) - markers[i].y;
194     }
195     return residuals;
196   }
197   const vector<Marker> &markers;
198   const ProjectiveReconstruction &reconstruction;
199 };
200
201 }  // namespace
202
203 bool ProjectiveIntersect(const vector<Marker> &markers,
204                          ProjectiveReconstruction *reconstruction) {
205   if (markers.size() < 2) {
206     return false;
207   }
208
209   // Get the cameras to use for the intersection.
210   vector<Mat34> cameras;
211   for (int i = 0; i < markers.size(); ++i) {
212     ProjectiveCamera *camera = reconstruction->CameraForImage(markers[i].image);
213     cameras.push_back(camera->P);
214   }
215
216   // Stack the 2D coordinates together as required by NViewTriangulate.
217   Mat2X points(2, markers.size());
218   for (int i = 0; i < markers.size(); ++i) {
219     points(0, i) = markers[i].x;
220     points(1, i) = markers[i].y;
221   }
222
223   Vec4 X;
224   LG << "Intersecting with " << markers.size() << " markers.";
225   NViewTriangulateAlgebraic(points, cameras, &X);
226   X /= X(3);
227
228   typedef LevenbergMarquardt<ProjectiveIntersectCostFunction> Solver;
229
230   ProjectiveIntersectCostFunction triangulate_cost(markers, *reconstruction);
231   Solver::SolverParameters params;
232   Solver solver(triangulate_cost);
233
234   Solver::Results results = solver.minimize(params, &X);
235   (void) results;  // TODO(keir): Ensure results are good.
236
237   // Try projecting the point; make sure it's in front of everyone.
238   for (int i = 0; i < cameras.size(); ++i) {
239     const ProjectiveCamera &camera =
240         *reconstruction->CameraForImage(markers[i].image);
241     Vec3 x = camera.P * X;
242     if (x(2) < 0) {
243       LOG(ERROR) << "POINT BEHIND CAMERA " << markers[i].image
244                  << ": " << x.transpose();
245     }
246   }
247
248   reconstruction->InsertPoint(markers[0].track, X);
249
250   // TODO(keir): Add proper error checking.
251   return true;
252 }
253
254 }  // namespace libmv