New LevMarq implementation #21018

savuor · 2021-11-07T22:09:07Z

Merge with extra: opencv/opencv_extra#942
Merge with contrib: opencv/opencv_contrib#3127

TODOs:

new interface: constructors, run(params) for linear, various factories, return report after optimization
(optional) implement geodesic acceleration
play with the params on existing code to find better combinations
add perf reports
remove old impl
remove all DEBUG code and fulfill all TODO statements

What?

New Levenberg-Marquadt algorithm implementation replaces the old one.

Why?

Because the old impl isn't as good as we want:

No sparse matrix support
No SO(3), SE(3) or any other manifold type supported for optimized params
No flexibility: fixed up/down coefficients and some deltas, a few options for a termination criteria

This stops it from using in some complex 3d-related problems such as pose graph optimization.

An algorithm termination deserves its own list of features:

Iteration calculation isn't fair: only successful ones are counted, a real amount of iterations elapsed can be several times bigger than reported
The optimization can stop and be considered successful even regardless of successful iterations elapsed count, cost function value drop or NaNs in a gradient
Step norm threshold is tied up to energy threshold

This is not OK even for a basic LevMarq solver.

How?

A new code fixes everything:

A 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 lets a user to use sparse matrices, exponential increments or fixed variables.
The algorithm is highly tunable:
- Lambda initial/up/down values can be changed by a user, diagonal clamping or upFactor doubling can be turned on/off
- A termination criteria can be composed of the following ones, each threshold is independent and tunable:
  - relative energy delta < threshold
  - gradient max value < threshold
  - step norm (L2 or Inf) < threshold
  - cost function < threshold
  - iterations count < maxIterations
Jacobian scaling, step quality metric and geodesic acceleration supported, they can improve the algorithm's speed/stability sometimes
A dense linear implementation converges at least as good as the old code does

This code is based on a current pose graph optimization routine. As a result, the pose graph has been rewritten too, now it uses the same LevMarq impl as other OpenCV functions.

Any numbers?

TLDR: new implementation converges more often in less iterations to approximately the same cost function values.

A convergence comparison across various use cases:

Convergence by function

function	old good	old total	new good	new total
calibrateCameraInternal	15	15	15	15
estimateAffine2D	11493	12111	12172	12172
solvePnPRefine	5	5	5	5
stereoCalibrateImpl	2	3	2	3
estimateAffinePartial2D	4300	6096	6858	6858
findExtrinsicCameraParams2	6018	6341	6238	6329
BundleAdjusterBase::estimate	783	783	709	709
findHomography	25044	30957	31351	31719

Final energy by function

function	old min	new min	old max	new max	old avg	new avg	old med	new med
calibrateCameraInternal	0.008321	0.008317	558.9	558.9	73.67	73.67	4.726	4.726
estimateAffine2D	0	0	3.267e+07	3.267e+07	7.014e+04	5.368e+05	0.009058	0.009339
solvePnPRefine	1.05e-26	2.985e-12	0.004971	0.004971	0.0009941	0.0009941	3.506e-17	2.179e-11
stereoCalibrateImpl	6.788	6.788	318.8	315.1	110.8	109.5	6.788	6.788
estimateAffinePartial2D	0	0	1209	1209	115.8	108.4	6.931e-09	3.247e-09
findExtrinsicCameraParams2	1.883e-29	2.958e-31	2.729e+42	2.729e+42	4.304e+38	4.313e+38	2.898e-10	4.199e-10
BundleAdjusterBase::estimate	26	26	4.598e+04	4.598e+04	1869	2274	266.2	764.6
findHomography	4.645e-27	0	2304	2304	34	30.6	0.003876	3.122e-05

Iterations elapsed till convergence by function

function	old min	new min	old max	new max	old avg	new avg	old med	new med
calibrateCameraInternal	5	4	157	54	60.07	24.73	70	26
estimateAffine2D	1	1	21	3	10.39	1.838	6	2
solvePnPRefine	3	2	7	4	5	3.6	5	4
stereoCalibrateImpl	45	1	71	1	58	1	58	1
estimateAffinePartial2D	1	1	21	6	4.561	1.655	4	1
findExtrinsicCameraParams2	1	1	42	20	3.015	1.158	2	1
BundleAdjusterBase::estimate	19	1	2100	38	64.32	3.224	55	4
findHomography	1	1	21	10	7.553	2.267	5	1

