6fbc4186fd
- Add support for refining the camera's intrinsic parameters
during a solve. Currently, refining supports only the following
combinations of intrinsic parameters:
f
f, cx, cy
f, cx, cy, k1, k2
f, k1
f, k1, k2
This is not the same as autocalibration, since the user must
still make a reasonable initial guess about the focal length and
other parameters, whereas true autocalibration would eliminate
the need for the user specify intrinsic parameters at all.
However, the solver works well with only rough guesses for the
focal length, so perhaps full autocalibation is not that
important.
Adding support for the last two combinations, (f, k1) and (f,
k1, k2) required changes to the library libmv depends on for
bundle adjustment, SSBA. These changes should get ported
upstream not just to libmv but to SSBA as well.
- Improved the region of convergence for bundle adjustment by
increasing the number of Levenberg-Marquardt iterations from 50
to 500. This way, the solver is able to crawl out of the bad
local minima it gets stuck in when changing from, for example,
bundling k1 and k2 to just k1 and resetting k2 to 0.
- Add several new region tracker implementations. A region tracker
is a libmv concept, which refers to tracking a template image
pattern through frames. The impact to end users is that tracking
should "just work better". I am reserving a more detailed
writeup, and maybe a paper, for later.
- Other libmv tweaks, such as detecting that a tracker is headed
outside of the image bounds.
This includes several changes made directly to the libmv extern
code rather expecting to get those changes through normal libmv
channels, because I, the libmv BDFL, decided it was faster to work
on libmv directly in Blender, then later reverse-port the libmv
changes from Blender back into libmv trunk. The interesting part
is that I added a full Levenberg-Marquardt loop to the region
tracking code, which should lead to a more stable solutions. I
also added a hacky implementation of "Efficient Second-Order
Minimization" for tracking, which works nicely. A more detailed
quantitative evaluation will follow.
Original patch by Keir, cleaned a bit by myself.
353 lines
14 KiB
C++
353 lines
14 KiB
C++
// -*- 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
|
|
FULL_BUNDLE_FOCAL_AND_RADIAL_K1 = 5, // f, k1
|
|
FULL_BUNDLE_FOCAL_AND_RADIAL = 6, // f, k1, k2
|
|
};
|
|
|
|
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;
|
|
case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
|
|
case FULL_BUNDLE_FOCAL_AND_RADIAL: return 3;
|
|
}
|
|
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;
|
|
case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
|
|
case FULL_BUNDLE_FOCAL_AND_RADIAL: return 3;
|
|
}
|
|
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
|