iPhone アプリ研究会

こんにちわ。Uedaです。

Eigenを使ってSVD(特異値分解)をしてみました。

特異値分解 ~wiki
http://ja.wikipedia.org/wiki/特異値分解

    Matrix3d P = Matrix3d::Random(3, 3);
    JacobiSVD svd(P, Eigen::ComputeFullU | Eigen::ComputeFullV);
    MatrixXd S = svd.singularValues();
    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();

コレスキー分解やLU分解も実装されているので, 便利ですね〜。

今回は以上です。

こんにちわ。Uedaです。

前回, 算出した回転行列の直行行列としての性質が保証されていなかったので, QR分解します。

直行行列 ~wiki
http://ja.wikipedia.org/wiki/直交行列

QR分解は行列を直行行列と上三角行列に分解する行列です。なので, 前回算出した回転行列もどきをQR分解して, 直行行列とそうでない行列に分けて, 直行行列の方を回転行列として抽出しようということです。

回転行列もどきRをQR分解して, 直行行列を抽出します。Eigen使ったコードです。

    HouseholderQR QR(R);
    Matrix3d Q = QR.householderQ();

出力結果
QR分解前
0.997029, 0.077192, -0.510096
-0.015336, 1.234692, -0.650774
-0.119096, 0.023709, 0.528783

QR分解後
-0.992825, -0.018295, 0.118166
0.015271, -0.999534, -0.026448
0.118594, -0.024454, 0.992642

直行行列にできたはいいですが, 符号がバラバラになってしまってる...

こんにちわ。Uedaです。

今回は下の資料を参考にカメラ運動パラメータを推定する計算をしてみました。

http://www.ime.info.hiroshima-cu.ac.jp/~hiura/lec/iip/geometry.pdf

最小2乗法の計算はEigenのjacobiSVDを使いました。

    const int DATA_COUNT = 100;
    const double ANS_P11 = 1.0, ANS_P12 = 0.0, ANS_P13 = 0.0, ANS_P14 = 1.0;
    const double ANS_P21 = 0.0, ANS_P22 = 1.0, ANS_P23 = 0.0, ANS_P24 = 1.0;
    const double ANS_P31 = 0.0, ANS_P32 = 0.0, ANS_P33 = 1.0, ANS_P34 = 1.0;
    
    const double NOIZE1 = 1.0;
    
    double *Xa = new double[DATA_COUNT];
    double *Ya = new double[DATA_COUNT];
    double *Za = new double[DATA_COUNT];
    double *ua = new double[DATA_COUNT];
    double *va = new double[DATA_COUNT];
    
    for (int i = 0; i < DATA_COUNT; i++)
    {
        double X = 0.5 - [self random];
        double Y = 0.5 - [self random];
        double Z = 0.5 - [self random];
        double u = (ANS_P11*X + ANS_P12*Y + ANS_P13*Z + ANS_P14) / (ANS_P31*X + ANS_P32*Y + ANS_P33*Z + ANS_P34) + (0.5 - [self random])*NOIZE1;
        double v = (ANS_P21*X + ANS_P22*Y + ANS_P23*Z + ANS_P24) / (ANS_P31*X + ANS_P32*Y + ANS_P33*Z + ANS_P34) + (0.5 - [self random])*NOIZE1;
        Xa[i] = X;
        Ya[i] = Y;
        Za[i] = Z;
        ua[i] = u;
        va[i] = v;
    }
    
    MatrixXd A(DATA_COUNT*2, 11);
    VectorXd b(DATA_COUNT*2);
    
    for (int i = 0, j = 0; j < DATA_COUNT; i+=2, j++)
    {
        b(i)   = ua[j];
        b(i+1) = va[j];
        
        A(i, 0) = Xa[j]; A(i, 1) = Ya[j]; A(i, 2) = Za[j]; A(i, 3) = 1.0;
        A(i, 4) = 0.0;   A(i, 5) = 0.0;   A(i, 6) = 0.0;   A(i, 7) = 0.0;
        A(i, 8) = -Xa[j]*ua[j]; A(i, 9) = -Ya[j]*ua[j]; A(i, 10) = -Za[j]*ua[j];
        
        A(i+1, 0) = 0.0;   A(i+1, 1) = 0.0;   A(i+1, 2) = 0.0;   A(i+1, 3) = 0.0;
        A(i+1, 4) = Xa[j]; A(i+1, 5) = Ya[j]; A(i+1, 6) = Za[j]; A(i+1, 7) = 1.0;
        A(i+1, 8) = -Xa[j]*va[j]; A(i+1, 9) = -Ya[j]*va[j]; A(i+1, 10) = -Za[j]*va[j];
    }
    
    MatrixXd ans = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
    // MatrixXd ans = (A.transpose() * A).inverse() * A.transpose() * b;

    NSLog(@"\n %f, %f, %f, %f \n %f, %f, %f, %f \n %f, %f, %f"
          , ans(0, 0), ans(1, 0), ans(2, 0), ans(3, 0),
          ans(4, 0), ans(5, 0), ans(6, 0), ans(7, 0),
          ans(8, 0), ans(9, 0), ans(10, 0));

