001/*******************************************************************************
002 * Permission to use and distribute this software is granted under the BSD 2-Clause
003 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause).
004 * Copyright (c) 2016-2023 Wilhelm Burger. All rights reserved.
005 * Visit https://imagingbook.com for additional details.
006 ******************************************************************************/
007package imagingbook.calibration.zhang;
008
009import imagingbook.calibration.zhang.util.MathUtil;
010import imagingbook.common.geometry.basic.Pnt2d;
011import imagingbook.common.math.Matrix;
012import org.apache.commons.math3.analysis.polynomials.PolynomialFunction;
013import org.apache.commons.math3.analysis.solvers.NewtonRaphsonSolver;
014import org.apache.commons.math3.analysis.solvers.UnivariateDifferentiableSolver;
015import org.apache.commons.math3.linear.MatrixUtils;
016import org.apache.commons.math3.linear.RealMatrix;
017import org.apache.commons.math3.linear.RealVector;
018
019
020/**
021 * A camera model with parameters as specified in Zhang's paper.
022 *
023 * @author WB
024 */
025public class Camera {
026        
027        /**  
028         * The camera's inner transformation matrix:
029         * <pre>
030         * | alpha  gamma  uc |
031         * |     0   beta  vc |
032         * </pre>
033         */
034        private final double[][] A;             // 2 x 3
035        private final double[] K;               // the vector of lens distortion coefficients
036                
037        // for the standard Zhang camera
038        public Camera(double alpha, double beta, double gamma, double uc, double vc, double k0, double k1) {
039                this.A = makeA(alpha, beta, gamma, uc, vc);
040                this.K = new double[] {k0, k1};
041        }
042
043        /**
044         * Creates a standard camera from a vector of intrinsic parameters
045         *
046         * @param s a vector of intrinsic camera parameters (alpha, beta, gamma, uc, vc, k0, k1).
047         */
048        public Camera (double[] s) {
049                //s = (alpha, beta, gamma, uc, vc, k0, k1);     
050                this.A = makeA(s[0], s[1], s[2], s[3], s[4]);
051                this.K = new double[] {s[5], s[6]};
052        }
053
054        /**
055         * Creates a standard camera from a transformation matrix and a vector of lens distortion coefficients.
056         *
057         * @param A the (min.) 2 x 3 matrix holding the intrinsic camera parameters
058         * @param K the radial distortion coefficients k0, k1, ... (may be {@code null})
059         */
060        public Camera(RealMatrix A, double[] K) {
061                this.K = (K == null) ? new double[0] : K.clone();
062                this.A = A.getSubMatrix(0, 1, 0, 2).getData();
063        }
064
065        /**
066         * Creates a simple pinhole camera (with no distortion whatsoever).
067         *
068         * @param f the camera's focal length (in pixel units).
069         * @param uc the x-position of the optical axis intersecting the image plane (in pixel units)
070         * @param vc the y-position of the optical axis intersecting the image plane (in pixel units)
071         */
072        public Camera(double f, double uc, double vc) {
073                this.K = new double[0];
074                this.A = makeA(f, f, 0, uc, vc);
075        }
076        
077        // --------------------------------------------------------------------------
078        
079        private double[][] makeA(double alpha, double beta, double gamma, double uc, double vc) {
080                return new double[][] {
081                                {alpha, gamma, uc},
082                                {    0,  beta, vc}};
083        }
084        
085        // P is assumed to be a X/Y point in the Z = 0 plane
086
087        /**
088         * Projects the X/Y world point (in the Z = 0 plane) to image coordinates under the given camera view (extrinsic
089         * transformation parameters).
090         *
091         * @param view the extrinsic transformation parameters
092         * @param P a single X/Y world point (with Z = 0)
093         * @return the projected 2D image coordinates
094         */
095        public double[] project(ViewTransform view, Pnt2d P) {
096                double[] XY0 = new double[] {P.getX(), P.getY(), 0}; 
097                return this.project(view, XY0);
098        }
099
100        /**
101         * Projects the X/Y world points (all in the Z = 0 plane) to image coordinates under the given camera view
102         * (extrinsic transformation parameters).
103         *
104         * @param view the extrinsic transformation parameters
105         * @param PP a set of X/Y world points (with Z = 0)
106         * @return the projected 2D image coordinates
107         */
108        public Pnt2d[] project(ViewTransform view, Pnt2d[] PP) {
109                Pnt2d[] imagePoints = new Pnt2d[PP.length];
110                for (int j = 0; j < PP.length; j++) {
111                        double[] uv = project(view, PP[j]);
112                        imagePoints[j] = MathUtil.toPnt2d(uv);
113                }
114                return imagePoints;
115        }
116
117        /**
118         * Projects the given 3D point onto the sensor plane of this camera for the provided extrinsic view parameters.
119         *
120         * @param view the extrinsic camera (view) parameters
121         * @param XYZ a point in 3D world coordinates
122         * @return the 2D sensor coordinates of the projected point
123         */
124        public double[] project(ViewTransform view, double[] XYZ) {
125                // map to the ideal projection plane (f = 1)
126                double[] xy = projectNormalized(view, XYZ);
127                // apply radial lens distortion to the ideal projection
128                double[] xyd = warp(xy);
129                // apply the intrinsic camera transformation:
130                double[] uv = mapToSensorPlane(xyd);
131                return uv;
132        }
133
134
135        /**
136         * Projects the given 3D point to ideal projection coordinates for the provided extrinsic view parameters. The world
137         * point is specified as a 2D coordinate in the Z = 0 plane.
138         *
139         * @param view the extrinsic camera (view) parameters
140         * @param P a point in 3D world coordinates (Z = 0)
141         * @return the 2D ideal projection
142         */
143        public double[] projectNormalized(ViewTransform view, Pnt2d P) {
144                double[] XY0 = {P.getX(), P.getY(), 0};
145                return projectNormalized(view, XY0);
146        }
147
148        /**
149         * Projects the given 3D point to ideal projection coordinates for the provided extrinsic view parameters.
150         *
151         * @param view the extrinsic camera (view) parameters
152         * @param XYZ a point in 3D world coordinates
153         * @return the 2D ideal projection
154         */
155        public double[] projectNormalized(ViewTransform view, double[] XYZ) {
156                double[] XYZc = view.applyTo(XYZ);
157                // Compute normalized projection coordinates (f = 1):
158                final double x = XYZc[0] / XYZc[2];
159                final double y = XYZc[1] / XYZc[2];
160                return new double[] {x, y};
161        }
162        
163        // not used in this form, just for symmetry
164        public double warp(double r) {
165                return r * (1 + D(r));
166        }
167
168        /**
169         * Applies radial distortion to a point in the ideal 2D projection.
170         *
171         * @param xy a 2D point in the ideal projection
172         * @return the lens-distorted position in the ideal projection
173         */
174        public double[] warp(double[] xy) {
175                final double x = xy[0];
176                final double y = xy[1];
177                final double r = Math.sqrt(x * x + y * y);
178                double d = (1 + D(r));
179                return new double[] {d * x, d * y};
180        }
181
182        /**
183         * Inverse radial distortion function. Finds the original (undistorted) radius r from the distorted radius R, both
184         * measured from the center = (0,0) of the ideal projection. Finds r as the root of the polynomial
185         * <pre>p(r) = - R + r + k0 * r^3 + k1 * r^5,</pre>
186         * where R is constant, by using a Newton-Raphson solver.
187         *
188         * @param R the distorted radius
189         * @return the undistorted radius
190         */
191        public double unwarp(double R) {
192                double k0 = K[0];
193                double k1 = K[1];
194                double[] coefficients = {-R, 1, 0, k0, 0, k1};
195                PolynomialFunction p = new PolynomialFunction(coefficients);
196                UnivariateDifferentiableSolver solver = new NewtonRaphsonSolver();
197                double rInit = R;
198                int maxEval = 20;
199                double r = solver.solve(maxEval, p, rInit);
200//              System.out.format("** solver iterations = %d\n", solver.getEvaluations());
201                return r;
202        }
203
204        /**
205         * Applies inverse radial distortion to a given point in the ideal image plane.
206         *
207         * @param xyd a distorted 2D point in the ideal image plane
208         * @return the undistorted point
209         */
210        public double[] unwarp(double[] xyd) {
211                final double xd = xyd[0];
212                final double yd = xyd[1];
213                final double R = Math.sqrt(xd * xd + yd * yd);  // distorted radius
214                final double r = unwarp(R);                                             // undistorted radius
215                final double s = r / R;
216                return new double[] {s * xd, s * yd};
217        }
218
219        /**
220         * Radial distortion function, to be applied in the form
221         * <pre>r' = r * (1 + D(r))</pre>
222         * to points in the ideal projection plane. Distortion coefficients k0, k1 are a property of the enclosing
223         * {@link Camera}.
224         *
225         * @param r the original radius of a point in the ideal projection plane
226         * @return the pos/neg deviation for the given radius
227         */
228        public double D(double r) {
229                final double k0 = (K.length > 0) ? K[0] : 0;
230                final double k1 = (K.length > 1) ? K[1] : 0;
231                final double r2 = r * r;
232                return (k0 + k1 * r2) * r2;             // D(r) = k0 * r^2 + k1 * r^4
233        }
234
235        /**
236         * Maps from the ideal projection plane to sensor coordinates, using the camera's intrinsic parameters.
237         *
238         * @param xyd a 2D point on the ideal projection plane
239         * @return the resulting 2D sensor coordinate
240         */
241        public double[] mapToSensorPlane(double[] xyd) {
242                final double x = xyd[0];
243                final double y = xyd[1];
244                final double u = A[0][0] * x + A[0][1] * y + A[0][2];
245                final double v =               A[1][1] * y + A[1][2];
246                return new double[] {u, v};     
247        }
248        
249        // -------------------------------------------------------------------
250
251        /**
252         * Returns the camera's inner parameters as a vector (alpha, beta, gamma, uc, vc, k0, k1).
253         *
254         * @return the camera's inner parameters
255         */
256        public double[] getParameterVector() {
257                return new double[] 
258                                {getAlpha(), getBeta(), getGamma(), getUc(), getVc(), K[0], K[1]};      
259        }
260
261        /**
262         * Returns the camera's alpha value.
263         *
264         * @return alpha
265         */
266        public double getAlpha() {
267                return A[0][0];
268        }
269
270        /**
271         * Returns the camera's beta value.
272         *
273         * @return beta
274         */
275        public double getBeta() {
276                return A[1][1];
277        }
278
279        /**
280         * Returns the camera's gamma value.
281         *
282         * @return gamma
283         */
284        public double getGamma() {
285                return A[0][1];
286        }
287
288        /**
289         * Returns the camera's uc value.
290         *
291         * @return uc
292         */
293        public double getUc() {
294                return A[0][2];
295        }
296
297        /**
298         * Returns the camera's vc value.
299         *
300         * @return vc
301         */
302        public double getVc() {
303                return A[1][2];
304        }
305
306        /**
307         * Returns the camera's lens distortion coefficients.
308         *
309         * @return the vector of lens distortion coefficients
310         */
311        public double[] getK() {
312                return K;
313        }
314
315        /**
316         * Returns a copy of the camera's inner transformation matrix with contents
317         * <pre>
318         *    alpha  gamma  uc
319         *        0   beta  vc
320         * </pre>
321         *
322         * @return the camara's inner transformation matrix (2 x 3)
323         */
324        public RealMatrix getA() {
325                return MatrixUtils.createRealMatrix(A);
326        }
327
328
329        /**
330         * Returns the inverse of the camera intrinsic matrix A as a 3x3 matrix (without the last row {0,0,1}). This version
331         * uses closed form matrix inversion. Used for rectifying images (i.e., removing lens distortion).
332         *
333         * @return the inverse of the camera intrinsic matrix A
334         */
335        public RealMatrix getInverseA() {
336                double alpha = A[0][0];
337                double beta = A[1][1];
338                double gamma = A[0][1];
339                double uc = A[0][2];
340                double vc = A[1][2];
341                double[][] Ai = {
342                        {1.0/alpha, -gamma/(alpha*beta), (gamma*vc - beta*uc)/(alpha*beta)},
343                        {0,         1.0/beta,            -vc/beta}};
344                return MatrixUtils.createRealMatrix(Ai);
345        }
346
347        /**
348         * Returns the homography for the given view as a 3 x 3 matrix.
349         *
350         * @param view the extrinsic view parameters
351         * @return the homography matrix
352         */
353        public RealMatrix getHomography(ViewTransform view) {
354                RealMatrix RT = view.getRotationMatrix();
355                RealVector T = view.getTranslationVector();
356                RT.setColumnVector(2, T);
357                
358                RealMatrix AM = MatrixUtils.createRealMatrix(3, 3); 
359                AM.setSubMatrix(A, 0, 0);
360                AM.setEntry(2, 2, 1);
361                
362                RealMatrix H = AM.multiply(RT); 
363                return H.scalarMultiply(1.0 / H.getEntry(2, 2));
364        }
365        
366        // -------------------------------------------------------------------
367
368        @Override
369        public String toString() {
370                return String.format("%s[alpha=%.4f, beta=%.4f, gamma=%.4f, uc=%.4f, vc=%.4f, K=%s]",
371                                this.getClass().getSimpleName(),
372                                getAlpha(), getBeta(), getGamma(), getUc(), getVc(), Matrix.toString(getK()));
373        }
374        
375        //---------------------------------------------------------------------
376
377//      public static void main(String[] args) {
378//              ViewTransform view = new ViewTransform();
379//
380//              System.out.println("Camera 1:");
381//              Camera camera1 = new Camera (
382//                              832.5, 832.53, 0.204494,        // alpha, beta, gamma,
383//                              303.959, 206.585,                       // c_x, c_y
384//                              -0.228601, 0.190353);           // k0, k1
385//              System.out.format("k0=%.4f, k1=%.4f\n", camera1.getK()[0], camera1.getK()[1]);
386//              double[] XYZ1 = {40, 70, 800};
387//              double[] uv1 = camera1.project(view, XYZ1);
388//              System.out.print(Matrix.toString(XYZ1) + " -> ");
389//              System.out.format("u=%.4f, u=%.4f\n", uv1[0], uv1[1]);
390//
391//              double r = 0.95;
392//              double rr = camera1.warp(r);
393//              System.out.format("radial distortion: r=%.4f -> rr=%.4f\n", r, rr);
394//              r = camera1.unwarp(rr);
395//              System.out.format("inv. radial distortion: rr=%.4f -> r=%.4f\n", rr, r);
396//
397//              System.out.println();
398//
399//              // distorted camera
400//              System.out.println("Camera 2:");
401//              RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
402//                              {832.5, 0.204494, 303.959},
403//                              {  0.0, 832.53, 206.585},
404//                              {  0.0,   0.0,     1.0}});
405//              Camera camera2 = new Camera(A, new double[] {-0.2, 0.190353});
406//              System.out.format("k0=%.4f, k1=%.4f\n", camera2.getK()[0], camera2.getK()[1]);
407//
408//              double[] XYZ2 = {40, 70, 800};
409//              double[] uv2 = camera2.project(view, XYZ2);
410//              System.out.print(Matrix.toString(XYZ2) + " -> ");
411//              System.out.format("u=%.4f, u=%.4f\n", uv2[0], uv2[1]);
412//
413//              r = 0.95;
414//              rr = camera2.warp(r);
415//              System.out.format("radial distortion: r=%.4f -> rr=%.4f\n", r, rr);
416//              r = camera2.unwarp(rr);
417//              System.out.format("inv. radial distortion: rr=%.4f -> r=%.4f\n", rr, r);
418//
419//              System.out.println("\nTesting radial lens distortion:");
420//              double[] xy2 = {0.3, -0.7};
421//              System.out.format("original x=%.4f, y=%.4f\n", xy2[0], xy2[1]);
422//              double[] xy2d = camera2.warp(xy2);
423//              System.out.format("distorted x=%.4f, y=%.4f\n", xy2d[0], xy2d[1]);
424//              double[] xy2u = camera2.unwarp(xy2d);
425//              System.out.format("undistorted x=%.4f, y=%.4f\n", xy2u[0], xy2u[1]);
426//
427//              System.out.println("\nTesting only radial lens distortion fun:");
428//              double ra = 0.10;
429//              double rb = camera2.warp(ra);
430//              double rc = camera2.unwarp(rb);
431//              System.out.format("ra=%.4f, rb=%.4f, rc=%.4f\n", ra, rb, rc);
432//      }
433        
434}