Other changes in this PR

Stereo calibration was broken during 4.x to 5.x porting, fixing it
Temporary fixes for Submap class and related stuff (anyway it'll be done using updated PoseGraph class)
minor changes

Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

I agree to contribute to the project under Apache 2 License.
To the best of my knowledge, the proposed patch is not based on a code under GPL or other license that is incompatible with OpenCV
The PR is proposed to proper branch
There is reference to original bug report and related work
There is accuracy test, performance test and test data in opencv_extra repository, if applicable
Patch to opencv_extra has the same branch name.
The feature is well documented and sample code can be built with the project CMake

… impl

alalek

We should revise public API.
Keep minimal part which is necessary for user application (tests).

Internal interfaces like "BaseLevMarq::Backend" should be hidden.

alalek · 2021-12-10T14:33:03Z

modules/3d/include/opencv2/3d.hpp

+For more details, please refer to Wikipedia page (https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm).
 */
-class CV_EXPORTS LMSolver : public Algorithm
+class CV_EXPORTS BaseLevMarq


BaseLevMarq -> LevMarqBase or LevMarqSolver

modules/3d/include/opencv2/3d.hpp

alalek · 2021-12-10T14:34:45Z

modules/3d/include/opencv2/3d.hpp

+
+    Ptr<Backend> pBackend;
+
+    BaseLevMarq(Ptr<Backend> backend_) :


Again, const reference for all input parameter.

no underscores in names if we don't need them.

alalek · 2021-12-10T14:36:22Z

modules/3d/include/opencv2/3d.hpp

+    // 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;


No fields in public API.
This is the primary requirement of public OpenCV API.
getter/setter should be exposed instead.

can we try to utilize https://github.com/opencv/opencv/wiki/OE-34.-Named-Parameters? It lets to define very nice API for C++ and Python (and maybe Swift after the corresponding binding generator is revised)

Implemented it as a Settings structure

alalek · 2021-12-10T14:41:33Z

modules/3d/include/opencv2/3d.hpp

+        double energy;
+    };
+
+    class Backend


Looks like it is not used by user code directly. Move to src or details.

The motivation to leave the Backend in public was to let a user to construct solvers that use sparse matrices, params of SE(3) and of more exotic groups, with fixed variables and so on.
I will try to move it to details to keep this possibility.
(However the interface is very complicated for a user)

Backend is moved to details header

alalek · 2021-12-10T14:46:46Z

modules/3d/include/opencv2/3d.hpp

+        bool found;
+        int iters;
+        double energy;


missing documentation

Added docs for Report structure

alalek · 2021-12-10T14:48:30Z

modules/core/include/opencv2/core/dualquaternion.inl.hpp

 inline Vec<T, 3> DualQuat<T>::getTranslation(QuatAssumeType assumeUnit) const
 {
-    Quat<T> trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit));
+    Quat<T> trans = T(2.0) * (getDualPart() * getRealPart().inv(assumeUnit));


Such changes should be backported to 4.x

Backported to #21319 and #3137@contrib

alalek · 2021-12-10T14:50:13Z

modules/calib/src/calibration.cpp

+        solver.maxIterations = (unsigned int)(termCrit.maxCount * 2.1);
+        solver.stepNormTolerance = termCrit.epsilon;
+        solver.smallEnergyTolerance = termCrit.epsilon * termCrit.epsilon;


We should have setter with TermCriteria parameter.

maxIterations = (unsigned int)(termCrit.maxCount * 2.1);
2.1

Why?

2.1 is a compatibility hack. Old impl counts successful iterations only, the new one counts all iterations.
The proportion between them for the same run is different per use case but in average it is 2.1: newIters = oldIters*2.1

I propose three solutions here:

to leave this hack as is (adding a comment about it in src)

