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.Line2D;
19 import com.irurueta.geometry.PinholeCamera;
20 import com.irurueta.geometry.Plane;
21 import com.irurueta.numerical.robust.InliersData;
22
23 import java.util.BitSet;
24 import java.util.List;
25
26 /**
27 * Base class for a pinhole camera refiner using line/plane correspondences.
28 * Implementations of this class refine a pinhole camera by taking into account
29 * an initial estimation, inlier line/plane matches and their residuals.
30 * This class can be used to find a solution that minimizes error of inliers in
31 * LMSE terms.
32 * Typically, a refiner is used by a robust estimator, however it can also be
33 * useful in some other situations.
34 */
35 public abstract class LinePlaneCorrespondencePinholeCameraRefiner extends PinholeCameraRefiner<Plane, Line2D> {
36
37 /**
38 * Plane to be reused when computing residuals.
39 */
40 private final Plane residualTestPlane = new Plane();
41
42 /**
43 * Constructor.
44 */
45 protected LinePlaneCorrespondencePinholeCameraRefiner() {
46 }
47
48 /**
49 * Constructor.
50 *
51 * @param initialEstimation initial estimation to be set.
52 * @param keepCovariance true if covariance of estimation must be kept after
53 * refinement, false otherwise.
54 * @param inliers set indicating which of the provided matches are inliers.
55 * @param residuals residuals for matched samples.
56 * @param numInliers number of inliers on initial estimation.
57 * @param samples1 1st set of paired samples.
58 * @param samples2 2nd set of paired samples.
59 * @param refinementStandardDeviation standard deviation used for
60 * Levenberg-Marquardt fitting.
61 */
62 protected LinePlaneCorrespondencePinholeCameraRefiner(
63 final PinholeCamera initialEstimation, final boolean keepCovariance,
64 final BitSet inliers, final double[] residuals, final int numInliers,
65 final List<Plane> samples1, final List<Line2D> samples2, final double refinementStandardDeviation) {
66 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2,
67 refinementStandardDeviation);
68 }
69
70 /**
71 * Constructor.
72 *
73 * @param initialEstimation initial estimation to be set.
74 * @param keepCovariance true if covariance of estimation must be kept after
75 * refinement, false otherwise.
76 * @param inliersData inlier data, typically obtained from a robust
77 * estimator.
78 * @param samples1 1st set of paired samples.
79 * @param samples2 2nd set of paired samples.
80 * @param refinementStandardDeviation standard deviation used for
81 * Levenberg-Marquardt fitting.
82 */
83 protected LinePlaneCorrespondencePinholeCameraRefiner(
84 final PinholeCamera initialEstimation, final boolean keepCovariance,
85 final InliersData inliersData, final List<Plane> samples1, final List<Line2D> samples2,
86 final double refinementStandardDeviation) {
87 super(initialEstimation, keepCovariance, inliersData, samples1, samples2, refinementStandardDeviation);
88 }
89
90 /**
91 * Total residual to be used during Powell refinement.
92 * Refinement uses Powell algorithm to minimize a cost function
93 * consisting on the sum of squared projection residuals plus the
94 * suggestion residual for any suggested terms.
95 *
96 * @param pinholeCamera camera to be checked.
97 * @param params camera parameters. In the following order:
98 * skewness, horizontal focal length, vertical focal length,
99 * horizontal principal point, vertical principal point, quaternion A,
100 * quaternion B, quaternion C, quaternion D, center x, center y, center z.
101 * @param weight weight for suggestion residual.
102 * @return total residual during Powell refinement.
103 */
104 protected double residualPowell(final PinholeCamera pinholeCamera, final double[] params, final double weight) {
105 return backprojectionResidual(pinholeCamera) + suggestionResidual(params, weight);
106 }
107
108 /**
109 * Computes total line back-projection residual for provided camera.
110 * This method computes the sum of the squared residuals for all inlier
111 * back-projected lines.
112 *
113 * @param pinholeCamera camera to compute residual for.
114 * @return total back-projection residual.
115 */
116 private double backprojectionResidual(final PinholeCamera pinholeCamera) {
117 pinholeCamera.normalize();
118
119 // back-projection inlier lines into test plane
120 final var nSamples = inliers.length();
121 var residual = 0.0;
122 for (int i = 0; i < nSamples; i++) {
123 if (inliers.get(i)) {
124 final var line = samples2.get(i);
125 final var plane = samples1.get(i);
126
127 line.normalize();
128 plane.normalize();
129
130 residual += Math.pow(singleBackprojectionResidual(pinholeCamera, line, plane), 2.0);
131 }
132 }
133
134 return residual;
135 }
136
137 /**
138 * Computes total residual to be used during Levenberg/Marquard covariance
139 * estimation.
140 *
141 * @param pinholeCamera camera to estimate covariance for.
142 * @param line 2D line to be back-projected with provided pinhole camera.
143 * @param plane plane to be compared with back-projected line.
144 * @param params camera parameters. In the following order:
145 * skewness, horizontal focal length, vertical focal length,
146 * horizontal principal point, vertical principal point, quaternion A,
147 * quaternion B, quaternion C, quaternion D, center x, center y, center z.
148 * @param weight weight for suggestion residual.
149 * @return total residual.
150 */
151 protected double residualLevenbergMarquardt(
152 final PinholeCamera pinholeCamera, final Line2D line, final Plane plane, final double[] params,
153 final double weight) {
154 var residual = singleBackprojectionResidual(pinholeCamera, line, plane);
155 if (hasSuggestions()) {
156 residual += suggestionResidual(params, weight);
157 }
158 return residual;
159 }
160
161 /**
162 * Back-projection residual/error for a single line using provided camera.
163 *
164 * @param pinholeCamera camera ot be checked.
165 * @param line line to be back-projected.
166 * @param plane plane to check against.
167 * @return dot product distance between back-projected line and plane.
168 */
169 @SuppressWarnings("DuplicatedCode")
170 private double singleBackprojectionResidual(final PinholeCamera pinholeCamera, final Line2D line,
171 final Plane plane) {
172 // back-project line into test plane
173 pinholeCamera.backProject(line, residualTestPlane);
174 residualTestPlane.normalize();
175
176 final var dotProduct = Math.abs(plane.getA() * residualTestPlane.getA()
177 + plane.getB() * residualTestPlane.getB()
178 + plane.getC() * residualTestPlane.getC()
179 + plane.getD() * residualTestPlane.getD());
180 return 1.0 - dotProduct;
181 }
182 }