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.InhomogeneousPoint3D;
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 inhomogeneous 3D point by taking into account an initial
34 * estimation, 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 InhomogeneousPoint3DRefiner extends Point3DRefiner<InhomogeneousPoint3D> {
42
43 /**
44 * Constructor.
45 */
46 public InhomogeneousPoint3DRefiner() {
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 InhomogeneousPoint3DRefiner(
63 final InhomogeneousPoint3D 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 InhomogeneousPoint3DRefiner(
82 final InhomogeneousPoint3D 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 InhomogeneousPoint3D refine() throws NotReadyException, LockedException, RefinerException {
99 final var result = new InhomogeneousPoint3D();
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 InhomogeneousPoint3D result) throws NotReadyException, LockedException,
120 RefinerException {
121 if (isLocked()) {
122 throw new LockedException();
123 }
124 if (!isReady()) {
125 throw new NotReadyException();
126 }
127
128 locked = true;
129
130 if (listener != null) {
131 listener.onRefineStart(this, initialEstimation);
132 }
133
134 final var initialTotalResidual = totalResidual(initialEstimation);
135
136 try {
137 final var initParams = initialEstimation.asArray();
138
139 // output values to be fitted/optimized will contain residuals
140 final var y = new double[numInliers];
141 // input values will contain planes to compute residuals
142 final var nDims = Plane.PLANE_NUMBER_PARAMS;
143 final var x = new Matrix(numInliers, nDims);
144 final var nSamples = inliers.length();
145 var pos = 0;
146 for (var i = 0; i < nSamples; i++) {
147 if (inliers.get(i)) {
148 // sample is inlier
149 final var plane = samples.get(i);
150 plane.normalize();
151 x.setElementAt(pos, 0, plane.getA());
152 x.setElementAt(pos, 1, plane.getB());
153 x.setElementAt(pos, 2, plane.getC());
154 x.setElementAt(pos, 3, plane.getD());
155
156 y[pos] = residuals[i];
157 pos++;
158 }
159 }
160
161 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
162
163 private final Plane plane = new Plane();
164
165 private final InhomogeneousPoint3D point = new InhomogeneousPoint3D();
166
167 private final GradientEstimator gradientEstimator = new GradientEstimator(p -> {
168 this.point.setCoordinates(p);
169 return residual(this.point, plane);
170 });
171
172 @Override
173 public int getNumberOfDimensions() {
174 return nDims;
175 }
176
177 @Override
178 public double[] createInitialParametersArray() {
179 return initParams;
180 }
181
182 @Override
183 public double evaluate(final int i, final double[] point, final double[] params,
184 final double[] derivatives) throws EvaluationException {
185 // point contains a,b,c,d values for plane
186 plane.setParameters(point);
187
188 // params contains coordinates of point
189 this.point.setCoordinates(params);
190
191 final var y = residual(this.point, plane);
192 gradientEstimator.gradient(params, derivatives);
193
194 return y;
195 }
196 };
197
198 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
199 getRefinementStandardDeviation());
200
201 fitter.fit();
202
203 // obtain estimated params
204 final var params = fitter.getA();
205
206 // update point
207 result.setCoordinates(params);
208
209 if (keepCovariance) {
210 // keep covariance
211 covariance = fitter.getCovar();
212 }
213
214 final var finalTotalResidual = totalResidual(result);
215 final var errorDecreased = finalTotalResidual < initialTotalResidual;
216
217 if (listener != null) {
218 listener.onRefineEnd(this, initialEstimation, result, errorDecreased);
219 }
220
221 return errorDecreased;
222 } catch (final Exception e) {
223 throw new RefinerException(e);
224 } finally {
225 locked = false;
226 }
227 }
228 }