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.calibration.estimators;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.ArrayUtils;
20 import com.irurueta.algebra.Matrix;
21 import com.irurueta.algebra.SingularValueDecomposer;
22 import com.irurueta.algebra.Utils;
23 import com.irurueta.geometry.CoordinatesType;
24 import com.irurueta.geometry.GeometryException;
25 import com.irurueta.geometry.MatrixRotation3D;
26 import com.irurueta.geometry.PinholeCamera;
27 import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
28 import com.irurueta.geometry.Point3D;
29 import com.irurueta.geometry.ProjectiveTransformation2D;
30 import com.irurueta.geometry.Rotation3D;
31 import com.irurueta.geometry.Transformation2D;
32
33 /**
34 * Estimates the camera pose for a given homography and pinhole camera intrinsic
35 * parameters.
36 * <p>
37 * This class is used for camera calibration but can also be used for
38 * virtual reality applications
39 * This class assumes that the homography is estimated as the result of
40 * projecting 3D points located in a plane.
41 * <p>
42 * This class is based on technique described at:
43 * Zhengyou Zhang. A Flexible New Technique for Camera Calibration. Technical
44 * Report. MSR-TR-98-71. December 2, 1998
45 */
46 public class CameraPoseEstimator {
47
48 /**
49 * Estimated camera rotation.
50 */
51 private Rotation3D rotation;
52
53 /**
54 * Estimated camera center.
55 */
56 private Point3D cameraCenter;
57
58 /**
59 * Estimated camera.
60 */
61 private PinholeCamera camera;
62
63 /**
64 * Estimates camera posed based on provided intrinsic parameters and
65 * 2D homography.
66 *
67 * @param intrinsic pinhole camera intrinsic parameters.
68 * @param homography a 2D homography.
69 * @throws AlgebraException if provided data is numerically unstable.
70 * @throws GeometryException if a proper camera rotation cannot be found.
71 */
72 public void estimate(final PinholeCameraIntrinsicParameters intrinsic, final Transformation2D homography)
73 throws AlgebraException, GeometryException {
74 rotation = new MatrixRotation3D();
75 cameraCenter = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
76 camera = new PinholeCamera();
77 estimate(intrinsic, homography, rotation, cameraCenter, camera);
78 }
79
80 /**
81 * Returns estimated camera rotation.
82 *
83 * @return estimated camera rotation.
84 */
85 public Rotation3D getRotation() {
86 return rotation;
87 }
88
89 /**
90 * Returns estimated camera center.
91 *
92 * @return estimated camera center.
93 */
94 public Point3D getCameraCenter() {
95 return cameraCenter;
96 }
97
98 /**
99 * Returns estimated camera.
100 *
101 * @return estimated camera.
102 */
103 public PinholeCamera getCamera() {
104 return camera;
105 }
106
107 /**
108 * Estimates camera pose based on provided intrinsic parameters and
109 * 2D homography.
110 *
111 * @param intrinsic pinhole camera intrinsic parameters.
112 * @param homography a 2D homography.
113 * @param estimatedRotation instance where estimated rotation will be stored.
114 * @param estimatedCameraCenter instance where estimated camera center will
115 * be stored.
116 * @param estimatedCamera instance where estimated camera will be stored.
117 * @throws AlgebraException if provided data is numerically unstable.
118 * @throws GeometryException if a proper camera rotation cannot be found.
119 */
120 public static void estimate(
121 final PinholeCameraIntrinsicParameters intrinsic,
122 final Transformation2D homography,
123 final Rotation3D estimatedRotation,
124 final Point3D estimatedCameraCenter,
125 final PinholeCamera estimatedCamera)
126 throws AlgebraException, GeometryException {
127
128 // inverse of intrinsic parameters matrix
129
130 // what follows next is equivalent to Utils.inverse(intrinsic.getInternalMatrix())
131 final var intrinsicInvMatrix = intrinsic.getInverseInternalMatrix();
132
133 if (homography instanceof ProjectiveTransformation2D projectiveTransformation2D) {
134 projectiveTransformation2D.normalize();
135 }
136 final var homographyMatrix = homography.asMatrix();
137
138 final var h1 = homographyMatrix.getSubmatrix(0, 0, 2, 0);
139 final var h2 = homographyMatrix.getSubmatrix(
140 0, 1, 2, 1);
141 final var h3 = homographyMatrix.getSubmatrix(0, 2, 2, 2);
142
143 final var matR1 = intrinsicInvMatrix.multiplyAndReturnNew(h1);
144 final var matR2 = intrinsicInvMatrix.multiplyAndReturnNew(h2);
145 final var matT = intrinsicInvMatrix.multiplyAndReturnNew(h3);
146
147 // because rotation matrices are orthonormal, we find norm of 1st or
148 // 2nd column (both should be equal, except for rounding errors)
149 // to normalize columns 1 and 2.
150 // Because norms might not be equal, we use average or norms of matR1 and
151 // matR2
152 final var norm1 = Utils.normF(matR1);
153 final var norm2 = Utils.normF(matR2);
154 final var invNorm = 2.0 / (norm1 + norm2);
155 matR1.multiplyByScalar(invNorm);
156 matR2.multiplyByScalar(invNorm);
157
158 // also normalize translation term, since pinhole camera is defined
159 // up to scale
160 matT.multiplyByScalar(invNorm);
161
162 final var r1 = matR1.getBuffer();
163 final var r2 = matR2.getBuffer();
164
165 // 3rd column of rotation must be orthogonal to 1st and 2nd columns and
166 // also have norm 1, because rotations are orthonormal
167 final var r3 = Utils.crossProduct(r1, r2);
168 ArrayUtils.normalize(r3);
169
170 final var rot = new Matrix(3, 3);
171 rot.setSubmatrix(0, 0, 2, 0, r1);
172 rot.setSubmatrix(0, 1, 2, 1, r2);
173 rot.setSubmatrix(0, 2, 2, 2, r3);
174
175 // because of noise in data during the estimation of both the homography
176 // and the intrinsic parameters, it might happen that r1 and r2 are not
177 // perfectly orthogonal, for that reason we approximate matrix rot for
178 // the closest orthonormal matrix to ensure that it is a valid rotation
179 // matrix. This is done by obtaining the SVD decomposition and setting
180 // all singular values to one, so that R = U*S*V' = U*V', S = I
181 final var decomposer = new SingularValueDecomposer(rot);
182 decomposer.decompose();
183 final var u = decomposer.getU();
184 final var v = decomposer.getV();
185 v.transpose();
186
187 // U and V are orthonormal, hence U*V' is also orthonormal and can be
188 // considered a rotation (up to sign)
189 u.multiply(v, rot);
190
191 // we need to ensure that rotation has determinant equal to 1, otherwise
192 // we change sign
193 final var det = Utils.det(rot);
194 if (det < 0.0) {
195 rot.multiplyByScalar(-1.0);
196 }
197
198 estimatedRotation.fromMatrix(rot);
199
200 // camera center
201
202 // t = K^-1*h3 = -R*C --> C=-R^-1*t=-R'*t
203 // p4 = -K*R*C = K*t = K*K^-1*h3 = h3 --> C= -(K*R)^-1*h3
204 final var invRot = rot.transposeAndReturnNew();
205 invRot.multiply(matT);
206 invRot.multiplyByScalar(-1.0);
207
208 final var centerBuffer = invRot.getBuffer();
209 estimatedCameraCenter.setInhomogeneousCoordinates(centerBuffer[0], centerBuffer[1], centerBuffer[2]);
210
211 // check that origin of coordinates (which is typically one of the
212 // pattern points) is located in front of the camera, otherwise
213 // fix camera
214 estimatedCamera.setIntrinsicAndExtrinsicParameters(intrinsic, estimatedRotation, estimatedCameraCenter);
215 }
216 }