to find true multiplier for each use case based on iterations elapsed statistics

remove the multiplier: it's 5.x now, we don't have to maintain such parameters compatibility

I think, having such a hack is fine.

If the problem solved is more or less well-defined (not ill-posed), the solver should converge faster than it will reach the maximum.

If the problem solved is ill-posed then we will do at least termCrit.maxCount iterations, so this hack may affect the speed, but not the quality. But, I believe, because the new solver uses the sparse structure of matrices, it should run even faster than the previous "dense" implementation.

Multiplier removed, statistics recalculated.
Regarding TermCriteria usage: I'm against it, it has only one epsilon parameter and it's not obvious to what threshold in this LevMarq impl it corresponds to.

alalek · 2021-12-23T01:00:23Z

modules/3d/include/opencv2/3d.hpp

+            initialLmDownFactor(3.0)
+        { }
+
+        bool operator==(const Settings& other) const


If users don't need this functionality, then it is better to create internal function.

Removed
(was made to compare a passed arg with default settings constant, default arg is used instead)

alalek · 2021-12-23T01:01:53Z

modules/3d/include/opencv2/3d.hpp

+        Settings() :
+            jacobiScaling(false),
+            upDouble(true),
+            useStepQuality(true),


Move ctor implementation to .cpp file.

alalek · 2021-12-23T01:04:11Z

modules/3d/include/opencv2/3d.hpp

+
+        Settings& jacobiScalingS          (bool         v) { jacobiScaling           = v; return *this; }
+        Settings& upDoubleS               (bool         v) { upDouble                = v; return *this; }
+        Settings& useStepQualityS         (bool         v) { useStepQuality          = v; return *this; }


Why is here 'S' suffix?
Where are you find that?

S stands for "set" but I didn't want to give them names like setEpsilon to make method names more compact.
Since you recommended not to use underscores, I seek other ways to name them.

Decided to use setValue names instead, looks more natural

alalek · 2021-12-23T01:04:55Z

modules/3d/include/opencv2/3d.hpp

+            return ok;
+        }
+
+        Settings& jacobiScalingS          (bool         v) { jacobiScaling           = v; return *this; }


inline for all setters

alalek · 2021-12-23T01:05:57Z

modules/3d/include/opencv2/3d.hpp

+        Settings& relEnergyDeltaToleranceS(double       v) { relEnergyDeltaTolerance = v; return *this; }
+        Settings& minGradientToleranceS   (double       v) { minGradientTolerance    = v; return *this; }
+        Settings& smallEnergyToleranceS   (double       v) { smallEnergyTolerance    = v; return *this; }
+        Settings& maxIterationsS          (unsigned int v) { maxIterations           = v; return *this; }


unsigned int

This type may be non-friendly with bindings.

Replaced by int

alalek · 2021-12-23T01:09:31Z

modules/3d/include/opencv2/3d.hpp