正解データ
1.0 0.0 0.0 1.0
0.0 1.0 0.0 1.0
0.0 0.0 1.0 1.0

出力結果
0.830548 -0.178821 -0.476796 1.108733
-0.103757 0.920693 -0.573100 1.011145
-0.072787 -0.137976 0.506778 1.0

前回計算で失敗した原因は, 関数を係数を定数倍しても意味が不変であることを考慮していなかったためだと思います。今回は参考資料して, そこのところを解消しました。
回転行列の性質を満たしていないのでこのまま使うことはできないですが, Eigenの最小2乗法計算手法jacobiSVDのテストも踏まえて計算してみました。

こんにちわ。Uedaです。

今回は ui = (Xi*p11 + Yi*P12 + Zi*P13 + P14) / (Xi*p31 + Yi*P32 + Zi*P33 + P34)
vi = (Xi*p21 + Yi*P22 + Zi*P23 + P24) / (Xi*p31 + Yi*P32 + Zi*P33 + P34)
の関数についてLevenberg-Marquardt法を使ってみます。

ちなみに、この関数は動画像から再投影誤差を最小にすることで、カメラの運動を推定するときに使ったりします。

前回の関数の定義を

    reprojection_functor(int inputs, int values, double *X, double *Y, double *Z, double *u, double *v)
    : inputs_(inputs), values_(values), X(X), Y(Y), Z(Z), u(u), v(v) {}
    
    double *X;
    double *Y;
    double *Z;
    double *u;
    double *v;
    int operator()(const VectorXd& b, VectorXd& fvec) const
    {
        for (int i = 0; i < values_; ++i)
        {
            fvec[i] = u[i] - ((b[0]*X[i] + b[1]*Y[i] + b[2]*Z[i] + b[3]) / (b[8]*X[i] + b[9]*Y[i] + b[10]*Z[i] + b[11]))
                    + v[i] - ((b[4]*X[i] + b[5]*Y[i] + b[6]*Z[i] + b[7]) / (b[8]*X[i] + b[9]*Y[i] + b[10]*Z[i] + b[11]));
        }
        return 0;
    }

に変更して、テストケースを作れば完成です。

テストケース

    const int DATA_COUNT = 100;
    const double ANS_P11 = 1.0, ANS_P12 = 4.0, ANS_P13 = 7.0, ANS_P14 = 10.0;
    const double ANS_P21 = 2.0, ANS_P22 = 5.0, ANS_P23 = 8.0, ANS_P24 = 11.0;
    const double ANS_P31 = 3.0, ANS_P32 = 6.0, ANS_P33 = 9.0, ANS_P34 = 12.0;
    
    const double NOIZE1 = 2.0;
    const double NOIZE2 = 2.0;
    
    double *Xa = new double[DATA_COUNT];
    double *Ya = new double[DATA_COUNT];
    double *Za = new double[DATA_COUNT];
    double *ua = new double[DATA_COUNT];
    double *va = new double[DATA_COUNT];
    
    double p11, p12, p13, p14, p21, p22, p23, p24, p31, p32, p33, p34;
    for (int i = 0; i < DATA_COUNT; i++)
    {
        double X = 0.5 - [self random];
        double Y = 0.5 - [self random];
        double Z = 0.5 - [self random];
        double u = (ANS_P11*X + ANS_P12*Y + ANS_P13*Z) / (ANS_P31*X + ANS_P32*Y + ANS_P33*Z) + (0.5 - [self random])*NOIZE1;
        double v = (ANS_P21*X + ANS_P22*Y + ANS_P23*Z) / (ANS_P31*X + ANS_P32*Y + ANS_P33*Z) + (0.5 - [self random])*NOIZE1;
        Xa[i] = X;
        Ya[i] = Y;
        Za[i] = Z;
        ua[i] = u;
        va[i] = v;
    }
    
    VectorXd p(12);
    p11 = ANS_P11 + (0.5 - [self random])*NOIZE2; p12 = ANS_P12 + (0.5 - [self random])*NOIZE2; p13 = ANS_P13 + (0.5 - [self random])*NOIZE2; p14 = ANS_P14 + (0.5 - [self random])*NOIZE2;
    p21 = ANS_P21 + (0.5 - [self random])*NOIZE2; p22 = ANS_P22 + (0.5 - [self random])*NOIZE2; p23 = ANS_P23 + (0.5 - [self random])*NOIZE2; p24 = ANS_P24 + (0.5 - [self random])*NOIZE2;
    p31 = ANS_P31 + (0.5 - [self random])*NOIZE2; p32 = ANS_P32 + (0.5 - [self random])*NOIZE2; p33 = ANS_P33 + (0.5 - [self random])*NOIZE2; p34 = ANS_P34 + (0.5 - [self random])*NOIZE2;
    p(0) = p11; p(1) = p12; p(2) = p13; p(3) = p14;
    p(4) = p21; p(5) = p22; p(6) = p23; p(7) = p24;
    p(8) = p31; p(9) = p32; p(10) = p33; p(11) = p34;

