076a9e6434662e8d671d2700a22fab1fed854325
[blender.git] / extern / libmv / third_party / ssba / Geometry / v3d_metricbundle.h
1 // -*- C++ -*-
2 /*
3 Copyright (c) 2008 University of North Carolina at Chapel Hill
4
5 This file is part of SSBA (Simple Sparse Bundle Adjustment).
6
7 SSBA is free software: you can redistribute it and/or modify it under the
8 terms of the GNU Lesser General Public License as published by the Free
9 Software Foundation, either version 3 of the License, or (at your option) any
10 later version.
11
12 SSBA is distributed in the hope that it will be useful, but WITHOUT ANY
13 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
14 A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
15 details.
16
17 You should have received a copy of the GNU Lesser General Public License along
18 with SSBA. If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 #ifndef V3D_METRICBUNDLE_H
22 #define V3D_METRICBUNDLE_H
23
24 # if defined(V3DLIB_ENABLE_SUITESPARSE)
25
26 #include "Math/v3d_optimization.h"
27 #include "Math/v3d_linear.h"
28 #include "Math/v3d_linear_utils.h"
29 #include "Geometry/v3d_cameramatrix.h"
30 #include "Geometry/v3d_distortion.h"
31
32 namespace V3D
33 {
34
35    // This structure provides some helper functions common to all metric BAs
36    struct MetricBundleOptimizerBase : public SparseLevenbergOptimizer
37    {
38          typedef SparseLevenbergOptimizer Base;
39
40          MetricBundleOptimizerBase(double inlierThreshold,
41                                    vector<CameraMatrix>& cams,
42                                    vector<Vector3d >& Xs,
43                                    vector<Vector2d > const& measurements,
44                                    vector<int> const& corrspondingView,
45                                    vector<int> const& corrspondingPoint,
46                                    int nAddParamsA, int nParamsC)
47             : SparseLevenbergOptimizer(2, cams.size(), 6+nAddParamsA, Xs.size(), 3, nParamsC,
48                                        corrspondingView, corrspondingPoint),
49               _cams(cams), _Xs(Xs), _measurements(measurements),
50               _savedTranslations(cams.size()), _savedRotations(cams.size()),
51               _savedXs(Xs.size()),
52               _inlierThreshold(inlierThreshold), _cachedParamLength(0.0)
53          {
54             // Since we assume that BA does not alter the inputs too much,
55             // we compute the overall length of the parameter vector in advance
56             // and return that value as the result of getParameterLength().
57             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
58             {
59                _cachedParamLength += sqrNorm_L2(_cams[i].getTranslation());
60                _cachedParamLength += 3.0; // Assume eye(3) for R.
61             }
62             for (int j = _nNonvaryingB; j < _nParametersB; ++j)
63                _cachedParamLength += sqrNorm_L2(_Xs[j]);
64
65             _cachedParamLength = sqrt(_cachedParamLength);
66          }
67
68          // Huber robust cost function.
69          virtual void fillWeights(VectorArray<double> const& residual, Vector<double>& w)
70          {
71             for (unsigned int k = 0; k < w.size(); ++k)
72             {
73                Vector<double> const& r = residual[k];
74                double const e = norm_L2(r);
75                w[k] = (e < _inlierThreshold) ? 1.0 : sqrt(_inlierThreshold / e);
76             } // end for (k)
77          }
78
79          virtual double getParameterLength() const
80          {
81             return _cachedParamLength;
82          }
83
84          virtual void updateParametersA(VectorArray<double> const& deltaAi);
85          virtual void updateParametersB(VectorArray<double> const& deltaBj);
86          virtual void updateParametersC(Vector<double> const& deltaC)
87          {
88             (void)deltaC;
89          }
90
91          virtual void saveAllParameters()
92          {
93             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
94             {
95                _savedTranslations[i] = _cams[i].getTranslation();
96                _savedRotations[i]    = _cams[i].getRotation();
97             }
98             _savedXs = _Xs;
99          }
100
101          virtual void restoreAllParameters()
102          {
103             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
104             {
105                _cams[i].setTranslation(_savedTranslations[i]);
106                _cams[i].setRotation(_savedRotations[i]);
107             }
108             _Xs = _savedXs;
109          }
110
111       protected:
112          typedef InlineMatrix<double, 3, 6> Matrix3x6d;
113
114          void poseDerivatives(int i, int j, Vector3d& XX,
115                               Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const;
116
117          vector<CameraMatrix>& _cams;
118          vector<Vector3d>&     _Xs;
119
120          vector<Vector2d> const& _measurements;
121
122          vector<Vector3d>   _savedTranslations;
123          vector<Matrix3x3d> _savedRotations;
124          vector<Vector3d>   _savedXs;
125
126          double const _inlierThreshold;
127          double       _cachedParamLength;
128    }; // end struct MetricBundleOptimizerBase
129
130    struct StdMetricBundleOptimizer : public MetricBundleOptimizerBase
131    {
132          typedef MetricBundleOptimizerBase Base;
133
134          StdMetricBundleOptimizer(double inlierThreshold,
135                                   vector<CameraMatrix>& cams,
136                                   vector<Vector3d >& Xs,
137                                   vector<Vector2d > const& measurements,
138                                   vector<int> const& corrspondingView,
139                                   vector<int> const& corrspondingPoint)
140             : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements,
141                                         corrspondingView, corrspondingPoint, 0, 0)
142          { }
143
144          virtual void evalResidual(VectorArray<double>& e)
145          {
146             for (unsigned int k = 0; k < e.count(); ++k)
147             {
148                int const i = _correspondingParamA[k];
149                int const j = _correspondingParamB[k];
150
151                Vector2d const q = _cams[i].projectPoint(_Xs[j]);
152                e[k][0] = q[0] - _measurements[k][0];
153                e[k][1] = q[1] - _measurements[k][1];
154             }
155          }
156
157          virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
158                                     int i, int j, int k);
159    }; // end struct StdMetricBundleOptimizer
160
161 //----------------------------------------------------------------------
162
163    enum
164    {
165       FULL_BUNDLE_METRIC = 0,
166       FULL_BUNDLE_FOCAL_LENGTH = 1,      // f
167       FULL_BUNDLE_FOCAL_LENGTH_PP = 2,   // f, cx, cy
168       FULL_BUNDLE_RADIAL = 3,            // f, cx, cy, k1, k2
169       FULL_BUNDLE_RADIAL_TANGENTIAL = 4  // f, cx, cy, k1, k2, p1, p2
170    };
171
172    struct CommonInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase
173    {
174          static int globalParamDimensionFromMode(int mode)
175          {
176             switch (mode)
177             {
178                case FULL_BUNDLE_METRIC:            return 0;
179                case FULL_BUNDLE_FOCAL_LENGTH:      return 1;
180                case FULL_BUNDLE_FOCAL_LENGTH_PP:   return 3;
181                case FULL_BUNDLE_RADIAL:            return 5;
182                case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7;
183             }
184             return 0;
185          }
186
187          typedef MetricBundleOptimizerBase Base;
188
189          CommonInternalsMetricBundleOptimizer(int mode,
190                                               double inlierThreshold,
191                                               Matrix3x3d& K,
192                                               StdDistortionFunction& distortion,
193                                               vector<CameraMatrix>& cams,
194                                               vector<Vector3d >& Xs,
195                                               vector<Vector2d > const& measurements,
196                                               vector<int> const& corrspondingView,
197                                               vector<int> const& corrspondingPoint)
198             : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements,
199                                         corrspondingView, corrspondingPoint,
200                                         0, globalParamDimensionFromMode(mode)),
201               _mode(mode), _K(K), _distortion(distortion)
202          {
203             _cachedAspectRatio = K[1][1] / K[0][0];
204          }
205
206          Vector2d projectPoint(Vector3d const& X, int i) const
207          {
208             Vector3d const XX = _cams[i].transformPointIntoCameraSpace(X);
209             Vector2d p;
210             p[0] = XX[0] / XX[2];
211             p[1] = XX[1] / XX[2];
212             p = _distortion(p);
213             Vector2d res;
214             res[0] = _K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2];
215             res[1] =                   _K[1][1] * p[1] + _K[1][2];
216             return res;
217          }
218
219          virtual void evalResidual(VectorArray<double>& e)
220          {
221             for (unsigned int k = 0; k < e.count(); ++k)
222             {
223                int const i = _correspondingParamA[k];
224                int const j = _correspondingParamB[k];
225
226                Vector2d const q = this->projectPoint(_Xs[j], i);
227                e[k][0] = q[0] - _measurements[k][0];
228                e[k][1] = q[1] - _measurements[k][1];
229             }
230          }
231
232          virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
233                                     int i, int j, int k);
234
235          virtual void updateParametersC(Vector<double> const& deltaC);
236
237          virtual void saveAllParameters()
238          {
239             Base::saveAllParameters();
240             _savedK = _K;
241             _savedDistortion = _distortion;
242          }
243
244          virtual void restoreAllParameters()
245          {
246             Base::restoreAllParameters();
247             _K = _savedK;
248             _distortion = _savedDistortion;
249          }
250
251       protected:
252          int                    _mode;
253          Matrix3x3d&            _K;
254          StdDistortionFunction& _distortion;
255
256          Matrix3x3d            _savedK;
257          StdDistortionFunction _savedDistortion;
258          double                _cachedAspectRatio;
259    }; // end struct CommonInternalsMetricBundleOptimizer
260
261 //----------------------------------------------------------------------
262
263    struct VaryingInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase
264    {
265          static int extParamDimensionFromMode(int mode)
266          {
267             switch (mode)
268             {
269                case FULL_BUNDLE_METRIC:            return 0;
270                case FULL_BUNDLE_FOCAL_LENGTH:      return 1;
271                case FULL_BUNDLE_FOCAL_LENGTH_PP:   return 3;
272                case FULL_BUNDLE_RADIAL:            return 5;
273                case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7;
274             }
275             return 0;
276          }
277
278          typedef MetricBundleOptimizerBase Base;
279
280          VaryingInternalsMetricBundleOptimizer(int mode,
281                                                double inlierThreshold,
282                                                std::vector<StdDistortionFunction>& distortions,
283                                                vector<CameraMatrix>& cams,
284                                                vector<Vector3d >& Xs,
285                                                vector<Vector2d > const& measurements,
286                                                vector<int> const& corrspondingView,
287                                                vector<int> const& corrspondingPoint)
288             : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements,
289                                         corrspondingView, corrspondingPoint,
290                                         extParamDimensionFromMode(mode), 0),
291               _mode(mode), _distortions(distortions),
292               _savedKs(cams.size()), _savedDistortions(cams.size())
293          { }
294
295          Vector2d projectPoint(Vector3d const& X, int i) const
296          {
297             return _cams[i].projectPoint(_distortions[i], X);
298          }
299
300          virtual void evalResidual(VectorArray<double>& e)
301          {
302             for (unsigned int k = 0; k < e.count(); ++k)
303             {
304                int const i = _correspondingParamA[k];
305                int const j = _correspondingParamB[k];
306
307                Vector2d const q = this->projectPoint(_Xs[j], i);
308                e[k][0] = q[0] - _measurements[k][0];
309                e[k][1] = q[1] - _measurements[k][1];
310             }
311          }
312
313          virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
314                                     int i, int j, int k);
315
316          virtual void updateParametersA(VectorArray<double> const& deltaAi);
317
318          virtual void saveAllParameters()
319          {
320             Base::saveAllParameters();
321             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
322                _savedKs[i] = _cams[i].getIntrinsic();
323             std::copy(_distortions.begin(), _distortions.end(), _savedDistortions.begin());
324          }
325
326          virtual void restoreAllParameters()
327          {
328             Base::restoreAllParameters();
329             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
330                _cams[i].setIntrinsic(_savedKs[i]);
331             std::copy(_savedDistortions.begin(), _savedDistortions.end(), _distortions.begin());
332          }
333
334       protected:
335          int                                 _mode;
336          std::vector<StdDistortionFunction>& _distortions;
337
338          std::vector<Matrix3x3d>            _savedKs;
339          std::vector<StdDistortionFunction> _savedDistortions;
340    }; // end struct VaryingInternalsMetricBundleOptimizer
341
342 } // end namespace V3D
343
344 # endif
345
346 #endif