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.util.ParameterBundle;
012import org.apache.commons.math3.linear.RealMatrix;
013
014import java.util.ArrayList;
015import java.util.List;
016
017
018/**
019 * This is an implementation of the camera calibration method 
020 * described in
021 *   Z. Zhang, "A flexible new technique for camera calibration",
022 *   IEEE Transactions on Pattern Analysis and Machine Intelligence, 
023 *   22(11), pp. 1330-1334, 2000.
024 * See also 
025 * <a href="http://research.microsoft.com/en-us/um/people/zhang/Calib/">
026 * http://research.microsoft.com/en-us/um/people/zhang/Calib/</a>
027 * and
028 * <a href="http://research.microsoft.com/en-us/um/people/zhang/Papers/TR98-71.pdf">
029 * http://research.microsoft.com/en-us/um/people/zhang/Papers/TR98-71.pdf</a>
030 * 
031 * @author W. Burger
032 * @version 2018/12/29
033 */
034public class Calibrator {
035
036        /**
037         * Inner class representing a set of parameters for instantiating new objects of type of {@link Calibrator}.
038         * Parameters can be specified by setting the associated public fields.
039         */
040        public static class Parameters implements ParameterBundle<Calibrator> {
041                /** Normalize point coordinates for numerical stability in {@link HomographyEstimator}. */
042                public boolean normalizePointCoordinates = true;
043                /** Assume that the camera has no skew (currently not used). */
044                public boolean assumeZeroSkew = false;
045                /** Use numeric (instead of analytic) calculation of the Jacobian in {@link NonlinearOptimizer}. */
046                public boolean useNumericJacobian = false;
047                /** Number of lens distortion coefficients (2 = simple polynomial model). */
048                public int lensDistortionKoeffients = 2;
049                /** Turn on debugging output. */
050                public boolean debug = false;                                   
051        }
052        
053        private int M;                                                          // the number of camera views
054        private final Pnt2d[] modelPts;                 // the sequence of 2D points in the planar model
055        private final List<Pnt2d[]> imgPntSet;  // list of vectors containing observed 2D image points for each view
056        
057        private Pnt2d[][] obsPts = null;
058        private final Parameters params;
059        private Camera initCam, finalCam;
060        private ViewTransform[] initViews, finalViews;
061        
062        // ------- constructors ------------------------------
063
064        /**
065         * The only constructor.
066         *
067         * @param params a parameter object (default parameters are used if {@code null} is passed)
068         * @param model a sequence of 2D points specifying the x/y coordinates of the planar calibration pattern (assuming
069         * zero z-coordinates)
070         */
071        public Calibrator(Parameters params, Pnt2d[] model) {
072                this.params = (params != null) ? params : new Parameters();
073                this.modelPts = model;
074                this.imgPntSet = new ArrayList<>();
075        }
076
077        /**
078         * Adds a new observation (a sequence of 2D image points) of the planar calibration pattern.
079         *
080         * @param pts a sequence of 2D image points
081         */
082        public void addView(Pnt2d[] pts) {
083                imgPntSet.add(pts);
084        }
085
086        /**
087         * Performs the actual camera calibration based on the provided sequence of views.
088         *
089         * @return the estimated camera intrinsics as a {@link Camera} object
090         */
091        public Camera calibrate() {
092                M = imgPntSet.size();   // number of views to process
093                if (M < 2) {
094                        throw new IllegalStateException("Calibration: at least two views needed");
095                }
096                
097                obsPts = imgPntSet.toArray(new Pnt2d[0][]);
098                
099                // Step 1: Calculate the homographies for each of the given N views:
100                HomographyEstimator hest = new HomographyEstimator(params.normalizePointCoordinates, true);
101                RealMatrix[] H_init = hest.estimateHomographies(modelPts, obsPts);
102                
103                // Step 2: Estimate the intrinsic parameters by linear optimization:
104                CameraIntrinsicsEstimator cis = new CameraIntrinsicsEstimator();
105                
106                RealMatrix A_init = cis.getCameraIntrinsics(H_init);
107                initCam = new Camera(A_init, new double[params.lensDistortionKoeffients]);
108                
109                // Step 3: calculate the extrinsic view parameters:
110                ExtrinsicViewEstimator eve = new ExtrinsicViewEstimator(A_init);
111                initViews = eve.getExtrinsics(H_init);
112                
113                // Step 4: Determine the lens distortion from initial estimates:
114                RadialDistortionEstimator rde = new RadialDistortionEstimator();
115                double[] distParams = rde.estimateLensDistortion(initCam, initViews, modelPts, obsPts);
116                Camera improvedCam = new Camera(A_init, distParams);
117                
118                // Step 5: Refine all parameters by non-linear optimization
119                NonlinearOptimizer optimizer = (params.useNumericJacobian) ?
120                                new NonlinearOptimizerNumeric(modelPts, obsPts) :
121                                new NonlinearOptimizerAnalytic(modelPts, obsPts);
122                optimizer.optimize(improvedCam, initViews);
123                finalCam = optimizer.getFinalCamera();
124                finalViews = optimizer.getFinalViews();
125                return finalCam;
126        }
127        
128        
129        //---------------------------------------------------------------------------
130        
131        // @SuppressWarnings("unused")
132        // private void printHomographies(RealMatrix[] homographies) {
133        //      int i = 0;
134        //      for (RealMatrix H : homographies) {
135        //              i++;
136        //              System.out.println("Homography " + i + ":");
137        //              System.out.println(Matrix.toString(H.getData()));
138        //      }
139        // }
140
141        /**
142         * Calculates the squared projection error for a single view, associated with a set of observed image points.
143         *
144         * @param cam a camera model (camera intrinsics)
145         * @param view a view transformation (camera extrinsics)
146         * @param observed a set of observed image points
147         * @return the squared projection error (measured in pixel units)
148         */
149    public double getProjectionError(Camera cam, ViewTransform view, Pnt2d[] observed) {
150        double sqError = 0;
151                for (int j = 0; j < modelPts.length; j++) {
152                        double[] uv = cam.project(view, modelPts[j]);
153                        double[] UV = MathUtil.toArray(observed[j]);
154                        double du = uv[0] - UV[0];
155                        double dv = uv[1] - UV[1];
156                        sqError = sqError + du * du + dv * dv;
157                }
158        return sqError;
159    }
160
161        /**
162         * Calculates the squared projection error for a sequence of views, associated with a sequence of observed image
163         * point sets.
164         *
165         * @param cam a camera model (camera intrinsics)
166         * @param views a sequence of view transformations (camera extrinsics)
167         * @param observed a sequence of sets of observed image points
168         * @return the squared projection error (measured in pixel units)
169         */
170    public double getProjectionError(Camera cam, ViewTransform[] views, Pnt2d[][] observed) {
171        double totalError = 0;
172        for (int i = 0; i < views.length; i++) {
173                totalError = totalError + getProjectionError(cam, views[i], observed[i]);
174        }
175        return totalError;
176    }
177    
178    // ----------------------------------------------------------------------
179
180        /**
181         * Returns the initial camera model (no lens distortion).
182         *
183         * @return the initial camera model
184         */
185    public Camera getInitialCamera() {
186        return initCam;
187    }
188
189        /**
190         * Returns the final camera model (including lens distortion).
191         *
192         * @return the final camera model
193         */
194    public Camera getFinalCamera() {
195        return finalCam;
196    }
197
198        /**
199         * Returns the sequence of initial camera views (extrinsics, no lens distortion).
200         *
201         * @return the sequence of initial camera views
202         */
203    public ViewTransform[] getInitialViews() {
204        return initViews;
205    }
206
207        /**
208         * Returns the sequence of final camera views (extrinsics, including lens distortion).
209         *
210         * @return the sequence of final camera views
211         */
212    public ViewTransform[] getFinalViews() {
213        return finalViews;
214    }
215    
216}