Kabsh.java

/**
 * @author Jonas Devlieghere <[email protected]>
 * @version 1.0
 *
 * The Kabsch algorithm calculates the optimal rotation matrix that minimizes 
 * the RMSD (root mean squared deviation) between two paired sets of points.
 *
 * The algorithm solves the equation Q = P*R + T where R is the rotation and
 * T the translation in order to align data sets P and Q as best as possible.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
public class Kabsch {
    /**
     * Data set P
     */
    SimpleMatrix P;
 
    /**
     * Data set Q
     */
    SimpleMatrix Q;
 
    /**
     * Rotation matrix
     */
    SimpleMatrix rotation;
 
    /**
     * Translation matrix
     */
    SimpleMatrix translation;
 
    public Kabsch(SimpleMatrix P, SimpleMatrix Q) throws IllegalArgumentException {
        if(!isValidInputMatrix(P) || !isValidInputMatrix(Q))
            throw new IllegalArgumentException();
        if(P.numCols() != Q.numCols() || P.numRows() != Q.numRows())
            throw new IllegalArgumentException();
        this.P = P;
        this.Q = Q;
        calculate();
    }
 
    /**
     * Check if the given matrix is a valid matrix for the Kabsch algorithm.
     *
     * @param     M
     *             The matrix to be checked.
     * @return    True if and only if the number of columns is at least two
     *             and the number of rows is at least the number of columns.
     */
    private boolean isValidInputMatrix(SimpleMatrix M) {
        return M.numCols() >= 2 && M.numRows() >= M.numCols();
    }
 
    /**
     * Calculate the covariance matrix for the matrices P and Q.
     *
     * @return    The covariance matrix
     */
    private SimpleMatrix getCovariance(){
        HashMap<String, SimpleMatrix> centroids = getCentroids();
        SimpleMatrix Pcentroid = centroids.get("P");
        SimpleMatrix Qcentroid = centroids.get("Q");
        int n = P.numCols();
        int m = P.numRows();
        SimpleMatrix A = new SimpleMatrix(new double[n][n]);
        for(int j = 0; j < m; j++){
            SimpleMatrix Pj = new SimpleMatrix(1,n);
            SimpleMatrix Qj = new SimpleMatrix(1,n);
            for(int i = 0; i < n; i++){
                Qj.set(0, i, Q.get(j, i) - Qcentroid.get(0,i));
                Pj.set(0, i, P.get(j, i) - Pcentroid.get(0,i));
            }
            SimpleMatrix S = Qj.transpose().mult(Pj);
            A = A.plus(S);
        }
        return A;
    }
 
    /**
     * Compute the Singular Value Decomposition of the Covariance Matrix. Use the
     * results to create the rotation and translation matrix.
     */
    private void calculate(){
        // Calculate Singular Value Decomposition of covariance matrix
        SimpleMatrix A = getCovariance();
        SimpleSVD<SimpleMatrix> svd = new SimpleSVD<SimpleMatrix>(A.getMatrix(),true);
        SimpleMatrix UT = svd.getU().transpose();
        SimpleMatrix V = svd.getV();
 
        // Decide whether we need to correct our rotation matrix to insure a right-handed coordinate system
        SimpleMatrix R = V.mult(UT);
        int d = (int) Math.signum(R.determinant());
 
        // Calculate the optimal rotation matrix
        SimpleMatrix dm = SimpleMatrix.identity(P.numCols());
        dm.set(P.numCols()-1, P.numCols()-1, d);
        this.rotation = V.mult(dm).mult(UT);
 
        // Calculate the translation matrix
        HashMap<String, SimpleMatrix> centroids = getCentroids();
        SimpleMatrix Pcentroid = centroids.get("P");
        SimpleMatrix Qcentroid = centroids.get("Q");
        this.translation = R.mult(Pcentroid.transpose()).scale(-1).plus(Qcentroid.transpose());
    }
 
    /**
     * Compute the centroids for matrix P and Q
     *
     * @return    HashMap containing the centroids for matrix P and Q
     */
    private HashMap<String, SimpleMatrix> getCentroids(){
        HashMap<String, SimpleMatrix> result = new HashMap<String, SimpleMatrix>();
        int n = P.numCols();
        int m = P.numRows();
        SimpleMatrix Pcentroid = new SimpleMatrix(1, n);
        SimpleMatrix Qcentroid = new SimpleMatrix(1, n);
        for(int i = 0; i < n; i++){
            double sumP = 0;
            double sumQ = 0;
            for(int j = 0; j < m; j++){
                sumP += P.get(j, i);
                sumQ += Q.get(j, i);
            }
            Pcentroid.set(0, i, sumP/m);
            Qcentroid.set(0, i, sumQ/m);
        }
        result.put("P", Pcentroid);
        result.put("Q", Qcentroid);
        return result;
    }
 
    /**
     * Get the translation vector.
     *
     * @return    The translation vector.
     */
    public SimpleMatrix getTranslationVector(){
        return this.translation;
    }
 
    /**
     * Get the translation matrix.
     *
     * @return  The translation matrix.
     */
    public SimpleMatrix getTranslationMatrix(){
        double[][] matrix = new double[3][3];
        SimpleMatrix transVec = getTranslationVector();
        for(int i = 0; i < 3; i++){
            for(int j = 0; j < 3; j++){
                matrix[j][i] = transVec.get(j,0);
            }
        }
        return new SimpleMatrix(matrix);
    }
 
    /**
     * Get the rotation matrix
     *
     * @return    The rotation matrix
     */
    public SimpleMatrix getRotation(){
        return this.rotation;
    }
 
}

How do you know that the center of the mass gives the optimal superposition?