コード例 #1
0
ファイル: sparseLM.cpp プロジェクト: CaptainFalco/OpenPilot
 VectorType model(const VectorType& uv, VectorType& x)
 {
   VectorType y; //Change this to use expression template
   int m = Base::values(); 
   int n = Base::inputs();
   eigen_assert(uv.size()%2 == 0);
   eigen_assert(uv.size() == n);
   eigen_assert(x.size() == m);
   y.setZero(m);
   int half = n/2;
   VectorBlock<const VectorType> u(uv, 0, half);
   VectorBlock<const VectorType> v(uv, half, half);
   Scalar coeff;
   for (int j = 0; j < m; j++)
   {
     for (int i = 0; i < half; i++) 
     {
       coeff = (x(j)-i)/v(i);
       coeff *= coeff;
       if (coeff < 1. && coeff > 0.)
         y(j) += u(i)*std::pow((1-coeff), 2);
     }
   }
   return y;
 }