Non-Linear Least Squares: Sparse Schur Complement

As optimization problems get larger they also tend to be more sparse. A common problem with sparse systems is fill in. What fill in refers to are zero elements becoming non-zero as a linear system is solved. This will destroy your performance. What the Schur Complement does is allow you to solve the problem in separate components which are structured for efficiency.

Bundle Adjustment is a classic problem from computer vision which is made possible by the Schur Complement. What would take seconds when solved with the Schur Complement can literally take hours or days without it. This example demonstrates the Schur Comlpement in a 2D version of bundle adjustment.

The parameters which are optimized are camera and landmarks locations. Both are them are described by an (x,y) coordinate. Each camera can observe each landmark with a bearings measurement.

\[\theta_{ij} = \mbox{atan}\left( \frac{l^j_y-c^i_y}{l^j_x-c^i_x}\right)\]

where \(\theta_{ij}\) is camera i’s observation of landmark j, \((c_x,c_y)\) is a camera’s location, and \((l_x,l_y)\) is a landmark’s location.

The actual code is shown below. This is more complex than our previous examples so be sure to read the in code comments. How the Jacobian is formulated is also different from the regular least-squaers problem. It’s broken up into a left and right side.

ExampleSchurComplementLeastSquares.java

  1public static void main(String[] args)
  2{
  3    //------------------------------------------------------------------
  4    // Randomly generate the world
  5    //
  6    Random rand = new Random(0xDEADBEEF);
  7    final int numCameras = 10;
  8    final int numLandmarks = 40;
  9
 10    // Cameras are placed along a line. This specifies the length of the line and how spread out the cameras
 11    // will be
 12    final double length = 10;
 13
 14    // how far away the landmark line is from the camera line
 15    final double depth = 20;
 16
 17    double[] observations = new double[numCameras*numLandmarks];
 18
 19    double[] optimal = new double[2*(numCameras+numLandmarks)];
 20    double[] initial = new double[optimal.length];
 21
 22    // Randomly create the world and observations
 23    List<Point2D> cameras = new ArrayList<>();
 24    List<Point2D> landmarks = new ArrayList<>();
 25
 26    int index = 0;
 27    for (int i = 0; i < numCameras; i++) {
 28        double x = 2*(rand.nextDouble()-0.5)*length;
 29        cameras.add( new Point2D(0,x) );
 30
 31        optimal[index++] = 0;
 32        optimal[index++] = x;
 33    }
 34    for (int i = 0; i < numLandmarks; i++) {
 35        double y = 2*(rand.nextDouble()-0.5)*length;
 36        landmarks.add( new Point2D(depth,y) );
 37
 38        optimal[index++] = depth;
 39        optimal[index++] = y;
 40    }
 41
 42    //------------------------------------------------------------------
 43    // Creating noisy initial estimate
 44    //
 45    for (int i = 0; i < optimal.length; i++) {
 46        initial[i] = optimal[i] + rand.nextGaussian()*5; // this is a lot of error!
 47    }
 48
 49    //------------------------------------------------------------------
 50    // Creating perfect observations.
 51    //   Unrealistic, but we know if it hit the optimal solution or not
 52    index = 0;
 53    for (int i = 0; i < numCameras; i++) {
 54        Point2D c = cameras.get(i);
 55        for (int j = 0; j < numLandmarks; j++, index++) {
 56            Point2D l = landmarks.get(j);
 57            observations[index] = Math.atan((l.y-c.y)/(l.x-c.x));
 58
 59            // sanity check
 60            if(UtilEjml.isUncountable(observations[index]))
 61                throw new RuntimeException("Egads");
 62        }
 63    }
 64
 65    //------------------------------------------------------------------
 66    // Create the optimizer and optimize!
 67    //
 68    UnconstrainedLeastSquaresSchur<DMatrixSparseCSC> optimizer =
 69            FactoryOptimizationSparse.levenbergMarquardtSchur(null);
 70
 71    // Send to standard out progress information
 72    optimizer.setVerbose(System.out,0);
 73
 74    // For large sparse systems it's strongly recommended that you use an analytic Jacobian. While this might
 75    // change in the future after a redesign, there isn't a way to efficiently compute the numerical Jacobian
 76    FuncGradient funcGrad = new FuncGradient(numCameras,numLandmarks,observations);
 77    optimizer.setFunction(funcGrad,funcGrad);
 78
 79    // provide it an extremely crude initial estimate of the line equation
 80    optimizer.initialize(initial,1e-12,1e-12);
 81
 82    // iterate 500 times or until it converges.
 83    // Manually iteration is possible too if more control over is required
 84    UtilOptimize.process(optimizer,500);
 85
 86    // see how accurately it found the solution. Optimally this value would be zero
 87    System.out.println("Final Error = "+optimizer.getFunctionValue());
 88}
 89
 90
 91/**
 92 * Implements the residual function and the gradient.
 93 */
 94public static class FuncGradient
 95        implements FunctionNtoM, SchurJacobian<DMatrixSparseCSC>
 96{
 97    int numCameras,numLandmarks;
 98    
 99    // observations of each landmark as seen from each camera as an angle measurement
100    // 2D array. cameras = rows, landmarks = columns
101    double observations[];
102    
103    List<Point2D> cameras = new ArrayList<>();
104    List<Point2D> landmarks = new ArrayList<>();
105
106    public FuncGradient(int numCameras, int numLandmarks,
107                        double observations[] ) {
108        this.numCameras = numCameras;
109        this.numLandmarks = numLandmarks;
110        this.observations = observations;
111    }
112
113    /**
114     * The function.
115     * 
116     * @param input Parameters for input model.
117     * @param output Storage for the output give the model.
118     */
119    @Override
120    public void process(double[] input, double[] output) {
121        decode(numCameras,numLandmarks,input,cameras, landmarks);
122
123        int index = 0;
124        for (int i = 0; i < cameras.size(); i++) {
125            Point2D c = cameras.get(i);
126            for (int j = 0; j < landmarks.size(); j++, index++) {
127                Point2D l = landmarks.get(j);
128                output[index] = Math.atan((l.y-c.y)/(l.x-c.x))-observations[index];
129
130                if(UtilEjml.isUncountable(output[index]))
131                    throw new RuntimeException("Egads");
132            }
133        }
134    }
135
136    @Override
137    public int getNumOfInputsN() {
138        return 2*numCameras + 2*numLandmarks;
139    }
140
141    @Override
142    public int getNumOfOutputsM() {
143        return numCameras*numLandmarks;
144    }
145
146    /**
147     * The Jaoobian. Split in a left and right hand side for the Schur Complement.
148     * 
149     * @param input Vector with input parameters.
150     * @param left (Output) left side of jacobian. Will be resized to fit.
151     * @param right (Output) right side of jacobian. Will be resized to fit.
152     */
153    @Override
154    public void process(double[] input, DMatrixSparseCSC left, DMatrixSparseCSC right) {
155        decode(numCameras,numLandmarks,input,cameras, landmarks);
156
157        int N = numCameras*numLandmarks;
158        DMatrixSparseTriplet tripletLeft = new DMatrixSparseTriplet(N,2*numCameras,1);
159        DMatrixSparseTriplet tripletRight = new DMatrixSparseTriplet(N,2*numLandmarks,1);
160
161        int output = 0;
162        for (int i = 0; i < numCameras; i++) {
163            Point2D c = cameras.get(i);
164            for (int j = 0; j < numLandmarks; j++, output++) {
165                Point2D l = landmarks.get(j);
166                double top = l.y-c.y;
167                double bottom = l.x-c.x;
168                double slope = top/bottom;
169
170                double a = 1.0/(1.0+slope*slope);
171
172                double dx = top/(bottom*bottom);
173                double dy = -1.0/bottom;
174
175                tripletLeft.addItemCheck(output,i*2+0,a*dx);
176                tripletLeft.addItemCheck(output,i*2+1,a*dy);
177            }
178        }
179
180        for (int i = 0; i < numLandmarks; i++) {
181            Point2D l = landmarks.get(i);
182            for (int j = 0; j < numCameras; j++) {
183                Point2D c = cameras.get(j);
184                double top = l.y-c.y;
185                double bottom = l.x-c.x;
186                double slope = top/bottom;
187
188                double a = 1.0/(1.0+slope*slope);
189
190                double dx = -top/(bottom*bottom);
191                double dy = 1.0/bottom;
192
193                output = j*numLandmarks + i;
194                tripletRight.addItemCheck(output,i*2+0,a*dx);
195                tripletRight.addItemCheck(output,i*2+1,a*dy);
196            }
197        }
198
199        DConvertMatrixStruct.convert(tripletLeft,left);
200        DConvertMatrixStruct.convert(tripletRight,right);
201    }
202}
203
204protected static void decode( int numCameras , int numLandmarks, 
205                              double[] input, List<Point2D> cameras , List<Point2D> landmarks ) {
206    cameras.clear();
207    landmarks.clear();
208    int index = 0;
209    for (int i = 0; i < numCameras; i++) {
210        cameras.add( new Point2D(input[index++],input[index++]) );
211    }
212    for (int i = 0; i < numLandmarks; i++) {
213        landmarks.add( new Point2D(input[index++],input[index++]) );
214    }
215}
216
217static class Point2D {
218    double x,y;
219
220    public Point2D(double x, double y) {
221        this.x = x;
222        this.y = y;
223    }
224}

When you run this example you should see something like the following:

Steps     fx        change      |step|   f-test     g-test    tr-ratio  lambda
0     2.124E+01   0.000E+00  0.000E+00  0.000E+00  0.000E+00    0.00   1.00E-04
1     1.521E+00  -1.972E+01  4.268E+01  9.284E-01  2.749E-02   0.942   3.33E-05
2     2.505E-02  -1.496E+00  2.687E+01  9.835E-01  7.797E-03   0.991   1.11E-05
3     5.300E-05  -2.500E-02  6.227E+00  9.979E-01  1.886E-04   0.999   3.70E-06
4     9.786E-10  -5.300E-05  3.123E-01  1.000E+00  1.415E-05   1.000   1.23E-06
5     1.303E-18  -9.786E-10  1.413E-03  1.000E+00  1.244E-07   1.000   4.12E-07
6     1.280E-30  -1.303E-18  5.313E-08  1.000E+00  7.759E-12   1.000   1.37E-07
Converged g-test
Final Error = 1.2803435700834499E-30