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.CoordinatesType;
19 import com.irurueta.geometry.PinholeCamera;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.geometry.Point3D;
22 import com.irurueta.numerical.robust.InliersData;
23
24 import java.util.BitSet;
25 import java.util.List;
26
27 /**
28 * Base class for a pinhole camera refiner using point correspondences.
29 * Implementations of this class refine a pinhole camera by taking into account
30 * an initial estimation, inlier point matches and their residuals.
31 * This class can be used to find a solution that minimizes error of inliers in
32 * LMSE terms.
33 * Typically, a refiner is used by a robust estimator, however it can also be
34 * useful in some other situations.
35 */
36 public abstract class PointCorrespondencePinholeCameraRefiner extends PinholeCameraRefiner<Point3D, Point2D> {
37
38 /**
39 * Point to be reused when computing residuals.
40 */
41 private final Point2D residualTestPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
42
43 /**
44 * Constructor.
45 */
46 protected PointCorrespondencePinholeCameraRefiner() {
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 samples1 1st set of paired samples.
59 * @param samples2 2nd set of paired samples.
60 * @param refinementStandardDeviation standard deviation used for
61 * Levenberg-Marquardt fitting.
62 */
63 protected PointCorrespondencePinholeCameraRefiner(
64 final PinholeCamera initialEstimation, final boolean keepCovariance,
65 final BitSet inliers, final double[] residuals, final int numInliers, final List<Point3D> samples1,
66 final List<Point2D> samples2, final double refinementStandardDeviation) {
67 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2,
68 refinementStandardDeviation);
69 }
70
71 /**
72 * Constructor.
73 *
74 * @param initialEstimation initial estimation to be set.
75 * @param keepCovariance true if covariance of estimation must be kept after
76 * refinement, false otherwise.
77 * @param inliersData inlier data, typically obtained from a robust
78 * estimator.
79 * @param samples1 1st set of paired samples.
80 * @param samples2 2nd set of paired samples.
81 * @param refinementStandardDeviation standard deviation used for
82 * Levenberg-Marquardt fitting.
83 */
84 protected PointCorrespondencePinholeCameraRefiner(
85 final PinholeCamera initialEstimation, final boolean keepCovariance, final InliersData inliersData,
86 final List<Point3D> samples1, final List<Point2D> samples2, final double refinementStandardDeviation) {
87 super(initialEstimation, keepCovariance, inliersData, samples1, samples2, refinementStandardDeviation);
88 }
89
90 /**
91 * Total residual to be used during Powell refinement.
92 * Powell's 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 projectionResidual(pinholeCamera) + suggestionResidual(params, weight);
106 }
107
108 /**
109 * Computes total point projection residual for provided camera.
110 * This method computes the sum of the squared residuals for all inlier
111 * projected points.
112 *
113 * @param pinholeCamera camera to compute residual for.
114 * @return total projection residual.
115 */
116 private double projectionResidual(final PinholeCamera pinholeCamera) {
117 pinholeCamera.normalize();
118
119 // project inlier 3D points into test point
120 final var nSamples = inliers.length();
121 final var projectedPoint2D = Point2D.create();
122 var residual = 0.0;
123 for (var i = 0; i < nSamples; i++) {
124 if (inliers.get(i)) {
125 final var point3D = samples1.get(i);
126 final var point2D = samples2.get(i);
127
128 point3D.normalize();
129 point2D.normalize();
130
131 pinholeCamera.project(point3D, projectedPoint2D);
132
133 projectedPoint2D.normalize();
134
135 residual += Math.pow(projectedPoint2D.distanceTo(point2D), 2.0);
136 }
137 }
138
139 return residual;
140 }
141
142 /**
143 * Computes total residual to be used during Levenberg/Marquardt covariance
144 * estimation.
145 *
146 * @param pinholeCamera camera to estimate covariance for.
147 * @param point3D 3D point to be projected with provided pinhole camera.
148 * @param point2D 2D point to be compared with projected point.
149 * @param params camera parameters. In the following order:
150 * skewness, horizontal focal length, vertical focal length,
151 * horizontal principal point, vertical principal point, quaternion A,
152 * quaternion B, quaternion C, quaternion D, center x, center y, center z.
153 * @param weight weight for suggestion residual.
154 * @return total residual.
155 */
156 protected double residualLevenbergMarquardt(
157 final PinholeCamera pinholeCamera, final Point3D point3D, final Point2D point2D, final double[] params,
158 final double weight) {
159 var residual = singleProjectionResidual(pinholeCamera, point3D, point2D);
160 if (hasSuggestions()) {
161 residual += suggestionResidual(params, weight);
162 }
163 return residual;
164 }
165
166 /**
167 * Projection residual/error for a single point using provided camera.
168 *
169 * @param pinholeCamera camera to be checked.
170 * @param point3D point to be projected.
171 * @param point2D point to check against.
172 * @return distance between projected point and 2D point.
173 */
174 private double singleProjectionResidual(
175 final PinholeCamera pinholeCamera, final Point3D point3D, final Point2D point2D) {
176 // project point3D into test point
177 pinholeCamera.project(point3D, residualTestPoint);
178
179 // compare test point and 2D point
180 return residualTestPoint.distanceTo(point2D);
181 }
182 }