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