1 /*
2 * Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry.refiners;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.GeometryException;
21 import com.irurueta.geometry.Line2D;
22 import com.irurueta.geometry.PinholeCamera;
23 import com.irurueta.geometry.Plane;
24 import com.irurueta.geometry.estimators.LockedException;
25 import com.irurueta.geometry.estimators.NotReadyException;
26 import com.irurueta.numerical.EvaluationException;
27 import com.irurueta.numerical.GradientEstimator;
28 import com.irurueta.numerical.MultiDimensionFunctionEvaluatorListener;
29 import com.irurueta.numerical.NumericalException;
30 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
31 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
32 import com.irurueta.numerical.optimization.PowellMultiOptimizer;
33 import com.irurueta.numerical.robust.InliersData;
34
35 import java.util.BitSet;
36 import java.util.List;
37
38 /**
39 * A pinhole camera refiner using line/plane correspondences and the
40 * Powell algorithm to try to decrease overall error in LMSE terms among
41 * inlier samples by taking the decomposed parameters of a pinhole camera.
42 * Typically, this refiner is used by a robust estimator, however it can also be
43 * useful in some other situations.
44 */
45 @SuppressWarnings("DuplicatedCode")
46 public class DecomposedLinePlaneCorrespondencePinholeCameraRefiner extends LinePlaneCorrespondencePinholeCameraRefiner {
47
48 /**
49 * Default value for minimum suggestion weight. This weight is used to
50 * slowly draw original camera parameters into desired suggested values.
51 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
52 * to find a solution so that the algorithm can converge into desired value.
53 * The faster the weights are increased the less likely that suggested
54 * values can be converged if they differ too much from the original ones.
55 */
56 public static final double DEFAULT_MIN_SUGGESTION_WEIGHT = 0.1;
57
58 /**
59 * Default value for maximum suggestion weight. This weight is used to
60 * slowly draw original camera parameters into desired suggested values.
61 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
62 * to find a solution so that the algorithm can converge into desired value.
63 * The faster the weights are increased the less likely that suggested
64 * values can be converged if they differ too much from the original ones.
65 */
66 public static final double DEFAULT_MAX_SUGGESTION_WEIGHT = 2.0;
67
68 /**
69 * Default value for the step to increase suggestion weight. This weight is
70 * used to slowly draw original camera parameters into desired suggested
71 * values. Suggestion weight slowly increases each time Levenberg-Marquardt
72 * is used to find a solution so that the algorithm can converge into
73 * desired value. The faster the weights are increased the less likely that
74 * suggested values can be converged if they differ too much from the
75 * original ones.
76 */
77 public static final double DEFAULT_SUGGESTION_WEIGHT_STEP = 0.475;
78
79 /**
80 * Dimensions for refinement.
81 */
82 private static final int REFINE_DIMS = 12;
83
84 /**
85 * Minimum suggestion weight. This weight is used to slowly draw original
86 * camera parameters into desired suggested values.
87 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
88 * to find a solution so that the algorithm can converge into desired value.
89 * The faster the weights are increased the less likely that suggested
90 * values can be converged if they differ too much from the original ones.
91 */
92 private double minSuggestionWeight = DEFAULT_MIN_SUGGESTION_WEIGHT;
93
94 /**
95 * Maximum suggestion weight. This weight is used to slowly draw original
96 * camera parameters into desired suggested values.
97 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
98 * to find a solution so that the algorithm can converge into desired value.
99 * The faster the weights are increased the less likely that suggested
100 * values can be converged if they differ too much from the original ones.
101 */
102 private double maxSuggestionWeight = DEFAULT_MAX_SUGGESTION_WEIGHT;
103
104 /**
105 * Step to increase suggestion weight. This weight is used to slowly draw
106 * original camera parameters into desired suggested values. Suggestion
107 * weight slowly increases each time Levenberg-Marquardt is used to find a
108 * solution so that the algorithm can converge into desired value. The
109 * faster the weights are increased the less likely that suggested values
110 * can be converged if they differ too much from the original ones.
111 */
112 private double suggestionWeightStep = DEFAULT_SUGGESTION_WEIGHT_STEP;
113
114 /**
115 * Instance of a pinhole camera to be reused during refinement.
116 */
117 private PinholeCamera refineCamera;
118
119 /**
120 * Current weight during refinement.
121 */
122 private double currentWeight;
123
124 /**
125 * Constructor.
126 */
127 public DecomposedLinePlaneCorrespondencePinholeCameraRefiner() {
128 }
129
130 /**
131 * Constructor.
132 *
133 * @param initialEstimation initial estimation to be set.
134 * @param keepCovariance true if covariance of estimation must be kept after
135 * refinement, false otherwise.
136 * @param inliers set indicating which of the provided matches are inliers.
137 * @param residuals residuals for matched samples.
138 * @param numInliers number of inliers on initial estimation.
139 * @param samples1 1st set of paired samples.
140 * @param samples2 2nd set of paired samples.
141 * @param refinementStandardDeviation standard deviation used for
142 * Levenberg-Marquardt fitting.
143 */
144 public DecomposedLinePlaneCorrespondencePinholeCameraRefiner(
145 final PinholeCamera initialEstimation, final boolean keepCovariance, final BitSet inliers,
146 final double[] residuals, final int numInliers, final List<Plane> samples1, final List<Line2D> samples2,
147 final double refinementStandardDeviation) {
148 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2,
149 refinementStandardDeviation);
150 }
151
152 /**
153 * Constructor.
154 *
155 * @param initialEstimation initial estimation to be set.
156 * @param keepCovariance true if covariance of estimation must be kept after
157 * refinement, false otherwise.
158 * @param inliersData inlier data, typically obtained from a robust
159 * estimator.
160 * @param samples1 1st set of paired samples.
161 * @param samples2 2nd set of paired samples.
162 * @param refinementStandardDeviation standard deviation used for
163 * Levenberg-Marquardt fitting.
164 */
165 public DecomposedLinePlaneCorrespondencePinholeCameraRefiner(
166 final PinholeCamera initialEstimation, final boolean keepCovariance, final InliersData inliersData,
167 final List<Plane> samples1, final List<Line2D> samples2, final double refinementStandardDeviation) {
168 super(initialEstimation, keepCovariance, inliersData, samples1, samples2, refinementStandardDeviation);
169 }
170
171 /**
172 * Gets minimum suggestion weight. This weight is used to slowly draw
173 * original camera parameters into desired suggested values.
174 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
175 * to find a solution so that the algorithm can converge into desired value.
176 * The faster the weights are increased the less likely that suggested
177 * values can be converged if they differ too much from the original ones.
178 *
179 * @return minimum suggestion weight.
180 */
181 public double getMinSuggestionWeight() {
182 return minSuggestionWeight;
183 }
184
185 /**
186 * Sets minimum suggestion weight. This weight is used to slowly draw
187 * original camera parameters into desired suggested values.
188 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
189 * to find a solution so that the algorithm can converge into desired value.
190 * The faster the weights are increased the less likely that suggested
191 * values can be converged if they differ too much from the original ones.
192 *
193 * @param minSuggestionWeight minimum suggestion weight.
194 * @throws LockedException if estimator is locked.
195 */
196 public void setMinSuggestionWeight(final double minSuggestionWeight) throws LockedException {
197 if (isLocked()) {
198 throw new LockedException();
199 }
200 this.minSuggestionWeight = minSuggestionWeight;
201 }
202
203 /**
204 * Gets maximum suggestion weight. This weight is used to slowly draw
205 * original camera parameters into desired suggested values.
206 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
207 * to find a solution so that the algorithm can converge into desired value.
208 * The faster the weights are increased the less likely that suggested
209 * values can be converged if they differ too much from the original ones.
210 *
211 * @return maximum suggestion weight.
212 */
213 public double getMaxSuggestionWeight() {
214 return maxSuggestionWeight;
215 }
216
217 /**
218 * Sets maximum suggestion weight. This weight is used to slowly draw
219 * original camera parameters into desired suggested values.
220 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
221 * to find a solution so that the algorithm can converge into desired value.
222 * The faster the weights are increased the less likely that suggested
223 * values can be converged if they differ too much from the original ones.
224 *
225 * @param maxSuggestionWeight maximum suggestion weight.
226 * @throws LockedException if estimator is locked.
227 */
228 public void setMaxSuggestionWeight(final double maxSuggestionWeight) throws LockedException {
229 if (isLocked()) {
230 throw new LockedException();
231 }
232 this.maxSuggestionWeight = maxSuggestionWeight;
233 }
234
235 /**
236 * Sets minimum and maximum suggestion weights. Suggestion weight is used to
237 * slowly draw original camera parameters into desired suggested values.
238 * Suggestion weight slowly increases each time Levenberg-Marquardt is used
239 * to find a solution so that the algorithm can converge into desired value.
240 * The faster the weights are increased the less likely that suggested
241 * values can be converged if they differ too much from the original ones.
242 *
243 * @param minSuggestionWeight minimum suggestion weight.
244 * @param maxSuggestionWeight maximum suggestion weight.
245 * @throws LockedException if estimator is locked.
246 * @throws IllegalArgumentException if minimum suggestion weight is greater
247 * or equal than maximum value.
248 */
249 public void setMinMaxSuggestionWeight(final double minSuggestionWeight, final double maxSuggestionWeight)
250 throws LockedException {
251 if (isLocked()) {
252 throw new LockedException();
253 }
254 if (minSuggestionWeight >= maxSuggestionWeight) {
255 throw new IllegalArgumentException();
256 }
257
258 this.minSuggestionWeight = minSuggestionWeight;
259 this.maxSuggestionWeight = maxSuggestionWeight;
260 }
261
262 /**
263 * Gets step to increase suggestion weight. This weight is used to slowly
264 * draw original camera parameters into desired suggested values. Suggestion
265 * weight slowly increases each time Levenberg-Marquardt is used to find a
266 * solution so that the algorithm can converge into desired value. The
267 * faster the weights are increased the less likely that suggested values
268 * can be converged if they differ too much from the original ones.
269 *
270 * @return step to increase suggestion weight.
271 */
272 public double getSuggestionWeightStep() {
273 return suggestionWeightStep;
274 }
275
276 /**
277 * Sets step to increase suggestion weight. This weight is used to slowly
278 * draw original camera parameters into desired suggested values. Suggestion
279 * weight slowly increases each time Levenberg-Marquardt is used to find a
280 * solution so that the algorithm can converge into desired value. The
281 * faster the weights are increased the less likely that suggested values
282 * can be converged if they differ too much from the original ones.
283 *
284 * @param suggestionWeightStep step to increase suggestion weight.
285 * @throws LockedException if estimator is locked.
286 * @throws IllegalArgumentException if provided step is negative or zero.
287 */
288 public void setSuggestionWeightStep(final double suggestionWeightStep) throws LockedException {
289 if (isLocked()) {
290 throw new LockedException();
291 }
292 if (suggestionWeightStep <= 0.0) {
293 throw new IllegalArgumentException();
294 }
295
296 this.suggestionWeightStep = suggestionWeightStep;
297 }
298
299 /**
300 * Refines provided initial estimation.
301 * This method always sets a value into provided result instance regardless
302 * of the fact that error has actually improved in LMSE terms or not.
303 *
304 * @param result instance where refined estimation will be stored.
305 * @return true if result improves (decreases) in LMSE terms respect to
306 * initial estimation, false if no improvement has been achieved.
307 * @throws NotReadyException if not enough input data has been provided.
308 * @throws LockedException if estimator is locked because refinement is
309 * already in progress.
310 */
311 @Override
312 public boolean refine(final PinholeCamera result) throws NotReadyException, LockedException {
313 if (isLocked()) {
314 throw new LockedException();
315 }
316 if (!isReady()) {
317 throw new NotReadyException();
318 }
319
320 locked = true;
321
322 if (listener != null) {
323 listener.onRefineStart(this, initialEstimation);
324 }
325
326 final var improved = refinePowell(result);
327
328 if (keepCovariance) {
329 covariance = estimateCovarianceLevenbergMarquardt(improved ? result : initialEstimation, currentWeight);
330 }
331
332 if (listener != null) {
333 listener.onRefineEnd(this, initialEstimation, result, improved);
334 }
335
336 locked = false;
337
338 return improved;
339 }
340
341 /**
342 * Estimates covariance matrix for provided estimated and refined camera
343 *
344 * @param pinholeCamera pinhole camera to estimate covariance for.
345 * @param weight weight for suggestion residual.
346 * @return estimated covariance or null if anything fails.
347 */
348 private Matrix estimateCovarianceLevenbergMarquardt(final PinholeCamera pinholeCamera, final double weight) {
349 try {
350 pinholeCamera.normalize();
351
352 // output values to be fitted/optimized will contain residuals
353 final var y = new double[numInliers];
354 // input values will contain 2D line and 3D plane to compute
355 // residuals
356 final var nDims = Plane.PLANE_NUMBER_PARAMS + Line2D.LINE_NUMBER_PARAMS;
357 final var x = new Matrix(numInliers, nDims);
358 final var nSamples = inliers.length();
359 var pos = 0;
360 final var initParams = new double[REFINE_DIMS];
361 cameraToParameters(pinholeCamera, initParams);
362
363 final var suggestionResidual = hasSuggestions() ? suggestionResidual(initParams, weight) : 0.0;
364 for (var i = 0; i < nSamples; i++) {
365 if (inliers.get(i)) {
366 // sample is inlier
367 final var plane = samples1.get(i);
368 final var line = samples2.get(i);
369 plane.normalize();
370 line.normalize();
371 x.setElementAt(pos, 0, line.getA());
372 x.setElementAt(pos, 1, line.getB());
373 x.setElementAt(pos, 2, line.getC());
374 x.setElementAt(pos, 3, plane.getA());
375 x.setElementAt(pos, 4, plane.getB());
376 x.setElementAt(pos, 5, plane.getC());
377 x.setElementAt(pos, 6, plane.getD());
378
379 y[pos] = Math.pow(residuals[i], 2.0) + suggestionResidual;
380 pos++;
381 }
382 }
383
384 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
385
386 private final Plane plane = new Plane();
387
388 private final Line2D line = new Line2D();
389
390 private final PinholeCamera pinholeCamera = new PinholeCamera();
391
392 private final GradientEstimator gradientEstimator = new GradientEstimator(params -> {
393 parametersToCamera(params, pinholeCamera);
394 return residualLevenbergMarquardt(pinholeCamera, line, plane, params, weight);
395 });
396
397 @Override
398 public int getNumberOfDimensions() {
399 return nDims;
400 }
401
402 @Override
403 public double[] createInitialParametersArray() {
404 return initParams;
405 }
406
407 @Override
408 public double evaluate(final int i, final double[] point, final double[] params,
409 final double[] derivatives) throws EvaluationException {
410
411 line.setParameters(point[0], point[1], point[2]);
412 plane.setParameters(point[3], point[4], point[5], point[6]);
413
414 parametersToCamera(params, pinholeCamera);
415 final var y = residualLevenbergMarquardt(pinholeCamera, line, plane, params, weight);
416 gradientEstimator.gradient(params, derivatives);
417
418 return y;
419 }
420 };
421
422 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
423 getRefinementStandardDeviation());
424
425 fitter.fit();
426
427 // obtain covariance
428 return fitter.getCovar();
429
430 } catch (final Exception e) {
431 // estimation failed, so we return null
432 return null;
433 }
434 }
435
436 /**
437 * Refines camera using Powell optimization to minimize a cost function
438 * consisting on the sum of squared projection residuals plus the
439 * suggestion residual for any suggested terms.
440 *
441 * @param result instance where refined estimation will be stored.
442 * @return true if result improves (decreases) in LMSE terms respect to
443 * initial estimation, false if no improvement has been achieved.
444 */
445 private boolean refinePowell(final PinholeCamera result) {
446 var improvedAtLeastOnce = false;
447 currentWeight = minSuggestionWeight;
448
449 if (hasSuggestions()) {
450 try {
451 // copy camera into a new instance
452 refineCamera = new PinholeCamera(new Matrix(initialEstimation.getInternalMatrix()));
453 refineCamera.normalize();
454
455 final var startPoint = new double[REFINE_DIMS];
456 final var listener = new RefinementMultiDimensionFunctionEvaluatorListener();
457 final var optimizer = new PowellMultiOptimizer(listener, PowellMultiOptimizer.DEFAULT_TOLERANCE);
458
459 boolean improved;
460 do {
461 improved = refinementStepPowell(optimizer, listener, startPoint, currentWeight);
462
463 if (improved) {
464 // update result
465 result.setInternalMatrix(new Matrix(refineCamera.getInternalMatrix()));
466 improvedAtLeastOnce = true;
467 }
468
469 currentWeight += suggestionWeightStep;
470 } while (currentWeight < maxSuggestionWeight && improved);
471
472 return improvedAtLeastOnce;
473 } catch (final GeometryException | NumericalException | AlgebraException e) {
474 // refinement failed, so we return input value
475 return improvedAtLeastOnce;
476 }
477 }
478 return false;
479 }
480
481 /**
482 * Computes one refinement step using Powell optimizer for a given weight
483 * on suggestion terms.
484 *
485 * @param optimizer Powell optimizer to be reused.
486 * @param listener Powell optimizer listener to be reused.
487 * @param startPoint starting point for powell optimization. This array is
488 * passed only for reuse purposes.
489 * @param weight suggestion terms weight.
490 * @return true if this refinement step decreased projection error in LMSE
491 * terms, false otherwise.
492 * @throws GeometryException if something failed.
493 * @throws NumericalException if something failed.
494 */
495 private boolean refinementStepPowell(
496 final PowellMultiOptimizer optimizer, final RefinementMultiDimensionFunctionEvaluatorListener listener,
497 final double[] startPoint, final double weight) throws GeometryException, NumericalException {
498
499 listener.weight = weight;
500 cameraToParameters(refineCamera, startPoint);
501 final var initResidual = residualPowell(refineCamera, startPoint, weight);
502
503 optimizer.setStartPoint(startPoint);
504 optimizer.minimize();
505
506 final var resultParams = optimizer.getResult();
507 parametersToCamera(resultParams, refineCamera);
508
509 final var finalResidual = residualPowell(refineCamera, resultParams, weight);
510
511 return finalResidual < initResidual;
512 }
513
514 /**
515 * Listener for powell optimizer to minimize cost function during
516 * refinement.
517 * A weight can be provided so that required parameters are slowly drawn
518 * to suggested values.
519 */
520 private class RefinementMultiDimensionFunctionEvaluatorListener implements MultiDimensionFunctionEvaluatorListener {
521 /**
522 * Weight to slowly draw parameters to suggested values.
523 */
524 double weight;
525
526 /**
527 * Evaluates cost function
528 *
529 * @param point parameters to evaluate cost function.
530 * @return cost value.
531 */
532 @Override
533 public double evaluate(final double[] point) {
534 parametersToCamera(point, refineCamera);
535 return residualPowell(refineCamera, point, weight);
536 }
537 }
538 }