mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 15:53:03 +04:00
Merge pull request #21018 from savuor:levmarqfromscratch
New LevMarq implementation * Hash TSDF fix: apply volume pose when fetching pose * DualQuat minor fix * Pose Graph: getEdgePose(), getEdgeInfo() * debugging code for pose graph * add edge to submap * pose averaging: DualQuats instead of matrix averaging * overlapping ratio: rise it up; minor comment * remove `Submap::addEdgeToSubmap` * test_pose_graph: minor * SparseBlockMatrix: support 1xN as well as Nx1 for residual vector * small changes to old LMSolver * new LevMarq impl * Pose Graph rewritten to use new impl * solvePnP(), findHomography() and findExtrinsicCameraParams2() use new impl * estimateAffine...2D() use new impl * calibration and stereo calibration use new impl * BundleAdjusterBase::estimate() uses new impl * new LevMarq interface * PoseGraph: changing opt interface * findExtrinsicCameraParams2(): opt interface updated * HomographyRefine: opt interface updated * solvePnPRefine opt interface fixed * Affine2DRefine opt interface fixed * BundleAdjuster::estimate() opt interface fixed * calibration: opt interface fixed + code refactored a little * minor warning fixes * geodesic acceleration, Impl -> Backend rename * calcFunc() always uses probe vars * solveDecomposed, fixing negation * fixing geodesic acceleration + minors * PoseGraph exposes its optimizer now + its tests updated to check better convegence * Rosenbrock test added for LevMarq * LevMarq params upgraded * Rosenbrock can do better * fixing stereo calibration * old implementation removed (as well as debug code) * more debugging code removed * fix warnings * fixing warnings * fixing Eigen dependency * trying to fix Eigen deps * debugging code for submat is now temporary * trying to fix Eigen dependency * relax sanity check for solvePnP * relaxing sanity check even more * trying to fix Eigen dependency * warning fix * Quat<T>: fixing warnings * more warning fixes * fixed warning * fixing *KinFu OCL tests * algo params -> struct Settings * Backend moved to details * BaseLevMarq -> LevMarqBase * detail/pose_graph.hpp -> detail/optimizer.hpp * fixing include stuff for details/optimizer.hpp * doc fix * LevMarqBase rework: Settings, pImpl, Backend * Impl::settings and ::backend fix * HashTSDFGPU fix * fixing compilation * warning fix for OdometryFrameImplTMat * docs fix + compile warnings * remake: new class LevMarq with pImpl and enums, LevMarqBase => detail, no Backend class, Settings() => .cpp, Settings==() removed, Settings.set...() inlines * fixing warnings & whitespace
This commit is contained in:
Rostislav Vasilikhin
committed by
GitHub
GitHub
parent
4d9365990a
commit
9d6f388809
@@ -468,77 +468,267 @@ can be found in:
|
||||
*/
|
||||
CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() );
|
||||
|
||||
/** Levenberg-Marquardt solver. Starting with the specified vector of parameters it
|
||||
optimizes the target vector criteria "err"
|
||||
(finds local minima of each target vector component absolute value).
|
||||
|
||||
When needed, it calls user-provided callback.
|
||||
/** @brief Type of matrix used in LevMarq solver
|
||||
|
||||
Matrix type can be dense, sparse or chosen automatically based on a matrix size, performance considerations or backend availability.
|
||||
|
||||
Note: only dense matrix is now supported
|
||||
*/
|
||||
class CV_EXPORTS LMSolver : public Algorithm
|
||||
enum class MatrixType
|
||||
{
|
||||
AUTO = 0,
|
||||
DENSE = 1,
|
||||
SPARSE = 2
|
||||
};
|
||||
|
||||
/** @brief Type of variables used in LevMarq solver
|
||||
|
||||
Variables can be linear, rotation (SO(3) group) or rigid transformation (SE(3) group) with corresponding jacobians and exponential updates.
|
||||
|
||||
Note: only linear variables are now supported
|
||||
*/
|
||||
enum class VariableType
|
||||
{
|
||||
LINEAR = 0,
|
||||
SO3 = 1,
|
||||
SE3 = 2
|
||||
};
|
||||
|
||||
/** @brief Levenberg-Marquadt solver
|
||||
|
||||
A Levenberg-Marquadt algorithm locally minimizes an objective function value (aka energy, cost or error) starting from
|
||||
current param vector.
|
||||
To do that, at each iteration it repeatedly calculates the energy at probe points until it's reduced.
|
||||
To calculate a probe point, a linear equation is solved: (J^T*J + lambda*D)*dx = -J^T*b where J is a function jacobian,
|
||||
b is a vector of residuals (aka errors or energy terms), D is a diagonal matrix generated from J^T*J diagonal
|
||||
and lambda changes for each probe point. Then the resulting dx is "added" to current variable and it forms
|
||||
a probe value. "Added" is quoted because in some groups (e.g. SO(3) group) such an increment can be a non-trivial operation.
|
||||
|
||||
For more details, please refer to Wikipedia page (https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm).
|
||||
|
||||
This solver supports fixed variables and two forms of callback function:
|
||||
1. Generating ordinary jacobian J and residual vector err ("long")
|
||||
2. Generating normal equation matrix J^T*J and gradient vector J^T*err
|
||||
|
||||
Currently the solver supports dense jacobian matrix and linear parameter increment.
|
||||
*/
|
||||
class CV_EXPORTS LevMarq
|
||||
{
|
||||
public:
|
||||
class CV_EXPORTS Callback
|
||||
/** @brief Optimization report
|
||||
|
||||
The structure is returned when optimization is over.
|
||||
*/
|
||||
struct CV_EXPORTS Report
|
||||
{
|
||||
public:
|
||||
virtual ~Callback() {}
|
||||
/**
|
||||
computes error and Jacobian for the specified vector of parameters
|
||||
|
||||
@param param the current vector of parameters
|
||||
@param err output vector of errors: err_i = actual_f_i - ideal_f_i
|
||||
@param J output Jacobian: J_ij = d(err_i)/d(param_j)
|
||||
|
||||
when J=noArray(), it means that it does not need to be computed.
|
||||
Dimensionality of error vector and param vector can be different.
|
||||
The callback should explicitly allocate (with "create" method) each output array
|
||||
(unless it's noArray()).
|
||||
*/
|
||||
virtual bool compute(InputArray param, OutputArray err, OutputArray J) const = 0;
|
||||
Report(bool isFound, int nIters, double finalEnergy) :
|
||||
found(isFound), iters(nIters), energy(finalEnergy)
|
||||
{ }
|
||||
// true if the cost function converged to a local minimum which is checked by check* fields, thresholds and other options
|
||||
// false if the cost function failed to converge because of error, amount of iterations exhausted or lambda explosion
|
||||
bool found;
|
||||
// amount of iterations elapsed until the optimization stopped
|
||||
int iters;
|
||||
// energy value reached by the optimization
|
||||
double energy;
|
||||
};
|
||||
|
||||
/**
|
||||
Runs Levenberg-Marquardt algorithm using the passed vector of parameters as the start point.
|
||||
The final vector of parameters (whether the algorithm converged or not) is stored at the same
|
||||
vector. The method returns the number of iterations used. If it's equal to the previously specified
|
||||
maxIters, there is a big chance the algorithm did not converge.
|
||||
/** @brief Structure to keep LevMarq settings
|
||||
|
||||
@param param initial/final vector of parameters.
|
||||
|
||||
Note that the dimensionality of parameter space is defined by the size of param vector,
|
||||
and the dimensionality of optimized criteria is defined by the size of err vector
|
||||
computed by the callback.
|
||||
The structure allows a user to pass algorithm parameters along with their names like this:
|
||||
@code
|
||||
MySolver solver(nVars, callback, MySolver::Settings().geodesicS(true).geoScale(1.0));
|
||||
@endcode
|
||||
*/
|
||||
virtual int run(InputOutputArray param) const = 0;
|
||||
struct CV_EXPORTS Settings
|
||||
{
|
||||
Settings();
|
||||
|
||||
inline Settings& setJacobiScaling (bool v) { jacobiScaling = v; return *this; }
|
||||
inline Settings& setUpDouble (bool v) { upDouble = v; return *this; }
|
||||
inline Settings& setUseStepQuality (bool v) { useStepQuality = v; return *this; }
|
||||
inline Settings& setClampDiagonal (bool v) { clampDiagonal = v; return *this; }
|
||||
inline Settings& setStepNormInf (bool v) { stepNormInf = v; return *this; }
|
||||
inline Settings& setCheckRelEnergyChange (bool v) { checkRelEnergyChange = v; return *this; }
|
||||
inline Settings& setCheckMinGradient (bool v) { checkMinGradient = v; return *this; }
|
||||
inline Settings& setCheckStepNorm (bool v) { checkStepNorm = v; return *this; }
|
||||
inline Settings& setGeodesic (bool v) { geodesic = v; return *this; }
|
||||
inline Settings& setHGeo (double v) { hGeo = v; return *this; }
|
||||
inline Settings& setGeoScale (double v) { geoScale = v; return *this; }
|
||||
inline Settings& setStepNormTolerance (double v) { stepNormTolerance = v; return *this; }
|
||||
inline Settings& setRelEnergyDeltaTolerance(double v) { relEnergyDeltaTolerance = v; return *this; }
|
||||
inline Settings& setMinGradientTolerance (double v) { minGradientTolerance = v; return *this; }
|
||||
inline Settings& setSmallEnergyTolerance (double v) { smallEnergyTolerance = v; return *this; }
|
||||
inline Settings& setMaxIterations (int v) { maxIterations = (unsigned int)v; return *this; }
|
||||
inline Settings& setInitialLambda (double v) { initialLambda = v; return *this; }
|
||||
inline Settings& setInitialLmUpFactor (double v) { initialLmUpFactor = v; return *this; }
|
||||
inline Settings& setInitialLmDownFactor (double v) { initialLmDownFactor = v; return *this; }
|
||||
|
||||
// normalize jacobian columns for better conditioning
|
||||
// slows down sparse solver, but maybe this'd be useful for some other solver
|
||||
bool jacobiScaling;
|
||||
// double upFactor until the probe is successful
|
||||
bool upDouble;
|
||||
// use stepQuality metrics for steps down
|
||||
bool useStepQuality;
|
||||
// clamp diagonal values added to J^T*J to pre-defined range of values
|
||||
bool clampDiagonal;
|
||||
// to use squared L2 norm or Inf norm for step size estimation
|
||||
bool stepNormInf;
|
||||
// to use relEnergyDeltaTolerance or not
|
||||
bool checkRelEnergyChange;
|
||||
// to use minGradientTolerance or not
|
||||
bool checkMinGradient;
|
||||
// to use stepNormTolerance or not
|
||||
bool checkStepNorm;
|
||||
// to use geodesic acceleration or not
|
||||
bool geodesic;
|
||||
// second directional derivative approximation step for geodesic acceleration
|
||||
double hGeo;
|
||||
// how much of geodesic acceleration is used
|
||||
double geoScale;
|
||||
// optimization stops when norm2(dx) drops below this value
|
||||
double stepNormTolerance;
|
||||
// optimization stops when relative energy change drops below this value
|
||||
double relEnergyDeltaTolerance;
|
||||
// optimization stops when max gradient value (J^T*b vector) drops below this value
|
||||
double minGradientTolerance;
|
||||
// optimization stops when energy drops below this value
|
||||
double smallEnergyTolerance;
|
||||
// optimization stops after a number of iterations performed
|
||||
unsigned int maxIterations;
|
||||
|
||||
// LevMarq up and down params
|
||||
double initialLambda;
|
||||
double initialLmUpFactor;
|
||||
double initialLmDownFactor;
|
||||
};
|
||||
|
||||
/** "Long" callback: f(param, &err, &J) -> bool
|
||||
Computes error and Jacobian for the specified vector of parameters,
|
||||
returns true on success.
|
||||
|
||||
param: the current vector of parameters
|
||||
err: output vector of errors: err_i = actual_f_i - ideal_f_i
|
||||
J: output Jacobian: J_ij = d(err_i)/d(param_j)
|
||||
|
||||
Param vector values may be changed by the callback only if they are fixed.
|
||||
Changing non-fixed variables may lead to incorrect results.
|
||||
When J=noArray(), it means that it does not need to be computed.
|
||||
Dimensionality of error vector and param vector can be different.
|
||||
The callback should explicitly allocate (with "create" method) each output array
|
||||
(unless it's noArray()).
|
||||
*/
|
||||
typedef std::function<bool(InputOutputArray, OutputArray, OutputArray)> LongCallback;
|
||||
|
||||
/** Normal callback: f(param, &JtErr, &JtJ, &errnorm) -> bool
|
||||
|
||||
Computes squared L2 error norm, normal equation matrix J^T*J and J^T*err vector
|
||||
where J is MxN Jacobian: J_ij = d(err_i)/d(param_j)
|
||||
err is Mx1 vector of errors: err_i = actual_f_i - ideal_f_i
|
||||
M is a number of error terms, N is a number of variables to optimize.
|
||||
Make sense to use this class instead of usual Callback if the number
|
||||
of error terms greatly exceeds the number of variables.
|
||||
|
||||
param: the current Nx1 vector of parameters
|
||||
JtErr: output Nx1 vector J^T*err
|
||||
JtJ: output NxN matrix J^T*J
|
||||
errnorm: output total error: dot(err, err)
|
||||
|
||||
Param vector values may be changed by the callback only if they are fixed.
|
||||
Changing non-fixed variables may lead to incorrect results.
|
||||
If JtErr or JtJ are empty, they don't have to be computed.
|
||||
The callback should explicitly allocate (with "create" method) each output array
|
||||
(unless it's noArray()).
|
||||
*/
|
||||
typedef std::function<bool(InputOutputArray, OutputArray, OutputArray, double&)> NormalCallback;
|
||||
|
||||
/**
|
||||
Sets the maximum number of iterations
|
||||
@param maxIters the number of iterations
|
||||
Creates a solver
|
||||
|
||||
@param nvars Number of variables in a param vector
|
||||
@param callback "Long" callback, produces jacobian and residuals for each energy term, returns true on success
|
||||
@param settings LevMarq settings structure, see LevMarqBase class for details
|
||||
@param mask Indicates what variables are fixed during optimization (zeros) and what vars to optimize (non-zeros)
|
||||
@param matrixType Type of matrix used in the solver; only DENSE and AUTO are supported now
|
||||
@param paramType Type of optimized parameters; only LINEAR is supported now
|
||||
@param nerrs Energy terms amount. If zero, callback-generated jacobian size is used instead
|
||||
@param solveMethod What method to use for linear system solving
|
||||
*/
|
||||
virtual void setMaxIters(int maxIters) = 0;
|
||||
LevMarq(int nvars, LongCallback callback, const Settings& settings = Settings(), InputArray mask = noArray(),
|
||||
MatrixType matrixType = MatrixType::AUTO, VariableType paramType = VariableType::LINEAR, int nerrs = 0, int solveMethod = DECOMP_SVD);
|
||||
/**
|
||||
Retrieves the current maximum number of iterations
|
||||
Creates a solver
|
||||
|
||||
@param nvars Number of variables in a param vector
|
||||
@param callback Normal callback, produces J^T*J and J^T*b directly instead of J and b, returns true on success
|
||||
@param settings LevMarq settings structure, see LevMarqBase class for details
|
||||
@param mask Indicates what variables are fixed during optimization (zeros) and what vars to optimize (non-zeros)
|
||||
@param matrixType Type of matrix used in the solver; only DENSE and AUTO are supported now
|
||||
@param paramType Type of optimized parameters; only LINEAR is supported now
|
||||
@param LtoR Indicates what part of symmetric matrix to copy to another part: lower or upper. Used only with alt. callback
|
||||
@param solveMethod What method to use for linear system solving
|
||||
*/
|
||||
virtual int getMaxIters() const = 0;
|
||||
LevMarq(int nvars, NormalCallback callback, const Settings& settings = Settings(), InputArray mask = noArray(),
|
||||
MatrixType matrixType = MatrixType::AUTO, VariableType paramType = VariableType::LINEAR, bool LtoR = false, int solveMethod = DECOMP_SVD);
|
||||
|
||||
/**
|
||||
Creates Levenberg-Marquard solver
|
||||
Creates a solver
|
||||
|
||||
@param cb callback
|
||||
@param maxIters maximum number of iterations that can be further
|
||||
modified using setMaxIters() method.
|
||||
@param param Input/output vector containing starting param vector and resulting optimized params
|
||||
@param callback "Long" callback, produces jacobian and residuals for each energy term, returns true on success
|
||||
@param settings LevMarq settings structure, see LevMarqBase class for details
|
||||
@param mask Indicates what variables are fixed during optimization (zeros) and what vars to optimize (non-zeros)
|
||||
@param matrixType Type of matrix used in the solver; only DENSE and AUTO are supported now
|
||||
@param paramType Type of optimized parameters; only LINEAR is supported now
|
||||
@param nerrs Energy terms amount. If zero, callback-generated jacobian size is used instead
|
||||
@param solveMethod What method to use for linear system solving
|
||||
*/
|
||||
static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters);
|
||||
static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
|
||||
LevMarq(InputOutputArray param, LongCallback callback, const Settings& settings = Settings(), InputArray mask = noArray(),
|
||||
MatrixType matrixType = MatrixType::AUTO, VariableType paramType = VariableType::LINEAR, int nerrs = 0, int solveMethod = DECOMP_SVD);
|
||||
/**
|
||||
Creates a solver
|
||||
|
||||
static int run(InputOutputArray param, InputArray mask,
|
||||
int nerrs, const TermCriteria& termcrit, int solveMethod,
|
||||
std::function<bool (Mat& param, Mat* err, Mat* J)> callb);
|
||||
static int runAlt(InputOutputArray param, InputArray mask,
|
||||
const TermCriteria& termcrit, int solveMethod, bool LtoR,
|
||||
std::function<bool (Mat& param, Mat* JtErr,
|
||||
Mat* JtJ, double* errnorm)> callb);
|
||||
@param param Input/output vector containing starting param vector and resulting optimized params
|
||||
@param callback Normal callback, produces J^T*J and J^T*b directly instead of J and b, returns true on success
|
||||
@param settings LevMarq settings structure, see LevMarqBase class for details
|
||||
@param mask Indicates what variables are fixed during optimization (zeros) and what vars to optimize (non-zeros)
|
||||
@param matrixType Type of matrix used in the solver; only DENSE and AUTO are supported now
|
||||
@param paramType Type of optimized parameters; only LINEAR is supported now
|
||||
@param LtoR Indicates what part of symmetric matrix to copy to another part: lower or upper. Used only with alt. callback
|
||||
@param solveMethod What method to use for linear system solving
|
||||
*/
|
||||
LevMarq(InputOutputArray param, NormalCallback callback, const Settings& settings = Settings(), InputArray mask = noArray(),
|
||||
MatrixType matrixType = MatrixType::AUTO, VariableType paramType = VariableType::LINEAR, bool LtoR = false, int solveMethod = DECOMP_SVD);
|
||||
|
||||
/**
|
||||
Runs Levenberg-Marquadt algorithm using current settings and given parameters vector.
|
||||
The method returns the optimization report.
|
||||
*/
|
||||
Report optimize();
|
||||
|
||||
/** @brief Runs optimization using the passed vector of parameters as the start point.
|
||||
|
||||
The final vector of parameters (whether the algorithm converged or not) is stored at the same
|
||||
vector.
|
||||
This method can be used instead of the optimize() method if rerun with different start points is required.
|
||||
The method returns the optimization report.
|
||||
|
||||
@param param initial/final vector of parameters.
|
||||
|
||||
Note that the dimensionality of parameter space is defined by the size of param vector,
|
||||
and the dimensionality of optimized criteria is defined by the size of err vector
|
||||
computed by the callback.
|
||||
*/
|
||||
Report run(InputOutputArray param);
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
Ptr<Impl> pImpl;
|
||||
};
|
||||
|
||||
|
||||
/** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp
|
||||
An example program about pose estimation from coplanar points
|
||||
|
||||
|
||||
@@ -0,0 +1,140 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#ifndef OPENCV_3D_DETAIL_OPTIMIZER_HPP
|
||||
#define OPENCV_3D_DETAIL_OPTIMIZER_HPP
|
||||
|
||||
#include "opencv2/core/affine.hpp"
|
||||
#include "opencv2/core/quaternion.hpp"
|
||||
#include "opencv2/3d.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
|
||||
/*
|
||||
This class provides functions required for Levenberg-Marquadt algorithm implementation.
|
||||
See LevMarqBase::optimize() source code for details.
|
||||
*/
|
||||
class CV_EXPORTS LevMarqBackend
|
||||
{
|
||||
public:
|
||||
virtual ~LevMarqBackend() { }
|
||||
|
||||
// enables geodesic acceleration support in a backend, returns true on success
|
||||
virtual bool enableGeo() = 0;
|
||||
|
||||
// calculates an energy and/or jacobian at probe param vector
|
||||
virtual bool calcFunc(double& energy, bool calcEnergy = true, bool calcJacobian = false) = 0;
|
||||
|
||||
// adds x to current variables and writes the sum to probe var
|
||||
// or to geodesic acceleration var if geo flag is set
|
||||
virtual void currentOplusX(const Mat_<double>& x, bool geo = false) = 0;
|
||||
|
||||
// allocates jtj, jtb and other resources for objective function calculation, sets probeX to current X
|
||||
virtual void prepareVars() = 0;
|
||||
// returns a J^T*b vector (aka gradient)
|
||||
virtual const Mat_<double> getJtb() = 0;
|
||||
// returns a J^T*J diagonal vector
|
||||
virtual const Mat_<double> getDiag() = 0;
|
||||
// sets a J^T*J diagonal
|
||||
virtual void setDiag(const Mat_<double>& d) = 0;
|
||||
// performs jacobi scaling if the option is turned on
|
||||
virtual void doJacobiScaling(const Mat_<double>& di) = 0;
|
||||
|
||||
// decomposes LevMarq matrix before solution
|
||||
virtual bool decompose() = 0;
|
||||
// solves LevMarq equation (J^T*J + lmdiag) * x = -right for current iteration using existing decomposition
|
||||
// right can be equal to J^T*b for LevMarq equation or J^T*rvv for geodesic acceleration equation
|
||||
virtual bool solveDecomposed(const Mat_<double>& right, Mat_<double>& x) = 0;
|
||||
|
||||
// calculates J^T*f(geo) where geo is geodesic acceleration variable
|
||||
// this is used for J^T*rvv calculation for geodesic acceleration
|
||||
// calculates J^T*rvv where rvv is second directional derivative of the function in direction v
|
||||
// rvv = (f(x0 + v*h) - f(x0))/h - J*v)/h
|
||||
// where v is a LevMarq equation solution
|
||||
virtual bool calcJtbv(Mat_<double>& jtbv) = 0;
|
||||
|
||||
// sets current params vector to probe params
|
||||
virtual void acceptProbe() = 0;
|
||||
};
|
||||
|
||||
/** @brief Base class for Levenberg-Marquadt solvers.
|
||||
|
||||
This class can be used for general local optimization using sparse linear solvers, exponential param update or fixed variables
|
||||
implemented in child classes.
|
||||
This base class does not depend on a type, layout or a group structure of a param vector or an objective function jacobian.
|
||||
A child class should provide a storage for that data and implement all virtual member functions that process it.
|
||||
This class does not support fixed/masked variables, this should also be implemented in child classes.
|
||||
*/
|
||||
class CV_EXPORTS LevMarqBase
|
||||
{
|
||||
public:
|
||||
virtual ~LevMarqBase() { }
|
||||
|
||||
// runs optimization using given termination conditions
|
||||
virtual LevMarq::Report optimize();
|
||||
|
||||
LevMarqBase(const Ptr<LevMarqBackend>& backend_, const LevMarq::Settings& settings_):
|
||||
backend(backend_), settings(settings_)
|
||||
{ }
|
||||
|
||||
Ptr<LevMarqBackend> backend;
|
||||
LevMarq::Settings settings;
|
||||
};
|
||||
|
||||
|
||||
// ATTENTION! This class is used internally in Large KinFu.
|
||||
// It has been pushed to publicly available headers for tests only.
|
||||
// Source compatibility of this API is not guaranteed in the future.
|
||||
|
||||
// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems
|
||||
// The pose graph format, cost function and optimization techniques
|
||||
// repeat the ones used in Ceres 3D Pose Graph Optimization:
|
||||
// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet
|
||||
class CV_EXPORTS_W PoseGraph
|
||||
{
|
||||
public:
|
||||
static Ptr<PoseGraph> create();
|
||||
virtual ~PoseGraph();
|
||||
|
||||
// Node may have any id >= 0
|
||||
virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0;
|
||||
virtual bool isNodeExist(size_t nodeId) const = 0;
|
||||
virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0;
|
||||
virtual bool isNodeFixed(size_t nodeId) const = 0;
|
||||
virtual Affine3d getNodePose(size_t nodeId) const = 0;
|
||||
virtual std::vector<size_t> getNodesIds() const = 0;
|
||||
virtual size_t getNumNodes() const = 0;
|
||||
|
||||
// Edges have consequent indices starting from 0
|
||||
virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation,
|
||||
const Matx66f& _information = Matx66f::eye()) = 0;
|
||||
virtual size_t getEdgeStart(size_t i) const = 0;
|
||||
virtual size_t getEdgeEnd(size_t i) const = 0;
|
||||
virtual Affine3d getEdgePose(size_t i) const = 0;
|
||||
virtual Matx66f getEdgeInfo(size_t i) const = 0;
|
||||
virtual size_t getNumEdges() const = 0;
|
||||
|
||||
// checks if graph is connected and each edge connects exactly 2 nodes
|
||||
virtual bool isValid() const = 0;
|
||||
|
||||
// creates an optimizer with user-defined settings and returns a pointer on it
|
||||
virtual Ptr<LevMarqBase> createOptimizer(const LevMarq::Settings& settings) = 0;
|
||||
// creates an optimizer with default settings and returns a pointer on it
|
||||
virtual Ptr<LevMarqBase> createOptimizer() = 0;
|
||||
|
||||
// Creates an optimizer (with default settings) if it wasn't created before and runs it
|
||||
// Returns number of iterations elapsed or -1 if failed to optimize
|
||||
virtual LevMarq::Report optimize() = 0;
|
||||
|
||||
// calculate cost function based on current nodes parameters
|
||||
virtual double calcEnergy() const = 0;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
@@ -1,60 +0,0 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#ifndef OPENCV_3D_DETAIL_POSE_GRAPH_HPP
|
||||
#define OPENCV_3D_DETAIL_POSE_GRAPH_HPP
|
||||
|
||||
#include "opencv2/core/affine.hpp"
|
||||
#include "opencv2/core/quaternion.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
|
||||
// ATTENTION! This class is used internally in Large KinFu.
|
||||
// It has been pushed to publicly available headers for tests only.
|
||||
// Source compatibility of this API is not guaranteed in the future.
|
||||
|
||||
// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems
|
||||
// The pose graph format, cost function and optimization techniques
|
||||
// repeat the ones used in Ceres 3D Pose Graph Optimization:
|
||||
// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet
|
||||
class CV_EXPORTS_W PoseGraph
|
||||
{
|
||||
public:
|
||||
static Ptr<PoseGraph> create();
|
||||
virtual ~PoseGraph();
|
||||
|
||||
// Node may have any id >= 0
|
||||
virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0;
|
||||
virtual bool isNodeExist(size_t nodeId) const = 0;
|
||||
virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0;
|
||||
virtual bool isNodeFixed(size_t nodeId) const = 0;
|
||||
virtual Affine3d getNodePose(size_t nodeId) const = 0;
|
||||
virtual std::vector<size_t> getNodesIds() const = 0;
|
||||
virtual size_t getNumNodes() const = 0;
|
||||
|
||||
// Edges have consequent indices starting from 0
|
||||
virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation,
|
||||
const Matx66f& _information = Matx66f::eye()) = 0;
|
||||
virtual size_t getEdgeStart(size_t i) const = 0;
|
||||
virtual size_t getEdgeEnd(size_t i) const = 0;
|
||||
virtual size_t getNumEdges() const = 0;
|
||||
|
||||
// checks if graph is connected and each edge connects exactly 2 nodes
|
||||
virtual bool isValid() const = 0;
|
||||
|
||||
// Termination criteria are max number of iterations and min relative energy change to current energy
|
||||
// Returns number of iterations elapsed or -1 if max number of iterations was reached or failed to optimize
|
||||
virtual int optimize(const cv::TermCriteria& tc = cv::TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6)) = 0;
|
||||
|
||||
// calculate cost function based on current nodes parameters
|
||||
virtual double calcEnergy() const = 0;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
@@ -7,7 +7,10 @@
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/core/affine.hpp>
|
||||
#include "opencv2/3d/detail/pose_graph.hpp"
|
||||
#include "opencv2/3d/detail/optimizer.hpp"
|
||||
|
||||
//TODO: remove it when it is rewritten to robust pose graph
|
||||
#include "opencv2/core/dualquaternion.hpp"
|
||||
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
@@ -32,10 +35,10 @@ public:
|
||||
|
||||
void accumulatePose(const Affine3f& _pose, int _weight = 1)
|
||||
{
|
||||
Matx44f accPose = estimatedPose.matrix * weight + _pose.matrix * _weight;
|
||||
weight += _weight;
|
||||
accPose /= float(weight);
|
||||
estimatedPose = Affine3f(accPose);
|
||||
DualQuatf accPose = DualQuatf::createFromAffine3(estimatedPose) * float(weight) + DualQuatf::createFromAffine3(_pose) * float(_weight);
|
||||
weight += _weight;
|
||||
accPose = accPose / float(weight);
|
||||
estimatedPose = accPose.toAffine3();
|
||||
}
|
||||
};
|
||||
typedef std::map<int, PoseConstraint> Constraints;
|
||||
@@ -280,7 +283,9 @@ bool SubmapManager<MatType>::shouldCreateSubmap(int currFrameId)
|
||||
Ptr<SubmapT> currSubmap = getSubmap(currSubmapId);
|
||||
float ratio = currSubmap->calcVisibilityRatio(currFrameId);
|
||||
|
||||
if (ratio < 0.2f)
|
||||
//TODO: fix this when a new pose graph is ready
|
||||
// if (ratio < 0.2f)
|
||||
if (ratio < 0.5f)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
@@ -368,20 +373,20 @@ int SubmapManager<MatType>::estimateConstraint(int fromSubmapId, int toSubmapId,
|
||||
}
|
||||
|
||||
int localInliers = 0;
|
||||
Matx44f inlierConstraint;
|
||||
DualQuatf inlierConstraint;
|
||||
for (int i = 0; i < int(weights.size()); i++)
|
||||
{
|
||||
if (weights[i] > INLIER_WEIGHT_THRESH)
|
||||
{
|
||||
localInliers++;
|
||||
if (i == int(weights.size() - 1))
|
||||
inlierConstraint += prevConstraint.matrix;
|
||||
inlierConstraint += DualQuatf::createFromMat(prevConstraint.matrix);
|
||||
else
|
||||
inlierConstraint += fromSubmapData.constraints[i].matrix;
|
||||
inlierConstraint += DualQuatf::createFromMat(fromSubmapData.constraints[i].matrix);
|
||||
}
|
||||
}
|
||||
inlierConstraint /= float(max(localInliers, 1));
|
||||
inlierPose = Affine3f(inlierConstraint);
|
||||
inlierConstraint = inlierConstraint * 1.0f/float(max(localInliers, 1));
|
||||
inlierPose = inlierConstraint.toAffine3();
|
||||
inliers = localInliers;
|
||||
|
||||
if (inliers >= MIN_INLIERS)
|
||||
|
||||
Reference in New Issue
Block a user