結果
正解
1.000000, 4.000000, 7.000000, 10.000000
2.000000, 5.000000, 8.000000, 11.000000
3.000000, 6.000000, 9.000000, 12.000000
推定
1.001588, 4.518335, 6.761741, 10.181617
2.237630, 4.923387, 7.199742, 10.156541
3.348539, 6.064186, 9.443100, 12.882333

以上です。

こんにちわ。Uedaです。

前回の補足します。

<問題> z = a / x + b / y + c の非線形の式のa, b, cを、ノイズを乗せたM個のデータxi, yi, zi(0 < i < M)から推定する。
・正解a, b, cはそれぞれ
a = 1.0 ,b = 2.0, c = 3.0
・初期データ_a, _b, _cはそれぞれ
_a = a + noise2, _b = b + noise2, _b = b + noise
・-0.5 < xi < 0.5, -0.5 < yi < 0.5, zi = z + noise1
・-2.5 < noise1 < 2.5
・-50.0 < noise2 < 50.0

をEigenで実装されているLM法を使って推定します。

まず、データを作っていきます。これらは直接計算に関係ありません。計算の準備です。

    const int DATA_COUNT = 100;
    const double ANS_A = 1.0;
    const double ANS_B = 2.0;
    const double ANS_C = 3.0;
    const double NOIZE1 = 5.0;
    const double NOIZE2 = 100.0;

    double *xa = new double[DATA_COUNT];
    double *ya = new double[DATA_COUNT];
    double *za = new double[DATA_COUNT];
    for (int i = 0; i < DATA_COUNT; i++)
    {
        double x = 0.5 - [self random];
        double y = 0.5 - [self random];
        double noize = (0.5 - [self random]) * NOIZE1;
        double z = ANS_A/x + ANS_B/y + ANS_C + noize;
        xa[i] = x;
        ya[i] = y;
        za[i] = z;
    }

初期値を適当に決定します。今回は正解データにnoise2を足します。(ただし, -50.0 < noise2 < 50.0

    VectorXd p(3); // beta1とbeta2の初期値(適当)
    double a = ANS_A + (0.5 - [self random]) * NOIZE2;
    double b = ANS_B + (0.5 - [self random]) * NOIZE2;
    double c = ANS_C + (0.5 - [self random]) * NOIZE2;
    p(0) = a;
    p(1) = b;
    p(2) = c;

関数を定義します。
int operator()(const VectorXd& b, VectorXd& fvec) constの第一引数 bは求めたい係数です。今回ではa, b, cにあたります。初期計算では初期値がこれになります。fvec[i]はあるデータi(xi, yi, zi)に対して, 推定中の関数だとどれだけの誤差があるかを入れるものです。

struct misra1a_functor : Functor
{
・・・
    int operator()(const VectorXd& b, VectorXd& fvec) const
    {
        for (int i = 0; i < values_; ++i) {
            fvec[i] = z[i] - b[0]/x[i] - b[1]/y[i] - b[2]; // fvec[i] = zi - a / xi - b / yi - c
        }
        return 0;
    }
・・・
}

実際に計算します。

    misra1a_functor functor(3, DATA_COUNT, xa, ya, za);
    
    NumericalDiff＜misra1a_functor＞ numDiff(functor);
    LevenbergMarquardt＜NumericalDiff＜misra1a_functor＞＞ lm(numDiff); // 注意 "＜"が全角
    int info = lm.minimize(p);

1行目: 関数を生成します。
2行目: 生成した関数の(遍)微分した形の関数を用意させています。手で関数の微分をしてもいいのですが, 数値微分させて代用しています。
3, 4行目: Eigenのレーベンバーグ・マーカート法を使って計算させる。結果を配列pに出力しています。

以上です。