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.util;
008
009import imagingbook.common.math.Arithmetic;
010import imagingbook.common.math.Matrix;
011import org.apache.commons.math3.geometry.euclidean.threed.NotARotationMatrixException;
012import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
013import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
014import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
015
016import static imagingbook.common.math.Arithmetic.isZero;
017import static imagingbook.common.math.Matrix.add;
018import static imagingbook.common.math.Matrix.idMatrix;
019import static imagingbook.common.math.Matrix.multiply;
020import static imagingbook.common.math.Matrix.normL2;
021import static imagingbook.common.math.Matrix.zeroVector;
022
023/**
024 * This class defines methods for converting between Rodrigues rotation vectors and 3D rotation matrices, plus some
025 * related utility methods. None of these methods is currently used in other parts of the calibration library.
026 */
027public class Rotations {
028
029    private static final double TWO_PI = 2 * Math.PI;
030
031    // ++++++++++++++   Rodrigues vector --> Rotation matrix  +++++++++++++++++++
032
033    /**
034     * Converts a given Rodrigues rotation vector to the equivalent 3D rotation matrix. Hand-made calculation (uses no
035     * library methods).
036     *
037     * @param rv Rodrigues rotation vector
038     * @return the 3D rotation matrix
039     */
040    public static double[][] toRotationMatrix(double[] rv) {
041        double theta = normL2(rv);
042        double rx = rv[0] / theta;
043        double ry = rv[1] / theta;
044        double rz = rv[2] / theta;
045        // System.out.println("rotation angle1 = " + theta);
046        // System.out.println("rotation axis1 = " + Matrix.toString(new double[] {rx, ry, rz}));
047        double[][] W = {
048                {0, -rz, ry},
049                {rz, 0, -rx},
050                {-ry, rx, 0}};
051        // System.out.println("W = \n" + Matrix.toString(W));
052        double[][] I = idMatrix(3);
053        double[][] R1 = add(I, multiply(Math.sin(theta), W));
054        double[][] R2 = multiply(1 - Math.cos(theta), multiply(W, W));
055        double[][] R = add(R1, R2);
056        return R;
057    }
058
059    /**
060     * Converts a given Rodrigues rotation vector to the equivalent 3D rotation matrix. For comparison, this version
061     * uses Apache Commons Math (ACM).
062     *
063     * @param rv Rodrigues rotation vector
064     * @return the 3D rotation matrix
065     */
066    static double[][] toRotationMatrixACM(double[] rv) {
067        double angle = normL2(rv);
068        Vector3D axis = new Vector3D(rv);
069        Rotation rotation = new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR);
070        System.out.println("rotation angle2 = " + rotation.getAngle());
071        System.out.println("rotation axis2 = " + rotation.getAxis(RotationConvention.VECTOR_OPERATOR));
072        return rotation.getMatrix();
073    }
074
075    // ++++++++++++++   Rotation matrix --> Rodrigues vector +++++++++++++++++++
076
077    /**
078     * Converts a 3D rotation matrix (R) to the equivalent Rodrigues rotation vector. From "Vector Representation of
079     * Rotations", Carlo Tomasi (https://www.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.pdf). Matlab code:
080     * http://www.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.m
081     *
082     * @param R a 3D rotation matrix
083     * @return the Rodrigues rotation vector
084     */
085    public static double[] toRodriguesVector(double[][] R) {
086        // final double eps = EPSILON_DOUBLE;
087        double[] p = {
088                0.5 * (R[2][1] - R[1][2]),
089                0.5 * (R[0][2] - R[2][0]),
090                0.5 * (R[1][0] - R[0][1])};
091        double s = normL2(p);
092        double c = 0.5 * (Matrix.trace(R) - 1);
093        if (isZero(s)) {                    // Rotation angle is either 0 or pi
094            if (isZero(c - 1)) {            // Case 1: c = 1, Rotation angle is 0
095                return zeroVector(3);
096            } else if (isZero(c + 1)) {        // Case 2: c = -1, Rotation angle is pi
097                // find the column of R + I with greatest norm (for better numerical results)
098                double[][] Rp = add(R, idMatrix(3));
099                double[] v = getMaxColumnVector(Rp);
100                double vn = normL2(v);
101                if (isZero(vn)) {        // this shouldn't really happen
102                    throw new RuntimeException("R is an inversion, not a rotation");
103                }
104                double[] u = multiply(1 / vn, v);    // unit vector
105                return multiply(Math.PI, normalizeSign(u));
106            } else {                  // how can this be?
107                throw new RuntimeException("sin(theta) is zero, bus cos(theta) is neither 1 nor -1!");
108            }
109        } else {   // (s != 0) : rotation strictly between 0 and pi
110            double[] u = multiply(1.0 / s, p);    // unit vector
111            double theta = Math.atan2(s, c);
112            return multiply(theta, u);
113        }
114    }
115
116    // http://math.stackexchange.com/questions/83874/efficient-and-accurate-numerical-implementation-of-the-inverse-rodrigues-rotatio
117
118    /**
119     * Converts a 3D rotation matrix (R) to the equivalent Rodrigues rotation vector. For comparison, this version uses
120     * Apache Commons Math (ACM).
121     *
122     * @param R
123     * @return
124     */
125    static double[] toRodriguesVectorACM(double[][] R) {
126        Rotation rot = new Rotation(R, 0.01);
127        double angle = rot.getAngle();
128        Vector3D axis = rot.getAxis(RotationConvention.VECTOR_OPERATOR);
129        double[] rv = axis.scalarMultiply(angle / axis.getNorm()).toArray();
130        return rv;
131    }
132
133    /**
134     * Changes the sign of a unit vector u so that it is on the proper half of the unit sphere.
135     *
136     * @param x unit vector
137     * @return same or inverted unit vector
138     */
139    private static double[] normalizeSign(double[] x) {
140        if ((x[0] < 0) ||
141                (isZero(x[0]) && x[1] < 0) ||
142                (isZero(x[0]) && isZero(x[1]) && x[2] < 0)) {
143            return multiply(-1, x);
144        } else {
145            return x;
146        }
147    }
148
149    /**
150     * Returns the column vector of the given matrix with the greatest norm.
151     *
152     * @param A a matrix
153     * @return the maximum-norm column vector
154     */
155    private static double[] getMaxColumnVector(double[][] A) {
156        final int rows = A.length;
157        final int cols = A[0].length;
158        int maxCol = 0;
159        double maxNorm = Double.NEGATIVE_INFINITY;
160        for (int c = 0; c < cols; c++) {
161            double csum = 0;
162            for (int r = 0; r < rows; r++) {
163                csum = csum + A[r][c] * A[r][c];
164            }
165            if (csum > maxNorm) {
166                maxNorm = csum;
167                maxCol = c;
168            }
169        }
170        return Matrix.getColumn(A, maxCol);
171    }
172
173    public static final double DefaultOrthogonalityThreshold = 1e-6;
174
175    /**
176     * Checks if the specified matrix is a rotation matrix under the given orthogonality threshold.
177     *
178     * @param R the matrix to be checked
179     * @param threshold the orthogonality threshold
180     * @return
181     */
182    public static boolean isRotationMatrix(double[][] R, double threshold) {
183        try {
184            Rotation rot = new Rotation(R, threshold);
185        } catch (NotARotationMatrixException e) {
186            return false;
187        }
188        return true;
189    }
190
191    /**
192     * Checks if the specified matrix is a rotation matrix using the default orthogonality threshold
193     * ({@link #DefaultOrthogonalityThreshold}).
194     *
195     * @param R the matrix to be checked
196     * @return
197     */
198    public static boolean isRotationMatrix(double[][] R) {
199        return isRotationMatrix(R, DefaultOrthogonalityThreshold);
200    }
201
202    /**
203     * Normalized the given angle to [-π,π].
204     *
205     * @param angle some angle (any finite value)
206     * @return the equivalent angle in [-π,π]
207     */
208    public static double normalizeAngle(double angle) {
209        // http://commons.apache.org/proper/commons-math/javadocs/api-3.6.1/org/apache/commons/math3/util/MathUtils.html
210        // return MathUtils.normalizeAngle(angle, 0.0);
211        return angle - TWO_PI * Math.floor((angle + Math.PI) / TWO_PI);
212    }
213
214    /**
215     * Creates a Rodrigues rotation vector from a given 3D rotation axis and angle. The angle (normalized to [0,2π])
216     * determines the norm of the resulting vector. Use with care, results are not unique!
217     *
218     * @param axis a 3D vector representing the rotation axis
219     * @param theta the rotation angle
220     * @return
221     */
222    private static double[] makeRodriguesVector(double[] axis, double theta) {
223        double s = normL2(axis);
224        if (Arithmetic.isZero(s)) {
225            throw new IllegalArgumentException("rotation axis vector must have nonzero norm");
226        }
227        return multiply(theta / s, axis);
228    }
229
230}