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.math.Matrix;
010import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
011import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
012import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
013import org.apache.commons.math3.linear.MatrixUtils;
014import org.apache.commons.math3.linear.RealMatrix;
015import org.apache.commons.math3.linear.RealVector;
016
017import java.io.StringWriter;
018import java.util.Arrays;
019
020/**
021 * Instances of this class represent extrinsic camera (view) parameters.
022 *
023 * @author WB
024 */
025public class ViewTransform {
026        
027        private static final double OrthogonalityThreshold = 0.01;
028        private final Rotation rotation;
029        private final double[] translation;
030        
031        // ----------------------------------------------------------------------------------
032        
033        public ViewTransform() {
034                this.rotation = Rotation.IDENTITY;
035                this.translation = new double[3];
036        }
037        
038        public ViewTransform(double rX, double rY, double rZ, double tX, double tY, double tZ) {
039                this.rotation = makeRotation(new double[] {rX, rY, rZ});
040                this.translation = new double[] {tX, tY, tZ};
041        }
042        
043        public ViewTransform(Rotation rot, double[] t) {
044                this.rotation = rot;
045                translation = t;
046        }
047        
048        public ViewTransform(RealMatrix RT) {   // RT is of size 3 x 4 (a homography)
049                if (RT.getRowDimension() != 3 || RT.getColumnDimension() != 4) {
050                        throw new IllegalArgumentException("View transform matrix must be 3 x 4");
051                }
052                RealMatrix R = RT.getSubMatrix(0, 2, 0, 2);
053                rotation = new Rotation(R.getData(), OrthogonalityThreshold);
054                translation = RT.getColumnVector(3).toArray();
055        }
056        
057        public ViewTransform(RealMatrix R, RealVector t) {      // R is of size 3 x 3 , t of size 3 x 1
058                this(new Rotation(R.getData(), OrthogonalityThreshold), t.toArray());
059        }
060        
061        public ViewTransform(double[] w) {
062                this.rotation = makeRotation(w);
063                this.translation = Arrays.copyOfRange(w, 3, 6);
064        }
065        
066        // ----------------------------------------------------------------------------------
067        
068        private Rotation makeRotation(double[] w) {
069                Vector3D axis = new Vector3D(w[0], w[1], w[2]);
070                double angle = axis.getNorm();
071                //return new Rotation(axis, angle);
072                return new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR);
073        }
074        
075        protected double[] getParameters() {
076                //double[] rotAxis = rotation.getAxis().toArray();
077                double[] rotAxis = rotation.getAxis(RotationConvention.VECTOR_OPERATOR).toArray();
078                double rotAngle = rotation.getAngle();
079                return new double[] {
080                        rotAxis[0] * rotAngle,
081                        rotAxis[1] * rotAngle,
082                        rotAxis[2] * rotAngle,
083                        translation[0], translation[1], translation[2]};
084        }
085        
086        public Rotation getRotation() {
087                return rotation;
088        }
089        
090        public double[] getRotationAxis() {
091                //double[] rotAxis = rotation.getAxis().toArray();
092                double[] rotAxis = rotation.getAxis(RotationConvention.VECTOR_OPERATOR).toArray();
093                double rotAngle = rotation.getAngle();
094                rotAxis[0] *= rotAngle;
095                rotAxis[1] *= rotAngle;
096                rotAxis[2] *= rotAngle;
097                return rotAxis;
098        }
099        
100        public RealMatrix getRotationMatrix() {
101                double[][] R = rotation.getMatrix();
102                return MatrixUtils.createRealMatrix(R);
103        }
104        
105        public double[] getTranslation() {
106                return translation;
107        }
108        
109        public RealVector getTranslationVector() {
110                return MatrixUtils.createRealVector(translation);
111        }
112        
113        // ----------------------------------------------------------------------------------
114
115        /**
116         * Moves point XYZ from 3D world coordinates to 3D camera coordinates, as specified by the transformations of this
117         * view. No projection is involved.
118         *
119         * @param XYZ a 3D (world) point
120         * @return the given world point mapped to 3D camera coordinates
121         */
122        protected double[] applyTo(double[] XYZ) {      // 3D vector XYZ assumed
123                double[] XYZc = new double[3];
124                rotation.applyTo(XYZ, XYZc);
125                for (int i = 0; i < 3; i++) {
126                        XYZc[i] = XYZc[i] + translation[i];
127                }
128                return XYZc;
129        }
130        
131        public String toString() {
132                RealMatrix R = this.getRotationMatrix();
133                RealVector T = this.getTranslationVector();
134                StringWriter writer = new StringWriter();
135                writer.append("R = \n");
136                writer.append(Matrix.toString(R.getData()));
137                writer.append("\n");
138                writer.append("T = ");
139                writer.append(Matrix.toString(T.toArray()));
140                return writer.toString();
141        }
142
143}