Htykuut

I want to calculate a rigid 3D transformation between two sets of 3D points. I googled myself, found no suitable implementation and implemented it myself with the help of the Apache Commons Math library, based on this guide. The implementation can be seen below:

  public static RigidTransformation3dAnswer computeRigidTransformation3D(RealMatrix src,

      RealMatrix dst) {

    if (src.getRowDimension() == dst.getRowDimension() && src.getColumnDimension() == dst

        .getColumnDimension()) {



      int n = src.getRowDimension();

      RealMatrix centroidSrc = computeCentroid(src);

      RealMatrix centroidDst = computeCentroid(dst);



      RealMatrix aa = src.subtract(tile(centroidSrc, n));

      RealMatrix bb = dst.subtract(tile(centroidDst, n));



      RealMatrix h = aa.transpose().multiply(bb);

      SingularValueDecomposition singularValueDecomposition = new SingularValueDecomposition(h);



      RealMatrix u = singularValueDecomposition.getU();

      RealMatrix vt = singularValueDecomposition.getVT();

      RealMatrix rotationMatrix = vt.transpose().multiply(u.transpose());



      if (new LUDecomposition(rotationMatrix).getDeterminant() < 0) {

        vt.setColumn(2, vt.getColumnVector(2).mapMultiplyToSelf(-1).toArray());

        rotationMatrix = vt.transpose().multiply(u.transpose());

      }



      RealMatrix transpose = (rotationMatrix.scalarMultiply(-1).multiply(centroidSrc.transpose()))

          .add(centroidDst.transpose());



      RigidTransformation3dAnswer answer = new RigidTransformation3dAnswer();

      answer.setRotationMatrix(rotationMatrix);

      answer.setTranslationMatrix(transpose);

      return answer;

    }

    return null;

  }



private static RealMatrix tile(RealMatrix a, int n) {

    RealMatrix realMatrix = new Array2DRowRealMatrix(n, a.getColumnDimension());

    for (int i = 0; i < n; i++) {

      realMatrix.setEntry(i, 0, a.getEntry(0, 0));

      realMatrix.setEntry(i, 1, a.getEntry(0, 1));

      realMatrix.setEntry(i, 2, a.getEntry(0, 2));

    }

    return realMatrix;

  }



private static RealMatrix computeCentroid(RealMatrix mat) {

    double sumX = 0;

    double sumY = 0;

    double sumZ = 0;

    double returnArray = new double[1][3];

    for (int i = 0; i < mat.getRowDimension(); i++) {

      double a = mat.getEntry(i, 0);

      sumX = sumX + a;

      a = mat.getEntry(i, 1);

      sumY = sumY + a;

      a = mat.getEntry(i, 2);

      sumZ = sumZ + a;

    }

    double centroidX = sumX / (double) mat.getRowDimension();

    double centroidY = sumY / (double) mat.getRowDimension();

    double centroidZ = sumZ / (double) mat.getRowDimension();

    returnArray[0][0] = centroidX;

    returnArray[0][1] = centroidY;

    returnArray[0][2] = centroidZ;

    return new Array2DRowRealMatrix(returnArray);

  }

Although this implementation works perfectly, it is not very efficient and takes around 50 ms on desktop, which is not suitable because I want to use it in an Android application.
So here are three question:

a) is there a more efficient library or framework method to use to compute a rigid 3D transformation?

b) If not, is there a heuristic, and therefore more efficient, implementation of such a transformation, if, for example, I exclude the translation and only want rotation around the Z (vertical) axis?

c) If neither of those two points has an answer, is there a way to improve my code efficiency-wise?