# 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

  1 public 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;
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;
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]))
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
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  */
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]))
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
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;
196             }
197         }
198
199         DConvertMatrixStruct.convert(tripletLeft,left);
200         DConvertMatrixStruct.convert(tripletRight,right);
201     }
202 }
203
204 protected 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++) {
211     }
212     for (int i = 0; i < numLandmarks; i++) {
214     }
215 }
216
217 static 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