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.common.geometry.basic.Pnt2d;
010import org.apache.commons.math3.linear.ArrayRealVector;
011import org.apache.commons.math3.linear.DecompositionSolver;
012import org.apache.commons.math3.linear.MatrixUtils;
013import org.apache.commons.math3.linear.RealMatrix;
014import org.apache.commons.math3.linear.RealVector;
015import org.apache.commons.math3.linear.SingularValueDecomposition;
016
017/**
018 * This class defines methods for estimating the radial lens distortion parameters
019 *
020 * @author WB
021 */
022public class RadialDistortionEstimator {
023
024        /**
025         * Estimates the lens distortion from multiple views, starting from an initial (linear) camera model.
026         *
027         * @param cam the initial (linear) camera model
028         * @param views a sequence of extrinsic view transformations
029         * @param modelPts the set of 2D model points (on the planar calibration target)
030         * @param obsPts a sequence of 2D image point sets, one set for each view
031         * @return a vector of lens distortion coefficients
032         */
033        protected double[] estimateLensDistortion(Camera cam, ViewTransform[] views, Pnt2d[] modelPts, Pnt2d[][] obsPts) {
034                final int M = views.length;             // the number of views
035                final int N = modelPts.length;  // the number of model points
036
037                final double uc = cam.getUc();
038                final double vc = cam.getVc();
039
040                RealMatrix D = MatrixUtils.createRealMatrix(2 * M * N, 2);
041                RealVector d = new ArrayRealVector(2 * M * N);
042
043                int l = 0;
044                for (int i = 0; i < M; i++) {
045                        Pnt2d[] obs = obsPts[i];
046
047                        for (int j = 0; j < N; j++) {
048                                // determine the radius in the ideal image plane
049                                double[] xy = cam.projectNormalized(views[i], modelPts[j]);
050                                double x = xy[0];
051                                double y = xy[1];
052                                double r2 = x * x + y * y;
053                                double r4 = r2 * r2;
054                                
055                                // project model point to image
056                                double[] uv = cam.project(views[i], modelPts[j]);
057                                double u = uv[0];
058                                double v = uv[1];
059                                double du = u - uc;     // distance to estim. projection center
060                                double dv = v - vc;
061                                
062                                D.setEntry(l * 2 + 0, 0, du * r2);
063                                D.setEntry(l * 2 + 0, 1, du * r4);
064                                D.setEntry(l * 2 + 1, 0, dv * r2);
065                                D.setEntry(l * 2 + 1, 1, dv * r4);
066                                
067                                // observed image point
068                                Pnt2d UV = obs[j];
069                                double U = UV.getX();
070                                double V = UV.getY();
071                                
072                                d.setEntry(l * 2 + 0, U - u);
073                                d.setEntry(l * 2 + 1, V - v);
074                                l++;
075                        }
076                }
077                
078                DecompositionSolver solver = new SingularValueDecomposition(D).getSolver();
079                RealVector k = solver.solve(d);
080                
081//              double err1 = D.operate(new ArrayRealVector(new double[] {0,0})).subtract(d).getNorm();
082//              double err2 = D.operate(k).subtract(d).getNorm();
083//              System.out.format("err1=%.2f, err2=%.2f \n", err1, err2);
084                
085                return k.toArray();
086        }
087
088}