0

在下面的代码中,我正在尝试实现此处介绍的仿射变换近似算法

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

using namespace Eigen;
using namespace std;

int main(int argc, char **argv)
{
  Vector3f x1 (3.0f, 0.0f, 0.0f);
  Vector3f x2 (0.0f, 2.0f, 0.0f);
  Vector3f x3 (0.0f, 0.0f, 1.0f);

  Vector3f translation(1.0f, -2.0f, 2.0f);

  Vector3f x_bar1 = x1 + translation;
  Vector3f x_bar2 = x2 + translation;
  Vector3f x_bar3 = x3 + translation;

  std::cerr << "x_bar1 = \n" << x_bar1 << std::endl;
  std::cerr << "x_bar2 = \n" << x_bar2 << std::endl;
  std::cerr << "x_bar3 = \n" << x_bar3 << std::endl;

  Vector3f c     = (x1+x2+x3)/3.0f;

  Vector3f c_bar = (x_bar1+x_bar2+x_bar3)/3.0f;

  Vector3f y1,y2,y3, y_bar1,y_bar2,y_bar3;
  y1  = x1 - c;
  y2  = x2 - c;
  y3  = x3 - c;
  y_bar1 = x_bar1 - c_bar;
  y_bar2 = x_bar2 - c_bar;
  y_bar3 = x_bar3 - c_bar;

  Matrix3f H;
  H =  y1*y_bar1.transpose()+y2*y_bar2.transpose()+y3*y_bar3.transpose();

  JacobiSVD<Matrix3f> svd(H, ComputeFullU | ComputeFullV);
  Matrix3f R; R = svd.matrixV()*svd.matrixU().transpose();
  Vector3f t; t = c-R*c_bar;

  std::cerr << "R = \n" << R << std::endl;
  std::cerr << "t = \n" << t << std::endl;
}

但我得到错误的答案:

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

问题出在实现上还是算法上?如果是这样,修正是什么?

4

2 回答 2

0

我将尝试插入从您的原始 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?
*/
}
于 2012-11-04T19:01:47.360 回答
0

您可以使用例如此SVD-demonstration检查您的计算。对于你的例子,我得到

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

所以算法是正确的!可能你的 H 是错误的。

于 2012-11-04T18:26:08.630 回答