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.math.Matrix;
011import org.apache.commons.math3.linear.CholeskyDecomposition;
012import org.apache.commons.math3.linear.MatrixUtils;
013import org.apache.commons.math3.linear.RealMatrix;
014
015/**
016 * This class defines methods for estimating the intrinsic camera parameters from multiple homographies. Alternative
017 * versions are provided (only one is actually used though).
018 *
019 * @author WB
020 */
021public class CameraIntrinsicsEstimator {
022
023        /**
024         * Version 1 (Zhang's original closed form solution). Estimates the the intrinsic camera parameters from multiple
025         * homographies.
026         *
027         * @param homographies a set of homography matrices
028         * @return the estimated 3 x 3 intrinsic transformation matrix
029         */
030        protected RealMatrix getCameraIntrinsicsZhang1(RealMatrix[] homographies) {
031                final int M = homographies.length;
032                int rows = 2 * M;
033                double[][] V = new double[rows][];
034
035                for (int i = 0; i < M; i++) {
036                        RealMatrix H = homographies[i];
037                        V[2*i] = getVpq(H, 0, 1); // v01
038                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11
039                }
040
041                if (M == 2) {
042                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };
043                }
044                
045                RealMatrix VM = MatrixUtils.createRealMatrix(V);
046//              MathUtil.print("estimateIntrinsics: V = ", VM);//WB
047                
048                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0
049                
050                final double vc         = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1] * b[1]);
051                final double lambda = b[5] - (b[3] * b[3] + vc * (b[1] * b[3] - b[0] * b[4])) / b[0];
052                final double alpha      = Math.sqrt(lambda / b[0]);
053                final double beta       = Math.sqrt(lambda * b[0] / (b[0] * b[2] - b[1] * b[1]));
054                final double gamma      = -b[1] * alpha * alpha * beta / lambda;
055                final double uc         = gamma * vc / alpha - b[3] * alpha * alpha / lambda;   // PAMI paper = WRONG!
056
057                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
058                                { alpha, gamma, uc },
059                                { 0, beta, vc },
060                                { 0, 0, 1 }
061                });
062
063                return A;
064        }
065
066        /**
067         * Version 2 (Zhang's corrected closed form solution). Estimates the the intrinsic camera parameters from multiple
068         * homographies.
069         *
070         * @param homographies a set of homography matrices
071         * @return the estimated 3 x 3 intrinsic transformation matrix
072         */
073        protected RealMatrix getCameraIntrinsicsZhang2(RealMatrix[] homographies) {
074                final int M = homographies.length;
075                int rows = 2 * M;
076                double[][] V = new double[rows][];
077
078                for (int i = 0; i < M; i++) {
079                        RealMatrix H = homographies[i];
080                        V[2*i] = getVpq(H, 0, 1); // v01
081                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11
082                }
083
084                if (M == 2) {
085                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };
086                }
087                
088                RealMatrix VM = MatrixUtils.createRealMatrix(V);
089                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0
090                
091                final double vc         = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1] * b[1]);
092                final double lambda = b[5] - (b[3] * b[3] + vc * (b[1] * b[3] - b[0] * b[4])) / b[0];
093                final double alpha      = Math.sqrt(lambda / b[0]);
094                final double beta       = Math.sqrt(lambda * b[0] / (b[0] * b[2] - b[1] * b[1]));
095                final double gamma      = -b[1] * alpha * alpha * beta / lambda;
096                final double uc         = gamma * vc / beta - b[3] * alpha * alpha / lambda;    // beta! 1998 report seems correct!
097
098                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
099                                { alpha, gamma, uc },
100                                { 0, beta, vc },
101                                { 0, 0, 1 }
102                });
103
104                return A;
105        }
106
107        /**
108         * Version 3 (WB's closed form solution). Estimates the the intrinsic camera parameters from multiple homographies.
109         *
110         * @param homographies a set of homography matrices
111         * @return the estimated 3 x 3 intrinsic transformation matrix
112         */
113        protected RealMatrix getCameraIntrinsicsZhang3(RealMatrix[] homographies) {
114                final int M = homographies.length;
115                int rows = 2 * M;
116                double[][] V = new double[rows][];
117
118                for (int i = 0; i < M; i++) {
119                        RealMatrix H = homographies[i];
120                        V[2*i] = getVpq(H, 0, 1); // v01
121                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11
122                }
123
124                if (M == 2) {
125                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };
126                }
127                
128                RealMatrix VM = MatrixUtils.createRealMatrix(V);
129                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0
130                
131                final double w = b[0]*b[2]*b[5] - b[1]*b[1]*b[5] - b[0]*b[4]*b[4] + 2*b[1]*b[3]*b[4] - b[2]*b[3]*b[3];
132                final double d = 
133                                b[0] * b[2] - b[1] * b[1];
134                final double uc = 
135                                (b[1] * b[4] - b[2] * b[3]) / d;
136                final double vc = 
137                                (b[1] * b[3] - b[0] * b[4]) / d;
138                final double alpha      = 
139                                Math.sqrt(w / (d * b[0]));
140                final double beta       = 
141                                Math.sqrt(w / (d * d) * b[0]);
142                final double gamma      = 
143                                  Math.sqrt(w / (d * d * b[0])) * b[1];
144                
145                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
146                                { alpha, gamma, uc },
147                                { 0, beta, vc },
148                                { 0, 0, 1 }
149                });
150
151                return A;
152        }
153
154
155        /**
156         * Final version by WB (this version is used by default). Estimates the intrinsic camera parameters from multiple
157         * homographies using a Cholesky decomposition.
158         *
159         * @param homographies a set of homography matrices
160         * @return the estimated 3 x 3 intrinsic transformation matrix
161         */
162        protected RealMatrix getCameraIntrinsics(RealMatrix[] homographies) {
163                final int M = homographies.length;
164                int rows = 2 * M;
165                double[][] V = new double[rows][];
166
167                for (int i = 0; i < M; i++) {
168                        RealMatrix H = homographies[i];
169                        V[2*i] = getVpq(H, 0, 1); // v01
170                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11
171                }
172
173                if (M == 2) {
174                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };
175                }
176                
177                RealMatrix VM = MatrixUtils.createRealMatrix(V);
178//              MathUtil.print("estimateIntrinsics: V = ", VM);//WB
179                
180                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0
181                
182                RealMatrix B = MatrixUtils.createRealMatrix(new double[][]
183                                {{b[0], b[1], b[3]},
184                                 {b[1], b[2], b[4]},
185                                 {b[3], b[4], b[5]}});
186                
187                if (B.getEntry(0, 0) < 0 || B.getEntry(1, 1) < 0 || B.getEntry(2, 2) < 0) {     
188                        B = B.scalarMultiply(-1);       // make sure B is positive definite 
189                }
190                
191                CholeskyDecomposition cd = new CholeskyDecomposition(B);
192                RealMatrix L = cd.getL();
193                RealMatrix A = MatrixUtils.inverse(L).transpose().scalarMultiply(L.getEntry(2, 2));
194                return A;
195        }
196
197//      private double[] getVpq(RealMatrix H, int p, int q) {
198//              H = H.transpose();
199//              final double[] vij = new double[] {
200//                              H.getEntry(p, 0) * H.getEntry(q, 0),
201//                              H.getEntry(p, 0) * H.getEntry(q, 1) + H.getEntry(p, 1) * H.getEntry(q, 0),
202//                              H.getEntry(p, 1) * H.getEntry(q, 1),
203//                              H.getEntry(p, 2) * H.getEntry(q, 0) + H.getEntry(p, 0) * H.getEntry(q, 2),
204//                              H.getEntry(p, 2) * H.getEntry(q, 1) + H.getEntry(p, 1) * H.getEntry(q, 2),
205//                              H.getEntry(p, 2) * H.getEntry(q, 2)
206//              };
207//              return vij;
208//      }
209        
210        // version without transpose
211        private double[] getVpq(RealMatrix H, int p, int q) {
212                final double[] vpq = new double[] {
213                                H.getEntry(0, p) * H.getEntry(0, q),
214                                H.getEntry(0, p) * H.getEntry(1, q) + H.getEntry(1, p) * H.getEntry(0, q),
215                                H.getEntry(1, p) * H.getEntry(1, q),
216                                H.getEntry(2, p) * H.getEntry(0, q) + H.getEntry(0, p) * H.getEntry(2, q),
217                                H.getEntry(2, p) * H.getEntry(1, q) + H.getEntry(1, p) * H.getEntry(2, q),
218                                H.getEntry(2, p) * H.getEntry(2, q)
219                };
220                return vpq;
221        }
222
223}