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.geometry.Plane;
19 import com.irurueta.geometry.Point3D;
20 import com.irurueta.geometry.estimators.LockedException;
21 import com.irurueta.numerical.robust.InliersData;
22
23 import java.util.BitSet;
24 import java.util.List;
25
26 /**
27 * Refines a 3D point by taking into account an initial estimation, inlier
28 * samples and their residuals.
29 * This class can be used to find a solution that minimizes error of inliers in
30 * LMSE terms.
31 * Typically, a refiner is used by a robust estimator, however it can also be
32 * useful in some other situations.
33 *
34 * @param <T> an implementation of a 3D point.
35 */
36 public abstract class Point3DRefiner<T extends Point3D> extends SamplesAndInliersDataRefiner<T, Plane> {
37
38 /**
39 * Standard deviation used for Levenberg-Marquardt fitting during
40 * refinement.
41 * Returned value gives an indication of how much variance each residual
42 * has.
43 * Typically, this value is related to the threshold used on each robust
44 * estimation, since residuals of found inliers are within the range of
45 * such threshold.
46 */
47 private double refinementStandardDeviation;
48
49 /**
50 * Constructor.
51 */
52 protected Point3DRefiner() {
53 }
54
55 /**
56 * Constructor.
57 *
58 * @param initialEstimation initial estimation to be set.
59 * @param keepCovariance true if covariance of estimation must be kept after
60 * refinement, false otherwise.
61 * @param inliers set indicating which of the provided matches are inliers.
62 * @param residuals residuals for matched samples.
63 * @param numInliers number of inliers on initial estimation.
64 * @param samples collection of samples.
65 * @param refinementStandardDeviation standard deviation used for
66 * Levenberg-Marquardt fitting.
67 */
68 protected Point3DRefiner(
69 final T initialEstimation, final boolean keepCovariance, final BitSet inliers, double[] residuals,
70 final int numInliers, final List<Plane> samples, final double refinementStandardDeviation) {
71 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples);
72 this.refinementStandardDeviation = refinementStandardDeviation;
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param initialEstimation initial estimation to be set.
79 * @param keepCovariance true if covariance of estimation must be kept after
80 * refinement, false otherwise.
81 * @param inliersData inlier data, typically obtained from a robust
82 * estimator.
83 * @param samples collection of samples.
84 * @param refinementStandardDeviation standard deviation used for
85 * Levenberg-Marquardt fitting.
86 */
87 protected Point3DRefiner(
88 final T initialEstimation, final boolean keepCovariance, final InliersData inliersData,
89 final List<Plane> samples, final double refinementStandardDeviation) {
90 super(initialEstimation, keepCovariance, inliersData, samples);
91 this.refinementStandardDeviation = refinementStandardDeviation;
92 }
93
94 /**
95 * Gets standard deviation used for Levenberg-Marquardt fitting during
96 * refinement.
97 * Returned value gives an indication of how much variance each residual
98 * has.
99 * Typically, this value is related to the threshold used on each robust
100 * estimation, since residuals of found inliers are within the range of such
101 * threshold.
102 *
103 * @return standard deviation used for refinement.
104 */
105 public double getRefinementStandardDeviation() {
106 return refinementStandardDeviation;
107 }
108
109 /**
110 * Sets standard deviation used for Levenberg-Marquardt fitting during
111 * refinement.
112 * Returned value gives an indication of how much variance each residual
113 * has.
114 * Typically, this value is related to the threshold used on each robust
115 * estimation, since residuals of found inliers are within the range of such
116 * threshold.
117 *
118 * @param refinementStandardDeviation standard deviation used for
119 * refinement.
120 * @throws LockedException if estimator is locked.
121 */
122 public void setRefinementStandardDeviation(final double refinementStandardDeviation) throws LockedException {
123 if (isLocked()) {
124 throw new LockedException();
125 }
126 this.refinementStandardDeviation = refinementStandardDeviation;
127 }
128
129 /**
130 * Computes the residual between a point and a plane as their distance.
131 *
132 * @param point a point.
133 * @param plane a plane.
134 * @return residual (distance between provided point and plane).
135 */
136 protected double residual(final Point3D point, final Plane plane) {
137 point.normalize();
138 plane.normalize();
139 return Math.abs(plane.signedDistance(point));
140 }
141
142 /**
143 * Computes total residual among all provided inlier samples.
144 *
145 * @param point a point.
146 * @return total residual.
147 */
148 protected double totalResidual(final Point3D point) {
149 var result = 0.0;
150
151 final var nSamples = inliers.length();
152 for (var i = 0; i < nSamples; i++) {
153 if (inliers.get(i)) {
154 // sample is inlier
155 final var plane = samples.get(i);
156 result += residual(point, plane);
157 }
158 }
159
160 return result;
161 }
162 }