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