Affine transformation approximation of two triangles using Eigen SVD

Question 1

You can check your computation using e.g. this SVD-demonstration. For your example I get

H =
6   -2  -1
-2  2.666666667 -0.666666667
-1  -0.666666667    0.666666667

U =
1   0   0
0   0.957092026 -0.289784149
0   -0.289784149    -0.957092026

V =
1   0   0
0   0.957092026 -0.289784149
0   -0.289784149    -0.957092026

R =
1 0 0
0 1 0
0 0 1

So the algorithm is correct! Probably your H is wrong.

Question 2

I will attempt to shove in a Java program derived from your original C++. Based on this I offer the following suggestions:

1) I think you have switched c and c_bar in t = c-R*c_bar;

2) The paper (which you can get freely at http://www.math.pku.edu.cn/teachers/yaoy/Fall2011/arun.pdf) suggests that having coplanar points Qi is uncommon, but in fact it is quite likely, because these points have their centroid subtracted, and so if you add them all up you get a zero vector. Therefore it is entirely likely that you will get a reflection back instead of a rotation and you need to switch column signs as in equation (18) and in my Java.

    package uk.co.demon.mcdowella.apachemathuser;
/*

Affine transformation approximation of two triangles using Eigen SVD


In the code below, I am trying to implement the algorithm for affine transformation approximation presented here
*/

/*
#include <Eigen/Eigen>
#include <iostream>

using namespace Eigen;
using namespace std;
*/
import org.apache.commons.math.linear.ArrayRealVector;
import java.util.Arrays;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.SingularValueDecompositionImpl;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;

public class FindRotation
{
  /** Utility function to return XY' */
  private static RealMatrix xyt(RealVector x, RealVector y)
  {
    Array2DRowRealMatrix xx = new Array2DRowRealMatrix(x.getData());
    return xx.multiply((
      new Array2DRowRealMatrix(y.getData())).transpose());
  }
  // int main(int argc, char **argv)
  public static void main(String[] s)
  {
    // Vector3f x1 (3.0f, 0.0f, 0.0f);
    ArrayRealVector x1 = new ArrayRealVector(new double[]
      {3.0f, 0.0f, 0.0f});
    // Vector3f x2 (0.0f, 2.0f, 0.0f);
    ArrayRealVector x2 = new ArrayRealVector(new double[]
      {0.0f, 2.0f, 0.0f});
    // Vector3f x3 (0.0f, 0.0f, 1.0f);
    ArrayRealVector x3 = new ArrayRealVector(new double[]
      {0.0f, 0.0f, 1.0f});

    // Vector3f translation(1.0f, -2.0f, 2.0f);
    ArrayRealVector translation = new ArrayRealVector(new double[]
      {1.0f, -2.0f, 2.0f});

    Array2DRowRealMatrix rot;
    if (true)
    { // test - do simple rotation
      rot = new Array2DRowRealMatrix(new double[][] {
        new double[] {1.0, 0.0,  0.0},
        new double[] {0.0, 0.0, -1.0},
        new double[] {0.0, 1.0,  0.0},
      });
      System.out.println("Rot determinant is " + rot.getDeterminant());
    }
    else
    {
      rot = new Array2DRowRealMatrix(new double[][] {
        new double[] {1.0, 0.0, 0.0},
        new double[] {0.0, 1.0, 0.0},
        new double[] {0.0, 0.0, 1.0},
      });
    }

    // Vector3f x_bar1 = x1 + translation;
    RealVector x_bar1 = rot.operate(x1).add(translation);
    // Vector3f x_bar2 = x2 + translation;
    RealVector x_bar2 = rot.operate(x2).add(translation);
    // Vector3f x_bar3 = x3 + translation;
    RealVector x_bar3 = rot.operate(x3).add(translation);

    // std::cerr << "x_bar1 = \n" << x_bar1 << std::endl;
    System.out.println("x_bar1 = ");
    System.out.println(x_bar1);
    // std::cerr << "x_bar2 = \n" << x_bar2 << std::endl;
    System.out.println("x_bar2 = ");
    System.out.println(x_bar2);
    // std::cerr << "x_bar3 = \n" << x_bar3 << std::endl;
    System.out.println("x_bar3 = ");
    System.out.println(x_bar3);

    // Vector3f c     = (x1+x2+x3)/3.0f;
    RealVector c     = x1.add(x2).add(x3).mapDivide(3.0f);

    // Vector3f c_bar = (x_bar1+x_bar2+x_bar3)/3.0f;
    RealVector c_bar     =
      x_bar1.add(x_bar2).add(x_bar3).mapDivide(3.0f);

    // Vector3f y1,y2,y3, y_bar1,y_bar2,y_bar3;
    // y1  = x1 - c;
    RealVector y1  = x1.subtract(c);
    // y2  = x2 - c;
    RealVector y2 = x2.subtract(c);
    // y3  = x3 - c;
    RealVector y3 = x3.subtract(c);
    // y_bar1 = x_bar1 - c_bar;
    RealVector y_bar1 = x_bar1.subtract(c_bar);
    // y_bar2 = x_bar2 - c_bar;
    RealVector y_bar2 = x_bar2.subtract(c_bar);
    // y_bar3 = x_bar3 - c_bar;
    RealVector y_bar3 = x_bar3.subtract(c_bar);

    System.out.println("Y1 " + y1 + " (Q1)");
    System.out.println("Y2 " + y2 + " (Q2)");
    System.out.println("Y3 " + y3 + " (Q3)");
    System.out.println("YB1 " + y_bar1);
    System.out.println("YB2 " + y_bar2);
    System.out.println("YB3 " + y_bar3);

    // Matrix3f H;
    // H =  y1*y_bar1.transpose()+y2*y_bar2.transpose()+y3*y_bar3.transpose();
    RealMatrix h = xyt(y1, y_bar1).add(xyt(y2,y_bar2)).add(
      xyt(y3, y_bar3));

    // JacobiSVD<Matrix3f> svd(H, ComputeFullU | ComputeFullV);
    SingularValueDecompositionImpl svd = 
      new SingularValueDecompositionImpl(h);
    System.out.println("Singular values are " + Arrays.toString(svd.getSingularValues()));
    // Matrix3f R; R = svd.matrixV()*svd.matrixU().transpose();
    RealMatrix r = svd.getV().multiply(svd.getUT());
    double rDeterminant = r.getDeterminant();
    System.out.println("Determinant " + rDeterminant);
    if (rDeterminant < 0.0)
    { // coplanar case - which is not surprising because Q in the original paper sum to 0.0
      // because centroid is subtracted from each of them. Try alternate r
      RealMatrix changeLastColumn = new Array2DRowRealMatrix(new double[][] {
        new double[] {1.0, 0.0, 0.0},
        new double[] {0.0, 1.0, 0.0},
        new double[] {0.0, 0.0, -1.0}});
      RealMatrix vd = svd.getV().multiply(changeLastColumn);
      r = vd.multiply(svd.getUT());
      rDeterminant = r.getDeterminant();
      System.out.println("Determinant at second go is " + rDeterminant);
    }
    // Vector3f t; t = c-R*c_bar;
    // Note - original transpose seems to be the wrong way round
    RealVector t = c_bar.subtract(r.operate(c));

    // std::cerr << "R = \n" << R << std::endl;
    System.out.println("R = ");
    System.out.println(r);
    // std::cerr << "t = \n" << t << std::endl;
    System.out.println("t = ");
    System.out.println(t);

    // Apply supposed answer
    RealVector z1 = r.operate(x1).add(t);   
    RealVector z2 = r.operate(x2).add(t);   
    RealVector z3 = r.operate(x3).add(t);   
    System.out.println("Z1  "+ z1);
    System.out.println("Z2  "+ z2);
    System.out.println("Z3  "+ z3);
  }

/*

But I get wrong answer:

R = 
 0.836735 -0.244898 -0.489796
-0.244898  0.632653 -0.734694
-0.489796 -0.734694 -0.469388
t = 
0.142857
3.71429
1.42857

Is the problem in the implementation or in the algorithm? If so, what is the correction?
*/
}