Allow using VVS and Lowe pose refinement on 3 points

[s-trinh]

trafficstars

• without noise, Lowe method (non-linear Levenberg-Marquardt pose minimization) produces lower residuals compared to VVS approach, with the default VVS parameters (number of iterations?, convergence threshold?)
• but since perfect data do not exist, with noise the two approaches seems to be more or less equivalent (what about parameters like the maximum number of iterations?, convergence rate?, ...)

Pose refinement on 4 non co-planar points without noise using VVS method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.09999995549  0.1999999997  0.3000000021
tvec error: 4.456247289e-08
tu_true: 0.1  -0.2  0.67
tu_refined: 0.09999996357  -0.1999999354  0.6700000158
tu error: 7.580579721e-08

Pose refinement on 4 non co-planar points without noise using Lowe method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1  0.2  0.3
tvec error: 1.415613947e-14
tu_true: 0.1  -0.2  0.67
tu_refined: 0.1  -0.2  0.67
tu error: 8.672440908e-14

Pose refinement on 4 non co-planar points with noise using VVS method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1014742971  0.1989016047  0.3009296775
tvec error: 0.002060175838
tu_true: 0.1  -0.2  0.67
tu_refined: 0.08685601143  -0.2151501354  0.6787435716
tu error: 0.0218801527

Pose refinement on 4 non co-planar points with noise using Lowe method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1014743353  0.1989016095  0.3009296789
tvec error: 0.002060201223
tu_true: 0.1  -0.2  0.67
tu_refined: 0.08685605312  -0.2151502044  0.6787435525
tu error: 0.02188016779

Pose refinement on 4 co-planar points without noise using VVS method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.09999996854  0.2000000043  0.3000000099
tvec error: 3.326094976e-08
tu_true: 0.1  -0.2  0.67
tu_refined: 0.09999992879  -0.1999999762  0.669999984
tu error: 7.677512577e-08

Pose refinement on 4 co-planar points without noise using Lowe method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1  0.2  0.3
tvec error: 2.177928571e-15
tu_true: 0.1  -0.2  0.67
tu_refined: 0.1  -0.2  0.67
tu error: 2.238414471e-15

Pose refinement on 4 co-planar points with noise using VVS method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1042042049  0.2026134386  0.3077661144
tvec error: 0.009209665211
tu_true: 0.1  -0.2  0.67
tu_refined: 0.07372693607  -0.2171247333  0.6859334964
tu error: 0.03517679189

Pose refinement on 4 co-planar points with noise using Lowe method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1042043253  0.2026133982  0.307765996
tvec error: 0.009209608846
tu_true: 0.1  -0.2  0.67
tu_refined: 0.07372723774  -0.2171248702  0.6859336419
tu error: 0.03517669912

Pose refinement on 3 points using VVS method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.09999996742  0.2000000038  0.3000000106
tvec error: 3.445552982e-08
tu_true: 0.1  -0.2  0.67
tu_refined: 0.09999993533  -0.1999999618  0.6699999873
tu error: 7.61952163e-08

Pose refinement on 3 points using Lowe method:
tvec_true: 0.1  0.2  0.3
tvec_refined: 0.1  0.2  0.3
tvec error: 2.95789022e-15
tu_true: 0.1  -0.2  0.67
tu_refined: 0.1  -0.2  0.67
tu error: 1.687849393e-15

[codecov[bot]]

Codecov Report

Merging #1034 (3ddd9b4) into master (8092c95) will increase coverage by 0.23%. The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #1034      +/-   ##
==========================================
+ Coverage   53.11%   53.34%   +0.23%     
==========================================
  Files         678      679       +1     
  Lines       80685    80928     +243     
==========================================
+ Hits        42852    43173     +321     
+ Misses      37833    37755      -78     

Impacted Files	Coverage Δ
modules/vision/src/pose-estimation/vpPose.cpp	52.83% <100.00%> (+0.88%)	:arrow_up:
modules/vision/test/pose/testPoseRefine.cpp	100.00% <100.00%> (ø)
modules/core/src/tools/optimization/vpLinProg.cpp	37.14% <0.00%> (-4.29%)	:arrow_down:
modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp	68.75% <0.00%> (-0.55%)	:arrow_down:
modules/core/src/math/matrix/vpMatrix_qr.cpp	62.79% <0.00%> (-0.47%)	:arrow_down:
modules/tracker/mbt/src/vpMbScanLine.cpp	97.32% <0.00%> (-0.39%)	:arrow_down:
...re/src/math/transformation/vpTranslationVector.cpp	54.60% <0.00%> (+3.68%)	:arrow_up:
...ision/src/pose-estimation/vpLevenbergMarquartd.cpp	79.03% <0.00%> (+24.36%)	:arrow_up:

