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.MatrixUtils; 012import org.apache.commons.math3.linear.RealMatrix; 013import org.apache.commons.math3.linear.RealVector; 014 015 016/** 017 * This class defines methods for estimating the extrinsic camera parameters from multiple homographies. 018 * 019 * @author WB 020 */ 021public class ExtrinsicViewEstimator { 022 023 private static boolean beVerbose = false; 024 private final RealMatrix A_inv; 025 026 /** 027 * The only constructor. 028 * 029 * @param A the 3 x 3 matrix with intrinsic camera parameters 030 */ 031 protected ExtrinsicViewEstimator(RealMatrix A) { 032 this.A_inv = MatrixUtils.inverse(A); 033 } 034 035 /** 036 * Estimates the extrinsic camera parameters for a sequence of homographies. 037 * 038 * @param homographies a set of homographies given as 3 x 3 matrices 039 * @return the sequence of extrinsic camera parameters (views), one view for each homography 040 */ 041 protected ViewTransform[] getExtrinsics(RealMatrix[] homographies) { 042 final int M = homographies.length; 043 ViewTransform[] views = new ViewTransform[M]; 044 // ExtrinsicViewEstimator eve = new ExtrinsicViewEstimator(A); 045 for (int i = 0; i < M; i++) { 046 views[i] = estimateViewTransform(homographies[i]); 047 } 048 return views; 049 } 050 051 private ViewTransform estimateViewTransform(RealMatrix H) { 052 RealVector h0 = H.getColumnVector(0); 053 RealVector h1 = H.getColumnVector(1); 054 RealVector h2 = H.getColumnVector(2); 055 056 double lambda = 1 / A_inv.operate(h0).getNorm(); 057 if (beVerbose) { 058 System.out.format("lambda = %f\n", lambda); 059 } 060 061 // compute the columns in the rotation matrix 062 RealVector r0 = A_inv.operate(h0).mapMultiplyToSelf(lambda); 063 RealVector r1 = A_inv.operate(h1).mapMultiplyToSelf(lambda); 064 RealVector r2 = MathUtil.crossProduct3x3(r0, r1); 065 RealVector t = A_inv.operate(h2).mapMultiplyToSelf(lambda); 066 067 if (beVerbose) { 068 System.out.println("r1 = " + Matrix.toString(r0.toArray())); 069 System.out.println("r2 = " + Matrix.toString(r1.toArray())); 070 System.out.println("t = " + Matrix.toString(t.toArray())); 071 } 072 073 RealMatrix R = MatrixUtils.createRealMatrix(3, 3); 074 R.setColumnVector(0, r0); 075 R.setColumnVector(1, r1); 076 R.setColumnVector(2, r2); 077 if (beVerbose) { 078 System.out.println("Rinit = \n" + Matrix.toString(R.getData())); 079 } 080 081 // the R matrix is probably not a real rotation matrix. So find 082 // the closest real rotation matrix (ViewTransform takes care of this): 083 ViewTransform view = new ViewTransform(R, t); 084 return view; 085 } 086 087}