PnPLepetitEPnP (geo 0.19 API)

java.lang.Object
- boofcv.alg.geo.pose.PnPLepetitEPnP

```
public class PnPLepetitEPnP
extends java.lang.Object
```
Implementation of the EPnP algorithm from [1] for solving the PnP problem when N >= 5 for the general case and N >= 4 for planar data (see note below). Given a calibrated camera, n pairs of 2D point observations and the known 3D world coordinates, it solves the for camera's pose.. This solution is non-iterative and claims to be much faster and more accurate than the alternatives. Works for both planar and non-planar configurations.

Expresses the N 3D point as a weighted sum of four virtual control points. Problem then becomes to estimate the coordinates of the control points in the camera referential, which can be done in O(n) time.

After estimating the control points the solution can be refined using Gauss Newton non-linear optimization. The objective function being optimizes reduces the difference between world and camera control point distances by adjusting the values of beta. Optimization is very fast, but can degrade accuracy if over optimized. See warning below. To turn on non-linear optimization set setNumIterations(int) to a positive number.

After experimentation there doesn't seem to be any universally best way to choose the control point distribution. To allow tuning for specific problems the 'magic number' has been provided. Larger values increase the control point distribution's size. In general smaller numbers appear to be better for noisier data, but can degrade results if too small.

MINIMUM POINTS: According to the original paper [1] only 4 points are needed for the general case. In practice it produces very poor results on average. The matlab code provided by the original author also exhibits instability in the minimum case. The article also does not show results for the minimum case, making it hard to judge what should be expected. However, I'm not ruling out there being a bug in relinearization. See that code for comments. Correspondence with the original author indicates that he did not expect results from relinearization to be as accurate as the other cases.

NOTES: This implementation deviates in some minor ways from what was described in the paper. However, their own example code (Matlab and C++) are mutually different in significant ways too. See how solutions are scored, linear systems are solved, and how world control points are computed. How control points are computed here is inspired from their C++ example (technique used in their matlab example has some stability issues), but there is probably room for more improvement.

WARNING: Setting the number of optimization iterations too high can actually degrade accuracy. The objective function being minimized is not observation residuals. Locally it appears to be a good approximation, but can diverge and actually produce worse results. Because of this behavior, more advanced optimization routines are unnecessary and counter productive.

[1] Vincent Lepetit, Francesc Moreno-Noguer, and Pascal Fua, "EPnP: An Accurate O(n) Solution to the PnP Problem" Int. J. Comput. Visionm, vol 81, issue 2, 2009

Field Summary

Fields
Modifier and Type	Field and Description
`protected org.ejml.data.DenseMatrix64F`	`alphas`
`protected org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64>`	`controlWorldPts`
`protected org.ejml.data.DenseMatrix64F`	`L_full`
`protected java.util.List<georegression.struct.point.Point3D_F64>[]`	`nullPts`
`protected int`	`numControl`
`protected org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64>`	`solutionPts`
`protected org.ejml.data.DenseMatrix64F`	`y`

Constructor Summary

Constructors
Constructor and Description
`PnPLepetitEPnP()` Constructor which uses the default magic number
`PnPLepetitEPnP(double magicNumber)` Constructor which allows configuration of the magic number.

Method Summary

All Methods Static Methods Instance Methods Concrete Methods
Modifier and Type	Method and Description
`protected void`	`computeBarycentricCoordinates(org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts, org.ejml.data.DenseMatrix64F alphas, java.util.List<georegression.struct.point.Point3D_F64> worldPts)` Given the control points it computes the 4 weights for each camera point.
`protected static void`	`constructM(java.util.List<georegression.struct.point.Point2D_F64> obsPts, org.ejml.data.DenseMatrix64F alphas, org.ejml.data.DenseMatrix64F M)` Constructs the linear system which is to be solved.
`protected void`	`estimateCase1(double[] betas)` Simple analytical solution.
`protected void`	`estimateCase2(double[] betas)`
`protected void`	`estimateCase3_planar(double[] betas)` If the data is planar use relinearize to estimate betas
`protected void`	`estimateCase3(double[] betas)`
`protected void`	`estimateCase4(double[] betas)`
`protected void`	`extractNullPoints(org.ejml.data.DenseMatrix64F M)` Computes M'*M and finds the null space.
`int`	`getMinPoints()` Returns the minimum number of points required to make an estimate.
`protected double`	`matchScale(java.util.List<georegression.struct.point.Point3D_F64> nullPts, org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts)` Examines the distance each point is from the centroid to determine the scaling difference between world control points and the null points.
`void`	`process(java.util.List<georegression.struct.point.Point3D_F64> worldPts, java.util.List<georegression.struct.point.Point2D_F64> observed, georegression.struct.se.Se3_F64 solutionModel)` Compute camera motion given a set of features with observations and 3D locations
`void`	`selectWorldControlPoints(java.util.List<georegression.struct.point.Point3D_F64> worldPts, org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts)` Selects control points along the data's axis and the data's centroid.
`void`	`setNumIterations(int numIterations)` Used to turn on and off non-linear optimization.

Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

