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