diff options
Diffstat (limited to 'extern/libmv/third_party/ssba')
| -rw-r--r-- | extern/libmv/third_party/ssba/COPYING.TXT | 165 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Geometry/v3d_cameramatrix.h | 204 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Geometry/v3d_distortion.h | 97 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp | 365 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h | 346 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Math/v3d_linear.h | 923 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Math/v3d_linear_utils.h | 391 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Math/v3d_mathutilities.h | 59 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Math/v3d_optimization.cpp | 955 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/Math/v3d_optimization.h | 273 | ||||
| -rw-r--r-- | extern/libmv/third_party/ssba/README.TXT | 92 | ||||
| -rwxr-xr-x | extern/libmv/third_party/ssba/README.libmv | 23 |
12 files changed, 3893 insertions, 0 deletions
diff --git a/extern/libmv/third_party/ssba/COPYING.TXT b/extern/libmv/third_party/ssba/COPYING.TXT new file mode 100644 index 00000000000..fc8a5de7edf --- /dev/null +++ b/extern/libmv/third_party/ssba/COPYING.TXT @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/> + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_cameramatrix.h b/extern/libmv/third_party/ssba/Geometry/v3d_cameramatrix.h new file mode 100644 index 00000000000..448ae9714e5 --- /dev/null +++ b/extern/libmv/third_party/ssba/Geometry/v3d_cameramatrix.h @@ -0,0 +1,204 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_CAMERA_MATRIX_H +#define V3D_CAMERA_MATRIX_H + +#include "Math/v3d_linear.h" +#include "Geometry/v3d_distortion.h" + +namespace V3D +{ + + struct CameraMatrix + { + CameraMatrix() + { + makeIdentityMatrix(_K); + makeIdentityMatrix(_R); + makeZeroVector(_T); + this->updateCachedValues(true, true); + } + + CameraMatrix(double f, double cx, double cy) + { + makeIdentityMatrix(_K); + _K[0][0] = f; + _K[1][1] = f; + _K[0][2] = cx; + _K[1][2] = cy; + makeIdentityMatrix(_R); + makeZeroVector(_T); + this->updateCachedValues(true, true); + } + + CameraMatrix(Matrix3x3d const& K, + Matrix3x3d const& R, + Vector3d const& T) + : _K(K), _R(R), _T(T) + { + this->updateCachedValues(true, true); + } + + void setIntrinsic(Matrix3x3d const& K) { _K = K; this->updateCachedValues(true, false); } + void setRotation(Matrix3x3d const& R) { _R = R; this->updateCachedValues(false, true); } + void setTranslation(Vector3d const& T) { _T = T; this->updateCachedValues(false, true); } + + template <typename Mat> + void setOrientation(Mat const& RT) + { + _R[0][0] = RT[0][0]; _R[0][1] = RT[0][1]; _R[0][2] = RT[0][2]; + _R[1][0] = RT[1][0]; _R[1][1] = RT[1][1]; _R[1][2] = RT[1][2]; + _R[2][0] = RT[2][0]; _R[2][1] = RT[2][1]; _R[2][2] = RT[2][2]; + _T[0] = RT[0][3]; _T[1] = RT[1][3]; _T[2] = RT[2][3]; + this->updateCachedValues(false, true); + } + + Matrix3x3d const& getIntrinsic() const { return _K; } + Matrix3x3d const& getRotation() const { return _R; } + Vector3d const& getTranslation() const { return _T; } + + Matrix3x4d getOrientation() const + { + Matrix3x4d RT; + RT[0][0] = _R[0][0]; RT[0][1] = _R[0][1]; RT[0][2] = _R[0][2]; + RT[1][0] = _R[1][0]; RT[1][1] = _R[1][1]; RT[1][2] = _R[1][2]; + RT[2][0] = _R[2][0]; RT[2][1] = _R[2][1]; RT[2][2] = _R[2][2]; + RT[0][3] = _T[0]; RT[1][3] = _T[1]; RT[2][3] = _T[2]; + return RT; + } + + Matrix3x4d getProjection() const + { + Matrix3x4d const RT = this->getOrientation(); + return _K * RT; + } + + double getFocalLength() const { return _K[0][0]; } + double getAspectRatio() const { return _K[1][1] / _K[0][0]; } + + Vector2d getPrincipalPoint() const + { + Vector2d pp; + pp[0] = _K[0][2]; + pp[1] = _K[1][2]; + return pp; + } + + Vector2d projectPoint(Vector3d const& X) const + { + Vector3d q = _K*(_R*X + _T); + Vector2d res; + res[0] = q[0]/q[2]; res[1] = q[1]/q[2]; + return res; + } + + template <typename Distortion> + Vector2d projectPoint(Distortion const& distortion, Vector3d const& X) const + { + Vector3d XX = _R*X + _T; + Vector2d p; + p[0] = XX[0] / XX[2]; + p[1] = XX[1] / XX[2]; + p = distortion(p); + + Vector2d res; + res[0] = _K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2]; + res[1] = _K[1][1] * p[1] + _K[1][2]; + return res; + } + + Vector3d unprojectPixel(Vector2d const &p, double depth = 1) const + { + Vector3d pp; + pp[0] = p[0]; pp[1] = p[1]; pp[2] = 1.0; + Vector3d ray = _invK * pp; + ray[0] *= depth/ray[2]; + ray[1] *= depth/ray[2]; + ray[2] = depth; + ray = _Rt * ray; + return _center + ray; + } + + Vector3d transformPointIntoCameraSpace(Vector3d const& p) const + { + return _R*p + _T; + } + + Vector3d transformPointFromCameraSpace(Vector3d const& p) const + { + return _Rt*(p-_T); + } + + Vector3d transformDirectionFromCameraSpace(Vector3d const& dir) const + { + return _Rt*dir; + } + + Vector3d const& cameraCenter() const + { + return _center; + } + + Vector3d opticalAxis() const + { + return this->transformDirectionFromCameraSpace(makeVector3(0.0, 0.0, 1.0)); + } + + Vector3d upVector() const + { + return this->transformDirectionFromCameraSpace(makeVector3(0.0, 1.0, 0.0)); + } + + Vector3d rightVector() const + { + return this->transformDirectionFromCameraSpace(makeVector3(1.0, 0.0, 0.0)); + } + + Vector3d getRay(Vector2d const& p) const + { + Vector3d pp = makeVector3(p[0], p[1], 1.0); + Vector3d ray = _invK * pp; + ray = _Rt * ray; + normalizeVector(ray); + return ray; + } + + protected: + void updateCachedValues(bool intrinsic, bool orientation) + { + if (intrinsic) _invK = invertedMatrix(_K); + + if (orientation) + { + makeTransposedMatrix(_R, _Rt); + _center = _Rt * (-1.0 * _T); + } + } + + Matrix3x3d _K, _R; + Vector3d _T; + Matrix3x3d _invK, _Rt; + Vector3d _center; + }; // end struct CameraMatrix + +} // end namespace V3D + +#endif diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_distortion.h b/extern/libmv/third_party/ssba/Geometry/v3d_distortion.h new file mode 100644 index 00000000000..d0816558314 --- /dev/null +++ b/extern/libmv/third_party/ssba/Geometry/v3d_distortion.h @@ -0,0 +1,97 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_DISTORTION_H +#define V3D_DISTORTION_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_utils.h" + +namespace V3D +{ + + struct StdDistortionFunction + { + double k1, k2, p1, p2; + + StdDistortionFunction() + : k1(0), k2(0), p1(0), p2(0) + { } + + Vector2d operator()(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + + Vector2d xd; + xd[0] = kr * xu[0] + 2*p1*xu[0]*xu[1] + p2*(r2 + 2*xu[0]*xu[0]); + xd[1] = kr * xu[1] + 2*p2*xu[0]*xu[1] + p1*(r2 + 2*xu[1]*xu[1]); + return xd; + } + + Matrix2x2d derivativeWrtRadialParameters(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + double const r4 = r2*r2; + //double const kr = 1 + k1*r2 + k2*r4; + + Matrix2x2d deriv; + + deriv[0][0] = xu[0] * r2; // d xd/d k1 + deriv[0][1] = xu[0] * r4; // d xd/d k2 + deriv[1][0] = xu[1] * r2; // d yd/d k1 + deriv[1][1] = xu[1] * r4; // d yd/d k2 + return deriv; + } + + Matrix2x2d derivativeWrtTangentialParameters(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + //double const r4 = r2*r2; + //double const kr = 1 + k1*r2 + k2*r4; + + Matrix2x2d deriv; + deriv[0][0] = 2*xu[0]*xu[1]; // d xd/d p1 + deriv[0][1] = r2 + 2*xu[0]*xu[0]; // d xd/d p2 + deriv[1][0] = r2 + 2*xu[1]*xu[1]; // d yd/d p1 + deriv[1][1] = deriv[0][0]; // d yd/d p2 + return deriv; + } + + Matrix2x2d derivativeWrtUndistortedPoint(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + double const dkr = 2*k1 + 4*k2*r2; + + Matrix2x2d deriv; + deriv[0][0] = kr + xu[0] * xu[0] * dkr + 2*p1*xu[1] + 6*p2*xu[0]; // d xd/d xu + deriv[0][1] = xu[0] * xu[1] * dkr + 2*p1*xu[0] + 2*p2*xu[1]; // d xd/d yu + deriv[1][0] = deriv[0][1]; // d yd/d xu + deriv[1][1] = kr + xu[1] * xu[1] * dkr + 6*p1*xu[1] + 2*p2*xu[0]; // d yd/d yu + return deriv; + } + }; // end struct StdDistortionFunction + +} // end namespace V3D + +#endif diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp new file mode 100644 index 00000000000..1c1f0cb2627 --- /dev/null +++ b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp @@ -0,0 +1,365 @@ +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#include "Geometry/v3d_metricbundle.h" + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + +namespace +{ + + typedef V3D::InlineMatrix<double, 2, 4> Matrix2x4d; + typedef V3D::InlineMatrix<double, 4, 2> Matrix4x2d; + typedef V3D::InlineMatrix<double, 2, 6> Matrix2x6d; + +} // end namespace <> + +namespace V3D +{ + + void + MetricBundleOptimizerBase::updateParametersA(VectorArray<double> const& deltaAi) + { + Vector3d T, omega; + Matrix3x3d R0, dR; + + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + T = _cams[i].getTranslation(); + T[0] += deltaAi[i][0]; + T[1] += deltaAi[i][1]; + T[2] += deltaAi[i][2]; + _cams[i].setTranslation(T); + + // Create incremental rotation using Rodriguez formula. + R0 = _cams[i].getRotation(); + omega[0] = deltaAi[i][3]; + omega[1] = deltaAi[i][4]; + omega[2] = deltaAi[i][5]; + createRotationMatrixRodriguez(omega, dR); + _cams[i].setRotation(dR * R0); + } // end for (i) + } // end MetricBundleOptimizerBase::updateParametersA() + + void + MetricBundleOptimizerBase::updateParametersB(VectorArray<double> const& deltaBj) + { + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + { + _Xs[j][0] += deltaBj[j][0]; + _Xs[j][1] += deltaBj[j][1]; + _Xs[j][2] += deltaBj[j][2]; + } + } // end MetricBundleOptimizerBase::updateParametersB() + + void + MetricBundleOptimizerBase::poseDerivatives(int i, int j, Vector3d& XX, + Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const + { + XX = _cams[i].transformPointIntoCameraSpace(_Xs[j]); + + // See Frank Dellaerts bundle adjustment tutorial. + // d(dR * R0 * X + t)/d omega = -[R0 * X]_x + Matrix3x3d J; + makeCrossProductMatrix(XX - _cams[i].getTranslation(), J); + scaleMatrixIP(-1.0, J); + + // Now the transformation from world coords into camera space is xx = Rx + T + // Hence the derivative of x wrt. T is just the identity matrix. + makeIdentityMatrix(d_dRT); + copyMatrixSlice(J, 0, 0, 3, 3, d_dRT, 0, 3); + + // The derivative of Rx+T wrt x is just R. + copyMatrix(_cams[i].getRotation(), d_dX); + } // end MetricBundleOptimizerBase::poseDerivatives() + + +//---------------------------------------------------------------------- + + void + StdMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak, + Matrix<double>& Bk, + Matrix<double>& Ck, + int i, int j, int k) + { + Vector3d XX; + Matrix3x6d d_dRT; + Matrix3x3d d_dX; + this->poseDerivatives(i, j, XX, d_dRT, d_dX); + + double const f = _cams[i].getFocalLength(); + double const ar = _cams[i].getAspectRatio(); + + Matrix2x3d dp_dX; + double const bx = f / (XX[2] * XX[2]); + double const by = ar * bx; + dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0; dp_dX[0][2] = -bx * XX[0]; + dp_dX[1][0] = 0; dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1]; + + multiply_A_B(dp_dX, d_dRT, Ak); + multiply_A_B(dp_dX, d_dX, Bk); + } // end StdMetricBundleOptimizer::fillJacobians() + + //---------------------------------------------------------------------- + + void + CommonInternalsMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak, + Matrix<double>& Bk, + Matrix<double>& Ck, + int i, int j, int k) + { + double const focalLength = _K[0][0]; + + Vector3d XX; + Matrix3x6d dXX_dRT; + Matrix3x3d dXX_dX; + this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX); + + Vector2d xu; // undistorted image point + xu[0] = XX[0] / XX[2]; + xu[1] = XX[1] / XX[2]; + + Vector2d const xd = _distortion(xu); // distorted image point + + Matrix2x2d dp_dxd; + dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0; + dp_dxd[1][0] = 0; dp_dxd[1][1] = _cachedAspectRatio * focalLength; + + { + // First, lets do the derivative wrt the structure and motion parameters. + Matrix2x3d dxu_dXX; + dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]); + dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]); + + Matrix2x2d dxd_dxu = _distortion.derivativeWrtUndistortedPoint(xu); + + Matrix2x2d dp_dxu = dp_dxd * dxd_dxu; + Matrix2x3d dp_dXX = dp_dxu * dxu_dXX; + + multiply_A_B(dp_dXX, dXX_dRT, Ak); + multiply_A_B(dp_dXX, dXX_dX, Bk); + } // end scope + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + Matrix2x2d dxd_dp1p2 = _distortion.derivativeWrtTangentialParameters(xu); + Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2; + copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ck, 0, 5); + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu); + Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2; + copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 3); + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + Ck[0][1] = 1; Ck[0][2] = 0; + Ck[1][1] = 0; Ck[1][2] = 1; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + Ck[0][0] = xd[0]; + Ck[1][0] = xd[1]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end CommonInternalsMetricBundleOptimizer::fillJacobians() + + void + CommonInternalsMetricBundleOptimizer::updateParametersC(Vector<double> const& deltaC) + { + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + _distortion.p1 += deltaC[5]; + _distortion.p2 += deltaC[6]; + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + _distortion.k1 += deltaC[3]; + _distortion.k2 += deltaC[4]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + _K[0][2] += deltaC[1]; + _K[1][2] += deltaC[2]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + _K[0][0] += deltaC[0]; + _K[1][1] = _cachedAspectRatio * _K[0][0]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end CommonInternalsMetricBundleOptimizer::updateParametersC() + + //---------------------------------------------------------------------- + + void + VaryingInternalsMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak, + Matrix<double>& Bk, + Matrix<double>& Ck, + int i, int j, int k) + { + Vector3d XX; + Matrix3x6d dXX_dRT; + Matrix3x3d dXX_dX; + this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX); + + Vector2d xu; // undistorted image point + xu[0] = XX[0] / XX[2]; + xu[1] = XX[1] / XX[2]; + + Vector2d const xd = _distortions[i](xu); // distorted image point + + double const focalLength = _cams[i].getFocalLength(); + double const aspectRatio = _cams[i].getAspectRatio(); + + Matrix2x2d dp_dxd; + dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0; + dp_dxd[1][0] = 0; dp_dxd[1][1] = aspectRatio * focalLength; + + { + // First, lets do the derivative wrt the structure and motion parameters. + Matrix2x3d dxu_dXX; + dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]); + dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]); + + Matrix2x2d dxd_dxu = _distortions[i].derivativeWrtUndistortedPoint(xu); + + Matrix2x2d dp_dxu = dp_dxd * dxd_dxu; + Matrix2x3d dp_dXX = dp_dxu * dxu_dXX; + + Matrix2x6d dp_dRT; + + multiply_A_B(dp_dXX, dXX_dRT, dp_dRT); + copyMatrixSlice(dp_dRT, 0, 0, 2, 6, Ak, 0, 0); + multiply_A_B(dp_dXX, dXX_dX, Bk); + } // end scope + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + Matrix2x2d dxd_dp1p2 = _distortions[i].derivativeWrtTangentialParameters(xu); + Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2; + copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ak, 0, 11); + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + Matrix2x2d dxd_dk1k2 = _distortions[i].derivativeWrtRadialParameters(xu); + Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2; + copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ak, 0, 9); + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + Ak[0][7] = 1; Ak[0][8] = 0; + Ak[1][7] = 0; Ak[1][8] = 1; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + Ak[0][6] = xd[0]; + Ak[1][6] = xd[1]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end VaryingInternalsMetricBundleOptimizer::fillJacobians() + + void + VaryingInternalsMetricBundleOptimizer::updateParametersA(VectorArray<double> const& deltaAi) + { + Vector3d T, omega; + Matrix3x3d R0, dR, K; + + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + Vector<double> const& deltaA = deltaAi[i]; + + T = _cams[i].getTranslation(); + T[0] += deltaA[0]; + T[1] += deltaA[1]; + T[2] += deltaA[2]; + _cams[i].setTranslation(T); + + // Create incremental rotation using Rodriguez formula. + R0 = _cams[i].getRotation(); + omega[0] = deltaA[3]; + omega[1] = deltaA[4]; + omega[2] = deltaA[5]; + createRotationMatrixRodriguez(omega, dR); + _cams[i].setRotation(dR * R0); + + K = _cams[i].getIntrinsic(); + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + _distortions[i].p1 += deltaA[11]; + _distortions[i].p2 += deltaA[12]; + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + _distortions[i].k1 += deltaA[9]; + _distortions[i].k2 += deltaA[10]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + K[0][2] += deltaA[7]; + K[1][2] += deltaA[8]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + double const ar = K[1][1] / K[0][0]; + K[0][0] += deltaA[6]; + K[1][1] = ar * K[0][0]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + _cams[i].setIntrinsic(K); + } // end for (i) + } // end VaryingInternalsMetricBundleOptimizer::updateParametersC() + +} // end namespace V3D + +#endif // defined(V3DLIB_ENABLE_SUITESPARSE) diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h new file mode 100644 index 00000000000..076a9e64346 --- /dev/null +++ b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h @@ -0,0 +1,346 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_METRICBUNDLE_H +#define V3D_METRICBUNDLE_H + +# if defined(V3DLIB_ENABLE_SUITESPARSE) + +#include "Math/v3d_optimization.h" +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_utils.h" +#include "Geometry/v3d_cameramatrix.h" +#include "Geometry/v3d_distortion.h" + +namespace V3D +{ + + // This structure provides some helper functions common to all metric BAs + struct MetricBundleOptimizerBase : public SparseLevenbergOptimizer + { + typedef SparseLevenbergOptimizer Base; + + MetricBundleOptimizerBase(double inlierThreshold, + vector<CameraMatrix>& cams, + vector<Vector3d >& Xs, + vector<Vector2d > const& measurements, + vector<int> const& corrspondingView, + vector<int> const& corrspondingPoint, + int nAddParamsA, int nParamsC) + : SparseLevenbergOptimizer(2, cams.size(), 6+nAddParamsA, Xs.size(), 3, nParamsC, + corrspondingView, corrspondingPoint), + _cams(cams), _Xs(Xs), _measurements(measurements), + _savedTranslations(cams.size()), _savedRotations(cams.size()), + _savedXs(Xs.size()), + _inlierThreshold(inlierThreshold), _cachedParamLength(0.0) + { + // Since we assume that BA does not alter the inputs too much, + // we compute the overall length of the parameter vector in advance + // and return that value as the result of getParameterLength(). + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _cachedParamLength += sqrNorm_L2(_cams[i].getTranslation()); + _cachedParamLength += 3.0; // Assume eye(3) for R. + } + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + _cachedParamLength += sqrNorm_L2(_Xs[j]); + + _cachedParamLength = sqrt(_cachedParamLength); + } + + // Huber robust cost function. + virtual void fillWeights(VectorArray<double> const& residual, Vector<double>& w) + { + for (unsigned int k = 0; k < w.size(); ++k) + { + Vector<double> const& r = residual[k]; + double const e = norm_L2(r); + w[k] = (e < _inlierThreshold) ? 1.0 : sqrt(_inlierThreshold / e); + } // end for (k) + } + + virtual double getParameterLength() const + { + return _cachedParamLength; + } + + virtual void updateParametersA(VectorArray<double> const& deltaAi); + virtual void updateParametersB(VectorArray<double> const& deltaBj); + virtual void updateParametersC(Vector<double> const& deltaC) + { + (void)deltaC; + } + + virtual void saveAllParameters() + { + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _savedTranslations[i] = _cams[i].getTranslation(); + _savedRotations[i] = _cams[i].getRotation(); + } + _savedXs = _Xs; + } + + virtual void restoreAllParameters() + { + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _cams[i].setTranslation(_savedTranslations[i]); + _cams[i].setRotation(_savedRotations[i]); + } + _Xs = _savedXs; + } + + protected: + typedef InlineMatrix<double, 3, 6> Matrix3x6d; + + void poseDerivatives(int i, int j, Vector3d& XX, + Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const; + + vector<CameraMatrix>& _cams; + vector<Vector3d>& _Xs; + + vector<Vector2d> const& _measurements; + + vector<Vector3d> _savedTranslations; + vector<Matrix3x3d> _savedRotations; + vector<Vector3d> _savedXs; + + double const _inlierThreshold; + double _cachedParamLength; + }; // end struct MetricBundleOptimizerBase + + struct StdMetricBundleOptimizer : public MetricBundleOptimizerBase + { + typedef MetricBundleOptimizerBase Base; + + StdMetricBundleOptimizer(double inlierThreshold, + vector<CameraMatrix>& cams, + vector<Vector3d >& Xs, + vector<Vector2d > const& measurements, + vector<int> const& corrspondingView, + vector<int> const& corrspondingPoint) + : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements, + corrspondingView, corrspondingPoint, 0, 0) + { } + + virtual void evalResidual(VectorArray<double>& e) + { + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + Vector2d const q = _cams[i].projectPoint(_Xs[j]); + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck, + int i, int j, int k); + }; // end struct StdMetricBundleOptimizer + +//---------------------------------------------------------------------- + + enum + { + FULL_BUNDLE_METRIC = 0, + FULL_BUNDLE_FOCAL_LENGTH = 1, // f + FULL_BUNDLE_FOCAL_LENGTH_PP = 2, // f, cx, cy + FULL_BUNDLE_RADIAL = 3, // f, cx, cy, k1, k2 + FULL_BUNDLE_RADIAL_TANGENTIAL = 4 // f, cx, cy, k1, k2, p1, p2 + }; + + struct CommonInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase + { + static int globalParamDimensionFromMode(int mode) + { + switch (mode) + { + case FULL_BUNDLE_METRIC: return 0; + case FULL_BUNDLE_FOCAL_LENGTH: return 1; + case FULL_BUNDLE_FOCAL_LENGTH_PP: return 3; + case FULL_BUNDLE_RADIAL: return 5; + case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7; + } + return 0; + } + + typedef MetricBundleOptimizerBase Base; + + CommonInternalsMetricBundleOptimizer(int mode, + double inlierThreshold, + Matrix3x3d& K, + StdDistortionFunction& distortion, + vector<CameraMatrix>& cams, + vector<Vector3d >& Xs, + vector<Vector2d > const& measurements, + vector<int> const& corrspondingView, + vector<int> const& corrspondingPoint) + : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements, + corrspondingView, corrspondingPoint, + 0, globalParamDimensionFromMode(mode)), + _mode(mode), _K(K), _distortion(distortion) + { + _cachedAspectRatio = K[1][1] / K[0][0]; + } + + Vector2d projectPoint(Vector3d const& X, int i) const + { + Vector3d const XX = _cams[i].transformPointIntoCameraSpace(X); + Vector2d p; + p[0] = XX[0] / XX[2]; + p[1] = XX[1] / XX[2]; + p = _distortion(p); + Vector2d res; + res[0] = _K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2]; + res[1] = _K[1][1] * p[1] + _K[1][2]; + return res; + } + + virtual void evalResidual(VectorArray<double>& e) + { + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + Vector2d const q = this->projectPoint(_Xs[j], i); + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck, + int i, int j, int k); + + virtual void updateParametersC(Vector<double> const& deltaC); + + virtual void saveAllParameters() + { + Base::saveAllParameters(); + _savedK = _K; + _savedDistortion = _distortion; + } + + virtual void restoreAllParameters() + { + Base::restoreAllParameters(); + _K = _savedK; + _distortion = _savedDistortion; + } + + protected: + int _mode; + Matrix3x3d& _K; + StdDistortionFunction& _distortion; + + Matrix3x3d _savedK; + StdDistortionFunction _savedDistortion; + double _cachedAspectRatio; + }; // end struct CommonInternalsMetricBundleOptimizer + +//---------------------------------------------------------------------- + + struct VaryingInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase + { + static int extParamDimensionFromMode(int mode) + { + switch (mode) + { + case FULL_BUNDLE_METRIC: return 0; + case FULL_BUNDLE_FOCAL_LENGTH: return 1; + case FULL_BUNDLE_FOCAL_LENGTH_PP: return 3; + case FULL_BUNDLE_RADIAL: return 5; + case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7; + } + return 0; + } + + typedef MetricBundleOptimizerBase Base; + + VaryingInternalsMetricBundleOptimizer(int mode, + double inlierThreshold, + std::vector<StdDistortionFunction>& distortions, + vector<CameraMatrix>& cams, + vector<Vector3d >& Xs, + vector<Vector2d > const& measurements, + vector<int> const& corrspondingView, + vector<int> const& corrspondingPoint) + : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements, + corrspondingView, corrspondingPoint, + extParamDimensionFromMode(mode), 0), + _mode(mode), _distortions(distortions), + _savedKs(cams.size()), _savedDistortions(cams.size()) + { } + + Vector2d projectPoint(Vector3d const& X, int i) const + { + return _cams[i].projectPoint(_distortions[i], X); + } + + virtual void evalResidual(VectorArray<double>& e) + { + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + Vector2d const q = this->projectPoint(_Xs[j], i); + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck, + int i, int j, int k); + + virtual void updateParametersA(VectorArray<double> const& deltaAi); + + virtual void saveAllParameters() + { + Base::saveAllParameters(); + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + _savedKs[i] = _cams[i].getIntrinsic(); + std::copy(_distortions.begin(), _distortions.end(), _savedDistortions.begin()); + } + + virtual void restoreAllParameters() + { + Base::restoreAllParameters(); + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + _cams[i].setIntrinsic(_savedKs[i]); + std::copy(_savedDistortions.begin(), _savedDistortions.end(), _distortions.begin()); + } + + protected: + int _mode; + std::vector<StdDistortionFunction>& _distortions; + + std::vector<Matrix3x3d> _savedKs; + std::vector<StdDistortionFunction> _savedDistortions; + }; // end struct VaryingInternalsMetricBundleOptimizer + +} // end namespace V3D + +# endif + +#endif diff --git a/extern/libmv/third_party/ssba/Math/v3d_linear.h b/extern/libmv/third_party/ssba/Math/v3d_linear.h new file mode 100644 index 00000000000..7d6e898169c --- /dev/null +++ b/extern/libmv/third_party/ssba/Math/v3d_linear.h @@ -0,0 +1,923 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_LINEAR_H +#define V3D_LINEAR_H + +#include <cassert> +#include <algorithm> +#include <vector> +#include <cmath> + +namespace V3D +{ + using namespace std; + + //! Unboxed vector type + template <typename Elem, int Size> + struct InlineVectorBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem const * const_iterator; + typedef Elem * iterator; + + static unsigned int size() { return Size; } + + Elem& operator[](unsigned int i) { return _vec[i]; } + Elem operator[](unsigned int i) const { return _vec[i]; } + + Elem& operator()(unsigned int i) { return _vec[i-1]; } + Elem operator()(unsigned int i) const { return _vec[i-1]; } + + const_iterator begin() const { return _vec; } + iterator begin() { return _vec; } + const_iterator end() const { return _vec + Size; } + iterator end() { return _vec + Size; } + + void newsize(unsigned int sz) const + { + assert(sz == Size); + } + + protected: + Elem _vec[Size]; + }; + + //! Boxed (heap allocated) vector. + template <typename Elem> + struct VectorBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem const * const_iterator; + typedef Elem * iterator; + + VectorBase() + : _size(0), _ownsVec(true), _vec(0) + { } + + VectorBase(unsigned int size) + : _size(size), _ownsVec(true), _vec(0) + { + if (size > 0) _vec = new Elem[size]; + } + + VectorBase(unsigned int size, Elem * values) + : _size(size), _ownsVec(false), _vec(values) + { } + + VectorBase(VectorBase<Elem> const& a) + : _size(0), _ownsVec(true), _vec(0) + { + _size = a._size; + if (_size == 0) return; + _vec = new Elem[_size]; + std::copy(a._vec, a._vec + _size, _vec); + } + + ~VectorBase() { if (_ownsVec && _vec != 0) delete [] _vec; } + + VectorBase& operator=(VectorBase<Elem> const& a) + { + if (this == &a) return *this; + + this->newsize(a._size); + std::copy(a._vec, a._vec + _size, _vec); + return *this; + } + + unsigned int size() const { return _size; } + + VectorBase<Elem>& newsize(unsigned int sz) + { + if (sz == _size) return *this; + assert(_ownsVec); + + __destroy(); + _size = sz; + if (_size > 0) _vec = new Elem[_size]; + + return *this; + } + + + Elem& operator[](unsigned int i) { return _vec[i]; } + Elem operator[](unsigned int i) const { return _vec[i]; } + + Elem& operator()(unsigned int i) { return _vec[i-1]; } + Elem operator()(unsigned int i) const { return _vec[i-1]; } + + const_iterator begin() const { return _vec; } + iterator begin() { return _vec; } + const_iterator end() const { return _vec + _size; } + iterator end() { return _vec + _size; } + + protected: + void __destroy() + { + assert(_ownsVec); + + if (_vec != 0) delete [] _vec; + _size = 0; + _vec = 0; + } + + unsigned int _size; + bool _ownsVec; + Elem * _vec; + }; + + template <typename Elem, int Rows, int Cols> + struct InlineMatrixBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem * iterator; + typedef Elem const * const_iterator; + + static unsigned int num_rows() { return Rows; } + static unsigned int num_cols() { return Cols; } + + Elem * operator[](unsigned int row) { return _m[row]; } + Elem const * operator[](unsigned int row) const { return _m[row]; } + + Elem& operator()(unsigned int row, unsigned int col) { return _m[row-1][col-1]; } + Elem operator()(unsigned int row, unsigned int col) const { return _m[row-1][col-1]; } + + template <typename Vec> + void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const + { + for (unsigned int c = first; c < last; ++c) dst[c-first] = _m[row][c]; + } + + template <typename Vec> + void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const + { + for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col]; + } + + void newsize(unsigned int rows, unsigned int cols) const + { + assert(rows == Rows && cols == Cols); + } + + const_iterator begin() const { return &_m[0][0]; } + iterator begin() { return &_m[0][0]; } + const_iterator end() const { return &_m[0][0] + Rows*Cols; } + iterator end() { return &_m[0][0] + Rows*Cols; } + + protected: + Elem _m[Rows][Cols]; + }; + + template <typename Elem> + struct MatrixBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem const * const_iterator; + typedef Elem * iterator; + + MatrixBase() + : _rows(0), _cols(0), _ownsData(true), _m(0) + { } + + MatrixBase(unsigned int rows, unsigned int cols) + : _rows(rows), _cols(cols), _ownsData(true), _m(0) + { + if (_rows * _cols == 0) return; + _m = new Elem[rows*cols]; + } + + MatrixBase(unsigned int rows, unsigned int cols, Elem * values) + : _rows(rows), _cols(cols), _ownsData(false), _m(values) + { } + + MatrixBase(MatrixBase<Elem> const& a) + : _ownsData(true), _m(0) + { + _rows = a._rows; _cols = a._cols; + if (_rows * _cols == 0) return; + _m = new Elem[_rows*_cols]; + std::copy(a._m, a._m+_rows*_cols, _m); + } + + ~MatrixBase() + { + if (_ownsData && _m != 0) delete [] _m; + } + + MatrixBase& operator=(MatrixBase<Elem> const& a) + { + if (this == &a) return *this; + + this->newsize(a.num_rows(), a.num_cols()); + + std::copy(a._m, a._m+_rows*_cols, _m); + return *this; + } + + void newsize(unsigned int rows, unsigned int cols) + { + if (rows == _rows && cols == _cols) return; + + assert(_ownsData); + + __destroy(); + + _rows = rows; + _cols = cols; + if (_rows * _cols == 0) return; + _m = new Elem[rows*cols]; + } + + unsigned int num_rows() const { return _rows; } + unsigned int num_cols() const { return _cols; } + + Elem * operator[](unsigned int row) { return _m + row*_cols; } + Elem const * operator[](unsigned int row) const { return _m + row*_cols; } + + Elem& operator()(unsigned int row, unsigned int col) { return _m[(row-1)*_cols + col-1]; } + Elem operator()(unsigned int row, unsigned int col) const { return _m[(row-1)*_cols + col-1]; } + + const_iterator begin() const { return _m; } + iterator begin() { return _m; } + const_iterator end() const { return _m + _rows*_cols; } + iterator end() { return _m + _rows*_cols; } + + template <typename Vec> + void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const + { + Elem const * v = (*this)[row]; + for (unsigned int c = first; c < last; ++c) dst[c-first] = v[c]; + } + + template <typename Vec> + void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const + { + for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col]; + } + + protected: + void __destroy() + { + assert(_ownsData); + if (_m != 0) delete [] _m; + _m = 0; + _rows = _cols = 0; + } + + unsigned int _rows, _cols; + bool _ownsData; + Elem * _m; + }; + + template <typename T> + struct CCS_Matrix + { + CCS_Matrix() + : _rows(0), _cols(0) + { } + + CCS_Matrix(int const rows, int const cols, vector<pair<int, int> > const& nonZeros) + : _rows(rows), _cols(cols) + { + this->initialize(nonZeros); + } + + CCS_Matrix(CCS_Matrix const& b) + : _rows(b._rows), _cols(b._cols), + _colStarts(b._colStarts), _rowIdxs(b._rowIdxs), _destIdxs(b._destIdxs), _values(b._values) + { } + + CCS_Matrix& operator=(CCS_Matrix const& b) + { + if (this == &b) return *this; + _rows = b._rows; + _cols = b._cols; + _colStarts = b._colStarts; + _rowIdxs = b._rowIdxs; + _destIdxs = b._destIdxs; + _values = b._values; + return *this; + } + + void create(int const rows, int const cols, vector<pair<int, int> > const& nonZeros) + { + _rows = rows; + _cols = cols; + this->initialize(nonZeros); + } + + unsigned int num_rows() const { return _rows; } + unsigned int num_cols() const { return _cols; } + + int getNonzeroCount() const { return _values.size(); } + + T const * getValues() const { return &_values[0]; } + T * getValues() { return &_values[0]; } + + int const * getDestIndices() const { return &_destIdxs[0]; } + int const * getColumnStarts() const { return &_colStarts[0]; } + int const * getRowIndices() const { return &_rowIdxs[0]; } + + void getRowRange(unsigned int col, unsigned int& firstRow, unsigned int& lastRow) const + { + firstRow = _rowIdxs[_colStarts[col]]; + lastRow = _rowIdxs[_colStarts[col+1]-1]+1; + } + + template <typename Vec> + void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const + { + unsigned int const last = first + len; + + for (int r = 0; r < len; ++r) dst[r] = 0; // Fill vector with zeros + + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + + int i = colStart; + int r; + // Skip rows less than the given start row + while (i < colEnd && (r = _rowIdxs[i]) < first) ++i; + + // Copy elements until the final row + while (i < colEnd && (r = _rowIdxs[i]) < last) + { + dst[r-first] = _values[i]; + ++i; + } + } // end getColumnSlice() + + int getColumnNonzeroCount(unsigned int col) const + { + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + return colEnd - colStart; + } + + template <typename VecA, typename VecB> + void getSparseColumn(unsigned int col, VecA& rows, VecB& values) const + { + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + int const nnz = colEnd - colStart; + + for (int i = 0; i < nnz; ++i) + { + rows[i] = _rowIdxs[colStart + i]; + values[i] = _values[colStart + i]; + } + } + + protected: + struct NonzeroInfo + { + int row, col, serial; + + // Sort wrt the column first + bool operator<(NonzeroInfo const& rhs) const + { + if (col < rhs.col) return true; + if (col > rhs.col) return false; + return row < rhs.row; + } + }; + + void initialize(std::vector<std::pair<int, int> > const& nonZeros) + { + using namespace std; + + int const nnz = nonZeros.size(); + + _colStarts.resize(_cols + 1); + _rowIdxs.resize(nnz); + + vector<NonzeroInfo> nz(nnz); + for (int k = 0; k < nnz; ++k) + { + nz[k].row = nonZeros[k].first; + nz[k].col = nonZeros[k].second; + nz[k].serial = k; + } + + // Sort in column major order + std::sort(nz.begin(), nz.end()); + + for (size_t k = 0; k < nnz; ++k) _rowIdxs[k] = nz[k].row; + + int curCol = -1; + for (int k = 0; k < nnz; ++k) + { + NonzeroInfo const& el = nz[k]; + if (el.col != curCol) + { + // Update empty cols between + for (int c = curCol+1; c < el.col; ++c) _colStarts[c] = k; + + curCol = el.col; + _colStarts[curCol] = k; + } // end if + } // end for (k) + + // Update remaining columns + for (int c = curCol+1; c <= _cols; ++c) _colStarts[c] = nnz; + + _destIdxs.resize(nnz); + for (int k = 0; k < nnz; ++k) _destIdxs[nz[k].serial] = k; + + _values.resize(nnz); + } // end initialize() + + int _rows, _cols; + std::vector<int> _colStarts; + std::vector<int> _rowIdxs; + std::vector<int> _destIdxs; + std::vector<T> _values; + }; // end struct CCS_Matrix + +//---------------------------------------------------------------------- + + template <typename Vec, typename Elem> + inline void + fillVector(Vec& v, Elem val) + { + // We do not use std::fill since we rely only on size() and operator[] member functions. + for (unsigned int i = 0; i < v.size(); ++i) v[i] = val; + } + + template <typename Vec> + inline void + makeZeroVector(Vec& v) + { + fillVector(v, 0); + } + + template <typename VecA, typename VecB> + inline void + copyVector(VecA const& src, VecB& dst) + { + assert(src.size() == dst.size()); + // We do not use std::fill since we rely only on size() and operator[] member functions. + for (unsigned int i = 0; i < src.size(); ++i) dst[i] = src[i]; + } + + template <typename VecA, typename VecB> + inline void + copyVectorSlice(VecA const& src, unsigned int srcStart, unsigned int srcLen, + VecB& dst, unsigned int dstStart) + { + unsigned int const end = std::min(srcStart + srcLen, src.size()); + unsigned int const sz = dst.size(); + unsigned int i0, i1; + for (i0 = srcStart, i1 = dstStart; i0 < end && i1 < sz; ++i0, ++i1) dst[i1] = src[i0]; + } + + template <typename Vec> + inline typename Vec::value_type + norm_L1(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res += fabs(v[i]); + return res; + } + + template <typename Vec> + inline typename Vec::value_type + norm_Linf(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res = std::max(res, fabs(v[i])); + return res; + } + + template <typename Vec> + inline typename Vec::value_type + norm_L2(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i]; + return sqrt((double)res); + } + + template <typename Vec> + inline typename Vec::value_type + sqrNorm_L2(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i]; + return res; + } + + template <typename Vec> + inline void + normalizeVector(Vec& v) + { + typename Vec::value_type norm(norm_L2(v)); + for (unsigned int i = 0; i < v.size(); ++i) v[i] /= norm; + } + + template<typename VecA, typename VecB> + inline typename VecA::value_type + innerProduct(VecA const& a, VecB const& b) + { + assert(a.size() == b.size()); + typename VecA::value_type res(0); + for (unsigned int i = 0; i < a.size(); ++i) res += a[i] * b[i]; + return res; + } + + template <typename Elem, typename VecA, typename VecB> + inline void + scaleVector(Elem s, VecA const& v, VecB& dst) + { + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = s * v[i]; + } + + template <typename Elem, typename Vec> + inline void + scaleVectorIP(Elem s, Vec& v) + { + typedef typename Vec::value_type Elem2; + for (unsigned int i = 0; i < v.size(); ++i) + v[i] = (Elem2)(v[i] * s); + } + + template <typename VecA, typename VecB, typename VecC> + inline void + makeCrossProductVector(VecA const& v, VecB const& w, VecC& dst) + { + assert(v.size() == 3); + assert(w.size() == 3); + assert(dst.size() == 3); + dst[0] = v[1]*w[2] - v[2]*w[1]; + dst[1] = v[2]*w[0] - v[0]*w[2]; + dst[2] = v[0]*w[1] - v[1]*w[0]; + } + + template <typename VecA, typename VecB, typename VecC> + inline void + addVectors(VecA const& v, VecB const& w, VecC& dst) + { + assert(v.size() == w.size()); + assert(v.size() == dst.size()); + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] + w[i]; + } + + template <typename VecA, typename VecB, typename VecC> + inline void + subtractVectors(VecA const& v, VecB const& w, VecC& dst) + { + assert(v.size() == w.size()); + assert(v.size() == dst.size()); + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] - w[i]; + } + + template <typename MatA, typename MatB> + inline void + copyMatrix(MatA const& src, MatB& dst) + { + unsigned int const rows = src.num_rows(); + unsigned int const cols = src.num_cols(); + assert(dst.num_rows() == rows); + assert(dst.num_cols() == cols); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[r][c] = src[r][c]; + } + + template <typename MatA, typename MatB> + inline void + copyMatrixSlice(MatA const& src, unsigned int rowStart, unsigned int colStart, unsigned int rowLen, unsigned int colLen, + MatB& dst, unsigned int dstRow, unsigned int dstCol) + { + unsigned int const rows = dst.num_rows(); + unsigned int const cols = dst.num_cols(); + + unsigned int const rowEnd = std::min(rowStart + rowLen, src.num_rows()); + unsigned int const colEnd = std::min(colStart + colLen, src.num_cols()); + + unsigned int c0, c1, r0, r1; + + for (c0 = colStart, c1 = dstCol; c0 < colEnd && c1 < cols; ++c0, ++c1) + for (r0 = rowStart, r1 = dstRow; r0 < rowEnd && r1 < rows; ++r0, ++r1) + dst[r1][c1] = src[r0][c0]; + } + + template <typename MatA, typename MatB> + inline void + makeTransposedMatrix(MatA const& src, MatB& dst) + { + unsigned int const rows = src.num_rows(); + unsigned int const cols = src.num_cols(); + assert(dst.num_cols() == rows); + assert(dst.num_rows() == cols); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[c][r] = src[r][c]; + } + + template <typename Mat> + inline void + fillMatrix(Mat& m, typename Mat::value_type val) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) m[r][c] = val; + } + + template <typename Mat> + inline void + makeZeroMatrix(Mat& m) + { + fillMatrix(m, 0); + } + + template <typename Mat> + inline void + makeIdentityMatrix(Mat& m) + { + makeZeroMatrix(m); + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + unsigned int n = std::min(rows, cols); + for (unsigned int i = 0; i < n; ++i) + m[i][i] = 1; + } + + template <typename Mat, typename Vec> + inline void + makeCrossProductMatrix(Vec const& v, Mat& m) + { + assert(v.size() == 3); + assert(m.num_rows() == 3); + assert(m.num_cols() == 3); + m[0][0] = 0; m[0][1] = -v[2]; m[0][2] = v[1]; + m[1][0] = v[2]; m[1][1] = 0; m[1][2] = -v[0]; + m[2][0] = -v[1]; m[2][1] = v[0]; m[2][2] = 0; + } + + template <typename Mat, typename Vec> + inline void + makeOuterProductMatrix(Vec const& v, Mat& m) + { + assert(m.num_cols() == m.num_rows()); + assert(v.size() == m.num_cols()); + unsigned const sz = v.size(); + for (unsigned r = 0; r < sz; ++r) + for (unsigned c = 0; c < sz; ++c) m[r][c] = v[r]*v[c]; + } + + template <typename Mat, typename VecA, typename VecB> + inline void + makeOuterProductMatrix(VecA const& u, VecB const& v, Mat& m) + { + assert(m.num_cols() == m.num_rows()); + assert(u.size() == m.num_cols()); + assert(v.size() == m.num_cols()); + unsigned const sz = u.size(); + for (unsigned r = 0; r < sz; ++r) + for (unsigned c = 0; c < sz; ++c) m[r][c] = u[r]*v[c]; + } + + template <typename MatA, typename MatB, typename MatC> + void addMatrices(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_cols()); + assert(a.num_rows() == b.num_rows()); + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + unsigned int const rows = a.num_rows(); + unsigned int const cols = a.num_cols(); + + for (unsigned r = 0; r < rows; ++r) + for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] + b[r][c]; + } + + template <typename MatA, typename MatB> + void addMatricesIP(MatA const& a, MatB& dst) + { + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + unsigned int const rows = a.num_rows(); + unsigned int const cols = a.num_cols(); + + for (unsigned r = 0; r < rows; ++r) + for (unsigned c = 0; c < cols; ++c) dst[r][c] += a[r][c]; + } + + template <typename MatA, typename MatB, typename MatC> + void subtractMatrices(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_cols()); + assert(a.num_rows() == b.num_rows()); + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + unsigned int const rows = a.num_rows(); + unsigned int const cols = a.num_cols(); + + for (unsigned r = 0; r < rows; ++r) + for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] - b[r][c]; + } + + template <typename MatA, typename Elem, typename MatB> + inline void + makeScaledMatrix(MatA const& m, Elem scale, MatB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[r][c] = m[r][c] * scale; + } + + template <typename Mat, typename Elem> + inline void + scaleMatrixIP(Elem scale, Mat& m) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) m[r][c] *= scale; + } + + template <typename Mat, typename VecA, typename VecB> + inline void + multiply_A_v(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == cols); + assert(dst.size() == rows); + + makeZeroVector(dst); + + for (unsigned int r = 0; r < rows; ++r) + for (unsigned int c = 0; c < cols; ++c) dst[r] += m[r][c] * in[c]; + } + + template <typename Mat, typename VecA, typename VecB> + inline void + multiply_A_v_projective(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == cols-1); + assert(dst.size() == rows-1); + + typename VecB::value_type w = m(rows-1, cols-1); + unsigned int r, i; + for (i = 0; i < cols-1; ++i) w += m(rows-1, i) * in[i]; + for (r = 0; r < rows-1; ++r) dst[r] = m(r, cols-1); + for (r = 0; r < rows-1; ++r) + for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c]; + for (i = 0; i < rows-1; ++i) dst[i] /= w; + } + + template <typename Mat, typename VecA, typename VecB> + inline void + multiply_A_v_affine(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == cols-1); + assert(dst.size() == rows); + + unsigned int r; + + for (r = 0; r < rows; ++r) dst[r] = m(r, cols-1); + for (r = 0; r < rows; ++r) + for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c]; + } + + template <typename Mat, typename VecA, typename VecB> + inline void + multiply_At_v(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == rows); + assert(dst.size() == cols); + + makeZeroVector(dst); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[c] += m[r][c] * in[r]; + } + + template <typename MatA, typename MatB> + inline void + multiply_At_A(MatA const& a, MatB& dst) + { + assert(dst.num_rows() == a.num_cols()); + assert(dst.num_cols() == a.num_cols()); + + typedef typename MatB::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_cols(); ++r) + for (unsigned int c = 0; c < a.num_cols(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * a[k][c]; + dst[r][c] = accum; + } + } + + template <typename MatA, typename MatB, typename MatC> + inline void + multiply_A_B(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_rows()); + assert(dst.num_rows() == a.num_rows()); + assert(dst.num_cols() == b.num_cols()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < b.num_cols(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[k][c]; + dst[r][c] = accum; + } + } + + template <typename MatA, typename MatB, typename MatC> + inline void + multiply_At_B(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_rows() == b.num_rows()); + assert(dst.num_rows() == a.num_cols()); + assert(dst.num_cols() == b.num_cols()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_cols(); ++r) + for (unsigned int c = 0; c < b.num_cols(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * b[k][c]; + dst[r][c] = accum; + } + } + + template <typename MatA, typename MatB, typename MatC> + inline void + multiply_A_Bt(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_cols()); + assert(dst.num_rows() == a.num_rows()); + assert(dst.num_cols() == b.num_rows()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < b.num_rows(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[c][k]; + dst[r][c] = accum; + } + } + + template <typename Mat> + inline void + transposeMatrixIP(Mat& a) + { + assert(a.num_rows() == a.num_cols()); + + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < r; ++c) + std::swap(a[r][c], a[c][r]); + } + +} // end namespace V3D + +#endif diff --git a/extern/libmv/third_party/ssba/Math/v3d_linear_utils.h b/extern/libmv/third_party/ssba/Math/v3d_linear_utils.h new file mode 100644 index 00000000000..969ec99694f --- /dev/null +++ b/extern/libmv/third_party/ssba/Math/v3d_linear_utils.h @@ -0,0 +1,391 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_LINEAR_UTILS_H +#define V3D_LINEAR_UTILS_H + +#include "Math/v3d_linear.h" + +#include <iostream> + +namespace V3D +{ + + template <typename Elem, int Size> + struct InlineVector : public InlineVectorBase<Elem, Size> + { + }; // end struct InlineVector + + template <typename Elem> + struct Vector : public VectorBase<Elem> + { + Vector() + : VectorBase<Elem>() + { } + + Vector(unsigned int size) + : VectorBase<Elem>(size) + { } + + Vector(unsigned int size, Elem * values) + : VectorBase<Elem>(size, values) + { } + + Vector(Vector<Elem> const& a) + : VectorBase<Elem>(a) + { } + + Vector<Elem>& operator=(Vector<Elem> const& a) + { + (VectorBase<Elem>::operator=)(a); + return *this; + } + + Vector<Elem>& operator+=(Vector<Elem> const& rhs) + { + addVectorsIP(rhs, *this); + return *this; + } + + Vector<Elem>& operator*=(Elem scale) + { + scaleVectorsIP(scale, *this); + return *this; + } + + Vector<Elem> operator+(Vector<Elem> const& rhs) const + { + Vector<Elem> res(this->size()); + addVectors(*this, rhs, res); + return res; + } + + Vector<Elem> operator-(Vector<Elem> const& rhs) const + { + Vector<Elem> res(this->size()); + subtractVectors(*this, rhs, res); + return res; + } + + Elem operator*(Vector<Elem> const& rhs) const + { + return innerProduct(*this, rhs); + } + + }; // end struct Vector + + template <typename Elem, int Rows, int Cols> + struct InlineMatrix : public InlineMatrixBase<Elem, Rows, Cols> + { + }; // end struct InlineMatrix + + template <typename Elem> + struct Matrix : public MatrixBase<Elem> + { + Matrix() + : MatrixBase<Elem>() + { } + + Matrix(unsigned int rows, unsigned int cols) + : MatrixBase<Elem>(rows, cols) + { } + + Matrix(unsigned int rows, unsigned int cols, Elem * values) + : MatrixBase<Elem>(rows, cols, values) + { } + + Matrix(Matrix<Elem> const& a) + : MatrixBase<Elem>(a) + { } + + Matrix<Elem>& operator=(Matrix<Elem> const& a) + { + (MatrixBase<Elem>::operator=)(a); + return *this; + } + + Matrix<Elem>& operator+=(Matrix<Elem> const& rhs) + { + addMatricesIP(rhs, *this); + return *this; + } + + Matrix<Elem>& operator*=(Elem scale) + { + scaleMatrixIP(scale, *this); + return *this; + } + + Matrix<Elem> operator+(Matrix<Elem> const& rhs) const + { + Matrix<Elem> res(this->num_rows(), this->num_cols()); + addMatrices(*this, rhs, res); + return res; + } + + Matrix<Elem> operator-(Matrix<Elem> const& rhs) const + { + Matrix<Elem> res(this->num_rows(), this->num_cols()); + subtractMatrices(*this, rhs, res); + return res; + } + + }; // end struct Matrix + +//---------------------------------------------------------------------- + + typedef InlineVector<float, 2> Vector2f; + typedef InlineVector<double, 2> Vector2d; + typedef InlineVector<float, 3> Vector3f; + typedef InlineVector<double, 3> Vector3d; + typedef InlineVector<float, 4> Vector4f; + typedef InlineVector<double, 4> Vector4d; + + typedef InlineMatrix<float, 2, 2> Matrix2x2f; + typedef InlineMatrix<double, 2, 2> Matrix2x2d; + typedef InlineMatrix<float, 3, 3> Matrix3x3f; + typedef InlineMatrix<double, 3, 3> Matrix3x3d; + typedef InlineMatrix<float, 4, 4> Matrix4x4f; + typedef InlineMatrix<double, 4, 4> Matrix4x4d; + + typedef InlineMatrix<float, 2, 3> Matrix2x3f; + typedef InlineMatrix<double, 2, 3> Matrix2x3d; + typedef InlineMatrix<float, 3, 4> Matrix3x4f; + typedef InlineMatrix<double, 3, 4> Matrix3x4d; + + template <typename Elem> + struct VectorArray + { + VectorArray(unsigned count, unsigned size) + : _count(count), _size(size), _values(0), _vectors(0) + { + unsigned const nTotal = _count * _size; + if (count > 0) _vectors = new Vector<Elem>[count]; + if (nTotal > 0) _values = new Elem[nTotal]; + for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector<Elem>(_size, _values + i*_size); + } + + VectorArray(unsigned count, unsigned size, Elem initVal) + : _count(count), _size(size), _values(0), _vectors(0) + { + unsigned const nTotal = _count * _size; + if (count > 0) _vectors = new Vector<Elem>[count]; + if (nTotal > 0) _values = new Elem[nTotal]; + for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector<Elem>(_size, _values + i*_size); + std::fill(_values, _values + nTotal, initVal); + } + + ~VectorArray() + { + delete [] _values; + delete [] _vectors; + } + + unsigned count() const { return _count; } + unsigned size() const { return _size; } + + //! Get the submatrix at position ix + Vector<Elem> const& operator[](unsigned ix) const + { + return _vectors[ix]; + } + + //! Get the submatrix at position ix + Vector<Elem>& operator[](unsigned ix) + { + return _vectors[ix]; + } + + protected: + unsigned _count, _size; + Elem * _values; + Vector<Elem> * _vectors; + + private: + VectorArray(VectorArray const&); + void operator=(VectorArray const&); + }; + + template <typename Elem> + struct MatrixArray + { + MatrixArray(unsigned count, unsigned nRows, unsigned nCols) + : _count(count), _rows(nRows), _columns(nCols), _values(0), _matrices(0) + { + unsigned const nTotal = _count * _rows * _columns; + if (count > 0) _matrices = new Matrix<Elem>[count]; + if (nTotal > 0) _values = new double[nTotal]; + for (unsigned i = 0; i < _count; ++i) + new (&_matrices[i]) Matrix<Elem>(_rows, _columns, _values + i*(_rows*_columns)); + } + + ~MatrixArray() + { + delete [] _matrices; + delete [] _values; + } + + //! Get the submatrix at position ix + Matrix<Elem> const& operator[](unsigned ix) const + { + return _matrices[ix]; + } + + //! Get the submatrix at position ix + Matrix<Elem>& operator[](unsigned ix) + { + return _matrices[ix]; + } + + unsigned count() const { return _count; } + unsigned num_rows() const { return _rows; } + unsigned num_cols() const { return _columns; } + + protected: + unsigned _count, _rows, _columns; + double * _values; + Matrix<Elem> * _matrices; + + private: + MatrixArray(MatrixArray const&); + void operator=(MatrixArray const&); + }; + +//---------------------------------------------------------------------- + + template <typename Elem, int Size> + inline InlineVector<Elem, Size> + operator+(InlineVector<Elem, Size> const& v, InlineVector<Elem, Size> const& w) + { + InlineVector<Elem, Size> res; + addVectors(v, w, res); + return res; + } + + template <typename Elem, int Size> + inline InlineVector<Elem, Size> + operator-(InlineVector<Elem, Size> const& v, InlineVector<Elem, Size> const& w) + { + InlineVector<Elem, Size> res; + subtractVectors(v, w, res); + return res; + } + + template <typename Elem, int Size> + inline InlineVector<Elem, Size> + operator*(Elem scale, InlineVector<Elem, Size> const& v) + { + InlineVector<Elem, Size> res; + scaleVector(scale, v, res); + return res; + } + + template <typename Elem, int Rows, int Cols> + inline InlineVector<Elem, Rows> + operator*(InlineMatrix<Elem, Rows, Cols> const& A, InlineVector<Elem, Cols> const& v) + { + InlineVector<Elem, Rows> res; + multiply_A_v(A, v, res); + return res; + } + + template <typename Elem, int RowsA, int ColsA, int ColsB> + inline InlineMatrix<Elem, RowsA, ColsB> + operator*(InlineMatrix<Elem, RowsA, ColsA> const& A, InlineMatrix<Elem, ColsA, ColsB> const& B) + { + InlineMatrix<Elem, RowsA, ColsB> res; + multiply_A_B(A, B, res); + return res; + } + + template <typename Elem, int Rows, int Cols> + inline InlineMatrix<Elem, Cols, Rows> + transposedMatrix(InlineMatrix<Elem, Rows, Cols> const& A) + { + InlineMatrix<Elem, Cols, Rows> At; + makeTransposedMatrix(A, At); + return At; + } + + template <typename Elem> + inline InlineMatrix<Elem, 3, 3> + invertedMatrix(InlineMatrix<Elem, 3, 3> const& A) + { + Elem a = A[0][0], b = A[0][1], c = A[0][2]; + Elem d = A[1][0], e = A[1][1], f = A[1][2]; + Elem g = A[2][0], h = A[2][1], i = A[2][2]; + + Elem const det = a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h; + + InlineMatrix<Elem, 3, 3> res; + res[0][0] = e*i-f*h; res[0][1] = c*h-b*i; res[0][2] = b*f-c*e; + res[1][0] = f*g-d*i; res[1][1] = a*i-c*g; res[1][2] = c*d-a*f; + res[2][0] = d*h-e*g; res[2][1] = b*g-a*h; res[2][2] = a*e-b*d; + + scaleMatrixIP(1.0/det, res); + return res; + } + + template <typename Elem> + inline InlineVector<Elem, 2> + makeVector2(Elem a, Elem b) + { + InlineVector<Elem, 2> res; + res[0] = a; res[1] = b; + return res; + } + + template <typename Elem> + inline InlineVector<Elem, 3> + makeVector3(Elem a, Elem b, Elem c) + { + InlineVector<Elem, 3> res; + res[0] = a; res[1] = b; res[2] = c; + return res; + } + + template <typename Vec> + inline void + displayVector(Vec const& v) + { + using namespace std; + + for (int r = 0; r < v.size(); ++r) + cout << v[r] << " "; + cout << endl; + } + + template <typename Mat> + inline void + displayMatrix(Mat const& A) + { + using namespace std; + + for (int r = 0; r < A.num_rows(); ++r) + { + for (int c = 0; c < A.num_cols(); ++c) + cout << A[r][c] << " "; + cout << endl; + } + } + +} // end namespace V3D + +#endif diff --git a/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h b/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h new file mode 100644 index 00000000000..9e38b92a94b --- /dev/null +++ b/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h @@ -0,0 +1,59 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_MATH_UTILITIES_H +#define V3D_MATH_UTILITIES_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_utils.h" + +#include <vector> + +namespace V3D +{ + + template <typename Vec, typename Mat> + inline void + createRotationMatrixRodriguez(Vec const& omega, Mat& R) + { + assert(omega.size() == 3); + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + double const theta = norm_L2(omega); + makeIdentityMatrix(R); + if (fabs(theta) > 1e-6) + { + Matrix3x3d J, J2; + makeCrossProductMatrix(omega, J); + multiply_A_B(J, J, J2); + double const c1 = sin(theta)/theta; + double const c2 = (1-cos(theta))/(theta*theta); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + R[i][j] += c1*J[i][j] + c2*J2[i][j]; + } + } // end createRotationMatrixRodriguez() + + template <typename T> inline double sqr(T x) { return x*x; } + +} // namespace V3D + +#endif diff --git a/extern/libmv/third_party/ssba/Math/v3d_optimization.cpp b/extern/libmv/third_party/ssba/Math/v3d_optimization.cpp new file mode 100644 index 00000000000..234815fcd1f --- /dev/null +++ b/extern/libmv/third_party/ssba/Math/v3d_optimization.cpp @@ -0,0 +1,955 @@ +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#include "Math/v3d_optimization.h" + +#if defined(V3DLIB_ENABLE_SUITESPARSE) +//# include "COLAMD/Include/colamd.h" +# include "colamd.h" +extern "C" +{ +//# include "LDL/Include/ldl.h" +# include "ldl.h" +} +#endif + +#include <iostream> +#include <map> + +#define USE_BLOCK_REORDERING 1 +#define USE_MULTIPLICATIVE_UPDATE 1 + +using namespace std; + +namespace +{ + + using namespace V3D; + + inline double + squaredResidual(VectorArray<double> const& e) + { + int const N = e.count(); + int const M = e.size(); + + double res = 0.0; + + for (int n = 0; n < N; ++n) + for (int m = 0; m < M; ++m) + res += e[n][m] * e[n][m]; + + return res; + } // end squaredResidual() + +} // end namespace <> + +namespace V3D +{ + + int optimizerVerbosenessLevel = 0; + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + + void + SparseLevenbergOptimizer::setupSparseJtJ() + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + int const bColumnStart = nVaryingA*_paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + int const nColumns = cColumnStart + nVaryingC; + + _jointNonzerosW.clear(); + _jointIndexW.resize(_nMeasurements); +#if 1 + { + map<pair<int, int>, int> jointNonzeroMap; + for (size_t k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (i >= 0 && j >= 0) + { + map<pair<int, int>, int>::const_iterator p = jointNonzeroMap.find(make_pair(i, j)); + if (p == jointNonzeroMap.end()) + { + jointNonzeroMap.insert(make_pair(make_pair(i, j), _jointNonzerosW.size())); + _jointIndexW[k] = _jointNonzerosW.size(); + _jointNonzerosW.push_back(make_pair(i, j)); + } + else + { + _jointIndexW[k] = (*p).second; + } // end if + } // end if + } // end for (k) + } // end scope +#else + for (size_t k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (i >= 0 && j >= 0) + { + _jointIndexW[k] = _jointNonzerosW.size(); + _jointNonzerosW.push_back(make_pair(i, j)); + } + } // end for (k) +#endif + +#if defined(USE_BLOCK_REORDERING) + int const bBlockColumnStart = nVaryingA; + int const cBlockColumnStart = bBlockColumnStart + nVaryingB; + + int const nBlockColumns = cBlockColumnStart + ((nVaryingC > 0) ? 1 : 0); + + //cout << "nBlockColumns = " << nBlockColumns << endl; + + // For the column reordering we treat the columns belonging to one set + // of parameters as one (logical) column. + + // Determine non-zeros of JtJ (we forget about the non-zero diagonal for now) + // Only consider nonzeros of Ai^t * Bj induced by the measurements. + vector<pair<int, int> > nz_blockJtJ(_jointNonzerosW.size()); + for (int k = 0; k < _jointNonzerosW.size(); ++k) + { + nz_blockJtJ[k].first = _jointNonzerosW[k].second + bBlockColumnStart; + nz_blockJtJ[k].second = _jointNonzerosW[k].first; + } + + if (nVaryingC > 0) + { + // We assume, that the global unknowns are linked to every other variable. + for (int i = 0; i < nVaryingA; ++i) + nz_blockJtJ.push_back(make_pair(cBlockColumnStart, i)); + for (int j = 0; j < nVaryingB; ++j) + nz_blockJtJ.push_back(make_pair(cBlockColumnStart, j + bBlockColumnStart)); + } // end if + + int const nnzBlock = nz_blockJtJ.size(); + + vector<int> permBlockJtJ(nBlockColumns + 1); + + if (nnzBlock > 0) + { +// cout << "nnzBlock = " << nnzBlock << endl; + + CCS_Matrix<int> blockJtJ(nBlockColumns, nBlockColumns, nz_blockJtJ); + +// cout << " nz_blockJtJ: " << endl; +// for (size_t k = 0; k < nz_blockJtJ.size(); ++k) +// cout << " " << nz_blockJtJ[k].first << ":" << nz_blockJtJ[k].second << endl; +// cout << endl; + + int * colStarts = (int *)blockJtJ.getColumnStarts(); + int * rowIdxs = (int *)blockJtJ.getRowIndices(); + +// cout << "blockJtJ_colStarts = "; +// for (int k = 0; k <= nBlockColumns; ++k) cout << colStarts[k] << " "; +// cout << endl; + +// cout << "blockJtJ_rowIdxs = "; +// for (int k = 0; k < nnzBlock; ++k) cout << rowIdxs[k] << " "; +// cout << endl; + + int stats[COLAMD_STATS]; + symamd(nBlockColumns, rowIdxs, colStarts, &permBlockJtJ[0], (double *) NULL, stats, &calloc, &free); + if (optimizerVerbosenessLevel >= 2) symamd_report(stats); + } + else + { + for (int k = 0; k < permBlockJtJ.size(); ++k) permBlockJtJ[k] = k; + } // end if + +// cout << "permBlockJtJ = "; +// for (int k = 0; k < permBlockJtJ.size(); ++k) +// cout << permBlockJtJ[k] << " "; +// cout << endl; + + // From the determined symbolic permutation with logical variables, determine the actual ordering + _perm_JtJ.resize(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC + 1); + + int curDstCol = 0; + for (int k = 0; k < permBlockJtJ.size()-1; ++k) + { + int const srcCol = permBlockJtJ[k]; + if (srcCol < nVaryingA) + { + for (int n = 0; n < _paramDimensionA; ++n) + _perm_JtJ[curDstCol + n] = srcCol*_paramDimensionA + n; + curDstCol += _paramDimensionA; + } + else if (srcCol >= bBlockColumnStart && srcCol < cBlockColumnStart) + { + int const bStart = nVaryingA*_paramDimensionA; + int const j = srcCol - bBlockColumnStart; + + for (int n = 0; n < _paramDimensionB; ++n) + _perm_JtJ[curDstCol + n] = bStart + j*_paramDimensionB + n; + curDstCol += _paramDimensionB; + } + else if (srcCol == cBlockColumnStart) + { + int const cStart = nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB; + + for (int n = 0; n < nVaryingC; ++n) + _perm_JtJ[curDstCol + n] = cStart + n; + curDstCol += nVaryingC; + } + else + { + cerr << "Should not reach " << __LINE__ << ":" << __LINE__ << "!" << endl; + assert(false); + } + } +#else + vector<pair<int, int> > nz, nzL; + this->serializeNonZerosJtJ(nz); + + for (int k = 0; k < nz.size(); ++k) + { + // Swap rows and columns, since serializeNonZerosJtJ() generates the + // upper triangular part but symamd wants the lower triangle. + nzL.push_back(make_pair(nz[k].second, nz[k].first)); + } + + _perm_JtJ.resize(nColumns+1); + + if (nzL.size() > 0) + { + CCS_Matrix<int> symbJtJ(nColumns, nColumns, nzL); + + int * colStarts = (int *)symbJtJ.getColumnStarts(); + int * rowIdxs = (int *)symbJtJ.getRowIndices(); + +// cout << "symbJtJ_colStarts = "; +// for (int k = 0; k <= nColumns; ++k) cout << colStarts[k] << " "; +// cout << endl; + +// cout << "symbJtJ_rowIdxs = "; +// for (int k = 0; k < nzL.size(); ++k) cout << rowIdxs[k] << " "; +// cout << endl; + + int stats[COLAMD_STATS]; + symamd(nColumns, rowIdxs, colStarts, &_perm_JtJ[0], (double *) NULL, stats, &calloc, &free); + if (optimizerVerbosenessLevel >= 2) symamd_report(stats); + } + else + { + for (int k = 0; k < _perm_JtJ.size(); ++k) _perm_JtJ[k] = k; + } //// end if +#endif + _perm_JtJ.back() = _perm_JtJ.size() - 1; + +// cout << "_perm_JtJ = "; +// for (int k = 0; k < _perm_JtJ.size(); ++k) cout << _perm_JtJ[k] << " "; +// cout << endl; + + // Finally, compute the inverse of the full permutation. + _invPerm_JtJ.resize(_perm_JtJ.size()); + for (size_t k = 0; k < _perm_JtJ.size(); ++k) + _invPerm_JtJ[_perm_JtJ[k]] = k; + + vector<pair<int, int> > nz_JtJ; + this->serializeNonZerosJtJ(nz_JtJ); + + for (int k = 0; k < nz_JtJ.size(); ++k) + { + int const i = nz_JtJ[k].first; + int const j = nz_JtJ[k].second; + + int pi = _invPerm_JtJ[i]; + int pj = _invPerm_JtJ[j]; + // Swap values if in lower triangular part + if (pi > pj) std::swap(pi, pj); + nz_JtJ[k].first = pi; + nz_JtJ[k].second = pj; + } + + int const nnz = nz_JtJ.size(); + +// cout << "nz_JtJ = "; +// for (int k = 0; k < nnz; ++k) cout << nz_JtJ[k].first << ":" << nz_JtJ[k].second << " "; +// cout << endl; + + _JtJ.create(nColumns, nColumns, nz_JtJ); + +// cout << "_colStart_JtJ = "; +// for (int k = 0; k < _JtJ.num_cols(); ++k) cout << _JtJ.getColumnStarts()[k] << " "; +// cout << endl; + +// cout << "_rowIdxs_JtJ = "; +// for (int k = 0; k < nnz; ++k) cout << _JtJ.getRowIndices()[k] << " "; +// cout << endl; + + vector<int> workFlags(nColumns); + + _JtJ_Lp.resize(nColumns+1); + _JtJ_Parent.resize(nColumns); + _JtJ_Lnz.resize(nColumns); + + ldl_symbolic(nColumns, (int *)_JtJ.getColumnStarts(), (int *)_JtJ.getRowIndices(), + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &workFlags[0], NULL, NULL); + + if (optimizerVerbosenessLevel >= 1) + cout << "SparseLevenbergOptimizer: Nonzeros in LDL decomposition: " << _JtJ_Lp[nColumns] << endl; + + } // end SparseLevenbergOptimizer::setupSparseJtJ() + + void + SparseLevenbergOptimizer::serializeNonZerosJtJ(vector<pair<int, int> >& dst) const + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + int const bColumnStart = nVaryingA*_paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + + dst.clear(); + + // Add the diagonal block matrices (only the upper triangular part). + + // Ui submatrices of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i * _paramDimensionA; + + for (int c = 0; c < _paramDimensionA; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(i0 + r, i0 + c)); + } + + // Vj submatrices of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*_paramDimensionB + bColumnStart; + + for (int c = 0; c < _paramDimensionB; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(j0 + r, j0 + c)); + } + + // Z submatrix of JtJ + for (int c = 0; c < nVaryingC; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(cColumnStart + r, cColumnStart + c)); + + // Add the elements i and j linked by an observation k + // W submatrix of JtJ + for (size_t n = 0; n < _jointNonzerosW.size(); ++n) + { + int const i0 = _jointNonzerosW[n].first; + int const j0 = _jointNonzerosW[n].second; + int const r0 = i0*_paramDimensionA; + int const c0 = j0*_paramDimensionB + bColumnStart; + + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < _paramDimensionB; ++c) + dst.push_back(make_pair(r0 + r, c0 + c)); + } // end for (n) + + if (nVaryingC > 0) + { + // Finally, add the dense columns linking i (resp. j) with the global parameters. + // X submatrix of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i*_paramDimensionA; + + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c) + dst.push_back(make_pair(i0 + r, cColumnStart + c)); + } + + // Y submatrix of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*_paramDimensionB + bColumnStart; + + for (int r = 0; r < _paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c) + dst.push_back(make_pair(j0 + r, cColumnStart + c)); + } + } // end if + } // end SparseLevenbergOptimizer::serializeNonZerosJtJ() + + void + SparseLevenbergOptimizer::fillSparseJtJ(MatrixArray<double> const& Ui, + MatrixArray<double> const& Vj, + MatrixArray<double> const& Wn, + Matrix<double> const& Z, + Matrix<double> const& X, + Matrix<double> const& Y) + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + int const bColumnStart = nVaryingA*_paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + + int const nCols = _JtJ.num_cols(); + int const nnz = _JtJ.getNonzeroCount(); + + // The following has to replicate the procedure as in serializeNonZerosJtJ() + + int serial = 0; + + double * values = _JtJ.getValues(); + int const * destIdxs = _JtJ.getDestIndices(); + + // Add the diagonal block matrices (only the upper triangular part). + + // Ui submatrices of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i * _paramDimensionA; + + for (int c = 0; c < _paramDimensionA; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Ui[i][r][c]; + } + + // Vj submatrices of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*_paramDimensionB + bColumnStart; + + for (int c = 0; c < _paramDimensionB; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Vj[j][r][c]; + } + + // Z submatrix of JtJ + for (int c = 0; c < nVaryingC; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Z[r][c]; + + // Add the elements i and j linked by an observation k + // W submatrix of JtJ + for (size_t n = 0; n < _jointNonzerosW.size(); ++n) + { + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < _paramDimensionB; ++c, ++serial) + values[destIdxs[serial]] = Wn[n][r][c]; + } // end for (k) + + if (nVaryingC > 0) + { + // Finally, add the dense columns linking i (resp. j) with the global parameters. + // X submatrix of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const r0 = i * _paramDimensionA; + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c, ++serial) + values[destIdxs[serial]] = X[r0+r][c]; + } + + // Y submatrix of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const r0 = j * _paramDimensionB; + for (int r = 0; r < _paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c, ++serial) + values[destIdxs[serial]] = Y[r0+r][c]; + } + } // end if + } // end SparseLevenbergOptimizer::fillSparseJtJ() + + void + SparseLevenbergOptimizer::minimize() + { + status = LEVENBERG_OPTIMIZER_TIMEOUT; + bool computeDerivatives = true; + + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + if (nVaryingA == 0 && nVaryingB == 0 && nVaryingC == 0) + { + // No degrees of freedom, nothing to optimize. + status = LEVENBERG_OPTIMIZER_CONVERGED; + return; + } + + this->setupSparseJtJ(); + + Vector<double> weights(_nMeasurements); + + MatrixArray<double> Ak(_nMeasurements, _measurementDimension, _paramDimensionA); + MatrixArray<double> Bk(_nMeasurements, _measurementDimension, _paramDimensionB); + MatrixArray<double> Ck(_nMeasurements, _measurementDimension, _paramDimensionC); + + MatrixArray<double> Ui(nVaryingA, _paramDimensionA, _paramDimensionA); + MatrixArray<double> Vj(nVaryingB, _paramDimensionB, _paramDimensionB); + + // Wn = Ak^t*Bk + MatrixArray<double> Wn(_jointNonzerosW.size(), _paramDimensionA, _paramDimensionB); + + Matrix<double> Z(nVaryingC, nVaryingC); + + // X = A^t*C + Matrix<double> X(nVaryingA*_paramDimensionA, nVaryingC); + // Y = B^t*C + Matrix<double> Y(nVaryingB*_paramDimensionB, nVaryingC); + + VectorArray<double> residuals(_nMeasurements, _measurementDimension); + VectorArray<double> residuals2(_nMeasurements, _measurementDimension); + + VectorArray<double> diagUi(nVaryingA, _paramDimensionA); + VectorArray<double> diagVj(nVaryingB, _paramDimensionB); + Vector<double> diagZ(nVaryingC); + + VectorArray<double> At_e(nVaryingA, _paramDimensionA); + VectorArray<double> Bt_e(nVaryingB, _paramDimensionB); + Vector<double> Ct_e(nVaryingC); + + Vector<double> Jt_e(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC); + + Vector<double> delta(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC); + Vector<double> deltaPerm(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC); + + VectorArray<double> deltaAi(_nParametersA, _paramDimensionA); + VectorArray<double> deltaBj(_nParametersB, _paramDimensionB); + Vector<double> deltaC(_paramDimensionC); + + double err = 0.0; + + for (currentIteration = 0; currentIteration < maxIterations; ++currentIteration) + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: currentIteration: " << currentIteration << endl; + if (computeDerivatives) + { + this->evalResidual(residuals); + this->fillWeights(residuals, weights); + for (int k = 0; k < _nMeasurements; ++k) + scaleVectorIP(weights[k], residuals[k]); + + err = squaredResidual(residuals); + + if (optimizerVerbosenessLevel >= 1) cout << "SparseLevenbergOptimizer: |residual|^2 = " << err << endl; + if (optimizerVerbosenessLevel >= 2) cout << "SparseLevenbergOptimizer: lambda = " << lambda << endl; + + for (int k = 0; k < residuals.count(); ++k) scaleVectorIP(-1.0, residuals[k]); + + this->setupJacobianGathering(); + this->fillAllJacobians(weights, Ak, Bk, Ck); + + // Compute the different parts of J^t*e + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) makeZeroVector(At_e[i]); + + Vector<double> tmp(_paramDimensionA); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + if (i < 0) continue; + multiply_At_v(Ak[k], residuals[k], tmp); + addVectors(tmp, At_e[i], At_e[i]); + } // end for (k) + } // end if + + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) makeZeroVector(Bt_e[j]); + + Vector<double> tmp(_paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (j < 0) continue; + multiply_At_v(Bk[k], residuals[k], tmp); + addVectors(tmp, Bt_e[j], Bt_e[j]); + } // end for (k) + } // end if + + if (nVaryingC > 0) + { + makeZeroVector(Ct_e); + + Vector<double> tmp(_paramDimensionC); + + for (int k = 0; k < _nMeasurements; ++k) + { + multiply_At_v(Ck[k], residuals[k], tmp); + for (int l = 0; l < nVaryingC; ++l) Ct_e[l] += tmp[_nNonvaryingC + l]; + } + } // end if + + int pos = 0; + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < _paramDimensionA; ++l, ++pos) + Jt_e[pos] = At_e[i][l]; + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < _paramDimensionB; ++l, ++pos) + Jt_e[pos] = Bt_e[j][l]; + for (int l = 0; l < nVaryingC; ++l, ++pos) + Jt_e[pos] = Ct_e[l]; + +// cout << "Jt_e = "; +// for (int k = 0; k < Jt_e.size(); ++k) cout << Jt_e[k] << " "; +// cout << endl; + + if (this->applyGradientStoppingCriteria(norm_Linf(Jt_e))) + { + status = LEVENBERG_OPTIMIZER_CONVERGED; + goto end; + } + + // The lhs J^t*J consists of several parts: + // [ U W X ] + // J^t*J = [ W^t V Y ] + // [ X^t Y^t Z ], + // where U, V and W are block-sparse matrices (due to the sparsity of A and B). + // X, Y and Z contain only a few columns (the number of global parameters). + + if (nVaryingA > 0) + { + // Compute Ui + Matrix<double> U(_paramDimensionA, _paramDimensionA); + + for (int i = 0; i < nVaryingA; ++i) makeZeroMatrix(Ui[i]); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + if (i < 0) continue; + multiply_At_A(Ak[k], U); + addMatricesIP(U, Ui[i]); + } // end for (k) + } // end if + + if (nVaryingB > 0) + { + // Compute Vj + Matrix<double> V(_paramDimensionB, _paramDimensionB); + + for (int j = 0; j < nVaryingB; ++j) makeZeroMatrix(Vj[j]); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (j < 0) continue; + multiply_At_A(Bk[k], V); + addMatricesIP(V, Vj[j]); + } // end for (k) + } // end if + + if (nVaryingC > 0) + { + Matrix<double> ZZ(_paramDimensionC, _paramDimensionC); + Matrix<double> Zsum(_paramDimensionC, _paramDimensionC); + + makeZeroMatrix(Zsum); + + for (int k = 0; k < _nMeasurements; ++k) + { + multiply_At_A(Ck[k], ZZ); + addMatricesIP(ZZ, Zsum); + } // end for (k) + + // Ignore the non-varying parameters + for (int i = 0; i < nVaryingC; ++i) + for (int j = 0; j < nVaryingC; ++j) + Z[i][j] = Zsum[i+_nNonvaryingC][j+_nNonvaryingC]; + } // end if + + if (nVaryingA > 0 && nVaryingB > 0) + { + for (int n = 0; n < Wn.count(); ++n) makeZeroMatrix(Wn[n]); + + Matrix<double> W(_paramDimensionA, _paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const n = _jointIndexW[k]; + if (n >= 0) + { + int const i0 = _jointNonzerosW[n].first; + int const j0 = _jointNonzerosW[n].second; + + multiply_At_B(Ak[k], Bk[k], W); + addMatricesIP(W, Wn[n]); + } // end if + } // end for (k) + } // end if + + if (nVaryingA > 0 && nVaryingC > 0) + { + Matrix<double> XX(_paramDimensionA, _paramDimensionC); + + makeZeroMatrix(X); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + // Ignore the non-varying parameters + if (i < 0) continue; + + multiply_At_B(Ak[k], Ck[k], XX); + + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c) + X[r+i*_paramDimensionA][c] += XX[r][c+_nNonvaryingC]; + } // end for (k) + } // end if + + if (nVaryingB > 0 && nVaryingC > 0) + { + Matrix<double> YY(_paramDimensionB, _paramDimensionC); + + makeZeroMatrix(Y); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - _nNonvaryingB; + // Ignore the non-varying parameters + if (j < 0) continue; + + multiply_At_B(Bk[k], Ck[k], YY); + + for (int r = 0; r < _paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c) + Y[r+j*_paramDimensionB][c] += YY[r][c+_nNonvaryingC]; + } // end for (k) + } // end if + + if (currentIteration == 0) + { + // Initialize lambda as tau*max(JtJ[i][i]) + double maxEl = -1e30; + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < _paramDimensionA; ++l) + maxEl = std::max(maxEl, Ui[i][l][l]); + } + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < _paramDimensionB; ++l) + maxEl = std::max(maxEl, Vj[j][l][l]); + } + if (nVaryingC > 0) + { + for (int l = 0; l < nVaryingC; ++l) + maxEl = std::max(maxEl, Z[l][l]); + } + + lambda = tau * maxEl; + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: initial lambda = " << lambda << endl; + } // end if (currentIteration == 0) + } // end if (computeDerivatives) + + for (int i = 0; i < nVaryingA; ++i) + { + for (int l = 0; l < _paramDimensionA; ++l) diagUi[i][l] = Ui[i][l][l]; + } // end for (i) + + for (int j = 0; j < nVaryingB; ++j) + { + for (int l = 0; l < _paramDimensionB; ++l) diagVj[j][l] = Vj[j][l][l]; + } // end for (j) + + for (int l = 0; l < nVaryingC; ++l) diagZ[l] = Z[l][l]; + + // Augment the diagonals with lambda (either by the standard additive update or by multiplication). +#if !defined(USE_MULTIPLICATIVE_UPDATE) + for (int i = 0; i < nVaryingA; ++i) + for (unsigned l = 0; l < _paramDimensionA; ++l) + Ui[i][l][l] += lambda; + + for (int j = 0; j < nVaryingB; ++j) + for (unsigned l = 0; l < _paramDimensionB; ++l) + Vj[j][l][l] += lambda; + + for (unsigned l = 0; l < nVaryingC; ++l) + Z[l][l] += lambda; +#else + for (int i = 0; i < nVaryingA; ++i) + for (unsigned l = 0; l < _paramDimensionA; ++l) + Ui[i][l][l] = std::max(Ui[i][l][l] * (1.0 + lambda), 1e-15); + + for (int j = 0; j < nVaryingB; ++j) + for (unsigned l = 0; l < _paramDimensionB; ++l) + Vj[j][l][l] = std::max(Vj[j][l][l] * (1.0 + lambda), 1e-15); + + for (unsigned l = 0; l < nVaryingC; ++l) + Z[l][l] = std::max(Z[l][l] * (1.0 + lambda), 1e-15); +#endif + + this->fillSparseJtJ(Ui, Vj, Wn, Z, X, Y); + + bool success = true; + double rho = 0.0; + { + int const nCols = _JtJ_Parent.size(); + int const nnz = _JtJ.getNonzeroCount(); + int const lnz = _JtJ_Lp.back(); + + vector<int> Li(lnz); + vector<double> Lx(lnz); + vector<double> D(nCols), Y(nCols); + vector<int> workPattern(nCols), workFlag(nCols); + + int * colStarts = (int *)_JtJ.getColumnStarts(); + int * rowIdxs = (int *)_JtJ.getRowIndices(); + double * values = _JtJ.getValues(); + + int const d = ldl_numeric(nCols, colStarts, rowIdxs, values, + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &Li[0], &Lx[0], &D[0], + &Y[0], &workPattern[0], &workFlag[0], + NULL, NULL); + + if (d == nCols) + { + ldl_perm(nCols, &deltaPerm[0], &Jt_e[0], &_perm_JtJ[0]); + ldl_lsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + ldl_dsolve(nCols, &deltaPerm[0], &D[0]); + ldl_ltsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + ldl_permt(nCols, &delta[0], &deltaPerm[0], &_perm_JtJ[0]); + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: LDL decomposition failed. Increasing lambda." << endl; + success = false; + } + } + + if (success) + { + double const deltaSqrLength = sqrNorm_L2(delta); + + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: ||delta||^2 = " << deltaSqrLength << endl; + + double const paramLength = this->getParameterLength(); + if (this->applyUpdateStoppingCriteria(paramLength, sqrt(deltaSqrLength))) + { + status = LEVENBERG_OPTIMIZER_SMALL_UPDATE; + goto end; + } + + // Copy the updates from delta to the respective arrays + int pos = 0; + + for (int i = 0; i < _nNonvaryingA; ++i) makeZeroVector(deltaAi[i]); + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + for (int l = 0; l < _paramDimensionA; ++l, ++pos) + deltaAi[i][l] = delta[pos]; + + for (int j = 0; j < _nNonvaryingB; ++j) makeZeroVector(deltaBj[j]); + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + for (int l = 0; l < _paramDimensionB; ++l, ++pos) + deltaBj[j][l] = delta[pos]; + + makeZeroVector(deltaC); + for (int l = _nNonvaryingC; l < _paramDimensionC; ++l, ++pos) + deltaC[l] = delta[pos]; + + saveAllParameters(); + if (nVaryingA > 0) updateParametersA(deltaAi); + if (nVaryingB > 0) updateParametersB(deltaBj); + if (nVaryingC > 0) updateParametersC(deltaC); + + this->evalResidual(residuals2); + for (int k = 0; k < _nMeasurements; ++k) + scaleVectorIP(weights[k], residuals2[k]); + + double const newErr = squaredResidual(residuals2); + rho = err - newErr; + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: |new residual|^2 = " << newErr << endl; + +#if !defined(USE_MULTIPLICATIVE_UPDATE) + double const denom1 = lambda * deltaSqrLength; +#else + double denom1 = 0.0f; + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + for (int l = 0; l < _paramDimensionA; ++l) + denom1 += deltaAi[i][l] * deltaAi[i][l] * diagUi[i-_nNonvaryingA][l]; + + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + for (int l = 0; l < _paramDimensionB; ++l) + denom1 += deltaBj[j][l] * deltaBj[j][l] * diagVj[j-_nNonvaryingB][l]; + + for (int l = _nNonvaryingC; l < _paramDimensionC; ++l) + denom1 += deltaC[l] * deltaC[l] * diagZ[l-_nNonvaryingC]; + + denom1 *= lambda; +#endif + double const denom2 = innerProduct(delta, Jt_e); + rho = rho / (denom1 + denom2); + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: rho = " << rho + << " denom1 = " << denom1 << " denom2 = " << denom2 << endl; + } // end if (success) + + if (success && rho > 0) + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: Improved solution - decreasing lambda." << endl; + // Improvement in the new solution + decreaseLambda(rho); + computeDerivatives = true; + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: Inferior solution - increasing lambda." << endl; + restoreAllParameters(); + increaseLambda(); + computeDerivatives = false; + + // Restore diagonal elements in Ui, Vj and Z. + for (int i = 0; i < nVaryingA; ++i) + { + for (int l = 0; l < _paramDimensionA; ++l) Ui[i][l][l] = diagUi[i][l]; + } // end for (i) + + for (int j = 0; j < nVaryingB; ++j) + { + for (int l = 0; l < _paramDimensionB; ++l) Vj[j][l][l] = diagVj[j][l]; + } // end for (j) + + for (int l = 0; l < nVaryingC; ++l) Z[l][l] = diagZ[l]; + } // end if + } // end for + + end:; + if (optimizerVerbosenessLevel >= 2) + cout << "Leaving SparseLevenbergOptimizer::minimize()." << endl; + } // end SparseLevenbergOptimizer::minimize() + +#endif // defined(V3DLIB_ENABLE_SUITESPARSE) + +} // end namespace V3D diff --git a/extern/libmv/third_party/ssba/Math/v3d_optimization.h b/extern/libmv/third_party/ssba/Math/v3d_optimization.h new file mode 100644 index 00000000000..27d2e12287f --- /dev/null +++ b/extern/libmv/third_party/ssba/Math/v3d_optimization.h @@ -0,0 +1,273 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_OPTIMIZATION_H +#define V3D_OPTIMIZATION_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_mathutilities.h" + +#include <vector> +#include <iostream> + +namespace V3D +{ + + enum + { + LEVENBERG_OPTIMIZER_TIMEOUT = 0, + LEVENBERG_OPTIMIZER_SMALL_UPDATE = 1, + LEVENBERG_OPTIMIZER_CONVERGED = 2 + }; + + extern int optimizerVerbosenessLevel; + + struct LevenbergOptimizerCommon + { + LevenbergOptimizerCommon() + : status(LEVENBERG_OPTIMIZER_TIMEOUT), currentIteration(0), maxIterations(50), + tau(1e-3), lambda(1e-3), + gradientThreshold(1e-10), updateThreshold(1e-10), + _nu(2.0) + { } + virtual ~LevenbergOptimizerCommon() {} + + // See Madsen et al., "Methods for non-linear least squares problems." + virtual void increaseLambda() + { + lambda *= _nu; _nu *= 2.0; + } + + virtual void decreaseLambda(double const rho) + { + double const r = 2*rho - 1.0; + lambda *= std::max(1.0/3.0, 1 - r*r*r); + if (lambda < 1e-10) lambda = 1e-10; + _nu = 2; + } + + bool applyGradientStoppingCriteria(double maxGradient) const + { + return maxGradient < gradientThreshold; + } + + bool applyUpdateStoppingCriteria(double paramLength, double updateLength) const + { + return updateLength < updateThreshold * (paramLength + updateThreshold); + } + + int status; + int currentIteration, maxIterations; + double tau, lambda; + double gradientThreshold, updateThreshold; + + protected: + double _nu; + }; // end struct LevenbergOptimizerCommon + +# if defined(V3DLIB_ENABLE_SUITESPARSE) + + struct SparseLevenbergOptimizer : public LevenbergOptimizerCommon + { + SparseLevenbergOptimizer(int measurementDimension, + int nParametersA, int paramDimensionA, + int nParametersB, int paramDimensionB, + int paramDimensionC, + std::vector<int> const& correspondingParamA, + std::vector<int> const& correspondingParamB) + : LevenbergOptimizerCommon(), + _nMeasurements(correspondingParamA.size()), + _measurementDimension(measurementDimension), + _nParametersA(nParametersA), _paramDimensionA(paramDimensionA), + _nParametersB(nParametersB), _paramDimensionB(paramDimensionB), + _paramDimensionC(paramDimensionC), + _nNonvaryingA(0), _nNonvaryingB(0), _nNonvaryingC(0), + _correspondingParamA(correspondingParamA), + _correspondingParamB(correspondingParamB) + { + assert(correspondingParamA.size() == correspondingParamB.size()); + } + + ~SparseLevenbergOptimizer() { } + + void setNonvaryingCounts(int nNonvaryingA, int nNonvaryingB, int nNonvaryingC) + { + _nNonvaryingA = nNonvaryingA; + _nNonvaryingB = nNonvaryingB; + _nNonvaryingC = nNonvaryingC; + } + + void getNonvaryingCounts(int& nNonvaryingA, int& nNonvaryingB, int& nNonvaryingC) const + { + nNonvaryingA = _nNonvaryingA; + nNonvaryingB = _nNonvaryingB; + nNonvaryingC = _nNonvaryingC; + } + + void minimize(); + + virtual void evalResidual(VectorArray<double>& residuals) = 0; + + virtual void fillWeights(VectorArray<double> const& residuals, Vector<double>& w) + { + (void)residuals; + std::fill(w.begin(), w.end(), 1.0); + } + + void fillAllJacobians(Vector<double> const& w, + MatrixArray<double>& Ak, + MatrixArray<double>& Bk, + MatrixArray<double>& Ck) + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + for (unsigned k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + if (i < _nNonvaryingA && j < _nNonvaryingB) continue; + + fillJacobians(Ak[k], Bk[k], Ck[k], i, j, k); + } // end for (k) + + if (nVaryingA > 0) + { + for (unsigned k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(w[k], Ak[k]); + } + if (nVaryingB > 0) + { + for (unsigned k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(w[k], Bk[k]); + } + if (nVaryingC > 0) + { + for (unsigned k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(w[k], Ck[k]); + } + } // end fillAllJacobians() + + virtual void setupJacobianGathering() { } + + virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck, + int i, int j, int k) = 0; + + virtual double getParameterLength() const = 0; + + virtual void updateParametersA(VectorArray<double> const& deltaAi) = 0; + virtual void updateParametersB(VectorArray<double> const& deltaBj) = 0; + virtual void updateParametersC(Vector<double> const& deltaC) = 0; + virtual void saveAllParameters() = 0; + virtual void restoreAllParameters() = 0; + + int currentIteration, maxIterations; + + protected: + void serializeNonZerosJtJ(std::vector<std::pair<int, int> >& dst) const; + void setupSparseJtJ(); + void fillSparseJtJ(MatrixArray<double> const& Ui, MatrixArray<double> const& Vj, MatrixArray<double> const& Wk, + Matrix<double> const& Z, Matrix<double> const& X, Matrix<double> const& Y); + + int const _nMeasurements, _measurementDimension; + int const _nParametersA, _paramDimensionA; + int const _nParametersB, _paramDimensionB; + int const _paramDimensionC; + + int _nNonvaryingA, _nNonvaryingB, _nNonvaryingC; + + std::vector<int> const& _correspondingParamA; + std::vector<int> const& _correspondingParamB; + + std::vector<pair<int, int> > _jointNonzerosW; + std::vector<int> _jointIndexW; + + std::vector<int> _JtJ_Lp, _JtJ_Parent, _JtJ_Lnz; + std::vector<int> _perm_JtJ, _invPerm_JtJ; + + CCS_Matrix<double> _JtJ; + }; // end struct SparseLevenbergOptimizer + + struct StdSparseLevenbergOptimizer : public SparseLevenbergOptimizer + { + StdSparseLevenbergOptimizer(int measurementDimension, + int nParametersA, int paramDimensionA, + int nParametersB, int paramDimensionB, + int paramDimensionC, + std::vector<int> const& correspondingParamA, + std::vector<int> const& correspondingParamB) + : SparseLevenbergOptimizer(measurementDimension, nParametersA, paramDimensionA, + nParametersB, paramDimensionB, paramDimensionC, + correspondingParamA, correspondingParamB), + curParametersA(nParametersA, paramDimensionA), savedParametersA(nParametersA, paramDimensionA), + curParametersB(nParametersB, paramDimensionB), savedParametersB(nParametersB, paramDimensionB), + curParametersC(paramDimensionC), savedParametersC(paramDimensionC) + { } + + virtual double getParameterLength() const + { + double res = 0.0; + for (int i = 0; i < _nParametersA; ++i) res += sqrNorm_L2(curParametersA[i]); + for (int j = 0; j < _nParametersB; ++j) res += sqrNorm_L2(curParametersB[j]); + res += sqrNorm_L2(curParametersC); + return sqrt(res); + } + + virtual void updateParametersA(VectorArray<double> const& deltaAi) + { + for (int i = 0; i < _nParametersA; ++i) addVectors(deltaAi[i], curParametersA[i], curParametersA[i]); + } + + virtual void updateParametersB(VectorArray<double> const& deltaBj) + { + for (int j = 0; j < _nParametersB; ++j) addVectors(deltaBj[j], curParametersB[j], curParametersB[j]); + } + + virtual void updateParametersC(Vector<double> const& deltaC) + { + addVectors(deltaC, curParametersC, curParametersC); + } + + virtual void saveAllParameters() + { + for (int i = 0; i < _nParametersA; ++i) savedParametersA[i] = curParametersA[i]; + for (int j = 0; j < _nParametersB; ++j) savedParametersB[j] = curParametersB[j]; + savedParametersC = curParametersC; + } + + virtual void restoreAllParameters() + { + for (int i = 0; i < _nParametersA; ++i) curParametersA[i] = savedParametersA[i]; + for (int j = 0; j < _nParametersB; ++j) curParametersB[j] = savedParametersB[j]; + curParametersC = savedParametersC; + } + + VectorArray<double> curParametersA, savedParametersA; + VectorArray<double> curParametersB, savedParametersB; + Vector<double> curParametersC, savedParametersC; + }; // end struct StdSparseLevenbergOptimizer + +# endif + +} // end namespace V3D + +#endif diff --git a/extern/libmv/third_party/ssba/README.TXT b/extern/libmv/third_party/ssba/README.TXT new file mode 100644 index 00000000000..734962b1df8 --- /dev/null +++ b/extern/libmv/third_party/ssba/README.TXT @@ -0,0 +1,92 @@ +Description + +This is an implementation of a sparse Levenberg-Marquardt optimization +procedure and several bundle adjustment modules based on it. There are three +versions of bundle adjustment: +1) Pure metric adjustment. Camera poses have 6 dof and 3D points have 3 dof. +2) Common, but adjustable intrinsic and distortion parameters. This is useful, + if the set of images are taken with the same camera under constant zoom + settings. +3) Variable intrinsics and distortion parameters for each view. This addresses + the "community photo collection" setting, where each image is captured with + a different camera and/or with varying zoom setting. + +There are two demo applications in the Apps directory, bundle_common and +bundle_varying, which correspond to item 2) and 3) above. + +The input data file for both applications is a text file with the following +numerical values: + +First, the number of 3D points, views and 2D measurements: +<M> <N> <K> +Then, the values of the intrinsic matrix + [ fx skew cx ] +K = [ 0 fy cy ] + [ 0 0 1 ], +and the distortion parameters according to the convention of the Bouget +toolbox: + + <fx> <skew> <cx> <fy> <cy> <k1> <k2> <p1> <p2> + +For the bundle_varying application this is given <N> times, one for each +camera/view. +Then the <M> 3D point positions are given: + + <point-id> <X> <Y> <Z> + +Note: the point-ids need not to be exactly from 0 to M-1, any (unique) ids +will do. +The camera poses are given subsequently: + + <view-id> <12 entries of the RT matrix> + +There is a lot of confusion how to specify the orientation of cameras. We use +projection matrix notation, i.e. P = K [R|T], and a 3D point X in world +coordinates is transformed into the camera coordinate system by XX=R*X+T. + +Finally, the <K> 2d image measurements (given in pixels) are provided: + + <view-id> <point-id> <x> <y> 1 + +See the example in the Dataset folder. + + +Performance + +This software is able to perform successful loop closing for a video sequence +containing 1745 views, 37920 3D points and 627228 image measurements in about +16min on a 2.2 GHz Core 2. The footprint in memory was <700MB. + + +Requirements + +Solving the augmented normal equation in the LM optimizer is done with LDL, a +Cholsky like decomposition method for sparse matrices (see +http://www.cise.ufl.edu/research/sparse/ldl). The appropriate column +reordering is done with COLAMD (see +http://www.cise.ufl.edu/research/sparse/colamd). Both packages are licensed +under the GNU LGPL. + +This software was developed under Linux, but should compile equally well on +other operating systems. + +-Christopher Zach (cmzach@cs.unc.edu) + +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ diff --git a/extern/libmv/third_party/ssba/README.libmv b/extern/libmv/third_party/ssba/README.libmv new file mode 100755 index 00000000000..45e0a31f6fc --- /dev/null +++ b/extern/libmv/third_party/ssba/README.libmv @@ -0,0 +1,23 @@ +Project: SSBA +URL: http://www.cs.unc.edu/~cmzach/opensource.html +License: LGPL3 +Upstream version: 1.0 + +Local modifications: + + * Added + SET(CMAKE_CXX_FLAGS "") + to CMakeLists.txt to prevent warnings from being treated as errors. + * Fixed "unused variable" in the header files. Warnings in the cpps files + are still there. + * Fixed a bug in CameraMatrix::opticalAxis() in file + Geometry/v3d_cameramatrix.h + * Deleted the Dataset directory. + * Added '#include <string>' to ssba/Apps/bundle_common.cpp and + ssba/Apps/bundle_varying.cpp to stop undefined references to strcmp + * Removed unnecessary elements from the CMakeLists.txt file, including the + obsoleted local_config.cmake and friends. + * Added a virtual destructor to V3D::LevenbergOptimizerCommon in + Math/v3d_optimization.h + * Added /EHsc WIN32-specific flag to CMakeLists.txt + * Remove unused variable Vector3d np in bundle_common.cpp and bundle_varying (in main() function). |