---

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update 8092c95...3ddd9b4. Read the comment docs.

[fspindle]

@s-trinh In therory to estimate the 6 pose parameters 3 points are enough, but if I'm right we have 4 possible solutions. In that case we cannot ensure that the solution that is returned is the good one. This is particularly the case when we have to deal with tracking. I will ask François...

[chaumett]

Fabien, you are fully right: 4 poses generally exist such that they produce the same image points. If we could be sure the initial pose given to VVS or Lowe method is "near" the correct global minimum, then indeed we could consider 3 points only. However, since we are not sure in practice about the initial pose, we prefered to restrict to 4 points. Just a note, as noted by @s-trinh, Lowe's method implementation corresponds to the Levenberg-Marquardt minimization of the image residuals while VVS implementation corresponds to the Gauss-Newton minimization of the same residuals. It is not surprising Lowe/LM is more accurate, but at a more expensive price.

[s-trinh]

This pull request allows using 3 points only for VIRTUAL_VS and LOWE. For all the other methods (e.g. DEMENTHON_VIRTUAL_VS), it will throw an exception.

Since for me when using VIRTUAL_VS it implicitly assumes you have provided an initial solution close to the global optimum, allowing 3 points or 10 points should be the same for me. Or did I miss something?

[chaumett]

Well, with more than 3 points, it is likely the Dementhon/Lagrange/any other method will provide an initial guess sufficiently close to the global minimum. Even a very coarse initialisation should be fine. This is not the case for 3 points. So I feel dangerous to give the possibility to a non-expert user to use a method that may converge to a non-expected other global minimum. I believe he would be angry against ViSP and not again himself... This is fully different for an expert user, as yourself. So at the end, I do not know what is the best option. Fabien, please decide (If @s-trinh needs this functionality, it would be a pity to not give it to him...)

[fspindle]

Considering the different pose algorithms,

  typedef enum {
    LAGRANGE,             /*!< Linear Lagrange approach (doesn't need an initialization) */
    DEMENTHON,            /*!< Linear Dementhon aproach (doesn't need an initialization) */
    LOWE,                 /*!< Lowe aproach based on a Levenberg Marquartd non linear
                             minimization scheme that needs an initialization from Lagrange or
                             Dementhon aproach */
    RANSAC,               /*!< Robust Ransac aproach (doesn't need an initialization) */
    LAGRANGE_LOWE,        /*!< Non linear Lowe aproach initialized by Lagrange
                             approach */
    DEMENTHON_LOWE,       /*!< Non linear Lowe aproach initialized by Dementhon
                             approach */
    VIRTUAL_VS,           /*!< Non linear virtual visual servoing approach that needs an
                             initialization from Lagrange or Dementhon aproach */
    DEMENTHON_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach
                             initialized by Dementhon approach */
    LAGRANGE_VIRTUAL_VS   /*!< Non linear virtual visual servoing approach
                             initialized by Lagrange approach */
  } vpPoseMethodType;

an option I see is to modify / introduce:

    LOWE,                 /*!< Lowe approach based on a Levenberg Marquartd non linear
                             minimization scheme that needs an initialization from Lagrange or
                             Dementhon approach and at least 4 points. */
    LOWE_3PTS,            /*!< Lowe approach based on a Levenberg Marquartd non linear
                             minimization scheme that needs an initialization very close to the global
                             minimum and at least 3 points. Reserved to experts. */
    VIRTUAL_VS,           /*!< Non linear virtual visual servoing approach that needs an
                             initialization from Lagrange or Dementhon aproach and at least 4 points.  */
    VIRTUAL_VS_3PTS,      /*!< Non linear virtual visual servoing approach that needs an
                             initialization very close to the global minimum and at least 3 points. 
                             Reserved to experts.  */

and thus simply modify vpPose::computePose() accordingly

[s-trinh]

Even a very coarse initialisation should be fine. This is not the case for 3 points.

This is interesting. I will try to remember that.

---

This is just some code I am using to test different things on my side.

So we can leave this PR open and see later if it can be useful for other people.

[fspindle]

We don't know if it will be used by other people, but my position is if it's useful for you, it may be useful for other people

[fspindle]

@s-trinh and @chaumett I will close this PR. Feel free to reopen it if needed in your projects.