我将尝试插入从您的原始 C++ 派生的 Java 程序。基于此，我提出以下建议：
1) 我认为你在 t = cR*c_bar; 中切换了 c 和 c_bar;
2) 论文（您可以在http://www.math.pku.edu.cn/teachers/yaoy/Fall2011/arun.pdf免费获得）表明具有共面点 Qi 并不常见，但实际上相当很可能，因为这些点的质心被减去，所以如果你把它们全部加起来，你会得到一个零向量。因此，您很可能会得到反射而不是旋转，并且您需要在等式（18）和我的 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?
*/
}