1 /*
2 * Copyright (C) 2015 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.ar.epipolar;
17
18 import com.irurueta.geometry.CoordinatesType;
19 import com.irurueta.geometry.Line2D;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.geometry.estimators.NotReadyException;
22
23 /**
24 * Fixes a single matched pair of points so that they perfectly follow a given
25 * epipolar geometry using the Sampson approximation.
26 * When matching points typically the matching precision is about 1 pixel,
27 * however this makes that matched points under a given epipolar geometry (i.e.
28 * fundamental or essential matrix), do not lie perfectly on the corresponding
29 * epipolar plane or epipolar lines.
30 * The consequence is that triangularization of these matches will fail or
31 * produce inaccurate results.
32 * By fixing matched points using a corrector following a given epipolar
33 * geometry, this effect is alleviated.
34 * This corrector uses the Sampson approximation which is capable to remove
35 * small errors when matches are close to their real epipolar lines. This method
36 * is faster than the Gold standard method, but can only correct small errors
37 * (1 or 2 pixels).
38 */
39 public class SampsonSingleCorrector extends SingleCorrector {
40
41 /**
42 * Constructor.
43 */
44 public SampsonSingleCorrector() {
45 super();
46 }
47
48 /**
49 * Constructor.
50 *
51 * @param fundamentalMatrix fundamental matrix defining the epipolar
52 * geometry.
53 */
54 public SampsonSingleCorrector(final FundamentalMatrix fundamentalMatrix) {
55 super(fundamentalMatrix);
56 }
57
58 /**
59 * Constructor.
60 *
61 * @param leftPoint matched point on left view to be corrected.
62 * @param rightPoint matched point on right view to be corrected.
63 */
64 public SampsonSingleCorrector(final Point2D leftPoint, final Point2D rightPoint) {
65 super(leftPoint, rightPoint);
66 }
67
68 /**
69 * Constructor.
70 *
71 * @param leftPoint matched point on left view to be corrected.
72 * @param rightPoint matched point on right view to be corrected.
73 * @param fundamentalMatrix fundamental matrix defining an epipolar geometry.
74 */
75 public SampsonSingleCorrector(final Point2D leftPoint, final Point2D rightPoint,
76 final FundamentalMatrix fundamentalMatrix) {
77 super(leftPoint, rightPoint, fundamentalMatrix);
78 }
79
80 /**
81 * Corrects the pair of provided matched points to be corrected.
82 *
83 * @throws NotReadyException if this instance is not ready (either points or
84 * fundamental matrix has not been provided yet).
85 */
86 @Override
87 public void correct() throws NotReadyException {
88 if (!isReady()) {
89 throw new NotReadyException();
90 }
91
92 leftCorrectedPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
93 rightCorrectedPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
94 correct(leftPoint, rightPoint, fundamentalMatrix, leftCorrectedPoint, rightCorrectedPoint);
95 }
96
97 /**
98 * Gets type of correction being used.
99 *
100 * @return type of correction.
101 */
102 @Override
103 public CorrectorType getType() {
104 return CorrectorType.SAMPSON_CORRECTOR;
105 }
106
107 /**
108 * Corrects the pair of provided matched points to be corrected using
109 * provided fundamental matrix and stores the corrected points into provided
110 * instances.
111 *
112 * @param leftPoint point on left view to be corrected.
113 * @param rightPoint point on right view to be corrected.
114 * @param fundamentalMatrix fundamental matrix defining the epipolar
115 * geometry.
116 * @param correctedLeftPoint point on left view after correction.
117 * @param correctedRightPoint point on right view after correction.
118 * @throws NotReadyException if provided fundamental matrix is not ready.
119 */
120 public static void correct(final Point2D leftPoint, final Point2D rightPoint,
121 final FundamentalMatrix fundamentalMatrix, final Point2D correctedLeftPoint,
122 final Point2D correctedRightPoint) throws NotReadyException {
123
124 final var leftEpipolarLine = new Line2D();
125 final var rightEpipolarLine = new Line2D();
126 correct(leftPoint, rightPoint, fundamentalMatrix, correctedLeftPoint, correctedRightPoint, leftEpipolarLine,
127 rightEpipolarLine);
128 }
129
130 /**
131 * Corrects the pair of provided matched points to be corrected using
132 * provided fundamental matrix and stores the corrected points into provided
133 * instances. Provided epipolar lines are used for memory efficiency
134 * purposes only, so that those instances can be reused.
135 *
136 * @param leftPoint point on left view to be corrected.
137 * @param rightPoint point on right view to be corrected.
138 * @param fundamentalMatrix fundamental matrix defining the epipolar
139 * geometry.
140 * @param correctedLeftPoint point on left view after correction.
141 * @param correctedRightPoint point on right view after correction.
142 * @param leftEpipolarLine left epipolar line to be reused for memory
143 * efficiency purposes.
144 * @param rightEpipolarLine right epipolar line to be reused for memory
145 * efficiency purposes.
146 * @throws NotReadyException if provided fundamental matrix is not ready.
147 */
148 protected static void correct(
149 final Point2D leftPoint, final Point2D rightPoint, final FundamentalMatrix fundamentalMatrix,
150 final Point2D correctedLeftPoint, final Point2D correctedRightPoint, final Line2D leftEpipolarLine,
151 final Line2D rightEpipolarLine) throws NotReadyException {
152
153 // normalize to increase accuracy
154 leftPoint.normalize();
155 rightPoint.normalize();
156 fundamentalMatrix.normalize();
157
158 fundamentalMatrix.leftEpipolarLine(rightPoint, leftEpipolarLine);
159 fundamentalMatrix.rightEpipolarLine(leftPoint, rightEpipolarLine);
160
161 leftEpipolarLine.normalize();
162 rightEpipolarLine.normalize();
163
164 final var numerator = rightPoint.getHomX() * rightEpipolarLine.getA()
165 + rightPoint.getHomY() * rightEpipolarLine.getB()
166 + rightPoint.getHomW() * rightEpipolarLine.getC();
167
168 final var denominator = Math.pow(rightEpipolarLine.getA(), 2.0)
169 + Math.pow(rightEpipolarLine.getB(), 2.0)
170 + Math.pow(leftEpipolarLine.getA(), 2.0)
171 + Math.pow(leftEpipolarLine.getB(), 2.0);
172
173 final var factor = numerator / denominator;
174
175 final var leftCorrectionX = factor * leftEpipolarLine.getA();
176 final var leftCorrectionY = factor * leftEpipolarLine.getB();
177 final var rightCorrectionX = factor * rightEpipolarLine.getA();
178 final var rightCorrectionY = factor * rightEpipolarLine.getB();
179
180 final var correctedLeftX = leftPoint.getInhomX() - leftCorrectionX;
181 final var correctedLeftY = leftPoint.getInhomY() - leftCorrectionY;
182 final var correctedRightX = rightPoint.getInhomX() - rightCorrectionX;
183 final var correctedRightY = rightPoint.getInhomY() - rightCorrectionY;
184
185 correctedLeftPoint.setInhomogeneousCoordinates(correctedLeftX, correctedLeftY);
186 correctedRightPoint.setInhomogeneousCoordinates(correctedRightX, correctedRightY);
187 }
188 }