+    /*
+    Defined in details header
+    */
+    class CV_EXPORTS Backend
    {
    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;
+        virtual ~Backend() { }


Defined in

Is it not a definition?

Why does forward declaration not work here?

Backend class removed

alalek · 2021-12-23T01:10:29Z

modules/3d/include/opencv2/3d.hpp

-       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.
+    LevMarqBase(const Ptr<Backend>& backend, const Settings& settings);


Users don't really need this constructor. As they can't use it.

Fixed, see the comment below

…, no Backend class, Settings() => .cpp, Settings==() removed, Settings.set...() inlines

savuor · 2021-12-26T23:03:02Z

LevMarqBase was moved to detail headers
LevMarqDenseLinear was replaced by a class LevMarq which takes enum args responsible for "dense" and "linear" properties respectively; other types of solvers are not implemented now and reserved for future.

alalek

Looks good to me 👍

savuor added 17 commits September 3, 2021 02:58

Hash TSDF fix: apply volume pose when fetching pose

b43d504

DualQuat minor fix

c5dc5c5

Pose Graph: getEdgePose(), getEdgeInfo()

5aae399

debugging code for pose graph

69d2d43

add edge to submap

b488e32

pose averaging: DualQuats instead of matrix averaging

d1bef6f

overlapping ratio: rise it up; minor comment

684c012

remove Submap::addEdgeToSubmap

7f69c16

test_pose_graph: minor

c5c3dc7

SparseBlockMatrix: support 1xN as well as Nx1 for residual vector

1b048b6

small changes to old LMSolver

4ac6626

new LevMarq impl

b55834e

Pose Graph rewritten to use new impl

d0d7353

solvePnP(), findHomography() and findExtrinsicCameraParams2() use new…

5c914c5

… impl

estimateAffine...2D() use new impl

6c21aa5

calibration and stereo calibration use new impl

b046043

BundleAdjusterBase::estimate() uses new impl

499e73e

savuor added the category: 3d module label Nov 7, 2021

savuor changed the base branch from next to 5.x November 8, 2021 10:04

savuor added 11 commits November 12, 2021 20:08

new LevMarq interface

c605318

PoseGraph: changing opt interface

3ae96eb

findExtrinsicCameraParams2(): opt interface updated

a202c8f

HomographyRefine: opt interface updated

e7e74ba

solvePnPRefine opt interface fixed

c3bf0ad

Affine2DRefine opt interface fixed

67d200e

BundleAdjuster::estimate() opt interface fixed

f4f9292

calibration: opt interface fixed + code refactored a little

43f2249

minor warning fixes

276e042

geodesic acceleration, Impl -> Backend rename

c8e5bc6

calcFunc() always uses probe vars

b2a3908

fixed warning

d55ff06

savuor marked this pull request as ready for review December 9, 2021 16:25

fixing *KinFu OCL tests

1345d08

savuor requested a review from alalek December 10, 2021 11:19

alalek reviewed Dec 10, 2021

View reviewed changes

savuor added 12 commits December 18, 2021 17:00

algo params -> struct Settings

88119f3

Backend moved to details

0ac6553

BaseLevMarq -> LevMarqBase

f4e72f6

detail/pose_graph.hpp -> detail/optimizer.hpp

fa0322a

fixing include stuff for details/optimizer.hpp

40bd830

doc fix

4ca597b

LevMarqBase rework: Settings, pImpl, Backend

bb50684

Impl::settings and ::backend fix

f06a028

HashTSDFGPU fix

addab4f

fixing compilation

31b5ce9

warning fix for OdometryFrameImplTMat

f391b66

docs fix + compile warnings

69cb4fb

This was referenced Dec 22, 2021

Warning fixes for quaternion headers #21319

Merged

HashTSDF fixes backported opencv/opencv_contrib#3137

Merged

savuor requested a review from alalek December 23, 2021 00:46

alalek reviewed Dec 23, 2021

View reviewed changes

remake: new class LevMarq with pImpl and enums, LevMarqBase => detail…

02903a0

…, no Backend class, Settings() => .cpp, Settings==() removed, Settings.set...() inlines

fixing warnings & whitespace

d18caa9

savuor requested a review from alalek December 27, 2021 11:27

alalek approved these changes Dec 27, 2021

View reviewed changes

alalek merged commit 9d6f388 into opencv:5.x Dec 27, 2021

savuor deleted the levmarqfromscratch branch December 27, 2021 21:57

savuor mentioned this pull request Sep 27, 2022

[doc] LMSolver poorly documented #22563

Closed

mshabunin mentioned this pull request Jun 12, 2024

Merge 4.x -> 5.x #25745

Merged

Uh oh!

New LevMarq implementation #21018

New LevMarq implementation #21018

Uh oh!

Conversation

savuor commented Nov 7, 2021 • edited by alalek Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

TODOs:

What?

Why?

How?

Any numbers?

Other changes in this PR

Pull Request Readiness Checklist

Uh oh!

alalek left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

vpisarev Dec 15, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

savuor Dec 21, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

savuor Dec 12, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

savuor commented Nov 7, 2021 •

edited by alalek

Loading

vpisarev Dec 15, 2021 •

edited

Loading

savuor Dec 21, 2021 •

edited

Loading

savuor Dec 12, 2021 •

edited

Loading