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.Matrix;
19 import com.irurueta.geometry.HomogeneousPoint3D;
20 import com.irurueta.geometry.Plane;
21 import com.irurueta.geometry.estimators.LockedException;
22 import com.irurueta.geometry.estimators.NotReadyException;
23 import com.irurueta.numerical.EvaluationException;
24 import com.irurueta.numerical.GradientEstimator;
25 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
26 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
27 import com.irurueta.numerical.robust.InliersData;
28
29 import java.util.BitSet;
30 import java.util.List;
31
32 /**
33 * Refines an homogeneous 3D point by taking into account an initial estimation,
34 * inlier samples and their residuals.
35 * This class can be used to find a solution that minimizes error of inliers in
36 * LMSE terms.
37 * Typically, a refiner is used by a robust estimator, however it can also be
38 * useful in some other situations.
39 */
40 @SuppressWarnings("DuplicatedCode")
41 public class HomogeneousPoint3DRefiner extends Point3DRefiner<HomogeneousPoint3D> {
42
43 /**
44 * Constructor.
45 */
46 public HomogeneousPoint3DRefiner() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param initialEstimation initial estimation to be set.
53 * @param keepCovariance true if covariance of estimation must be kept after
54 * refinement, false otherwise.
55 * @param inliers set indicating which of the provided matches are inliers.
56 * @param residuals residuals for matched samples.
57 * @param numInliers number of inliers on initial estimation.
58 * @param samples collection of samples.
59 * @param refinementStandardDeviation standard deviation used for
60 * Levenberg-Marquardt fitting.
61 */
62 public HomogeneousPoint3DRefiner(
63 final HomogeneousPoint3D initialEstimation, final boolean keepCovariance, final BitSet inliers,
64 final double[] residuals, final int numInliers, final List<Plane> samples,
65 final double refinementStandardDeviation) {
66 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples, refinementStandardDeviation);
67 }
68
69 /**
70 * Constructor.
71 *
72 * @param initialEstimation initial estimation to be set.
73 * @param keepCovariance true if covariance of estimation must be kept after
74 * refinement, false otherwise.
75 * @param inliersData inlier data, typically obtained from a robust
76 * estimator.
77 * @param samples collection of samples.
78 * @param refinementStandardDeviation standard deviation used for
79 * Levenberg-Marquardt fitting.
80 */
81 public HomogeneousPoint3DRefiner(
82 final HomogeneousPoint3D initialEstimation, final boolean keepCovariance, final InliersData inliersData,
83 final List<Plane> samples, final double refinementStandardDeviation) {
84 super(initialEstimation, keepCovariance, inliersData, samples, refinementStandardDeviation);
85 }
86
87 /**
88 * Refines provided initial estimation.
89 *
90 * @return refined estimation.
91 * @throws NotReadyException if not enough input data has been provided.
92 * @throws LockedException if estimator is locked because refinement is
93 * already in progress.
94 * @throws RefinerException if refinement fails for some reason (e.g. unable
95 * to converge to a result).
96 */
97 @Override
98 public HomogeneousPoint3D refine() throws NotReadyException, LockedException, RefinerException {
99 final var result = new HomogeneousPoint3D();
100 refine(result);
101 return result;
102 }
103
104 /**
105 * Refines provided initial estimation.
106 * This method always sets a value into provided result instance regardless
107 * of the fact that error has actually improved in LMSE terms or not.
108 *
109 * @param result instance where refined estimation will be stored.
110 * @return true if result improved (decreases) in LMSE terms respect to
111 * initial estimation, false if no improvement has been achieved.
112 * @throws NotReadyException if not enough input data has been provided.
113 * @throws LockedException if estimator is locked because refinement is
114 * already in progress.
115 * @throws RefinerException if refinement fails for some reason (e.g. unable
116 * to converge to a result).
117 */
118 @Override
119 public boolean refine(final HomogeneousPoint3D result) throws NotReadyException, LockedException, RefinerException {
120 if (isLocked()) {
121 throw new LockedException();
122 }
123 if (!isReady()) {
124 throw new NotReadyException();
125 }
126
127 locked = true;
128
129 if (listener != null) {
130 listener.onRefineStart(this, initialEstimation);
131 }
132
133 final var initialTotalResidual = totalResidual(initialEstimation);
134
135 try {
136 final var initParams = initialEstimation.asArray();
137
138 // output values to be fitted/optimized will contain residuals
139 final var y = new double[numInliers];
140 // input values will contain planes to compute residuals
141 final var nDims = Plane.PLANE_NUMBER_PARAMS;
142 final var x = new Matrix(numInliers, nDims);
143 final var nSamples = inliers.length();
144 var pos = 0;
145 for (var i = 0; i < nSamples; i++) {
146 if (inliers.get(i)) {
147 // sample is inlier
148 final var plane = samples.get(i);
149 plane.normalize();
150 x.setElementAt(pos, 0, plane.getA());
151 x.setElementAt(pos, 1, plane.getB());
152 x.setElementAt(pos, 2, plane.getC());
153 x.setElementAt(pos, 3, plane.getD());
154
155 y[pos] = residuals[i];
156 pos++;
157 }
158 }
159
160 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
161
162 private final Plane plane = new Plane();
163
164 private final HomogeneousPoint3D point = new HomogeneousPoint3D();
165
166 private final GradientEstimator gradientEstimator = new GradientEstimator(p -> {
167 this.point.setCoordinates(p);
168 return residual(this.point, plane);
169 });
170
171 @Override
172 public int getNumberOfDimensions() {
173 return nDims;
174 }
175
176 @Override
177 public double[] createInitialParametersArray() {
178 return initParams;
179 }
180
181 @Override
182 public double evaluate(final int i, final double[] point, final double[] params,
183 final double[] derivatives) throws EvaluationException {
184 // point contains a,b,c,d values for plane
185 plane.setParameters(point);
186
187 // params contains coordinates of point
188 this.point.setCoordinates(params);
189
190 final var y = residual(this.point, plane);
191 gradientEstimator.gradient(params, derivatives);
192
193 return y;
194 }
195 };
196
197 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
198 getRefinementStandardDeviation());
199
200 fitter.fit();
201
202 // obtain estimated params
203 final var params = fitter.getA();
204
205 // update point
206 result.setCoordinates(params);
207
208 if (keepCovariance) {
209 // keep covariance
210 covariance = fitter.getCovar();
211 }
212
213 final var finalTotalResidual = totalResidual(result);
214 final var errorDecreased = finalTotalResidual < initialTotalResidual;
215
216 if (listener != null) {
217 listener.onRefineEnd(this, initialEstimation, result, errorDecreased);
218 }
219
220 return errorDecreased;
221 } catch (final Exception e) {
222 throw new RefinerException(e);
223 } finally {
224 locked = false;
225 }
226 }
227 }