From 9d6f38880992f134d51e8a3d2d375a1b1b44f30d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 28 Dec 2021 00:51:32 +0300 Subject: [PATCH] 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: 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 --- modules/3d/include/opencv2/3d.hpp | 290 +++++- .../include/opencv2/3d/detail/optimizer.hpp | 140 +++ .../include/opencv2/3d/detail/pose_graph.hpp | 60 -- .../3d/include/opencv2/3d/detail/submap.hpp | 27 +- modules/3d/perf/perf_pnp.cpp | 3 +- modules/3d/src/calibration_base.cpp | 83 +- modules/3d/src/fundam.cpp | 108 +- modules/3d/src/levmarq.cpp | 934 +++++++++++++----- modules/3d/src/precomp.hpp | 2 +- modules/3d/src/ptsetreg.cpp | 216 ++-- modules/3d/src/rgbd/hash_tsdf.cpp | 9 +- modules/3d/src/rgbd/odometry_frame_impl.cpp | 11 +- modules/3d/src/rgbd/pose_graph.cpp | 669 +++++++------ modules/3d/src/rgbd/sparse_block_matrix.hpp | 86 +- modules/3d/src/solvepnp.cpp | 95 +- modules/3d/test/test_pose_graph.cpp | 308 +++++- modules/calib/src/calibration.cpp | 148 +-- modules/calib/test/test_cameracalibration.cpp | 4 +- .../opencv2/core/dualquaternion.inl.hpp | 2 +- .../include/opencv2/core/quaternion.inl.hpp | 20 +- modules/stitching/src/motion_estimators.cpp | 36 +- 21 files changed, 2184 insertions(+), 1067 deletions(-) create mode 100644 modules/3d/include/opencv2/3d/detail/optimizer.hpp delete mode 100644 modules/3d/include/opencv2/3d/detail/pose_graph.hpp diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index a176da07c5..5815015aa7 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -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 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 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 create(const Ptr& cb, int maxIters); - static Ptr create(const Ptr& 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 callb); - static int runAlt(InputOutputArray param, InputArray mask, - const TermCriteria& termcrit, int solveMethod, bool LtoR, - std::function 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 pImpl; }; + /** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp An example program about pose estimation from coplanar points diff --git a/modules/3d/include/opencv2/3d/detail/optimizer.hpp b/modules/3d/include/opencv2/3d/detail/optimizer.hpp new file mode 100644 index 0000000000..fa480f13cc --- /dev/null +++ b/modules/3d/include/opencv2/3d/detail/optimizer.hpp @@ -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_& 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_ getJtb() = 0; + // returns a J^T*J diagonal vector + virtual const Mat_ getDiag() = 0; + // sets a J^T*J diagonal + virtual void setDiag(const Mat_& d) = 0; + // performs jacobi scaling if the option is turned on + virtual void doJacobiScaling(const Mat_& 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_& right, Mat_& 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_& 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& backend_, const LevMarq::Settings& settings_): + backend(backend_), settings(settings_) + { } + + Ptr 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 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 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 createOptimizer(const LevMarq::Settings& settings) = 0; + // creates an optimizer with default settings and returns a pointer on it + virtual Ptr 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 diff --git a/modules/3d/include/opencv2/3d/detail/pose_graph.hpp b/modules/3d/include/opencv2/3d/detail/pose_graph.hpp deleted file mode 100644 index d2e5d42a54..0000000000 --- a/modules/3d/include/opencv2/3d/detail/pose_graph.hpp +++ /dev/null @@ -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 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 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 diff --git a/modules/3d/include/opencv2/3d/detail/submap.hpp b/modules/3d/include/opencv2/3d/detail/submap.hpp index e50b4abe24..e8a77b5d1f 100644 --- a/modules/3d/include/opencv2/3d/detail/submap.hpp +++ b/modules/3d/include/opencv2/3d/detail/submap.hpp @@ -7,7 +7,10 @@ #include #include -#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 #include @@ -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 Constraints; @@ -280,7 +283,9 @@ bool SubmapManager::shouldCreateSubmap(int currFrameId) Ptr 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::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) diff --git a/modules/3d/perf/perf_pnp.cpp b/modules/3d/perf/perf_pnp.cpp index 997fba4e06..4b6764ef5a 100644 --- a/modules/3d/perf/perf_pnp.cpp +++ b/modules/3d/perf/perf_pnp.cpp @@ -53,7 +53,8 @@ PERF_TEST_P(PointsNum_Algo, solvePnP, } SANITY_CHECK(rvec, 1e-4); - SANITY_CHECK(tvec, 1e-4); + // the check is relaxed from 1e-4 to 2e-2 after LevMarq replacement + SANITY_CHECK(tvec, 2e-2); } PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, diff --git a/modules/3d/src/calibration_base.cpp b/modules/3d/src/calibration_base.cpp index 0a9dcdd856..6e2b0afcf7 100644 --- a/modules/3d/src/calibration_base.cpp +++ b/modules/3d/src/calibration_base.cpp @@ -1098,50 +1098,6 @@ void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraM eulerAngles.convertTo(_eulerAngles, depth); } -class SolvePnPCallback CV_FINAL : public LMSolver::Callback -{ -public: - SolvePnPCallback(const Mat& _objpt, const Mat& _imgpt, - const Mat& _cameraMatrix, const Mat& _distCoeffs) - { - objpt = _objpt; - imgpt = _imgpt; - cameraMatrix = _cameraMatrix; - distCoeffs = _distCoeffs; - } - - bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE - { - Mat param = _param.getMat(); - CV_Assert((param.cols == 1 || param.rows == 1) && param.total() == 6 && param.type() == CV_64F); - double* pdata = param.ptr(); - Mat rvec(3, 1, CV_64F, pdata); - Mat tvec(3, 1, CV_64F, pdata + 3); - int count = objpt.rows + objpt.cols - 1; - _err.create(count*2, 1, CV_64F); - Mat err = _err.getMat(); - err = err.reshape(2, count); - if( _Jac.needed() ) - { - _Jac.create(count*2, 6, CV_64F); - Mat Jac = _Jac.getMat(); - Mat dpdr = Jac.colRange(0, 3); - Mat dpdt = Jac.colRange(3, 6); - projectPoints( objpt, rvec, tvec, cameraMatrix, distCoeffs, - err, dpdr, dpdt, noArray(), noArray(), noArray(), noArray()); - } - else - { - projectPoints( objpt, rvec, tvec, cameraMatrix, distCoeffs, err); - } - err = err - imgpt; - err = err.reshape(1, 2*count); - return true; - } - - Mat objpt, imgpt, cameraMatrix, distCoeffs; -}; - void cv::findExtrinsicCameraParams2( const Mat& objectPoints, const Mat& imagePoints, const Mat& A, @@ -1316,9 +1272,42 @@ void cv::findExtrinsicCameraParams2( const Mat& objectPoints, _mn = _mn.reshape(2, 1); // refine extrinsic parameters using iterative algorithm - Ptr callb = makePtr(matM, _m, matA, distCoeffs); - Ptr solver = LMSolver::create(callb, max_iter, (double)FLT_EPSILON); - solver->run(_param); + auto callback = [matM, _m, matA, distCoeffs] + (InputOutputArray param_, OutputArray _err, OutputArray _Jac) -> bool + { + const Mat& objpt = matM; + const Mat& imgpt = _m; + const Mat& cameraMatrix = matA; + Mat x = param_.getMat(); + CV_Assert((x.cols == 1 || x.rows == 1) && x.total() == 6 && x.type() == CV_64F); + double* pdata = x.ptr(); + Mat rv(3, 1, CV_64F, pdata); + Mat tv(3, 1, CV_64F, pdata + 3); + int errCount = objpt.rows + objpt.cols - 1; + _err.create(errCount * 2, 1, CV_64F); + Mat err = _err.getMat(); + err = err.reshape(2, errCount); + if (_Jac.needed()) + { + _Jac.create(errCount * 2, 6, CV_64F); + Mat Jac = _Jac.getMat(); + Mat dpdr = Jac.colRange(0, 3); + Mat dpdt = Jac.colRange(3, 6); + projectPoints(objpt, rv, tv, cameraMatrix, distCoeffs, + err, dpdr, dpdt, noArray(), noArray(), noArray(), noArray()); + } + else + { + projectPoints(objpt, rv, tv, cameraMatrix, distCoeffs, err); + } + err = err - imgpt; + err = err.reshape(1, 2 * errCount); + return true; + }; + + LevMarq solver(_param, callback, LevMarq::Settings().setMaxIterations((unsigned int)max_iter).setGeodesic(true)); + solver.optimize(); + _param.rowRange(0, 3).copyTo(rvec); _param.rowRange(3, 6).copyTo(tvec); } diff --git a/modules/3d/src/fundam.cpp b/modules/3d/src/fundam.cpp index 921db14e34..48a124ef69 100644 --- a/modules/3d/src/fundam.cpp +++ b/modules/3d/src/fundam.cpp @@ -212,63 +212,6 @@ public: }; -class HomographyRefineCallback CV_FINAL : public LMSolver::Callback -{ -public: - HomographyRefineCallback(InputArray _src, InputArray _dst) - { - src = _src.getMat(); - dst = _dst.getMat(); - } - - bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE - { - int i, count = src.checkVector(2); - Mat param = _param.getMat(); - _err.create(count*2, 1, CV_64F); - Mat err = _err.getMat(), J; - if( _Jac.needed()) - { - _Jac.create(count*2, param.rows, CV_64F); - J = _Jac.getMat(); - CV_Assert( J.isContinuous() && J.cols == 8 ); - } - - const Point2f* M = src.ptr(); - const Point2f* m = dst.ptr(); - const double* h = param.ptr(); - double* errptr = err.ptr(); - double* Jptr = J.data ? J.ptr() : 0; - - for( i = 0; i < count; i++ ) - { - double Mx = M[i].x, My = M[i].y; - double ww = h[6]*Mx + h[7]*My + 1.; - ww = fabs(ww) > DBL_EPSILON ? 1./ww : 0; - double xi = (h[0]*Mx + h[1]*My + h[2])*ww; - double yi = (h[3]*Mx + h[4]*My + h[5])*ww; - errptr[i*2] = xi - m[i].x; - errptr[i*2+1] = yi - m[i].y; - - if( Jptr ) - { - Jptr[0] = Mx*ww; Jptr[1] = My*ww; Jptr[2] = ww; - Jptr[3] = Jptr[4] = Jptr[5] = 0.; - Jptr[6] = -Mx*ww*xi; Jptr[7] = -My*ww*xi; - Jptr[8] = Jptr[9] = Jptr[10] = 0.; - Jptr[11] = Mx*ww; Jptr[12] = My*ww; Jptr[13] = ww; - Jptr[14] = -Mx*ww*yi; Jptr[15] = -My*ww*yi; - - Jptr += 16; - } - } - - return true; - } - - Mat src, dst; -}; - static bool createAndRunRHORegistrator(double confidence, int maxIters, double ransacReprojThreshold, @@ -415,7 +358,56 @@ Mat findHomography( InputArray _points1, InputArray _points2, if( method == RANSAC || method == LMEDS ) cb->runKernel( src, dst, H ); Mat H8(8, 1, CV_64F, H.ptr()); - LMSolver::create(makePtr(src, dst), 10)->run(H8); + + auto homographyRefineCallback = [src, dst](InputOutputArray _param, OutputArray _err, OutputArray _Jac) -> bool + { + int i, count = src.checkVector(2); + Mat param = _param.getMat(); + _err.create(count * 2, 1, CV_64F); + Mat err = _err.getMat(), J; + if (_Jac.needed()) + { + _Jac.create(count * 2, param.rows, CV_64F); + J = _Jac.getMat(); + CV_Assert(J.isContinuous() && J.cols == 8); + } + + const Point2f* M = src.ptr(); + const Point2f* m = dst.ptr(); + const double* h = param.ptr(); + double* errptr = err.ptr(); + double* Jptr = J.data ? J.ptr() : 0; + + for (i = 0; i < count; i++) + { + double Mx = M[i].x, My = M[i].y; + double ww = h[6] * Mx + h[7] * My + 1.; + ww = fabs(ww) > DBL_EPSILON ? 1. / ww : 0; + double xi = (h[0] * Mx + h[1] * My + h[2]) * ww; + double yi = (h[3] * Mx + h[4] * My + h[5]) * ww; + errptr[i * 2] = xi - m[i].x; + errptr[i * 2 + 1] = yi - m[i].y; + + if (Jptr) + { + Jptr[0] = Mx * ww; Jptr[1] = My * ww; Jptr[2] = ww; + Jptr[3] = Jptr[4] = Jptr[5] = 0.; + Jptr[6] = -Mx * ww * xi; Jptr[7] = -My * ww * xi; + Jptr[8] = Jptr[9] = Jptr[10] = 0.; + Jptr[11] = Mx * ww; Jptr[12] = My * ww; Jptr[13] = ww; + Jptr[14] = -Mx * ww * yi; Jptr[15] = -My * ww * yi; + + Jptr += 16; + } + } + + return true; + }; + LevMarq solver(H8, homographyRefineCallback, + LevMarq::Settings() + .setMaxIterations(10) + .setGeodesic(true)); + solver.optimize(); } } diff --git a/modules/3d/src/levmarq.cpp b/modules/3d/src/levmarq.cpp index d4c188f17c..d9047d4990 100644 --- a/modules/3d/src/levmarq.cpp +++ b/modules/3d/src/levmarq.cpp @@ -43,260 +43,746 @@ #include "precomp.hpp" #include -/* - This is a translation to C++ from the Matlab's LMSolve package by Miroslav Balda. - Here is the original copyright: - ============================================================================ - - Copyright (c) 2007, Miroslav Balda - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are - met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the distribution - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. -*/ - namespace cv { -static void subMatrix(const Mat& src, Mat& dst, - const Mat& mask) -{ - CV_Assert(src.type() == CV_64F && dst.type() == CV_64F); - int m = src.rows, n = src.cols; - int i1 = 0, j1 = 0; - for(int i = 0; i < m; i++) - { - if(mask.at(i)) - { - const double* srcptr = src.ptr(i); - double* dstptr = dst.ptr(i1++); +LevMarq::Settings::Settings(): + jacobiScaling(false), + upDouble(true), + useStepQuality(true), + clampDiagonal(true), + stepNormInf(false), + checkRelEnergyChange(true), + checkMinGradient(true), + checkStepNorm(true), + geodesic(false), + hGeo(1e-4), + geoScale(0.5), + stepNormTolerance(1e-6), + relEnergyDeltaTolerance(1e-6), + minGradientTolerance(1e-6), + smallEnergyTolerance(0), // not used by default + maxIterations(500), + initialLambda(0.0001), + initialLmUpFactor(2.0), + initialLmDownFactor(3.0) +{ } - for(int j = j1 = 0; j < n; j++) + +// from Ceres, equation energy change: +// eq. energy = 1/2 * (residuals + J * step)^2 = +// 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) +// eq. energy change = 1/2 * residuals^2 - eq. energy = +// 1/2 * residuals^2 - 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) = +// 1/2 * ( residuals^2 - residuals^2 - 2 * residuals^T * J * step - (J*step)^T * J * step) = +// - 1/2 * ( 2 * residuals^T * J * step + (J*step)^T * J * step) = +// - 1/2 * ( 2 * residuals^T * J + (J*step)^T * J ) * step = +// - 1/2 * ( 2 * residuals^T * J + step^T * J^T * J ) * step = +// - 1/2 * step^T * ( 2 * J^t * residuals + J^T * J * step ) = +// - 1/2 * step^T * ( 2 * J^t * residuals + (J^T * J + LMDiag - LMDiag) * step ) = +// - 1/2 * step^T * ( 2 * J^t * residuals + (J^T * J + LMDiag) * step - LMDiag * step ) = +// - 1/2 * step^T * ( 2 * J^t * residuals - J^T * residuals - LMDiag * step ) = +// - 1/2 * step^T * ( J^t * residuals - LMDiag * step ) = +// - 1/2 * x^T * ( jtb - LMDiag * x ) +static double calcJacCostChangeLm(const cv::Mat_& jtb, const cv::Mat_& x, const cv::Mat_& lmDiag) +{ + return -0.5 * cv::sum(x.mul(jtb - lmDiag.mul(x)))[0]; +} + + +// 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 +// J^T*rvv = J^T*((f(x0 + v*h) - f(x0))/h - J*v)/h = +// J^T*(f(x0 + v*h) - f(x0) - J*v*h)/h^2 = +// (J^T*f(x0 + v*h) - J^T*f(x0) - J^T*J*v*h)/h^2 = +// < using (J^T*J + lmdiag) * v = J^T*b, also f(x0 + v*h) = b_v, f(x0) = b > +// (J^T*b_v - J^T*b - (J^T*J + lmdiag - lmdiag)*v*h)/h^2 = +// (J^T*b_v - J^T*b + J^t*b*h + lmdiag*v*h)/h^2 = +// (J^T*b_v - J^t*b*(1 - h) + lmdiag*v*h)/h^2 +static void calcJtrvv(const Mat_& jtbv, const Mat_& jtb, const Mat_& lmdiag, + const Mat_& v, const double hGeo, + Mat_& jtrvv) +{ + jtrvv = (jtbv + jtb * (hGeo - 1.0) + lmdiag.mul(v, hGeo)) / (hGeo * hGeo); +} + + +LevMarq::Report detail::LevMarqBase::optimize() +{ + if (settings.geodesic && !backend->enableGeo()) + { + CV_LOG_INFO(NULL, "The backend does not support geodesic acceleration, please turn off the corresponding option"); + return LevMarq::Report(false, 0, 0); // not found + } + + // this function sets probe vars to current X + backend->prepareVars(); + + double energy = 0.0; + if (!backend->calcFunc(energy, /*calcEnergy*/ true, /*calcJacobian*/ true) || energy < 0) + { + CV_LOG_INFO(NULL, "Error while calculating energy function"); + return LevMarq::Report(false, 0, 0); // not found + } + + double oldEnergy = energy; + + CV_LOG_INFO(NULL, "#s" << " energy: " << energy); + + // diagonal clamping options + const double minDiag = 1e-6; + const double maxDiag = 1e32; + // limit lambda inflation + const double maxLambda = 1e32; + + // finish reasons + bool tooLong = false; // => not found + bool bigLambda = false; // => not found + bool smallGradient = false; // => found + bool smallStep = false; // => found + bool smallEnergyDelta = false; // => found + bool smallEnergy = false; // => found + + // column scale inverted, for jacobian scaling + Mat_ di; + + // do the jacobian conditioning improvement used in Ceres + if (settings.jacobiScaling) + { + // L2-normalize each jacobian column + // vec d = {d_j = sum(J_ij^2) for each column j of J} = get_diag{ J^T * J } + // di = { 1/(1+sqrt(d_j)) }, extra +1 to avoid div by zero + Mat_ ds; + const Mat_ diag = backend->getDiag(); + cv::sqrt(diag, ds); + di = 1.0 / (ds + 1.0); + } + + double lmUpFactor = settings.initialLmUpFactor; + double lambdaLevMarq = settings.initialLambda; + + unsigned int iter = 0; + bool done = false; + while (!done) + { + // At this point we should have jtj and jtb built + + CV_LOG_INFO(NULL, "#LM#s" << " energy: " << energy); + + // do the jacobian conditioning improvement used in Ceres + if (settings.jacobiScaling) + { + backend->doJacobiScaling(di); + } + + const Mat_ jtb = backend->getJtb(); + + double gradientMax = cv::norm(jtb, NORM_INF); + + // Save original diagonal of jtj matrix for LevMarq + const Mat_ diag = backend->getDiag(); + + // Solve using LevMarq and get delta transform + bool enoughLm = false; + + while (!enoughLm && !done) + { + // form LevMarq matrix + Mat_ lmDiag, jtjDiag; + lmDiag = diag * lambdaLevMarq; + if (settings.clampDiagonal) + lmDiag = cv::min(cv::max(lmDiag, minDiag), maxDiag); + jtjDiag = lmDiag + diag; + backend->setDiag(jtjDiag); + + CV_LOG_INFO(NULL, "linear decompose..."); + + bool decomposed = backend->decompose(); + + CV_LOG_INFO(NULL, (decomposed ? "OK" : "FAIL")); + + CV_LOG_INFO(NULL, "linear solve..."); + // use double or convert everything to float + Mat_ x((int)jtb.rows, 1); + bool solved = decomposed && backend->solveDecomposed(jtb, x); + + CV_LOG_INFO(NULL, (solved ? "OK" : "FAIL")); + + double costChange = 0.0; + double jacCostChange = 0.0; + double stepQuality = 0.0; + double xNorm = 0.0; + if (solved) { - if(n < m || mask.at(j)) - dstptr[j1++] = srcptr[j]; + // what energy drop should be according to local model estimation + jacCostChange = calcJacCostChangeLm(jtb, x, lmDiag); + + // x norm + xNorm = cv::norm(x, settings.stepNormInf ? NORM_INF : NORM_L2SQR); + + // undo jacobi scaling + if (settings.jacobiScaling) + { + x = x.mul(di); + } + + if (settings.geodesic) + { + backend->currentOplusX(x * settings.hGeo, /*geo*/ true); + + Mat_ jtbv(jtb.rows, 1); + if (backend->calcJtbv(jtbv)) + { + Mat_ jtrvv(jtb.rows, 1); + calcJtrvv(jtbv, jtb, lmDiag, x, settings.hGeo, jtrvv); + + Mat_ xgeo((int)jtb.rows, 1); + bool geoSolved = backend->solveDecomposed(jtrvv, xgeo); + + if (geoSolved) + { + double truncerr = sqrt(xgeo.dot(xgeo) / x.dot(x)); + bool geoIsGood = (truncerr < 1.0); + if (geoIsGood) + x += xgeo * settings.geoScale; + + CV_LOG_INFO(NULL, "Geo truncerr: " << truncerr << (geoIsGood ? ", use it" : ", skip it") ); + } + else + { + CV_LOG_INFO(NULL, "Geo: failed to solve"); + } + } + } + + // calc energy with current delta x + backend->currentOplusX(x, /*geo*/ false); + + bool success = backend->calcFunc(energy, /*calcEnergy*/ true, /*calcJacobian*/ false); + if (!success || energy < 0 || std::isnan(energy)) + { + CV_LOG_INFO(NULL, "Error while calculating energy function"); + return LevMarq::Report(false, iter, oldEnergy); // not found + } + + costChange = oldEnergy - energy; + + stepQuality = costChange / jacCostChange; + + CV_LOG_INFO(NULL, "#LM#" << iter + << " energy: " << energy + << " deltaEnergy: " << costChange + << " deltaEqEnergy: " << jacCostChange + << " max(J^T*b): " << gradientMax + << (settings.stepNormInf ? " normInf(x): " : " norm2(x): ") << xNorm + << " deltaEnergy/energy: " << costChange / energy); + } + + // zero cost change is treated like an algorithm failure if checkRelEnergyChange is off + if (!solved || costChange < 0 || (!settings.checkRelEnergyChange && abs(costChange) < DBL_EPSILON)) + { + // failed to optimize, increase lambda and repeat + + lambdaLevMarq *= lmUpFactor; + if (settings.upDouble) + lmUpFactor *= 2.0; + + CV_LOG_INFO(NULL, "LM goes up, lambda: " << lambdaLevMarq << ", old energy: " << oldEnergy); + } + else + { + // optimized successfully, decrease lambda and set variables for next iteration + enoughLm = true; + + if (settings.useStepQuality) + lambdaLevMarq *= std::max(1.0 / settings.initialLmDownFactor, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); + else + lambdaLevMarq *= 1.0 / settings.initialLmDownFactor; + lmUpFactor = settings.initialLmUpFactor; + + smallGradient = (gradientMax < settings.minGradientTolerance); + smallStep = (xNorm < settings.stepNormTolerance); + smallEnergyDelta = (costChange / energy < settings.relEnergyDeltaTolerance); + smallEnergy = (energy < settings.smallEnergyTolerance); + + backend->acceptProbe(); + + CV_LOG_INFO(NULL, "#" << iter << " energy: " << energy); + + oldEnergy = energy; + + CV_LOG_INFO(NULL, "LM goes down, lambda: " << lambdaLevMarq << " step quality: " << stepQuality); + } + + iter++; + + tooLong = (iter >= settings.maxIterations); + bigLambda = (lambdaLevMarq >= maxLambda); + + done = tooLong || bigLambda; + done = done || (settings.checkMinGradient && smallGradient); + done = done || (settings.checkStepNorm && smallStep); + done = done || (settings.checkRelEnergyChange && smallEnergyDelta); + done = done || (smallEnergy); + } + + // calc jacobian for next iteration + if (!done) + { + double dummy; + if (!backend->calcFunc(dummy, /*calcEnergy*/ false, /*calcJacobian*/ true)) + { + CV_LOG_INFO(NULL, "Error while calculating jacobian"); + return LevMarq::Report(false, iter, oldEnergy); // not found } } } + + bool found = (smallGradient || smallStep || smallEnergyDelta || smallEnergy); + + CV_LOG_INFO(NULL, "Finished: " << (found ? "" : "not ") << "found"); + std::string fr = "Finish reason: "; + if (settings.checkMinGradient && smallGradient) + CV_LOG_INFO(NULL, fr + "gradient max val dropped below threshold"); + if (settings.checkStepNorm && smallStep) + CV_LOG_INFO(NULL, fr + "step size dropped below threshold"); + if (settings.checkRelEnergyChange && smallEnergyDelta) + CV_LOG_INFO(NULL, fr + "relative energy change between iterations dropped below threshold"); + if (smallEnergy) + CV_LOG_INFO(NULL, fr + "energy dropped below threshold"); + if (tooLong) + CV_LOG_INFO(NULL, fr + "max number of iterations reached"); + if (bigLambda) + CV_LOG_INFO(NULL, fr + "lambda has grown above the threshold, the trust region is too small"); + + return LevMarq::Report(found, iter, oldEnergy); } -class LMSolverImpl CV_FINAL : public LMSolver + +struct LevMarqDenseLinearBackend : public detail::LevMarqBackend +{ + // all variables including fixed ones + size_t nVars; + size_t allVars; + Mat_ jtj, jtb; + Mat_ probeX, currentX; + // for oplus operation + Mat_ delta; + + // "Long" callback: f(x, &b, &J) -> bool + // Produces jacobian and residuals for each energy term + LevMarq::LongCallback cb; + // "Normal" callback: f(x, &jtb, &jtj, &energy) -> bool + // Produces J^T*J and J^T*b directly instead of J and b + LevMarq::NormalCallback cb_alt; + + Mat_ mask; + // full matrices containing all vars including fixed ones + // used only when mask is not empty + Mat_ jtjFull, jtbFull; + + // used only with long callback + Mat_ jLong, bLong; + size_t nerrs; + // used only with alt. callback + // What part of symmetric matrix is to copy to another part + bool LtoR; + // What method to use for linear system solving + int solveMethod; + + // for geodesic acceleration + bool useGeo; + // x0 + v*h variable + Mat_ geoX; + // J^T*rvv vector + Mat_ jtrvv; + + LevMarqDenseLinearBackend(int nvars_, LevMarq::LongCallback callback_, InputArray mask_, int nerrs_, int solveMethod_) : + LevMarqDenseLinearBackend(noArray(), nvars_, callback_, nullptr, nerrs_, false, mask_, solveMethod_) + { } + LevMarqDenseLinearBackend(int nvars_, LevMarq::NormalCallback callback_, InputArray mask_, bool LtoR_, int solveMethod_) : + LevMarqDenseLinearBackend(noArray(), nvars_, nullptr, callback_, 0, LtoR_, mask_, solveMethod_) + { } + LevMarqDenseLinearBackend(InputOutputArray param_, LevMarq::LongCallback callback_, InputArray mask_, int nerrs_, int solveMethod_) : + LevMarqDenseLinearBackend(param_, 0, callback_, nullptr, nerrs_, false, mask_, solveMethod_) + { } + LevMarqDenseLinearBackend(InputOutputArray param_, LevMarq::NormalCallback callback_, InputArray mask_, bool LtoR_, int solveMethod_) : + LevMarqDenseLinearBackend(param_, 0, nullptr, callback_, 0, LtoR_, mask_, solveMethod_) + { } + + LevMarqDenseLinearBackend(InputOutputArray currentX_, int nvars, + LevMarq::LongCallback cb_ = nullptr, + LevMarq::NormalCallback cb_alt_ = nullptr, + size_t nerrs_ = 0, + bool LtoR_ = false, + InputArray mask_ = noArray(), + int solveMethod_ = DECOMP_SVD) : + LevMarqBackend(), + // these fields will be initialized at prepareVars() + jtj(), + jtb(), + probeX(), + delta(), + jtjFull(), + jtbFull(), + jLong(), + bLong(), + useGeo() + { + if (!currentX_.empty()) + { + CV_Assert(currentX_.type() == CV_64F); + CV_Assert(currentX_.rows() == 1 || currentX_.cols() == 1); + this->allVars = currentX_.size().area(); + this->currentX = currentX_.getMat().reshape(1, (int)this->allVars); + } + else + { + CV_Assert(nvars > 0); + this->allVars = nvars; + this->currentX = Mat_((int)this->allVars, 1); + } + + CV_Assert((cb_ || cb_alt_) && !(cb_ && cb_alt_)); + this->cb = cb_; + this->cb_alt = cb_alt_; + + this->nerrs = nerrs_; + this->LtoR = LtoR_; + this->solveMethod = solveMethod_; + + if (!mask_.empty()) + { + CV_Assert(mask_.depth() == CV_8U); + CV_Assert(mask_.size() == currentX_.size()); + int maskSize = mask_.size().area(); + this->mask.create(maskSize, 1); + mask_.copyTo(this->mask); + } + else + this->mask = Mat_(); + + this->nVars = this->mask.empty() ? this->allVars : countNonZero(this->mask); + CV_Assert(this->nVars > 0); + } + + virtual bool enableGeo() CV_OVERRIDE + { + useGeo = true; + return true; + } + + virtual void prepareVars() CV_OVERRIDE + { + probeX = currentX.clone(); + delta = Mat_((int)allVars, 1); + + // Allocate vars for use with mask + if (!mask.empty()) + { + jtjFull = Mat_((int)allVars, (int)allVars); + jtbFull = Mat_((int)allVars, 1); + jtj = Mat_((int)nVars, (int)nVars); + jtb = Mat_((int)nVars, 1); + } + else + { + jtj = Mat_((int)nVars, (int)nVars); + jtb = Mat_((int)nVars, 1); + jtjFull = jtj; + jtbFull = jtb; + } + + if (nerrs) + { + jLong = Mat_((int)nerrs, (int)allVars); + bLong = Mat_((int)nerrs, 1, CV_64F); + } + + if (useGeo) + { + geoX = currentX.clone(); + jtrvv = jtb.clone(); + } + } + + // adds x to current variables and writes result to probe vars + virtual void currentOplusX(const Mat_& x, bool geo) CV_OVERRIDE + { + // 'unpack' the param delta + int j = 0; + if (!mask.empty()) + { + for (int i = 0; i < (int)allVars; i++) + { + delta.at(i) = (mask.at(i) != 0) ? x(j++) : 0.0; + } + } + else + delta = x; + + if (geo) + { + if (useGeo) + { + geoX = currentX + delta; + } + else + { + CV_Error(CV_StsBadArg, "Geodesic acceleration is disabled"); + } + } + else + { + probeX = currentX + delta; + } + } + + virtual void acceptProbe() CV_OVERRIDE + { + probeX.copyTo(currentX); + } + + static void subMatrix(const Mat_& src, Mat_& dst, const Mat_& mask) + { + CV_Assert(src.type() == CV_64F && dst.type() == CV_64F); + int m = src.rows, n = src.cols; + int i1 = 0, j1 = 0; + for (int i = 0; i < m; i++) + { + if (mask(i)) + { + const double* srcptr = src[i]; + double* dstptr = dst[i1++]; + + for (int j = j1 = 0; j < n; j++) + { + if (n < m || mask(j)) + dstptr[j1++] = srcptr[j]; + } + } + } + } + + virtual bool calcFunc(double& energy, bool calcEnergy = true, bool calcJacobian = false) CV_OVERRIDE + { + Mat_ xd = probeX; + + double sd = 0.0; + if (calcJacobian) + { + jtbFull.setZero(); + jtjFull.setZero(); + + if (!cb_alt) + { + jLong.setZero(); + } + } + + if (cb_alt) + { + bool r = calcJacobian ? cb_alt(xd, jtbFull, jtjFull, sd) : cb_alt(xd, noArray(), noArray(), sd); + if (!r) + return false; + } + else + { + bLong.setZero(); + bool r = calcJacobian ? cb(xd, bLong, jLong) : cb(xd, bLong, noArray()); + if (!r) + return false; + } + + if (calcJacobian) + { + if (cb_alt) + { + completeSymm(jtjFull, LtoR); + } + else + { + mulTransposed(jLong, jtjFull, true); + gemm(jLong, bLong, 1, noArray(), 0, jtbFull, GEMM_1_T); + } + } + + if (calcEnergy) + { + if (cb_alt) + { + energy = sd; + } + else + { + energy = norm(bLong, NORM_L2SQR); + } + } + + if (!mask.empty()) + { + subMatrix(jtjFull, jtj, mask); + subMatrix(jtbFull, jtb, mask); + } + else + { + jtj = jtjFull; + jtb = jtbFull; + } + + return true; + } + + virtual const Mat_ getDiag() CV_OVERRIDE + { + return jtj.diag().clone(); + } + + virtual const Mat_ getJtb() CV_OVERRIDE + { + return jtb; + } + + virtual void setDiag(const Mat_& d) CV_OVERRIDE + { + d.copyTo(jtj.diag()); + } + + virtual void doJacobiScaling(const Mat_& di) CV_OVERRIDE + { + // J := J * d_inv, d_inv = make_diag(di) + // J^T*J := (J * d_inv)^T * J * d_inv = diag(di) * (J^T * J) * diag(di) = eltwise_mul(J^T*J, di*di^T) + // J^T*b := (J * d_inv)^T * b = d_inv^T * J^T*b = eltwise_mul(J^T*b, di) + // scaling J^T*J + for (int i = 0; i < (int)nVars; i++) + { + double* jtjrow = jtj.ptr(i); + for (int j = 0; j < (int)nVars; j++) + { + jtjrow[j] *= di(i) * di(j); + } + } + // scaling J^T*b + for (int i = 0; i < (int)nVars; i++) + { + jtb(i) *= di(i); + } + } + + virtual bool decompose() CV_OVERRIDE + { + //TODO: do the real decomposition + return true; + } + + virtual bool solveDecomposed(const Mat_& right, Mat_& x) CV_OVERRIDE + { + return cv::solve(jtj, -right, x, solveMethod); + } + + // calculates J^T*f(geo) + virtual bool calcJtbv(Mat_& jtbv) CV_OVERRIDE + { + if (cb_alt) + { + CV_Error(CV_StsNotImplemented, "Geodesic acceleration is not implemented for normal callbacks, please use \"long\" callbacks"); + } + else + { + Mat_ b_v = bLong.clone(); + bool r = cb(geoX, b_v, noArray()); + if (!r) + return false; + + Mat_ jLongFiltered(jLong.rows, (int)nVars); + int ctr = 0; + for (int i = 0; i < (int)allVars; i++) + { + if (mask.empty() || mask(i)) + { + jLong.col(i).copyTo(jLongFiltered.col(ctr)); + ctr++; + } + } + + jtbv = jLongFiltered.t() * b_v; + return true; + } + } +}; + +class LevMarq::Impl : public detail::LevMarqBase { public: - LMSolverImpl(const Ptr& _cb, int _maxIters, double _eps = FLT_EPSILON) - : cb(_cb), eps(_eps), maxIters(_maxIters) + Impl(const Ptr& backend_, const LevMarq::Settings& settings_) : + LevMarqBase(backend_, settings_) + { } + + Report run(InputOutputArray param) { + CV_Assert(!param.empty() && (param.type() == CV_64F) && (param.rows() == 1 || param.cols() == 1)); + backend.dynamicCast()->currentX = param.getMat().reshape(1, param.size().area()); + return optimize(); } - - int run(InputOutputArray param0) const CV_OVERRIDE - { - return LMSolver::run(param0, noArray(), 0, - TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, maxIters, eps), DECOMP_SVD, - [&](Mat& param, Mat* err, Mat* J)->bool - { - return cb->compute(param, err ? _OutputArray(*err) : _OutputArray(), - J ? _OutputArray(*J) : _OutputArray()); - }); - } - - void setMaxIters(int iters) CV_OVERRIDE { CV_Assert(iters > 0); maxIters = iters; } - int getMaxIters() const CV_OVERRIDE { return maxIters; } - - Ptr cb; - double eps; - int maxIters; }; -Ptr LMSolver::create(const Ptr& cb, int maxIters) +LevMarq::LevMarq(int nvars, LongCallback callback, const Settings& settings, InputArray mask, + MatrixType matrixType, VariableType paramType, int nerrs, int solveMethod) { - return makePtr(cb, maxIters); + if (matrixType != MatrixType::AUTO && matrixType != MatrixType::DENSE) + CV_Error(CV_StsNotImplemented, "General purpuse sparse solver for LevMarq is not implemented yet"); + if (paramType != VariableType::LINEAR) + CV_Error(CV_StsNotImplemented, "SO(3) and SE(3) params for LevMarq are not implemented yet"); + + auto backend = makePtr(nvars, callback, mask, nerrs, solveMethod); + pImpl = makePtr(backend, settings); } -Ptr LMSolver::create(const Ptr& cb, int maxIters, double eps) +LevMarq::LevMarq(int nvars, NormalCallback callback, const Settings& settings, InputArray mask, + MatrixType matrixType, VariableType paramType, bool LtoR, int solveMethod) { - return makePtr(cb, maxIters, eps); + if (matrixType != MatrixType::AUTO && matrixType != MatrixType::DENSE) + CV_Error(CV_StsNotImplemented, "General purpuse sparse solver for LevMarq is not implemented yet"); + if (paramType != VariableType::LINEAR) + CV_Error(CV_StsNotImplemented, "SO(3) and SE(3) params for LevMarq are not implemented yet"); + + auto backend = makePtr(nvars, callback, mask, LtoR, solveMethod); + pImpl = makePtr(backend, settings); } -static int LMSolver_run(InputOutputArray _param0, InputArray _mask, - int nerrs, const TermCriteria& termcrit, - int solveMethod, bool LtoR, - std::function* cb, - std::function* cb_alt) +LevMarq::LevMarq(InputOutputArray param, LongCallback callback, const Settings& settings, InputArray mask, + MatrixType matrixType, VariableType paramType, int nerrs, int solveMethod) { - int lambdaLg10 = -3; - Mat mask = _mask.getMat(); - Mat param0 = _param0.getMat(); - Mat x, xd, r, rd, J, A, Ap, v, temp_d, d, Am, vm, dm; - int p0type = param0.type(); - int maxIters = termcrit.type & TermCriteria::COUNT ? termcrit.maxCount : 1000; - double epsx = termcrit.type & TermCriteria::EPS ? termcrit.epsilon : 0, epsf = epsx; + if (matrixType != MatrixType::AUTO && matrixType != MatrixType::DENSE) + CV_Error(CV_StsNotImplemented, "General purpuse sparse solver for LevMarq is not implemented yet"); + if (paramType != VariableType::LINEAR) + CV_Error(CV_StsNotImplemented, "SO(3) and SE(3) params for LevMarq are not implemented yet"); - CV_Assert( (param0.cols == 1 || param0.rows == 1) && (p0type == CV_32F || p0type == CV_64F)); - CV_Assert( cb || cb_alt ); - - int lx = param0.rows + param0.cols - 1; - param0.convertTo(x, CV_64F); - d.create(lx, 1, CV_64F); - - CV_Assert(!mask.data || - (mask.depth() == CV_8U && - (mask.cols == 1 || mask.rows == 1) && - (mask.rows + mask.cols - 1 == lx))); - int lxm = mask.data ? countNonZero(mask) : lx; - if (lxm < lx) { - Am.create(lxm, lxm, CV_64F); - vm.create(lxm, 1, CV_64F); - } - - if( x.cols != 1 ) - transpose(x, x); - - A.create(lx, lx, CV_64F); - v.create(lx, 1, CV_64F); - - if (nerrs > 0) { - J.create(nerrs, lx, CV_64F); - r.create(nerrs, 1, CV_64F); - rd.create(nerrs, 1, CV_64F); - } - - double S = 0; - int nfJ = 1; - if (cb_alt) { - if( !(*cb_alt)(x, &v, &A, &S) ) - return -1; - completeSymm(A, LtoR); - } else { - if( !(*cb)(x, &r, &J) ) - return -1; - S = norm(r, NORM_L2SQR); - mulTransposed(J, A, true); - gemm(J, r, 1, noArray(), 0, v, GEMM_1_T); - } - - int i, iter = 0; - - for( ;; ) - { - CV_Assert( A.type() == CV_64F && A.rows == lx ); - A.copyTo(Ap); - double lambda = exp(lambdaLg10*log(10.)); - for( i = 0; i < lx; i++ ) - Ap.at(i, i) *= (1 + lambda); - if (lxm < lx) { - // remove masked-out rows & cols from JtJ and JtErr - subMatrix(Ap, Am, mask); - subMatrix(v, vm, mask); - solve(Am, vm, dm, solveMethod); - int j = 0; - // 'unpack' the param delta - for(i = j = 0; i < lx; i++) - d.at(i) = mask.at(i) != 0 ? dm.at(j++) : 0.; - } else { - solve(Ap, v, d, solveMethod); - } - subtract(x, d, xd); - - double Sd = 0.; - - if (cb_alt) { - if( !(*cb_alt)(xd, 0, 0, &Sd) ) - return -1; - } else { - if( !(*cb)(xd, &rd, 0) ) - return -1; - Sd = norm(rd, NORM_L2SQR); - } - - nfJ++; - if( Sd < S ) - { - nfJ++; - S = Sd; - lambdaLg10 = MAX(lambdaLg10-1, -16); - iter++; - std::swap(x, xd); - if (cb_alt) { - v.setZero(); - A.setZero(); - Sd = 0.; - if( !(*cb_alt)(x, &v, &A, &Sd) ) - return -1; - completeSymm(A, LtoR); - } else { - r.setZero(); - J.setZero(); - if( !(*cb)(x, &r, &J) ) - return -1; - mulTransposed(J, A, true); - gemm(J, r, 1, noArray(), 0, v, GEMM_1_T); - } - } else { - iter += lambdaLg10 == 16; - lambdaLg10 = MIN(lambdaLg10+1, 16); - } - - bool proceed = iter < maxIters && norm(d, NORM_INF) >= epsx && S >= epsf*epsf; - - /*if(lxm < lx) - { - printf("lambda=%g. delta:", lambda); - int j; - for(i = j = 0; i < lx; i++) { - double delta = d.at(i); - j += delta != 0; - if(j < 10) - printf(" %.2g", delta); - } - printf("\n"); - printf("%c %d %d, err=%g, param[0]=%g, d[0]=%g, lg10(lambda)=%d\n", - (proceed ? ' ' : '*'), iter, nfJ, S, x.at(0), d.at(0), lambdaLg10); - }*/ - if(!proceed) - break; - } - - if( param0.size() != x.size() ) - transpose(x, x); - - x.convertTo(param0, p0type); - if( iter == maxIters ) - iter = -iter; - - return iter; + auto backend = makePtr(param, callback, mask, nerrs, solveMethod); + pImpl = makePtr(backend, settings); } -int LMSolver::run(InputOutputArray param, InputArray mask, int nerrs, - const TermCriteria& termcrit, int solveMethod, - std::function cb) +LevMarq::LevMarq(InputOutputArray param, NormalCallback callback, const Settings& settings, InputArray mask, + MatrixType matrixType, VariableType paramType, bool LtoR, int solveMethod) { - return LMSolver_run(param, mask, nerrs, termcrit, solveMethod, true, &cb, 0); + if (matrixType != MatrixType::AUTO && matrixType != MatrixType::DENSE) + CV_Error(CV_StsNotImplemented, "General purpuse sparse solver for LevMarq is not implemented yet"); + if (paramType != VariableType::LINEAR) + CV_Error(CV_StsNotImplemented, "SO(3) and SE(3) params for LevMarq are not implemented yet"); + + auto backend = makePtr(param, callback, mask, LtoR, solveMethod); + pImpl = makePtr(backend, settings); } -int LMSolver::runAlt(InputOutputArray param, InputArray mask, - const TermCriteria& termcrit, int solveMethod, bool LtoR, - std::function cb_alt) +LevMarq::Report LevMarq::optimize() { - return LMSolver_run(param, mask, 0, termcrit, solveMethod, LtoR, 0, &cb_alt); + return pImpl->optimize(); +} + +LevMarq::Report LevMarq::run(InputOutputArray param) +{ + return pImpl->run(param); } } diff --git a/modules/3d/src/precomp.hpp b/modules/3d/src/precomp.hpp index 7ff3d8d304..7d96a4becc 100644 --- a/modules/3d/src/precomp.hpp +++ b/modules/3d/src/precomp.hpp @@ -63,7 +63,7 @@ #include "opencv2/core/ocl.hpp" #include "opencv2/core/hal/intrin.hpp" -#include "opencv2/3d/detail/pose_graph.hpp" +#include "opencv2/3d/detail/optimizer.hpp" #include "opencv2/3d/detail/kinfu_frame.hpp" #include diff --git a/modules/3d/src/ptsetreg.cpp b/modules/3d/src/ptsetreg.cpp index 6482d872be..bb5cbe6774 100644 --- a/modules/3d/src/ptsetreg.cpp +++ b/modules/3d/src/ptsetreg.cpp @@ -761,119 +761,6 @@ public: } }; -class Affine2DRefineCallback : public LMSolver::Callback -{ -public: - Affine2DRefineCallback(InputArray _src, InputArray _dst) - { - src = _src.getMat(); - dst = _dst.getMat(); - } - - bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE - { - int i, count = src.checkVector(2); - Mat param = _param.getMat(); - _err.create(count*2, 1, CV_64F); - Mat err = _err.getMat(), J; - if( _Jac.needed()) - { - _Jac.create(count*2, param.rows, CV_64F); - J = _Jac.getMat(); - CV_Assert( J.isContinuous() && J.cols == 6 ); - } - - const Point2f* M = src.ptr(); - const Point2f* m = dst.ptr(); - const double* h = param.ptr(); - double* errptr = err.ptr(); - double* Jptr = J.data ? J.ptr() : 0; - - for( i = 0; i < count; i++ ) - { - double Mx = M[i].x, My = M[i].y; - double xi = h[0]*Mx + h[1]*My + h[2]; - double yi = h[3]*Mx + h[4]*My + h[5]; - errptr[i*2] = xi - m[i].x; - errptr[i*2+1] = yi - m[i].y; - - /* - Jacobian should be: - {x, y, 1, 0, 0, 0} - {0, 0, 0, x, y, 1} - */ - if( Jptr ) - { - Jptr[0] = Mx; Jptr[1] = My; Jptr[2] = 1.; - Jptr[3] = Jptr[4] = Jptr[5] = 0.; - Jptr[6] = Jptr[7] = Jptr[8] = 0.; - Jptr[9] = Mx; Jptr[10] = My; Jptr[11] = 1.; - - Jptr += 6*2; - } - } - - return true; - } - - Mat src, dst; -}; - -class AffinePartial2DRefineCallback : public LMSolver::Callback -{ -public: - AffinePartial2DRefineCallback(InputArray _src, InputArray _dst) - { - src = _src.getMat(); - dst = _dst.getMat(); - } - - bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE - { - int i, count = src.checkVector(2); - Mat param = _param.getMat(); - _err.create(count*2, 1, CV_64F); - Mat err = _err.getMat(), J; - if( _Jac.needed()) - { - _Jac.create(count*2, param.rows, CV_64F); - J = _Jac.getMat(); - CV_Assert( J.isContinuous() && J.cols == 4 ); - } - - const Point2f* M = src.ptr(); - const Point2f* m = dst.ptr(); - const double* h = param.ptr(); - double* errptr = err.ptr(); - double* Jptr = J.data ? J.ptr() : 0; - - for( i = 0; i < count; i++ ) - { - double Mx = M[i].x, My = M[i].y; - double xi = h[0]*Mx - h[1]*My + h[2]; - double yi = h[1]*Mx + h[0]*My + h[3]; - errptr[i*2] = xi - m[i].x; - errptr[i*2+1] = yi - m[i].y; - - /* - Jacobian should be: - {x, -y, 1, 0} - {y, x, 0, 1} - */ - if( Jptr ) - { - Jptr[0] = Mx; Jptr[1] = -My; Jptr[2] = 1.; Jptr[3] = 0.; - Jptr[4] = My; Jptr[5] = Mx; Jptr[6] = 0.; Jptr[7] = 1.; - - Jptr += 4*2; - } - } - - return true; - } - - Mat src, dst; -}; int estimateAffine3D(InputArray _from, InputArray _to, OutputArray _out, OutputArray _inliers, @@ -1065,7 +952,57 @@ Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers, Mat src = from.rowRange(0, inliers_count); Mat dst = to.rowRange(0, inliers_count); Mat Hvec = H.reshape(1, 6); - LMSolver::create(makePtr(src, dst), static_cast(refineIters))->run(Hvec); + + auto affine2DRefineCallback = [src, dst](InputOutputArray _param, OutputArray _err, OutputArray _Jac) -> bool + { + int i, errCount = src.checkVector(2); + Mat param = _param.getMat(); + _err.create(errCount * 2, 1, CV_64F); + Mat err = _err.getMat(), J; + if (_Jac.needed()) + { + _Jac.create(errCount * 2, param.rows, CV_64F); + J = _Jac.getMat(); + CV_Assert(J.isContinuous() && J.cols == 6); + } + + const Point2f* M = src.ptr(); + const Point2f* m = dst.ptr(); + const double* h = param.ptr(); + double* errptr = err.ptr(); + double* Jptr = J.data ? J.ptr() : 0; + + for (i = 0; i < errCount; i++) + { + double Mx = M[i].x, My = M[i].y; + double xi = h[0] * Mx + h[1] * My + h[2]; + double yi = h[3] * Mx + h[4] * My + h[5]; + errptr[i * 2] = xi - m[i].x; + errptr[i * 2 + 1] = yi - m[i].y; + + /* + Jacobian should be: + {x, y, 1, 0, 0, 0} + {0, 0, 0, x, y, 1} + */ + if (Jptr) + { + Jptr[0] = Mx; Jptr[1] = My; Jptr[2] = 1.; + Jptr[3] = Jptr[4] = Jptr[5] = 0.; + Jptr[6] = Jptr[7] = Jptr[8] = 0.; + Jptr[9] = Mx; Jptr[10] = My; Jptr[11] = 1.; + + Jptr += 6 * 2; + } + } + + return true; + }; + LevMarq solver(Hvec, affine2DRefineCallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)refineIters) + .setGeodesic(true)); + solver.optimize(); } } @@ -1158,7 +1095,56 @@ Mat estimateAffinePartial2D(InputArray _from, InputArray _to, OutputArray _inlie double *Hptr = H.ptr(); double Hvec_buf[4] = {Hptr[0], Hptr[3], Hptr[2], Hptr[5]}; Mat Hvec (4, 1, CV_64F, Hvec_buf); - LMSolver::create(makePtr(src, dst), static_cast(refineIters))->run(Hvec); + + auto affinePartial2dRefineCallback = [src, dst](InputOutputArray _param, OutputArray _err, OutputArray _Jac) -> bool + { + int i, errCount = src.checkVector(2); + Mat param = _param.getMat(); + _err.create(errCount * 2, 1, CV_64F); + Mat err = _err.getMat(), J; + if (_Jac.needed()) + { + _Jac.create(errCount * 2, param.rows, CV_64F); + J = _Jac.getMat(); + CV_Assert(J.isContinuous() && J.cols == 4); + } + + const Point2f* M = src.ptr(); + const Point2f* m = dst.ptr(); + const double* h = param.ptr(); + double* errptr = err.ptr(); + double* Jptr = J.data ? J.ptr() : 0; + + for (i = 0; i < errCount; i++) + { + double Mx = M[i].x, My = M[i].y; + double xi = h[0] * Mx - h[1] * My + h[2]; + double yi = h[1] * Mx + h[0] * My + h[3]; + errptr[i * 2] = xi - m[i].x; + errptr[i * 2 + 1] = yi - m[i].y; + + /* + Jacobian should be: + {x, -y, 1, 0} + {y, x, 0, 1} + */ + if (Jptr) + { + Jptr[0] = Mx; Jptr[1] = -My; Jptr[2] = 1.; Jptr[3] = 0.; + Jptr[4] = My; Jptr[5] = Mx; Jptr[6] = 0.; Jptr[7] = 1.; + + Jptr += 4 * 2; + } + } + + return true; + }; + LevMarq solver(Hvec, affinePartial2dRefineCallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)refineIters) + .setGeodesic(true)); + solver.optimize(); + // update H with refined parameters Hptr[0] = Hptr[4] = Hvec_buf[0]; Hptr[1] = -Hvec_buf[1]; diff --git a/modules/3d/src/rgbd/hash_tsdf.cpp b/modules/3d/src/rgbd/hash_tsdf.cpp index 87982bd8af..b57ddc19f1 100644 --- a/modules/3d/src/rgbd/hash_tsdf.cpp +++ b/modules/3d/src/rgbd/hash_tsdf.cpp @@ -786,11 +786,11 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor if (voxel.tsdf != -128 && voxel.weight != 0) { Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); - localPoints.push_back(toPtype(point)); + localPoints.push_back(toPtype(this->pose * point)); if (needNormals) { Point3f normal = volume.getNormalVoxel(point); - localNormals.push_back(toPtype(normal)); + localNormals.push_back(toPtype(this->pose.rotation() * normal)); } } } @@ -1679,12 +1679,11 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor if (voxel.tsdf != -128 && voxel.weight != 0) { Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); - - localPoints.push_back(toPtype(point)); + localPoints.push_back(toPtype(this->pose * point)); if (needNormals) { Point3f normal = volume.getNormalVoxel(point); - localNormals.push_back(toPtype(normal)); + localNormals.push_back(toPtype(this->pose.rotation() * normal)); } } } diff --git a/modules/3d/src/rgbd/odometry_frame_impl.cpp b/modules/3d/src/rgbd/odometry_frame_impl.cpp index 9f4bdbfcbe..234839eddd 100644 --- a/modules/3d/src/rgbd/odometry_frame_impl.cpp +++ b/modules/3d/src/rgbd/odometry_frame_impl.cpp @@ -140,17 +140,18 @@ void OdometryFrameImplTMat::getGrayImage(OutputArray _image) const template void OdometryFrameImplTMat::setDepth(InputArray _depth) { - TMat depth_tmp, depth_flt; depth_tmp = getTMat(_depth); - // Odometry works with depth values in range [0, 10) - // if the max depth value more than 10, it needs to devide by 5000 + // Odometry works well with depth values in range [0, 10) + // If it's bigger, let's scale it down by 5000, a typical depth factor double max; - cv::minMaxLoc(depth_tmp, NULL, &max); + cv::minMaxLoc(depth_tmp, nullptr, &max); if (max > 10) { depth_tmp.convertTo(depth_flt, CV_32FC1, 1.f / 5000.f); - depth_flt.setTo(std::numeric_limits::quiet_NaN(), getTMat(depth_flt) < FLT_EPSILON); + TMat depthMask; + cv::compare(depth_flt, Scalar(FLT_EPSILON), depthMask, cv::CMP_LT); + depth_flt.setTo(std::numeric_limits::quiet_NaN(), depthMask); depth_tmp = depth_flt; } this->depth = depth_tmp; diff --git a/modules/3d/src/rgbd/pose_graph.cpp b/modules/3d/src/rgbd/pose_graph.cpp index 0b597c5b40..ff3a0de35b 100644 --- a/modules/3d/src/rgbd/pose_graph.cpp +++ b/modules/3d/src/rgbd/pose_graph.cpp @@ -4,7 +4,14 @@ #include "../precomp.hpp" #include "sparse_block_matrix.hpp" -#include "opencv2/3d/detail/pose_graph.hpp" +#include "opencv2/3d/detail/optimizer.hpp" + +namespace cv +{ +namespace detail +{ + +#if defined(HAVE_EIGEN) // matrix form of conjugation static const cv::Matx44d M_Conj{ 1, 0, 0, 0, @@ -40,8 +47,6 @@ static inline cv::Matx44d m_right(cv::Quatd q) z, y, -x, w }; } -// precaution against "unused function" warning when there's no Eigen -#if defined(HAVE_EIGEN) // jacobian of quaternionic (exp(x)*q) : R_3 -> H near x == 0 static inline cv::Matx43d expQuatJacobian(cv::Quatd q) { @@ -51,7 +56,6 @@ static inline cv::Matx43d expQuatJacobian(cv::Quatd q) -z, w, x, y, -x, w); } -#endif // concatenate matrices vertically template static inline @@ -98,13 +102,21 @@ cv::Matx<_Tp, m, n + k> concatHor(const cv::Matx<_Tp, m, n>& a, const cv::Matx<_ return res; } -namespace cv -{ -namespace detail +class PoseGraphImpl; +class PoseGraphLevMarqBackend; + +class PoseGraphLevMarq : public LevMarqBase { +public: + PoseGraphLevMarq(PoseGraphImpl* pg, const LevMarq::Settings& settings_ = LevMarq::Settings()) : + LevMarqBase(makePtr(pg), settings_) + { } +}; + class PoseGraphImpl : public detail::PoseGraph { +public: struct Pose3d { Vec3d t; @@ -145,6 +157,25 @@ class PoseGraphImpl : public detail::PoseGraph { q = q.normalize(); } + + // jacobian of exponential (exp(x)* q) : R_6->SE(3) near x == 0 + inline Matx expJacobian() + { + Matx43d qj = expQuatJacobian(q); + // x node layout is (rot_x, rot_y, rot_z, trans_x, trans_y, trans_z) + // pose layout is (q_w, q_x, q_y, q_z, trans_x, trans_y, trans_z) + return concatVert(concatHor(qj, Matx43d()), + concatHor(Matx33d(), Matx33d::eye())); + } + + inline Pose3d oplus(const Vec6d dx) + { + Vec3d deltaRot(dx[0], dx[1], dx[2]), deltaTrans(dx[3], dx[4], dx[5]); + Pose3d p; + p.q = Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * this->q; + p.t = this->t + deltaTrans; + return p; + } }; /*! \class GraphNode @@ -200,10 +231,9 @@ class PoseGraphImpl : public detail::PoseGraph Matx66f sqrtInfo; }; - -public: - PoseGraphImpl() : nodes(), edges() + PoseGraphImpl() : nodes(), edges(), lm() { } + virtual ~PoseGraphImpl() CV_OVERRIDE { } @@ -277,6 +307,16 @@ public: return edges[i].targetNodeId; } + virtual Affine3d getEdgePose(size_t i) const CV_OVERRIDE + { + return edges[i].pose.getAffine(); + } + virtual Matx66f getEdgeInfo(size_t i) const CV_OVERRIDE + { + Matx66f s = edges[i].sqrtInfo; + return s * s; + } + virtual size_t getNumEdges() const CV_OVERRIDE { return edges.size(); @@ -291,12 +331,33 @@ public: // calculate cost function based on provided nodes parameters double calcEnergyNodes(const std::map& newNodes) const; - // Termination criteria are max number of iterations and min relative energy change to current energy + // creates an optimizer + virtual Ptr createOptimizer(const LevMarq::Settings& settings) CV_OVERRIDE + { + lm = makePtr(this, settings); + + return lm; + } + + // creates an optimizer + virtual Ptr createOptimizer() CV_OVERRIDE + { + lm = makePtr(this, LevMarq::Settings() + .setMaxIterations(100) + .setCheckRelEnergyChange(true) + .setRelEnergyDeltaTolerance(1e-6) + .setGeodesic(true)); + + return lm; + } + // 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)) CV_OVERRIDE; + virtual LevMarq::Report optimize() CV_OVERRIDE; std::map nodes; - std::vector edges; + std::vector edges; + + Ptr lm; }; @@ -515,38 +576,10 @@ double PoseGraphImpl::calcEnergyNodes(const std::map& newNodes) co } -#if defined(HAVE_EIGEN) - -// from Ceres, equation energy change: -// eq. energy = 1/2 * (residuals + J * step)^2 = -// 1/2 * ( residuals^2 + 2 * residuals^T * J * step + (J*step)^T * J * step) -// eq. energy change = 1/2 * residuals^2 - eq. energy = -// residuals^T * J * step + 1/2 * (J*step)^T * J * step = -// (residuals^T * J + 1/2 * step^T * J^T * J) * step = -// step^T * ((residuals^T * J)^T + 1/2 * (step^T * J^T * J)^T) = -// 1/2 * step^T * (2 * J^T * residuals + J^T * J * step) = -// 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag - LMDiag) * step) = -// 1/2 * step^T * (2 * J^T * residuals + (J^T * J + LMDiag) * step - LMDiag * step) = -// 1/2 * step^T * (J^T * residuals - LMDiag * step) = -// 1/2 * x^T * (jtb - lmDiag^T * x) -static inline double calcJacCostChange(const std::vector& jtb, - const std::vector& x, - const std::vector& lmDiag) -{ - double jdiag = 0.0; - for (size_t i = 0; i < x.size(); i++) - { - jdiag += x[i] * (jtb[i] - lmDiag[i] * x[i]); - } - double costChange = jdiag * 0.5; - return costChange; -} - - // J := J * d_inv, d_inv = make_diag(di) -// J^T*J := (J * d_inv)^T * J * d_inv = diag(di)* (J^T * J)* diag(di) = eltwise_mul(J^T*J, di*di^T) +// J^T*J := (J * d_inv)^T * J * d_inv = diag(di) * (J^T * J) * diag(di) = eltwise_mul(J^T*J, di*di^T) // J^T*b := (J * d_inv)^T * b = d_inv^T * J^T*b = eltwise_mul(J^T*b, di) -static inline void doJacobiScaling(BlockSparseMat& jtj, std::vector& jtb, const std::vector& di) +static void doJacobiScalingSparse(BlockSparseMat& jtj, Mat_& jtb, const Mat_& di) { // scaling J^T*J for (auto& ijv : jtj.ijValue) @@ -558,350 +591,352 @@ static inline void doJacobiScaling(BlockSparseMat& jtj, std::vecto for (int j = 0; j < 6; j++) { Point2i pt(bpt.x * 6 + i, bpt.y * 6 + j); - m(i, j) *= di[pt.x] * di[pt.y]; + m(i, j) *= di(pt.x) * di(pt.y); } } } // scaling J^T*b - for (size_t i = 0; i < di.size(); i++) - { - jtb[i] *= di[i]; - } + jtb = jtb.mul(di); } - -int PoseGraphImpl::optimize(const cv::TermCriteria& tc) +//TODO: robustness +class PoseGraphLevMarqBackend : public detail::LevMarqBackend { - if (!isValid()) +public: + PoseGraphLevMarqBackend(PoseGraphImpl* pg_) : + LevMarqBackend(), + pg(pg_), + jtj(0), + jtb(), + tempNodes(), + useGeo(), + geoNodes(), + jtrvv(), + jtCached(), + decomposition(), + numNodes(), + numEdges(), + placesIds(), + idToPlace(), + nVarNodes() { - CV_LOG_INFO(NULL, "Invalid PoseGraph that is either not connected or has invalid nodes"); - return -1; - } - - size_t numNodes = getNumNodes(); - size_t numEdges = getNumEdges(); - - // Allocate indices for nodes - std::vector placesIds; - std::map idToPlace; - for (const auto& ni : nodes) - { - if (!ni.second.isFixed) + if (!pg->isValid()) { - idToPlace[ni.first] = placesIds.size(); - placesIds.push_back(ni.first); + CV_Error(cv::Error::Code::StsBadArg, "Invalid PoseGraph that is either not connected or has invalid nodes"); } + + this->numNodes = pg->getNumNodes(); + this->numEdges = pg->getNumEdges(); + + // Allocate indices for nodes + for (const auto& ni : pg->nodes) + { + if (!ni.second.isFixed) + { + this->idToPlace[ni.first] = this->placesIds.size(); + this->placesIds.push_back(ni.first); + } + } + + this->nVarNodes = this->placesIds.size(); + if (!this->nVarNodes) + { + CV_Error(cv::Error::Code::StsBadArg, "PoseGraph contains no non-constant nodes, skipping optimization"); + } + + if (!this->numEdges) + { + CV_Error(cv::Error::Code::StsBadArg, "PoseGraph has no edges, no optimization to be done"); + } + + CV_LOG_INFO(NULL, "Optimizing PoseGraph with " << this->numNodes << " nodes and " << this->numEdges << " edges"); + + this->nVars = this->nVarNodes * 6; } - size_t nVarNodes = placesIds.size(); - if (!nVarNodes) + + virtual bool calcFunc(double& energy, bool calcEnergy = true, bool calcJacobian = false) CV_OVERRIDE { - CV_LOG_INFO(NULL, "PoseGraph contains no non-constant nodes, skipping optimization"); - return -1; - } + std::map& nodes = tempNodes; - if (numEdges == 0) - { - CV_LOG_INFO(NULL, "PoseGraph has no edges, no optimization to be done"); - return -1; - } - - CV_LOG_INFO(NULL, "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges"); - - size_t nVars = nVarNodes * 6; - BlockSparseMat jtj(nVarNodes); - std::vector jtb(nVars); - - double energy = calcEnergyNodes(nodes); - double oldEnergy = energy; - - CV_LOG_INFO(NULL, "#s" << " energy: " << energy); - - // options - // stop conditions - bool checkIterations = (tc.type & TermCriteria::COUNT); - bool checkEps = (tc.type & TermCriteria::EPS); - const unsigned int maxIterations = tc.maxCount; - const double minGradientTolerance = 1e-6; - const double stepNorm2Tolerance = 1e-6; - const double relEnergyDeltaTolerance = tc.epsilon; - // normalize jacobian columns for better conditioning - // slows down sparse solver, but maybe this'd be useful for some other solver - const bool jacobiScaling = false; - const double minDiag = 1e-6; - const double maxDiag = 1e32; - - const double initialLambdaLevMarq = 0.0001; - const double initialLmUpFactor = 2.0; - const double initialLmDownFactor = 3.0; - - // finish reasons - bool tooLong = false; // => not found - bool smallGradient = false; // => found - bool smallStep = false; // => found - bool smallEnergyDelta = false; // => found - - // column scale inverted, for jacobian scaling - std::vector di(nVars); - - double lmUpFactor = initialLmUpFactor; - double lambdaLevMarq = initialLambdaLevMarq; - - unsigned int iter = 0; - bool done = false; - while (!done) - { - jtj.clear(); - std::fill(jtb.begin(), jtb.end(), 0.0); - - // caching nodes jacobians std::vector> cachedJac; - for (auto id : placesIds) + if (calcJacobian) { - Pose3d p = nodes.at(id).pose; - Matx43d qj = expQuatJacobian(p.q); - // x node layout is (rot_x, rot_y, rot_z, trans_x, trans_y, trans_z) - // pose layout is (q_w, q_x, q_y, q_z, trans_x, trans_y, trans_z) - Matx j = concatVert(concatHor(qj, Matx43d()), - concatHor(Matx33d(), Matx33d::eye())); - cachedJac.push_back(j); + jtj.clear(); + std::fill(jtb.begin(), jtb.end(), 0.0); + + // caching nodes jacobians + for (auto id : placesIds) + { + cachedJac.push_back(nodes.at(id).pose.expJacobian()); + } + + if (useGeo) + jtCached.clear(); } - // fill jtj and jtb - for (const auto& e : edges) + double totalErr = 0.0; + for (const auto& e : pg->edges) { size_t srcId = e.sourceNodeId, dstId = e.targetNodeId; - const Node& srcNode = nodes.at(srcId); - const Node& dstNode = nodes.at(dstId); + const PoseGraphImpl::Node& srcNode = nodes.at(srcId); + const PoseGraphImpl::Node& dstNode = nodes.at(dstId); - Pose3d srcP = srcNode.pose; - Pose3d tgtP = dstNode.pose; + const PoseGraphImpl::Pose3d& srcP = srcNode.pose; + const PoseGraphImpl::Pose3d& tgtP = dstNode.pose; bool srcFixed = srcNode.isFixed; bool dstFixed = dstNode.isFixed; Vec6d res; Matx stj, ttj; Matx sqj, tqj; + + double err = poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, + /* needJacobians = */ calcJacobian, sqj, stj, tqj, ttj, res); + totalErr += err; + + if (calcJacobian) + { + size_t srcPlace = (size_t)(-1), dstPlace = (size_t)(-1); + Matx66d sj, tj; + if (!srcFixed) + { + srcPlace = idToPlace.at(srcId); + sj = concatHor(sqj, stj) * cachedJac[srcPlace]; + + jtj.refBlock(srcPlace, srcPlace) += sj.t() * sj; + + Vec6d jtbSrc = sj.t() * res; + for (int i = 0; i < 6; i++) + { + jtb(6 * (int)srcPlace + i) += jtbSrc[i]; + } + } + + if (!dstFixed) + { + dstPlace = idToPlace.at(dstId); + tj = concatHor(tqj, ttj) * cachedJac[dstPlace]; + + jtj.refBlock(dstPlace, dstPlace) += tj.t() * tj; + + Vec6d jtbDst = tj.t() * res; + for (int i = 0; i < 6; i++) + { + jtb(6 * (int)dstPlace + i) += jtbDst[i]; + } + } + + if (!(srcFixed || dstFixed)) + { + Matx66d sjttj = sj.t() * tj; + jtj.refBlock(srcPlace, dstPlace) += sjttj; + jtj.refBlock(dstPlace, srcPlace) += sjttj.t(); + } + + if (useGeo) + { + jtCached.push_back({ sj, tj }); + } + } + } + + if (calcEnergy) + { + energy = totalErr * 0.5; + } + + return true; + } + + virtual bool enableGeo() CV_OVERRIDE + { + useGeo = true; + return true; + } + + // adds d to current variables and writes result to probe vars or geo vars + virtual void currentOplusX(const Mat_& d, bool geo = false) CV_OVERRIDE + { + if (geo && !useGeo) + { + CV_Error(CV_StsBadArg, "Geodesic acceleration is disabled"); + } + + std::map& nodes = geo ? geoNodes : tempNodes; + + nodes = pg->nodes; + + for (size_t i = 0; i < nVarNodes; i++) + { + Vec6d dx(d[0] + (i * 6)); + PoseGraphImpl::Pose3d& p = nodes.at(placesIds[i]).pose; + + p = p.oplus(dx); + } + } + + virtual void prepareVars() CV_OVERRIDE + { + jtj = BlockSparseMat(nVarNodes); + jtb = Mat_((int)nVars, 1); + tempNodes = pg->nodes; + if (useGeo) + geoNodes = pg->nodes; + } + + // decomposes LevMarq matrix before solution + virtual bool decompose() CV_OVERRIDE + { + return jtj.decompose(decomposition, false); + } + + // 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_& right, Mat_& x) CV_OVERRIDE + { + return jtj.solveDecomposed(decomposition, -right, x); + } + + // calculates J^T*f(geo) + virtual bool calcJtbv(Mat_& jtbv) CV_OVERRIDE + { + jtbv.setZero(); + + int ei = 0; + for (const auto& e : pg->edges) + { + size_t srcId = e.sourceNodeId, dstId = e.targetNodeId; + const PoseGraphImpl::Node& srcNode = geoNodes.at(srcId); + const PoseGraphImpl::Node& dstNode = geoNodes.at(dstId); + + const PoseGraphImpl::Pose3d& srcP = srcNode.pose; + const PoseGraphImpl::Pose3d& tgtP = dstNode.pose; + bool srcFixed = srcNode.isFixed; + bool dstFixed = dstNode.isFixed; + + Vec6d res; + // dummy vars + Matx stj, ttj; + Matx sqj, tqj; + poseError(srcP.q, srcP.t, tgtP.q, tgtP.t, e.pose.q, e.pose.t, e.sqrtInfo, - /* needJacobians = */ true, sqj, stj, tqj, ttj, res); + /* needJacobians = */ false, sqj, stj, tqj, ttj, res); size_t srcPlace = (size_t)(-1), dstPlace = (size_t)(-1); - Matx66d sj, tj; + Matx66d sj = jtCached[ei].first, tj = jtCached[ei].second; + if (!srcFixed) { srcPlace = idToPlace.at(srcId); - sj = concatHor(sqj, stj) * cachedJac[srcPlace]; - jtj.refBlock(srcPlace, srcPlace) += sj.t() * sj; - - Vec6f jtbSrc = sj.t() * res; + Vec6d jtbSrc = sj.t() * res; for (int i = 0; i < 6; i++) { - jtb[6 * srcPlace + i] += -jtbSrc[i]; + jtbv(6 * (int)srcPlace + i) += jtbSrc[i]; } } if (!dstFixed) { dstPlace = idToPlace.at(dstId); - tj = concatHor(tqj, ttj) * cachedJac[dstPlace]; - jtj.refBlock(dstPlace, dstPlace) += tj.t() * tj; - - Vec6f jtbDst = tj.t() * res; + Vec6d jtbDst = tj.t() * res; for (int i = 0; i < 6; i++) { - jtb[6 * dstPlace + i] += -jtbDst[i]; + jtbv(6 * (int)dstPlace + i) += jtbDst[i]; } } - if (!(srcFixed || dstFixed)) - { - Matx66d sjttj = sj.t() * tj; - jtj.refBlock(srcPlace, dstPlace) += sjttj; - jtj.refBlock(dstPlace, srcPlace) += sjttj.t(); - } + ei++; } - CV_LOG_INFO(NULL, "#LM#s" << " energy: " << energy); + return true; + } - // do the jacobian conditioning improvement used in Ceres - if (jacobiScaling) - { - // L2-normalize each jacobian column - // vec d = {d_j = sum(J_ij^2) for each column j of J} = get_diag{ J^T * J } - // di = { 1/(1+sqrt(d_j)) }, extra +1 to avoid div by zero - if (iter == 0) - { - for (size_t i = 0; i < nVars; i++) - { - double ds = sqrt(jtj.valElem(i, i)) + 1.0; - di[i] = 1.0 / ds; - } - } + virtual const Mat_ getDiag() CV_OVERRIDE + { + return jtj.diagonal(); + } - doJacobiScaling(jtj, jtb, di); - } + virtual const Mat_ getJtb() CV_OVERRIDE + { + return jtb; + } - double gradientMax = 0.0; - // gradient max + virtual void setDiag(const Mat_& d) CV_OVERRIDE + { for (size_t i = 0; i < nVars; i++) { - gradientMax = std::max(gradientMax, abs(jtb[i])); - } - - // Save original diagonal of jtj matrix for LevMarq - std::vector diag(nVars); - for (size_t i = 0; i < nVars; i++) - { - diag[i] = jtj.valElem(i, i); - } - - // Solve using LevMarq and get delta transform - bool enoughLm = false; - - decltype(nodes) tempNodes = nodes; - - while (!enoughLm && !done) - { - // form LevMarq matrix - std::vector lmDiag(nVars); - for (size_t i = 0; i < nVars; i++) - { - double v = diag[i]; - double ld = std::min(max(v * lambdaLevMarq, minDiag), maxDiag); - lmDiag[i] = ld; - jtj.refElem(i, i) = v + ld; - } - - CV_LOG_INFO(NULL, "sparse solve..."); - - // use double or convert everything to float - std::vector x; - bool solved = jtj.sparseSolve(jtb, x, false); - - CV_LOG_INFO(NULL, (solved ? "OK" : "FAIL")); - - double costChange = 0.0; - double jacCostChange = 0.0; - double stepQuality = 0.0; - double xNorm2 = 0.0; - if (solved) - { - jacCostChange = calcJacCostChange(jtb, x, lmDiag); - - // x squared norm - for (size_t i = 0; i < nVars; i++) - { - xNorm2 += x[i] * x[i]; - } - - // undo jacobi scaling - if (jacobiScaling) - { - for (size_t i = 0; i < nVars; i++) - { - x[i] *= di[i]; - } - } - - tempNodes = nodes; - - // Update temp nodes using x - for (size_t i = 0; i < nVarNodes; i++) - { - Vec6d dx(&x[i * 6]); - Vec3d deltaRot(dx[0], dx[1], dx[2]), deltaTrans(dx[3], dx[4], dx[5]); - Pose3d& p = tempNodes.at(placesIds[i]).pose; - - p.q = Quatd(0, deltaRot[0], deltaRot[1], deltaRot[2]).exp() * p.q; - p.t += deltaTrans; - } - - // calc energy with temp nodes - energy = calcEnergyNodes(tempNodes); - - costChange = oldEnergy - energy; - - stepQuality = costChange / jacCostChange; - - CV_LOG_INFO(NULL, "#LM#" << iter - << " energy: " << energy - << " deltaEnergy: " << costChange - << " deltaEqEnergy: " << jacCostChange - << " max(J^T*b): " << gradientMax - << " norm2(x): " << xNorm2 - << " deltaEnergy/energy: " << costChange / energy); - } - - if (!solved || costChange < 0) - { - // failed to optimize, increase lambda and repeat - - lambdaLevMarq *= lmUpFactor; - lmUpFactor *= 2.0; - - CV_LOG_INFO(NULL, "LM goes up, lambda: " << lambdaLevMarq << ", old energy: " << oldEnergy); - } - else - { - // optimized successfully, decrease lambda and set variables for next iteration - enoughLm = true; - - lambdaLevMarq *= std::max(1.0 / initialLmDownFactor, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); - lmUpFactor = initialLmUpFactor; - - smallGradient = (gradientMax < minGradientTolerance); - smallStep = (xNorm2 < stepNorm2Tolerance); - smallEnergyDelta = (costChange / energy < relEnergyDeltaTolerance); - - nodes = tempNodes; - - CV_LOG_INFO(NULL, "#" << iter << " energy: " << energy); - - oldEnergy = energy; - - CV_LOG_INFO(NULL, "LM goes down, lambda: " << lambdaLevMarq << " step quality: " << stepQuality); - } - - iter++; - - tooLong = (iter >= maxIterations); - - done = ( (checkIterations && tooLong) || smallGradient || smallStep || (checkEps && smallEnergyDelta) ); + jtj.refElem(i, i) = d((int)i); } } - // if maxIterations is given as a stop criteria, let's consider tooLong as a successful finish - bool found = (smallGradient || smallStep || smallEnergyDelta || (checkIterations && tooLong)); + virtual void doJacobiScaling(const Mat_& di) CV_OVERRIDE + { + doJacobiScalingSparse(jtj, jtb, di); + } - CV_LOG_INFO(NULL, "Finished: " << (found ? "" : "not") << "found"); - if (smallGradient) - CV_LOG_INFO(NULL, "Finish reason: gradient max val dropped below threshold"); - if (smallStep) - CV_LOG_INFO(NULL, "Finish reason: step size dropped below threshold"); - if (smallEnergyDelta) - CV_LOG_INFO(NULL, "Finish reason: relative energy change between iterations dropped below threshold"); - if (tooLong) - CV_LOG_INFO(NULL, "Finish reason: max number of iterations reached"); - return (found ? iter : -1); -} + virtual void acceptProbe() CV_OVERRIDE + { + pg->nodes = tempNodes; + } -#else -int PoseGraphImpl::optimize(const cv::TermCriteria& /*tc*/) + PoseGraphImpl* pg; + + // J^T*J matrix + BlockSparseMat jtj; + // J^T*b vector + Mat_ jtb; + + // Probe variable for different lambda tryout + std::map tempNodes; + + // For geodesic acceleration + bool useGeo; + std::map geoNodes; + Mat_ jtrvv; + std::vector> jtCached; + + // Used for keeping intermediate matrix decomposition for further linear solve operations + BlockSparseMat::Decomposition decomposition; + + // The rest members are generated from pg + size_t nVars; + size_t numNodes; + size_t numEdges; + + // Structures to convert node id to place in variables vector and back + std::vector placesIds; + std::map idToPlace; + + size_t nVarNodes; +}; + + +LevMarq::Report PoseGraphImpl::optimize() { - CV_Error(Error::StsNotImplemented, "Eigen library required for sparse matrix solve during pose graph optimization, dense solver is not implemented"); + if (!lm) + createOptimizer(); + return lm->optimize(); } -#endif - Ptr detail::PoseGraph::create() { return makePtr(); } +#else + +Ptr detail::PoseGraph::create() +{ + CV_Error(Error::StsNotImplemented, "Eigen library required for sparse matrix solve during pose graph optimization, dense solver is not implemented"); +} + +#endif + PoseGraph::~PoseGraph() { } } // namespace detail diff --git a/modules/3d/src/rgbd/sparse_block_matrix.hpp b/modules/3d/src/rgbd/sparse_block_matrix.hpp index 07d442f1cf..f8e0969750 100644 --- a/modules/3d/src/rgbd/sparse_block_matrix.hpp +++ b/modules/3d/src/rgbd/sparse_block_matrix.hpp @@ -83,7 +83,7 @@ struct BlockSparseMat Mat diagonal() const { // Diagonal max length is the number of columns in the sparse matrix - int diagLength = blockN * nBlocks; + int diagLength =int( blockN * nBlocks ); cv::Mat diag = cv::Mat::zeros(diagLength, 1, cv::DataType<_Tp>::type); for (int i = 0; i < diagLength; i++) @@ -136,57 +136,83 @@ struct BlockSparseMat } #if defined(HAVE_EIGEN) - //! Function to solve a sparse linear system of equations HX = B - //! Requires Eigen - bool sparseSolve(InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) const - { - Eigen::SparseMatrix<_Tp> bigA = toEigen(); - Mat mb = B.getMat().t(); - Eigen::Matrix<_Tp, -1, 1> bigB; - cv2eigen(mb, bigB); - Eigen::SparseMatrix<_Tp> bigAtranspose = bigA.transpose(); - if(checkSymmetry && !bigA.isApprox(bigAtranspose)) + // Decomposes matrix for further solution + // Sometimes it's required to consequently solve A*x = b then A*x = c then A*x = d... + // Splitting the solution procedure into two parts let us reuse the matrix' decomposition + struct Decomposition + { + Eigen::SparseMatrix<_Tp> bigA; + Eigen::SimplicialLDLT> solver; + }; + + bool decompose(Decomposition& d, bool checkSymmetry = true) const + { + d.bigA = this->toEigen(); + + Eigen::SparseMatrix<_Tp> bigAtranspose = d.bigA.transpose(); + if (checkSymmetry && !d.bigA.isApprox(bigAtranspose)) { CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); return false; } - Eigen::SimplicialLDLT> solver; - - solver.compute(bigA); - if (solver.info() != Eigen::Success) + d.solver.compute(d.bigA); + bool r = (d.solver.info() == Eigen::Success); + if (!r) { CV_LOG_INFO(NULL, "Failed to eigen-decompose"); + } + + return r; + } + + static bool solveDecomposed(const Decomposition& d, InputArray B, OutputArray X, OutputArray predB = cv::noArray()) + { + Mat mb = B.getMat(); + mb = mb.cols == 1 ? mb : mb.t(); + Eigen::Matrix<_Tp, -1, 1> bigB; + cv2eigen(mb, bigB); + + Eigen::Matrix<_Tp, -1, 1> solutionX = d.solver.solve(bigB); + if (d.solver.info() != Eigen::Success) + { + CV_LOG_INFO(NULL, "Failed to eigen-solve"); return false; } else { - Eigen::Matrix<_Tp, -1, 1> solutionX = solver.solve(bigB); - if (solver.info() != Eigen::Success) + eigen2cv(solutionX, X); + if (predB.needed()) { - CV_LOG_INFO(NULL, "Failed to eigen-solve"); - return false; - } - else - { - eigen2cv(solutionX, X); - if (predB.needed()) - { - Eigen::Matrix<_Tp, -1, 1> predBEigen = bigA * solutionX; - eigen2cv(predBEigen, predB); - } - return true; + Eigen::Matrix<_Tp, -1, 1> predBEigen = d.bigA * solutionX; + eigen2cv(predBEigen, predB); } + return true; } } + #else - bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) const + struct Decomposition { }; + + bool decompose(Decomposition& /*_d*/, bool /*checkSymmetry*/ = true) const + { + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); + } + + bool solveDecomposed(const Decomposition& /*d*/, InputArray /*B*/, OutputArray /*X*/, OutputArray /*predB*/ = cv::noArray()) const { CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); } #endif + //! Function to solve a sparse linear system of equations HX = B + bool sparseSolve(InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) const + { + Decomposition d; + return decompose(d, checkSymmetry) && solveDecomposed(d, B, X, predB); + } + static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001); size_t nBlocks; IDtoBlockValueMap ijValue; diff --git a/modules/3d/src/solvepnp.cpp b/modules/3d/src/solvepnp.cpp index 2d67126309..6cee6a7485 100644 --- a/modules/3d/src/solvepnp.cpp +++ b/modules/3d/src/solvepnp.cpp @@ -551,57 +551,6 @@ int solveP3P( InputArray _opoints, InputArray _ipoints, return solutions; } -class SolvePnPRefineLMCallback CV_FINAL : public LMSolver::Callback -{ -public: - SolvePnPRefineLMCallback(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs) - { - objectPoints = _opoints.getMat(); - imagePoints = _ipoints.getMat(); - npoints = std::max(objectPoints.checkVector(3, CV_32F), objectPoints.checkVector(3, CV_64F)); - imagePoints0 = imagePoints.reshape(1, npoints*2); - cameraMatrix = _cameraMatrix.getMat(); - distCoeffs = _distCoeffs.getMat(); - } - - bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE - { - Mat param = _param.getMat(); - _err.create(npoints*2, 1, CV_64FC1); - - if(_Jac.needed()) - { - _Jac.create(npoints*2, param.rows, CV_64FC1); - } - - Mat rvec = param(Rect(0, 0, 1, 3)), tvec = param(Rect(0, 3, 1, 3)); - - Mat J, projectedPts; - projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray()); - - if (_Jac.needed()) - { - Mat Jac = _Jac.getMat(); - for (int i = 0; i < Jac.rows; i++) - { - for (int j = 0; j < Jac.cols; j++) - { - Jac.at(i,j) = J.at(i,j); - } - } - } - - Mat err = _err.getMat(); - projectedPts = projectedPts.reshape(1, npoints*2); - err = projectedPts - imagePoints0; - - return true; - } - - Mat objectPoints, imagePoints, imagePoints0; - Mat cameraMatrix, distCoeffs; - int npoints; -}; /** * @brief Compute the Interaction matrix and the residuals for the current pose. @@ -750,7 +699,49 @@ static void solvePnPRefine(InputArray _objectPoints, InputArray _imagePoints, params.at(i+3,0) = tvec.at(i,0); } - LMSolver::create(makePtr(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params); + int npts = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); + Mat imagePoints0 = ipoints.reshape(1, npts * 2); + auto solvePnPRefineLMCallback = [opoints, imagePoints0, npts, cameraMatrix, distCoeffs] + (InputOutputArray _param, OutputArray _err, OutputArray _Jac) -> bool + { + Mat param = _param.getMat(); + _err.create(npts * 2, 1, CV_64FC1); + + if (_Jac.needed()) + { + _Jac.create(npts * 2, param.rows, CV_64FC1); + } + + Mat prvec = param(Rect(0, 0, 1, 3)), ptvec = param(Rect(0, 3, 1, 3)); + + Mat J, projectedPts; + projectPoints(opoints, prvec, ptvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray()); + + if (_Jac.needed()) + { + Mat Jac = _Jac.getMat(); + for (int i = 0; i < Jac.rows; i++) + { + for (int j = 0; j < Jac.cols; j++) + { + Jac.at(i, j) = J.at(i, j); + } + } + } + + Mat err = _err.getMat(); + projectedPts = projectedPts.reshape(1, npts * 2); + err = projectedPts - imagePoints0; + + return true; + }; + LevMarq solver(params, solvePnPRefineLMCallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)_criteria.maxCount) + .setStepNormTolerance((double)_criteria.epsilon) + .setSmallEnergyTolerance((double)_criteria.epsilon * (double)_criteria.epsilon) + .setGeodesic(true)); + solver.optimize(); params.rowRange(0, 3).convertTo(rvec0, rvec0.depth()); params.rowRange(3, 6).convertTo(tvec0, tvec0.depth()); diff --git a/modules/3d/test/test_pose_graph.cpp b/modules/3d/test/test_pose_graph.cpp index 1480cdfd4d..be1676a8eb 100644 --- a/modules/3d/test/test_pose_graph.cpp +++ b/modules/3d/test/test_pose_graph.cpp @@ -3,12 +3,16 @@ // of this distribution and at http://opencv.org/license.html #include "test_precomp.hpp" -#include +#include + +#include namespace opencv_test { namespace { using namespace cv; +#ifdef HAVE_EIGEN + static Affine3d readAffine(std::istream& input) { Vec3d p; @@ -94,7 +98,7 @@ static Ptr readG2OFile(const std::string& g2oFileName) } -TEST( PoseGraph, sphereG2O ) +TEST(PoseGraph, sphereG2O) { // Test takes 15+ sec in Release mode and 400+ sec in Debug mode applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG); @@ -106,20 +110,26 @@ TEST( PoseGraph, sphereG2O ) // In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015. std::string filename = cvtest::TS::ptr()->get_data_path() + "/cv/rgbd/sphere_bignoise_vertex3.g2o"; + Ptr pg = readG2OFile(filename); -#ifdef HAVE_EIGEN // You may change logging level to view detailed optimization report // For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO - int iters = pg->optimize(); + // geoScale=1 is experimental, not guaranteed to work on other problems + // the rest are default params + pg->createOptimizer(LevMarq::Settings().setGeoScale(1.0) + .setMaxIterations(100) + .setCheckRelEnergyChange(true) + .setRelEnergyDeltaTolerance(1e-6) + .setGeodesic(true)); - ASSERT_GE(iters, 0); - ASSERT_LE(iters, 36); // should converge in 36 iterations + auto r = pg->optimize(); - double energy = pg->calcEnergy(); + EXPECT_TRUE(r.found); + EXPECT_LE(r.iters, 20); // should converge in 31 iterations - ASSERT_LE(energy, 1.47723e+06); // should converge to 1.47722e+06 or less + EXPECT_LE(r.energy, 1.47723e+06); // should converge to 1.47722e+06 or less // Add the "--test_debug" to arguments to see resulting pose graph nodes positions if (cvtest::debugLevel > 0) @@ -143,9 +153,291 @@ TEST( PoseGraph, sphereG2O ) of.close(); } +} + +// ------------------------------------------------------------------------------------------ + +// Wireframe meshes for debugging visualization purposes +struct Mesh +{ + std::vector pts; + std::vector lines; + + Mesh join(const Mesh& m2) const + { + Mesh mo; + + size_t sz1 = this->pts.size(); + std::copy(this->pts.begin(), this->pts.end(), std::back_inserter(mo.pts)); + std::copy(m2.pts.begin(), m2.pts.end(), std::back_inserter(mo.pts)); + + std::copy(this->lines.begin(), this->lines.end(), std::back_inserter(mo.lines)); + std::transform(m2.lines.begin(), m2.lines.end(), std::back_inserter(mo.lines), + [sz1](Vec2i ab) { return Vec2i(ab[0] + (int)sz1, ab[1] + (int)sz1); }); + + return mo; + } + + Mesh transform(Affine3f a, float scale = 1.f) const + { + Mesh out; + out.lines = this->lines; + for (Point3f p : this->pts) + { + out.pts.push_back(a * (p * scale)); + } + return out; + } + + // 0-2 - min, 3-5 - max + Vec6f getBoundingBox() const + { + float maxv = std::numeric_limits::max(); + Vec3f xmin(maxv, maxv, maxv), xmax(-maxv, -maxv, -maxv); + for (Point3f p : this->pts) + { + xmin[0] = min(p.x, xmin[0]); xmin[1] = min(p.y, xmin[1]); xmin[2] = min(p.z, xmin[2]); + xmax[0] = max(p.x, xmax[0]); xmax[1] = max(p.y, xmax[1]); xmax[2] = max(p.z, xmax[2]); + } + return Vec6f(xmin[0], xmin[1], xmin[2], xmax[0], xmax[1], xmax[2]); + } +}; + + +Mesh seg7(int d) +{ + const std::vector pt = { {0, 0, 0}, {0, 1, 0}, + {1, 0, 0}, {1, 1, 0}, + {2, 0, 0}, {2, 1, 0} }; + + std::vector seg(7); + seg[0].pts = { pt[0], pt[1] }; + seg[1].pts = { pt[1], pt[3] }; + seg[2].pts = { pt[3], pt[5] }; + seg[3].pts = { pt[5], pt[4] }; + seg[4].pts = { pt[4], pt[2] }; + seg[5].pts = { pt[2], pt[0] }; + seg[6].pts = { pt[2], pt[3] }; + for (int i = 0; i < 7; i++) + seg[i].lines = { {0, 1} }; + + vector digits = { + seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]), // 0 + seg[1].join(seg[2]), // 1 + seg[0].join(seg[1]).join(seg[3]).join(seg[4]).join(seg[6]), // 2 + seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[6]), // 3 + seg[1].join(seg[2]).join(seg[5]).join(seg[6]), // 4 + seg[0].join(seg[2]).join(seg[3]).join(seg[5]).join(seg[6]), // 5 + seg[0].join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]).join(seg[6]), // 6 + seg[0].join(seg[1]).join(seg[2]), // 7 + seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]).join(seg[6]), // 8 + seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[5]).join(seg[6]), // 9 + seg[6], // - + }; + + return digits[d]; +} + +Mesh drawId(size_t x) +{ + vector digits; + do + { + digits.push_back(x % 10); + x /= 10; + } + while (x > 0); + float spacing = 0.2f; + Mesh m; + for (size_t i = 0; i < digits.size(); i++) + { + Mesh digit = seg7(digits[digits.size() - 1 - i]); + Vec6f bb = digit.getBoundingBox(); + digit = digit.transform(Affine3f().translate(-Vec3f(0, bb[1], 0))); + Vec3f tr; + if (m.pts.empty()) + tr = Vec3f(); + else + tr = Vec3f(0, (m.getBoundingBox()[4] + spacing), 0); + m = m.join(digit.transform( Affine3f().translate(tr) )); + } + return m; +} + + +Mesh drawFromTo(size_t f, size_t t) +{ + Mesh m; + + Mesh df = drawId(f); + Mesh dp = seg7(10); + Mesh dt = drawId(t); + + float spacing = 0.2f; + m = m.join(df).join(dp.transform(Affine3f().translate(Vec3f(0, df.getBoundingBox()[4] + spacing, 0)))) + .join(dt.transform(Affine3f().translate(Vec3f(0, df.getBoundingBox()[4] + 2*spacing + 1, 0)))); + + return m; +} + +Mesh drawPoseGraph(Ptr pg) +{ + Mesh marker; + marker.pts = { {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {1, 1, 0} }; + marker.lines = { {0, 1}, {0, 2}, {0, 3}, {1, 4} }; + + Mesh allMeshes; + Affine3f margin = Affine3f().translate(Vec3f(0.1f, 0.1f, 0)); + std::vector ids = pg->getNodesIds(); + for (const size_t& id : ids) + { + Affine3f pose = pg->getNodePose(id); + + Mesh m = marker.join(drawId(id).transform(margin, 0.25f)).transform(pose); + allMeshes = allMeshes.join(m); + } + + // edges + margin = Affine3f().translate(Vec3f(0.05f, 0.05f, 0)); + for (size_t i = 0; i < pg->getNumEdges(); i++) + { + Affine3f pose = pg->getEdgePose(i); + size_t sid = pg->getEdgeStart(i); + size_t did = pg->getEdgeEnd(i); + Affine3f spose = pg->getNodePose(sid); + Affine3f dpose = spose * pose; + + Mesh m = marker.join(drawFromTo(sid, did).transform(margin, 0.125f)).transform(dpose); + allMeshes = allMeshes.join(m); + } + + return allMeshes; +} + +void writeObj(const std::string& fname, const Mesh& m) +{ + // Write edge-only model of how nodes are located in space + std::fstream of(fname, std::fstream::out); + for (const Point3f& d : m.pts) + { + of << "v " << d.x << " " << d.y << " " << d.z << std::endl; + } + + for (const Vec2i& v : m.lines) + { + of << "l " << v[0] + 1 << " " << v[1] + 1 << std::endl; + } + + of.close(); +} + + +TEST(PoseGraph, simple) +{ + + Ptr pg = detail::PoseGraph::create(); + + DualQuatf true0(1, 0, 0, 0, 0, 0, 0, 0); + DualQuatf true1 = DualQuatf::createFromPitch((float)CV_PI / 3.0f, 10.0f, Vec3f(1, 1.5f, 1.2f), Vec3f()); + + DualQuatf pose0 = true0; + vector noise(7); + for (size_t i = 0; i < noise.size(); i++) + { + float angle = cv::theRNG().uniform(-1.f, 1.f); + float shift = cv::theRNG().uniform(-2.f, 2.f); + Matx31f axis = Vec3f::randu(0.f, 1.f), moment = Vec3f::randu(0.f, 1.f); + noise[i] = DualQuatf::createFromPitch(angle, shift, + Vec3f(axis(0), axis(1), axis(2)), + Vec3f(moment(0), moment(1), moment(2))); + } + + DualQuatf pose1 = noise[0] * true1; + + DualQuatf diff = true1 * true0.inv(); + vector cfrom = { diff, diff * noise[1], noise[2] * diff }; + DualQuatf diffInv = diff.inv(); + vector cto = { diffInv, diffInv * noise[3], noise[4] * diffInv }; + + pg->addNode(123, pose0.toAffine3(), true); + pg->addNode(456, pose1.toAffine3(), false); + + Matx66f info = Matx66f::eye(); + for (int i = 0; i < 3; i++) + { + pg->addEdge(123, 456, cfrom[i].toAffine3(), info); + pg->addEdge(456, 123, cto[i].toAffine3(), info); + } + + Mesh allMeshes = drawPoseGraph(pg); + + // Add the "--test_debug" to arguments to see resulting pose graph nodes positions + if (cvtest::debugLevel > 0) + { + writeObj("pg_simple_in.obj", allMeshes); + } + + auto r = pg->optimize(); + + Mesh after = drawPoseGraph(pg); + + // Add the "--test_debug" to arguments to see resulting pose graph nodes positions + if (cvtest::debugLevel > 0) + { + writeObj("pg_simple_out.obj", after); + } + + EXPECT_TRUE(r.found); +} #else + +TEST(PoseGraph, sphereG2O) +{ throw SkipTestException("Build with Eigen required for pose graph optimization"); +} + +TEST(PoseGraph, simple) +{ + throw SkipTestException("Build with Eigen required for pose graph optimization"); +} #endif + +TEST(LevMarq, Rosenbrock) +{ + auto f = [](double x, double y) -> double + { + return (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x); + }; + + auto j = [](double x, double y) -> Matx12d + { + return {/*dx*/ -2.0 + 2.0 * x - 400.0 * x * y + 400.0 * x*x*x, + /*dy*/ 200.0 * y - 200.0 * x*x, + }; + }; + + LevMarq solver(2, [f, j](InputOutputArray param, OutputArray err, OutputArray jv) -> bool + { + Vec2d v = param.getMat(); + double x = v[0], y = v[1]; + err.create(1, 1, CV_64F); + err.getMat().at(0) = f(x, y); + if (jv.needed()) + { + jv.create(1, 2, CV_64F); + Mat(j(x, y)).copyTo(jv); + } + return true; + }, + LevMarq::Settings().setGeodesic(true)); + + Mat_ x (Vec2d(1, 3)); + + auto r = solver.run(x); + + EXPECT_TRUE(r.found); + EXPECT_LT(r.energy, 0.035); + EXPECT_LE(r.iters, 17); } diff --git a/modules/calib/src/calibration.cpp b/modules/calib/src/calibration.cpp index a80d38ec37..9fcf939430 100644 --- a/modules/calib/src/calibration.cpp +++ b/modules/calib/src/calibration.cpp @@ -167,8 +167,8 @@ static void subMatrix(const Mat& src, Mat& dst, static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints, const Mat& npoints, Mat& allErrors, - Mat& _param, Mat* _JtErr, Mat* _JtJ, double* _errnorm, - double aspectRatio, Mat* perViewErrors, + Mat& _param, bool calcJ, Mat& JtErr, Mat& JtJ, double& errnorm, + double aspectRatio, Mat& perViewErrors, int flags, bool optimizeObjPoints) { const int NINTRINSIC = CALIB_NINTRINSIC; @@ -177,7 +177,6 @@ static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints, Mat _k(14, 1, CV_64F, k); double* param = _param.ptr(); int nparams = (int)_param.total(); - bool calcJ = _JtErr != 0; int ni0 = npoints.at(0); Mat _Je(ni0*2, 6, CV_64F), _Ji(ni0*2, NINTRINSIC, CV_64F), _Jo, _err(ni*2, 1, CV_64F); @@ -192,10 +191,8 @@ static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints, 0, 0, 1); std::copy(param + 4, param + 4 + 14, k); - if (_JtJ) - _JtJ->setZero(); - if (_JtErr) - _JtErr->setZero(); + JtJ.setZero(); + JtErr.setZero(); if(optimizeObjPoints) _Jo.create(ni0*2, ni0*3, CV_64F); @@ -263,8 +260,6 @@ static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints, if( calcJ ) { - Mat JtJ = *_JtJ, JtErr = *_JtErr; - // see HZ: (A6.14) for details on the structure of the Jacobian JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += _Ji.t() * _Ji; JtJ(Rect(NINTRINSIC + i * 6, NINTRINSIC + i * 6, 6, 6)) = _Je.t() * _Je; @@ -295,14 +290,13 @@ static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints, printf("\n"); }*/ - if( perViewErrors ) - perViewErrors->at(i) = std::sqrt(viewErr / ni); + if( !perViewErrors.empty() ) + perViewErrors.at(i) = std::sqrt(viewErr / ni); reprojErr += viewErr; } - if(_errnorm) - *_errnorm = reprojErr; + errnorm = reprojErr; } static double calibrateCameraInternal( const Mat& objectPoints, @@ -571,22 +565,35 @@ static double calibrateCameraInternal( const Mat& objectPoints, //std::cout << "single camera calib. mask: " << mask0.t() << "\n"; // 3. run the optimization - LMSolver::runAlt(param0, mask0, termCrit, solveMethod, false, - [&](Mat& _param, Mat* _JtErr, Mat* _JtJ, double* _errnorm) + + auto calibCameraLMCallback = [matM, _m, npoints, &allErrors, aspectRatio, &perViewErrors, flags, releaseObject] + (InputOutputArray _param, OutputArray JtErr, OutputArray JtJ, double& errnorm) -> bool { - cameraCalcJErr(matM, _m, npoints, allErrors, _param, _JtErr, _JtJ, _errnorm, - aspectRatio, perViewErrors, flags, releaseObject); + Mat jterr = JtErr.getMat(), jtj = JtJ.getMat(), perViewErr = perViewErrors ? *perViewErrors : Mat(); + Mat mparam = _param.getMat(); + cameraCalcJErr(matM, _m, npoints, allErrors, mparam, /* calcJ */ JtErr.needed() && JtJ.needed(), + jterr, jtj, errnorm, + aspectRatio, perViewErr, flags, releaseObject); return true; - }); + }; + + LevMarq solver(param0, calibCameraLMCallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)termCrit.maxCount) + .setStepNormTolerance(termCrit.epsilon) + .setSmallEnergyTolerance(termCrit.epsilon * termCrit.epsilon), + mask0, MatrixType::AUTO, VariableType::LINEAR, /* LtoR */ false, solveMethod); + // geodesic is not supported for normal callbacks + solver.optimize(); //std::cout << "single camera calib. param after LM: " << param0.t() << "\n"; Mat JtErr(nparams, 1, CV_64F), JtJ(nparams, nparams, CV_64F), JtJinv, JtJN; double reprojErr = 0; - JtErr.setZero(); JtJ.setZero(); - cameraCalcJErr(matM, _m, npoints, allErrors, param0, - stdDevs ? &JtErr : 0, stdDevs ? &JtJ : 0, &reprojErr, - aspectRatio, 0, flags, releaseObject); + JtErr.setZero(); JtJ.setZero(); Mat dummy; + cameraCalcJErr(matM, _m, npoints, allErrors, param0, (stdDevs != nullptr), + JtErr, JtJ, reprojErr, + aspectRatio, dummy, flags, releaseObject); if (stdDevs) { int nparams_nz = countNonZero(mask0); @@ -673,12 +680,11 @@ static double stereoCalibrateImpl( TermCriteria termCrit ) { const int NINTRINSIC = 18; - double reprojErr = 0; double dk[2][14]={{0}}; Mat Dist[2]; - Matx33d A[2], R_LR; - int i, k, p, ni = 0, pos, pointsTotal = 0, maxPoints = 0, nparams; + Matx33d A[2]; + int pointsTotal = 0, maxPoints = 0, nparams; bool recomputeIntrinsics = false; double aspectRatio[2] = {0}; @@ -689,10 +695,10 @@ static double stereoCalibrateImpl( _npoints.type() == CV_32S ); int nimages = (int)_npoints.total(); - for( i = 0; i < nimages; i++ ) + for(int i = 0; i < nimages; i++ ) { - ni = _npoints.at(i); - maxPoints = MAX(maxPoints, ni); + int ni = _npoints.at(i); + maxPoints = std::max(maxPoints, ni); pointsTotal += ni; } @@ -700,7 +706,7 @@ static double stereoCalibrateImpl( _objectPoints.convertTo(objectPoints, CV_64F); objectPoints = objectPoints.reshape(3, 1); - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { const Mat& points = k == 0 ? _imagePoints1 : _imagePoints2; const Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; @@ -750,7 +756,7 @@ static double stereoCalibrateImpl( if( flags & CALIB_FIX_ASPECT_RATIO ) { - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) aspectRatio[k] = A[k](0, 0)/A[k](1, 1); } @@ -765,7 +771,8 @@ static double stereoCalibrateImpl( // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters). nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0); - std::vector mask(nparams, (uchar)0); + std::vector mask(nparams, (uchar)1); + std::vector param(nparams, 0.); if( recomputeIntrinsics ) @@ -825,13 +832,14 @@ static double stereoCalibrateImpl( om = median(om_ref_list) T = median(T_ref_list) */ - for( i = pos = 0; i < nimages; pos += ni, i++ ) + int pos = 0; + for(int i = 0; i < nimages; i++ ) { - ni = _npoints.at(i); + int ni = _npoints.at(i); Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr() + pos*3); Matx33d R[2]; Vec3d rv, T[2]; - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { Mat imgpt_ik = Mat(1, ni, CV_64FC2, imagePoints[k].ptr() + pos*2); solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); @@ -859,6 +867,8 @@ static double stereoCalibrateImpl( RT0data[i + nimages*3] = T[1][0]; RT0data[i + nimages*4] = T[1][1]; RT0data[i + nimages*5] = T[1][2]; + + pos += ni; } if(flags & CALIB_USE_EXTRINSIC_GUESS) @@ -881,7 +891,7 @@ static double stereoCalibrateImpl( else { // find the medians and save the first 6 parameters - for( i = 0; i < 6; i++ ) + for(int i = 0; i < 6; i++ ) { double* rti = RT0data + i*nimages; qsort( rti, nimages, sizeof(*rti), dbCmp ); @@ -890,7 +900,7 @@ static double stereoCalibrateImpl( } if( recomputeIntrinsics ) - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { double* iparam = ¶m[(nimages+1)*6 + k*NINTRINSIC]; if( flags & CALIB_ZERO_TANGENT_DIST ) @@ -911,17 +921,15 @@ static double stereoCalibrateImpl( //std::cout << "param before LM: " << Mat(param, false).t() << "\n"; - LMSolver::runAlt(param, mask, termCrit, DECOMP_SVD, false, - [&](Mat& _param, Mat* _JtErr, Mat* _JtJ, double* _errnorm) + auto lmcallback = [&](InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm) { - double* param_p = _param.ptr(); + double* param_p = _param.getMat().ptr(); Vec3d om_LR(param_p[0], param_p[1], param_p[2]); Vec3d T_LR(param_p[3], param_p[4], param_p[5]); Vec3d om[2], T[2]; Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2; - reprojErr = 0; - Rodrigues(om_LR, R_LR); + double reprojErr = 0; if( recomputeIntrinsics ) { @@ -942,7 +950,7 @@ static double stereoCalibrateImpl( //ipparam[0] = ipparam[1]*aspectRatio[0]; //ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1]; } - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { A[k] = Matx33d(iparam[k*NINTRINSIC+0], 0, iparam[k*NINTRINSIC+2], 0, iparam[k*NINTRINSIC+1], iparam[k*NINTRINSIC+3], @@ -952,21 +960,22 @@ static double stereoCalibrateImpl( } } - for( i = pos = 0; i < nimages; pos += ni, i++ ) + int ptPos = 0; + for(int i = 0; i < nimages; i++ ) { - ni = _npoints.at(i); + int ni = _npoints.at(i); double* pi = param_p + (i+1)*6; om[0] = Vec3d(pi[0], pi[1], pi[2]); T[0] = Vec3d(pi[3], pi[4], pi[5]); - if( _JtJ || _JtErr ) + if( JtJ_.needed() || JtErr_.needed() ) composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(), dr3dr2, noArray(), noArray(), dt3dt1, dt3dr2, dt3dt2 ); else composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1] ); - Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr() + pos*3); + Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr() + ptPos*3); err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2); Mat tmpImagePoints = err.reshape(2, 1); @@ -976,30 +985,30 @@ static double stereoCalibrateImpl( Mat dpdrot = Je.colRange(0, 3); Mat dpdt = Je.colRange(3, 6); - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { - Mat imgpt_ik(1, ni, CV_64FC2, imagePoints[k].ptr() + pos*2); + Mat imgpt_ik(1, ni, CV_64FC2, imagePoints[k].ptr() + ptPos*2); - if( _JtJ || _JtErr ) + if( JtJ_.needed() || JtErr_.needed() ) projectPoints(objpt_i, om[k], T[k], A[k], Dist[k], - tmpImagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, noArray(), - (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0.); + tmpImagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, noArray(), + (flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0.); else projectPoints(objpt_i, om[k], T[k], A[k], Dist[k], tmpImagePoints); subtract( tmpImagePoints, imgpt_ik, tmpImagePoints ); - if( _JtJ ) + if( JtJ_.needed() ) { - Mat& JtErr = *_JtErr; - Mat& JtJ = *_JtJ; + Mat JtErr = JtErr_.getMat(); + Mat JtJ = JtJ_.getMat(); int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6; - assert( _JtJ && _JtErr ); + assert( JtJ_.needed() && JtErr_.needed() ); if( k == 1 ) { // d(err_{x|y}R) ~ de3 // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2} - for( p = 0; p < ni*2; p++ ) + for(int p = 0; p < ni*2; p++ ) { Mat de3dr3( 1, 3, CV_64F, Je.ptr(p)); Mat de3dt3( 1, 3, CV_64F, de3dr3.ptr() + 3 ); @@ -1045,21 +1054,38 @@ static double stereoCalibrateImpl( perViewErr->at(i, k) = std::sqrt(viewErr/ni); reprojErr += viewErr; } + + ptPos += ni; } - if(_errnorm) - *_errnorm = reprojErr; + errnorm = reprojErr; return true; - }); + }; + + double reprojErr = 0; + if (countNonZero(mask)) + { + LevMarq solver(param, lmcallback, + LevMarq::Settings() + .setMaxIterations((unsigned int)termCrit.maxCount) + .setStepNormTolerance(termCrit.epsilon) + .setSmallEnergyTolerance(termCrit.epsilon * termCrit.epsilon), + mask); + // geodesic not supported for normal callbacks + LevMarq::Report r = solver.optimize(); + + reprojErr = r.energy; + } Vec3d om_LR(param[0], param[1], param[2]); Vec3d T_LR(param[3], param[4], param[5]); + Matx33d R_LR; Rodrigues( om_LR, R_LR ); if( matR->rows == 1 || matR->cols == 1 ) om_LR.convertTo(*matR, matR->depth()); else R_LR.convertTo(*matR, matR->depth()); T_LR.convertTo(*matT, matT->depth()); - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { double* iparam = ¶m[(nimages+1)*6 + k*NINTRINSIC]; A[k] = Matx33d(iparam[0], 0, iparam[2], 0, iparam[1], iparam[3], 0, 0, 1); @@ -1067,7 +1093,7 @@ static double stereoCalibrateImpl( if( recomputeIntrinsics ) { - for( k = 0; k < 2; k++ ) + for(int k = 0; k < 2; k++ ) { Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; diff --git a/modules/calib/test/test_cameracalibration.cpp b/modules/calib/test/test_cameracalibration.cpp index c5608faaef..d34f0874fc 100644 --- a/modules/calib/test/test_cameracalibration.cpp +++ b/modules/calib/test/test_cameracalibration.cpp @@ -1946,8 +1946,8 @@ TEST(Calib3d_StereoCalibrate_CPP, extended) double res1 = stereoCalibrate( objpt, imgpt1, imgpt2, K1, c1, K2, c2, imageSize, R, T, E, F, err, flags); - printf("res0 = %g, res1 = %g\n", res0, res1); - EXPECT_LE(res1, res0*1.1); + + EXPECT_LE(res1, res0); EXPECT_TRUE(err.total() == 2); } diff --git a/modules/core/include/opencv2/core/dualquaternion.inl.hpp b/modules/core/include/opencv2/core/dualquaternion.inl.hpp index 4aec961dd2..814838521c 100644 --- a/modules/core/include/opencv2/core/dualquaternion.inl.hpp +++ b/modules/core/include/opencv2/core/dualquaternion.inl.hpp @@ -158,7 +158,7 @@ inline Quat DualQuat::getRotation(QuatAssumeType assumeUnit) const template inline Vec DualQuat::getTranslation(QuatAssumeType assumeUnit) const { - Quat trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit)); + Quat trans = T(2.0) * (getDualPart() * getRealPart().inv(assumeUnit)); return Vec{trans[1], trans[2], trans[3]}; } diff --git a/modules/core/include/opencv2/core/quaternion.inl.hpp b/modules/core/include/opencv2/core/quaternion.inl.hpp index 3c2fce10af..29a16d9f7d 100644 --- a/modules/core/include/opencv2/core/quaternion.inl.hpp +++ b/modules/core/include/opencv2/core/quaternion.inl.hpp @@ -54,7 +54,7 @@ Quat Quat::createFromAngleAxis(const T angle, const Vec &axis) { CV_Error(Error::StsBadArg, "this quaternion does not represent a rotation"); } - const T angle_half = angle * 0.5; + const T angle_half = angle * T(0.5); w = std::cos(angle_half); const T sin_v = std::sin(angle_half); const T sin_norm = sin_v / vNorm; @@ -79,35 +79,35 @@ Quat Quat::createFromRotMat(InputArray _R) T trace = R(0, 0) + R(1, 1) + R(2, 2); if (trace > 0) { - S = std::sqrt(trace + 1) * 2; + S = std::sqrt(trace + 1) * T(2); x = (R(1, 2) - R(2, 1)) / S; y = (R(2, 0) - R(0, 2)) / S; z = (R(0, 1) - R(1, 0)) / S; - w = -0.25 * S; + w = -T(0.25) * S; } else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) { - S = std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2; - x = -0.25 * S; + S = std::sqrt(T(1.0) + R(0, 0) - R(1, 1) - R(2, 2)) * T(2); + x = -T(0.25) * S; y = -(R(1, 0) + R(0, 1)) / S; z = -(R(0, 2) + R(2, 0)) / S; w = (R(1, 2) - R(2, 1)) / S; } else if (R(1, 1) > R(2, 2)) { - S = std::sqrt(1.0 - R(0, 0) + R(1, 1) - R(2, 2)) * 2; + S = std::sqrt(T(1.0) - R(0, 0) + R(1, 1) - R(2, 2)) * T(2); x = (R(0, 1) + R(1, 0)) / S; - y = 0.25 * S; + y = T(0.25) * S; z = (R(1, 2) + R(2, 1)) / S; w = (R(0, 2) - R(2, 0)) / S; } else { - S = std::sqrt(1.0 - R(0, 0) - R(1, 1) + R(2, 2)) * 2; + S = std::sqrt(T(1.0) - R(0, 0) - R(1, 1) + R(2, 2)) * T(2); x = (R(0, 2) + R(2, 0)) / S; y = (R(1, 2) + R(2, 1)) / S; - z = 0.25 * S; + z = T(0.25) * S; w = -(R(0, 1) - R(1, 0)) / S; } return Quat (w, x, y, z); @@ -268,7 +268,7 @@ inline Quat& Quat::operator/=(const T a) template inline Quat Quat::operator/(const T a) const { - const T a_inv = 1.0 / a; + const T a_inv = T(1.0) / a; return Quat(w * a_inv, x * a_inv, y * a_inv, z * a_inv); } diff --git a/modules/stitching/src/motion_estimators.cpp b/modules/stitching/src/motion_estimators.cpp index b4ae36a3d0..394aeb8961 100644 --- a/modules/stitching/src/motion_estimators.cpp +++ b/modules/stitching/src/motion_estimators.cpp @@ -253,16 +253,34 @@ bool BundleAdjusterBase::estimate(const std::vector &features, edges_[i].second].num_inliers); int nerrs = total_num_matches_ * num_errs_per_measurement_; - LMSolver::run(cam_params_, Mat(), nerrs, term_criteria_, DECOMP_SVD, - [&](Mat& param, Mat* err, Mat* jac) + + auto callb = [&](InputOutputArray param, OutputArray err, OutputArray jac) -> bool + { + // workaround against losing value + Mat backup = cam_params_.clone(); + param.copyTo(cam_params_); + if (jac.needed()) { - param.copyTo(cam_params_); - if (jac) - calcJacobian(*jac); - if (err) - calcError(*err); - return true; - }); + Mat m = jac.getMat(); + calcJacobian(m); + } + if (err.needed()) + { + Mat m = err.getMat(); + calcError(m); + } + backup.copyTo(cam_params_); + return true; + }; + + LevMarq solver(cam_params_, callb, + LevMarq::Settings() + .setMaxIterations((unsigned int)term_criteria_.maxCount) + .setStepNormTolerance(term_criteria_.epsilon) + .setSmallEnergyTolerance(term_criteria_.epsilon * term_criteria_.epsilon) + .setGeodesic(true), + noArray(), MatrixType::AUTO, VariableType::LINEAR, nerrs); + solver.optimize(); LOGLN_CHAT(""); LOGLN_CHAT("Bundle adjustment, final RMS error: " << std::sqrt(err.dot(err) / total_num_matches_));