Eigenを使って非線形最小2乗法 ~Levenberg-Marquardt法 2の修正

こんにちわ。Uedaです。

前回のプログラムに誤りがあったので修正します。

一番はじめの関数定義の部分です。

struct reprojection_functor : Functor
{
    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])) * (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])) * (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 inputs_;
    const int values_;
    int inputs() const { return inputs_; }
    int values() const { return values_; }
};

なお、前回のテストケースでの実験ではなかなかいい結果を得ることができませんでした。ということで、少しのノイズを軽減させ、正解データも変えました。

    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;
    const double NOIZE2 = 1.0;

正しい解に向かっている場合の結果

2013-05-22 17:11:22.906 TestEigen[3914:c07] 正解 
 1.000000, 0.000000, 0.000000, 1.000000 
 0.000000, 1.000000, 0.000000, 1.000000 
 0.000000, 0.000000, 1.000000, 1.000000
2013-05-22 17:11:22.907 TestEigen[3914:c07] 推定前 
 1.383749, -0.266086, 0.055873, 1.341386 
 0.277851, 1.302277, 0.388317, 1.318758 
 -0.484422, -0.412105, 0.862323, 1.235912
2013-05-22 17:11:23.062 TestEigen[3914:c07] 推定後　5 
 1.048085, -0.024046, 0.006782, 0.932006 
 0.143338, 0.929105, -0.020849, 0.913913 
 0.040038, -0.014241, 0.921208, 0.932719

うーん。テストケースをいじっても、3回に1度くらいしか正解らしい結果は得られず、失敗すると大きく誤った結果を返してきます。。

失敗した結果

2013-05-22 17:22:22.372 TestEigen[3956:c07] 正解 
 1.000000, 0.000000, 0.000000, 1.000000 
 0.000000, 1.000000, 0.000000, 1.000000 
 0.000000, 0.000000, 1.000000, 1.000000
2013-05-22 17:22:22.374 TestEigen[3956:c07] 推定前 
 1.151559, 0.088669, -0.113892, 1.024381 
 -0.037348, 0.584864, 0.104453, 1.257278 
 0.182822, 0.103723, 0.785262, 0.762810
2013-05-22 17:22:22.542 TestEigen[3956:c07] 推定後　5 
 88.457310, -9.044245, -13.995677, 84.093021 
 1.123463, 78.481974, -10.546093, 81.876606 
 5.283922, -2.218911, 70.051671, 80.691307