- Field Detail
  - alphas
```
protected org.ejml.data.DenseMatrix64F alphas
```
  - L_full
```
protected org.ejml.data.DenseMatrix64F L_full
```
  - y
```
protected org.ejml.data.DenseMatrix64F y
```
  - numControl
```
protected int numControl
```
  - nullPts
```
protected java.util.List<georegression.struct.point.Point3D_F64>[] nullPts
```
  - controlWorldPts
```
protected org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts
```
  - solutionPts
```
protected org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> solutionPts
```
- Constructor Detail
  - PnPLepetitEPnP
```
public PnPLepetitEPnP()
```
    Constructor which uses the default magic number
  - PnPLepetitEPnP
```
public PnPLepetitEPnP(double magicNumber)
```
    Constructor which allows configuration of the magic number.
    
    Parameters:
    
    magicNumber - Magic number changes distribution of world control points. Values less than one seem to work best. Try 0.1
- Method Detail
  - setNumIterations
```
public void setNumIterations(int numIterations)
```
    Used to turn on and off non-linear optimization. To turn on set to a positive number. See warning in class description about setting the number of iterations too high.
    
    Parameters:
    
    numIterations - Number of iterations. Try 10.
  - process
```
public void process(java.util.List<georegression.struct.point.Point3D_F64> worldPts,
                    java.util.List<georegression.struct.point.Point2D_F64> observed,
                    georegression.struct.se.Se3_F64 solutionModel)
```
    Compute camera motion given a set of features with observations and 3D locations
    
    Parameters:
    
    worldPts - Known location of features in 3D world coordinates
    
    observed - Observed location of features in normalized camera coordinates
    
    solutionModel - Output: Storage for the found solution.
  - selectWorldControlPoints
```
public void selectWorldControlPoints(java.util.List<georegression.struct.point.Point3D_F64> worldPts,
                                     org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts)
```
    Selects control points along the data's axis and the data's centroid. If the data is determined to be planar then only 3 control points are selected. The data's axis is determined by computing the covariance matrix then performing SVD. The axis is contained along the
  - computeBarycentricCoordinates
```
protected void computeBarycentricCoordinates(org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts,
                                             org.ejml.data.DenseMatrix64F alphas,
                                             java.util.List<georegression.struct.point.Point3D_F64> worldPts)
```
    Given the control points it computes the 4 weights for each camera point. This is done by solving the following linear equation: C*α=X. where C is the control point matrix, α is the 4 by n matrix containing the solution, and X is the camera point matrix. N is the number of points.
    
    C = [ controlPts' ; ones(1,4) ]
    X = [ cameraPts' ; ones(1,N) ]
  - constructM
```
protected static void constructM(java.util.List<georegression.struct.point.Point2D_F64> obsPts,
                                 org.ejml.data.DenseMatrix64F alphas,
                                 org.ejml.data.DenseMatrix64F M)
```
    Constructs the linear system which is to be solved. sum a_ij*x_j - a_ij*u_i*z_j = 0 sum a_ij*y_j - a_ij*v_i*z_j = 0 where (x,y,z) is the control point to be solved for. (u,v) is the observed normalized point
  - extractNullPoints
```
protected void extractNullPoints(org.ejml.data.DenseMatrix64F M)
```
    Computes M'*M and finds the null space. The 4 eigenvectors with the smallest eigenvalues are found and the null points extracted from them.
  - matchScale
```
protected double matchScale(java.util.List<georegression.struct.point.Point3D_F64> nullPts,
                            org.ddogleg.struct.FastQueue<georegression.struct.point.Point3D_F64> controlWorldPts)
```
    Examines the distance each point is from the centroid to determine the scaling difference between world control points and the null points.
  - estimateCase1
```
protected void estimateCase1(double[] betas)
```
    Simple analytical solution. Just need to solve for the scale difference in one set of potential control points.
  - estimateCase2
```
protected void estimateCase2(double[] betas)
```
  - estimateCase3
```
protected void estimateCase3(double[] betas)
```
  - estimateCase3_planar
```
protected void estimateCase3_planar(double[] betas)
```
    If the data is planar use relinearize to estimate betas
  - estimateCase4
```
protected void estimateCase4(double[] betas)
```
  - getMinPoints
```
public int getMinPoints()
```
    Returns the minimum number of points required to make an estimate. Technically it is 4 for general and 3 for planar. The minimum number of point cases seem to be a bit unstable in some situations. minimum + 1 or more is stable.
    
    Returns:
    
    minimum number of points

Class PnPLepetitEPnP

Field Summary

Constructor Summary

Method Summary

Methods inherited from class java.lang.Object

Field Detail

alphas

L_full

y

numControl

nullPts

controlWorldPts

solutionPts

Constructor Detail

PnPLepetitEPnP

PnPLepetitEPnP

Method Detail

setNumIterations

process

selectWorldControlPoints

computeBarycentricCoordinates

constructM

extractNullPoints

matchScale

estimateCase1

estimateCase2

estimateCase3

estimateCase3_planar

estimateCase4

getMinPoints