Assorted camera tracker improvements
[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       FULL_BUNDLE_FOCAL_AND_RADIAL_K1 = 5,      // f, k1
171       FULL_BUNDLE_FOCAL_AND_RADIAL = 6,      // f, k1, k2
172    };
173
174    struct CommonInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase
175    {
176          static int globalParamDimensionFromMode(int mode)
177          {
178             switch (mode)
179             {
180                case FULL_BUNDLE_METRIC:              return 0;
181                case FULL_BUNDLE_FOCAL_LENGTH:        return 1;
182                case FULL_BUNDLE_FOCAL_LENGTH_PP:     return 3;
183                case FULL_BUNDLE_RADIAL:              return 5;
184                case FULL_BUNDLE_RADIAL_TANGENTIAL:   return 7;
185                case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
186                case FULL_BUNDLE_FOCAL_AND_RADIAL:    return 3;
187             }
188             return 0;
189          }
190
191          typedef MetricBundleOptimizerBase Base;
192
193          CommonInternalsMetricBundleOptimizer(int mode,
194                                               double inlierThreshold,
195                                               Matrix3x3d& K,
196                                               StdDistortionFunction& distortion,
197                                               vector<CameraMatrix>& cams,
198                                               vector<Vector3d >& Xs,
199                                               vector<Vector2d > const& measurements,
200                                               vector<int> const& corrspondingView,
201                                               vector<int> const& corrspondingPoint)
202             : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements,
203                                         corrspondingView, corrspondingPoint,
204                                         0, globalParamDimensionFromMode(mode)),
205               _mode(mode), _K(K), _distortion(distortion)
206          {
207             _cachedAspectRatio = K[1][1] / K[0][0];
208          }
209
210          Vector2d projectPoint(Vector3d const& X, int i) const
211          {
212             Vector3d const XX = _cams[i].transformPointIntoCameraSpace(X);
213             Vector2d p;
214             p[0] = XX[0] / XX[2];
215             p[1] = XX[1] / XX[2];
216             p = _distortion(p);
217             Vector2d res;
218             res[0] = _K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2];
219             res[1] =                   _K[1][1] * p[1] + _K[1][2];
220             return res;
221          }
222
223          virtual void evalResidual(VectorArray<double>& e)
224          {
225             for (unsigned int k = 0; k < e.count(); ++k)
226             {
227                int const i = _correspondingParamA[k];
228                int const j = _correspondingParamB[k];
229
230                Vector2d const q = this->projectPoint(_Xs[j], i);
231                e[k][0] = q[0] - _measurements[k][0];
232                e[k][1] = q[1] - _measurements[k][1];
233             }
234          }
235
236          virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
237                                     int i, int j, int k);
238
239          virtual void updateParametersC(Vector<double> const& deltaC);
240
241          virtual void saveAllParameters()
242          {
243             Base::saveAllParameters();
244             _savedK = _K;
245             _savedDistortion = _distortion;
246          }
247
248          virtual void restoreAllParameters()
249          {
250             Base::restoreAllParameters();
251             _K = _savedK;
252             _distortion = _savedDistortion;
253          }
254
255       protected:
256          int                    _mode;
257          Matrix3x3d&            _K;
258          StdDistortionFunction& _distortion;
259
260          Matrix3x3d            _savedK;
261          StdDistortionFunction _savedDistortion;
262          double                _cachedAspectRatio;
263    }; // end struct CommonInternalsMetricBundleOptimizer
264
265 //----------------------------------------------------------------------
266
267    struct VaryingInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase
268    {
269          static int extParamDimensionFromMode(int mode)
270          {
271             switch (mode)
272             {
273                case FULL_BUNDLE_METRIC:              return 0;
274                case FULL_BUNDLE_FOCAL_LENGTH:        return 1;
275                case FULL_BUNDLE_FOCAL_LENGTH_PP:     return 3;
276                case FULL_BUNDLE_RADIAL:              return 5;
277                case FULL_BUNDLE_RADIAL_TANGENTIAL:   return 7;
278                case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
279                case FULL_BUNDLE_FOCAL_AND_RADIAL:    return 3;
280             }
281             return 0;
282          }
283
284          typedef MetricBundleOptimizerBase Base;
285
286          VaryingInternalsMetricBundleOptimizer(int mode,
287                                                double inlierThreshold,
288                                                std::vector<StdDistortionFunction>& distortions,
289                                                vector<CameraMatrix>& cams,
290                                                vector<Vector3d >& Xs,
291                                                vector<Vector2d > const& measurements,
292                                                vector<int> const& corrspondingView,
293                                                vector<int> const& corrspondingPoint)
294             : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements,
295                                         corrspondingView, corrspondingPoint,
296                                         extParamDimensionFromMode(mode), 0),
297               _mode(mode), _distortions(distortions),
298               _savedKs(cams.size()), _savedDistortions(cams.size())
299          { }
300
301          Vector2d projectPoint(Vector3d const& X, int i) const
302          {
303             return _cams[i].projectPoint(_distortions[i], X);
304          }
305
306          virtual void evalResidual(VectorArray<double>& e)
307          {
308             for (unsigned int k = 0; k < e.count(); ++k)
309             {
310                int const i = _correspondingParamA[k];
311                int const j = _correspondingParamB[k];
312
313                Vector2d const q = this->projectPoint(_Xs[j], i);
314                e[k][0] = q[0] - _measurements[k][0];
315                e[k][1] = q[1] - _measurements[k][1];
316             }
317          }
318
319          virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
320                                     int i, int j, int k);
321
322          virtual void updateParametersA(VectorArray<double> const& deltaAi);
323
324          virtual void saveAllParameters()
325          {
326             Base::saveAllParameters();
327             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
328                _savedKs[i] = _cams[i].getIntrinsic();
329             std::copy(_distortions.begin(), _distortions.end(), _savedDistortions.begin());
330          }
331
332          virtual void restoreAllParameters()
333          {
334             Base::restoreAllParameters();
335             for (int i = _nNonvaryingA; i < _nParametersA; ++i)
336                _cams[i].setIntrinsic(_savedKs[i]);
337             std::copy(_savedDistortions.begin(), _savedDistortions.end(), _distortions.begin());
338          }
339
340       protected:
341          int                                 _mode;
342          std::vector<StdDistortionFunction>& _distortions;
343
344          std::vector<Matrix3x3d>            _savedKs;
345          std::vector<StdDistortionFunction> _savedDistortions;
346    }; // end struct VaryingInternalsMetricBundleOptimizer
347
348 } // end namespace V3D
349
350 # endif
351
352 #endif