// RK4 integration
void double_integrator_dynamics( double *z, double *f, void *user_data ) {

    // Double integrator dynamics, 2d
    int num_states = 4;
    int num_controls = 2;
    MatrixXd A(num_states, num_states);
    MatrixXd B(num_states, num_controls);
    Vector4d x_t(z[0], z[1], z[2], z[3]);
    Vector2d u_t(z[4], z[5]);
    double t = z[6];
    //double t = ((double*) user_data)[0];

    A.topRightCorner(num_states/2, num_states/2).setIdentity();
    B.bottomRows(num_controls).setIdentity();

    Vector4d k1 = A * x_t + B * u_t;
    Vector4d k2 = A * (x_t + (k1.array()/2.0).matrix()) + B * u_t;
    Vector4d k3 = A * (x_t + (k2.array()/2.0).matrix()) + B * u_t;
    Vector4d k4 = A * (x_t + k3) + B * u_t;

    Vector4d x_new = (x_t.array() + t/6 * (k1.array() + 2*k2.array() + 2*k3.array() + k4.array())).matrix();

    f[0] = x_new(0);
    f[1] = x_new(1);
    f[2] = x_new(2);
    f[3] = x_new(3);

}
Beispiel #2
0
double EW_CHMN::CA_q(QCD::quark q) const
{
    switch (q) {
        case QCD::UP:
            return ( 3.1377 + 0.00014 * x_t() + 0.0041 * x_s());
        case QCD::DOWN:
        case QCD::STRANGE:
            return ( 3.0956 - 0.00015 * x_t() + 0.0019 * x_s());
        case QCD::CHARM:
            return ( 3.1369 + 0.00014 * x_t() + 0.0043 * x_s());
        case QCD::BOTTOM:
            return ( 3.0758 - 0.00015 * x_t() + 0.0028 * x_s());
        case QCD::TOP:
        default:
            throw std::runtime_error("Error in EW_CHMN::CA_q()");
    }
}
    Real HestonRNDCalculator::cdf(Real x, Time t) const {
        return GaussLobattoIntegral(
            maxIntegrationIterations_, 0.1*integrationEps_)(
            boost::bind(&CpxPv_Helper::p0,
                CpxPv_Helper(getHestonParams(hestonProcess_), x_t(x,t),t),_1),
            0.0, 1.0)/M_TWOPI + 0.5;

    }
Beispiel #4
0
double EW_CHMN::gL_q(const QCD::quark q) const
{
    double c1, c2, c3;
    switch (q) {
        case QCD::UP:
        case QCD::CHARM:
            c1 = 0.34675;
            c2 = 0.31309;
            c3 = -0.66793;
            break;
        case QCD::DOWN:
        case QCD::STRANGE:
            c1 = -0.42434;
            c2 = -0.38279;
            c3 = 0.33166;
            break;
        case QCD::BOTTOM:
            c1 = -0.42116;
            c2 = -0.38279;
            c3 = 0.33166;
            c1 += -0.000058 * x_h() + 0.000128 * x_t();
            break;
        case QCD::TOP:
        default:
            throw std::runtime_error("Error in EW_CHMN::gL_q()");
    }

    double deltaGVq = 0.0, deltaGAq = 0.0;
    if (SM.getModelName().compare("StandardModel") != 0) {
        deltaGVq = (dynamic_cast<const NPbase*> (&SM))->deltaGV_f(SM.getQuarks(q));
        deltaGAq = (dynamic_cast<const NPbase*> (&SM))->deltaGA_f(SM.getQuarks(q));
    }

    return ( c1 + c2 * Delta_gbarZ2() + c3 * Delta_sbar2()
            + (deltaGVq + deltaGAq) / 2.0);
}
Beispiel #5
0
template<typename PointSource, typename PointTarget> double
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
                                                                                  double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
                                                                                  PointCloudSource &trans_cloud)
{
  // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
  double phi_0 = -score;
  // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
  double d_phi_0 = -(score_gradient.dot (step_dir));

  Eigen::Matrix<double, 6, 1>  x_t;

  if (d_phi_0 >= 0)
  {
    // Not a decent direction
    if (d_phi_0 == 0)
      return 0;
    else
    {
      // Reverse step direction and calculate optimal step.
      d_phi_0 *= -1;
      step_dir *= -1;

    }
  }

  // The Search Algorithm for T(mu) [More, Thuente 1994]

  int max_step_iterations = 10;
  int step_iterations = 0;

  // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
  double mu = 1.e-4;
  // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
  double nu = 0.9;

  // Initial endpoints of Interval I,
  double a_l = 0, a_u = 0;

  // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
  double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
  double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);

  double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
  double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);

  // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
  bool interval_converged = (step_max - step_min) > 0, open_interval = true;

  double a_t = step_init;
  a_t = std::min (a_t, step_max);
  a_t = std::max (a_t, step_min);

  x_t = x + step_dir * a_t;

  final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
                           Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
                           Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
                           Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();

  // New transformed point cloud
  transformPointCloud (*input_, trans_cloud, final_transformation_);

  // Updates score, gradient and hessian.  Hessian calculation is unessisary but testing showed that most step calculations use the
  // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
  score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);

  // Calculate phi(alpha_t)
  double phi_t = -score;
  // Calculate phi'(alpha_t)
  double d_phi_t = -(score_gradient.dot (step_dir));

  // Calculate psi(alpha_t)
  double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
  // Calculate psi'(alpha_t)
  double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

  // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
  while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
  {
    // Use auxilary function if interval I is not closed
    if (open_interval)
    {
      a_t = trialValueSelectionMT (a_l, f_l, g_l,
                                   a_u, f_u, g_u,
                                   a_t, psi_t, d_psi_t);
    }
    else
    {
      a_t = trialValueSelectionMT (a_l, f_l, g_l,
                                   a_u, f_u, g_u,
                                   a_t, phi_t, d_phi_t);
    }

    a_t = std::min (a_t, step_max);
    a_t = std::max (a_t, step_min);

    x_t = x + step_dir * a_t;

    final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
                             Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
                             Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
                             Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();

    // New transformed point cloud
    // Done on final cloud to prevent wasted computation
    transformPointCloud (*input_, trans_cloud, final_transformation_);

    // Updates score, gradient. Values stored to prevent wasted computation.
    score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);

    // Calculate phi(alpha_t+)
    phi_t = -score;
    // Calculate phi'(alpha_t+)
    d_phi_t = -(score_gradient.dot (step_dir));

    // Calculate psi(alpha_t+)
    psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
    // Calculate psi'(alpha_t+)
    d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

    // Check if I is now a closed interval
    if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
    {
      open_interval = false;

      // Converts f_l and g_l from psi to phi
      f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
      g_l = g_l + mu * d_phi_0;

      // Converts f_u and g_u from psi to phi
      f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
      g_u = g_u + mu * d_phi_0;
    }

    if (open_interval)
    {
      // Update interval end points using Updating Algorithm [More, Thuente 1994]
      interval_converged = updateIntervalMT (a_l, f_l, g_l,
                                             a_u, f_u, g_u,
                                             a_t, psi_t, d_psi_t);
    }
    else
    {
      // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
      interval_converged = updateIntervalMT (a_l, f_l, g_l,
                                             a_u, f_u, g_u,
                                             a_t, phi_t, d_phi_t);
    }

    step_iterations++;
  }

  // If inner loop was run then hessian needs to be calculated.
  // Hessian is unnessisary for step length determination but gradients are required
  // so derivative and transform data is stored for the next iteration.
  if (step_iterations)
    computeHessian (hessian, trans_cloud, x_t);

  return (a_t);
}
Beispiel #6
0
double EW_CHMN::DeltaMw_SM() const
{
    return ( -0.137 * x_h() - 0.019 * x_h() * x_h() + 0.018 * x_t()
            - 0.005 * x_alpha() - 0.002 * x_s());
}
Beispiel #7
0
double EW_CHMN::DeltaTz_SM() const
{
    return ( -0.0995 * x_h() - 0.2858 * x_h() * x_h() + 0.1175 * x_h() * x_h() * x_h()
            + 0.0367 * x_t() + 0.00026 * x_t() * x_t() - 0.0017 * x_h() * x_t()
            - 0.0033 * x_s() - 0.0001 * x_t() * x_s());
}
Beispiel #8
0
double EW_CHMN::DeltaSz_SM() const
{
    return ( 0.2217 * x_h() - 0.1188 * x_h() * x_h() + 0.0320 * x_h() * x_h() * x_h()
            - 0.0014 * x_t() + 0.0005 * x_s());
}
Beispiel #9
0
bool
Date::parseISO_8601(std::string str0)
{
    // ISO 8601 Format: yyyy-mm-ddThh:mm:ss
    double year, month, day, hour;

    // defaults:
    year = 0.;
    month = 1.;
    day = 1.;
    hour=0.;

    Split x_str0(str0, " T", true);

    Split x_d(x_str0[0],"-");

    if( x_d.size() > 2 )
    {
        year  = hdhC::string2Double( x_d[0] );
        month = hdhC::string2Double( x_d[1] );
        day   = hdhC::string2Double( x_d[2] );
    }
    else if( x_d.size() > 1 )
    {
        year  = hdhC::string2Double( x_d[0] );
        month = hdhC::string2Double( x_d[1] );
    }
    else
        year  = hdhC::string2Double( x_d[0] );

    if( x_str0.size() > 1 )
    {
        Split x_t(x_str0[1], ':') ;

        if( x_t.size() > 2 )
        {
            hour  = hdhC::string2Double( x_t[0] );
            hour += hdhC::string2Double( x_t[1] ) / 60.;
            hour += hdhC::string2Double( x_t[2] ) / 3600.;
        }
        else if( x_t.size() > 1 )
        {
            hour  = hdhC::string2Double( x_t[0] );
            hour += hdhC::string2Double( x_t[1] ) / 60.;
        }
        else
            hour  = hdhC::string2Double( x_t[0] );
    }

    if( x_str0.size() == 3 )
    {
        // local time zone
        Split x_t(x_str0[2], ':') ;
        if( x_t.size() > 1 )
        {
            hour += hdhC::string2Double(x_t[0]) ;
            hour += hdhC::string2Double(x_t[1]) / 60. ;  // minutes
        }
        else
            hour += hdhC::string2Double(x_t[0]) ;

    }

    jul = date2Julian( year, month, day, 0., 0., 0.);
    jul += hour/24.;
    isDateSet=true;

    return false ;
}
Beispiel #10
0
void NNLSOptimizer::estimateExpressionParameters(const vector<Point2f> &featurePoints,
                                 const Mat &cameraMatrix, const Mat& lensDist,
                                 Face* face_ptr,const vector<int> &point_indices,
                                 const Mat &rotation, const Mat &translation,
                                 vector<double> &weights_ex)
{
    Mat_<double> weakCamera(2,3);
    //matrices for A_ex * w_ex = f and A_id * w_id = f
    Mat_<double> A_ex, seg_A_ex;
    //matrix Z = kron(weights, Identity_matrix)
    Mat_<double> Z_ex;
    Mat_<double> ZU;
    Mat_<double> pr, Pt;

    //core tensor rows
    Mat_<double> Mi;

    //featurePoints converted to 2x1 matrices
    Mat_<double> fi;
    Mat_<double> f;


    int index = 0;

    const int exr_size = model->getExpSize();
    const int id_size = model->getIdSize();

    double *w_id = new double[id_size];
    double *w_exp = new double[exr_size];

    Mat_<double> rmatrix;
    Mat_<double> x_t(1,exr_size);
    Mat_<double> y_t(1,id_size);
    Mat_<double> x(exr_size,1);
    Mat_<double> y(id_size,1);
    //used for x_t*U_ex and y_t*U_id
    Mat_<double> lin_comb_x(exr_size,1);
    Mat_<double> lin_comb_y(id_size,1);

    double Z_avg;
    double average_depth;
    Point3f p;
    Mat_<double> proP;
    Mat_<double> pM(3,1);

    Rodrigues(rotation,rmatrix);

    //get weights from the current face instance
    face_ptr->getWeights(w_id,id_size,w_exp,exr_size);

    //find average depth
    average_depth = face_ptr->getAverageDepth();    
    pM(0,0) = 1;
    pM(1,0) = 1;
    pM(2,0) = average_depth;
    proP = cameraMatrix*rmatrix*pM;
    proP = proP + cameraMatrix*translation;
    Z_avg = proP(0,2);

    /***************************************************************/
    /*create weak-perspecitve camera matrix from camera matrix*/
    /***************************************************************/
    for(int i=0;i<2;i++)
        for(int j=0;j<3;j++)
            weakCamera(i,j) = cameraMatrix.at<double>(i,j);

    f = Mat_<double>(featurePoints.size()*2,1);
    fi = Mat_<double>(2,1);

    //precompute translation
    Pt = (1./translation.at<double>(2,0)) * (weakCamera*translation);

     //preprocess points and core tensor rows
    //for points subtract translation too
    for(unsigned int i=0;i<featurePoints.size();i++)
    {
        fi = Mat_<double>(2,1);
        fi(0,0) = featurePoints[i].x;
        fi(1,0) = featurePoints[i].y;

        fi = fi - Pt;
        f.row(2*i) = fi.row(0) + 0.0;
        f.row(2*i+1) = fi.row(1) + 0.0;
    }    

    pr = (1.0/Z_avg)*weakCamera*rmatrix;
    A_ex = Mat_<double>::zeros(2*featurePoints.size(),exr_size);
    seg_A_ex = Mat_<double>::zeros(2*featurePoints.size(),exr_size);    

    for(int i=0;i<id_size;i++)
        y_t(0,i) = w_id[i];
    lin_comb_y = (y_t*u_id).t();
    Z_ex = Matrix::kronecker(lin_comb_y,Mat_<double>::eye(exr_size,exr_size));
    ZU = Z_ex*(u_ex.t());

    for(unsigned int i=0;i<point_indices.size();++i)
    {
        index = point_indices[i];
        seg_A_ex = pr*M[index]*ZU;
        A_ex.row(2*i) = seg_A_ex.row(0) + 0.0;
        A_ex.row(2*i+1) = seg_A_ex.row(1) + 0.0;
    }

    //do not use the guess in the first frame but do for every following frame
    for(int i=0;i<exr_size;i++)
        x_t(0,i) = x(i,0) = w_exp[i];

//    Mat_<double> temp = (A_ex*x - f);
//    Mat_<double> e = temp.t()*temp;
//    cout << "exp, error before " << e(0,0)<<endl;
    //op .. lets see if we get here
    this->scannls(A_ex,f,x);
//    temp = (A_ex*x - f);
//    e = temp.t()*temp;
//    cout << "exp, error after " << e(0,0)<<endl;


    for(int i=0;i<exr_size;i++){
        w_exp[i] = x(i,0);
    }

    for(int i=0;i<exr_size;i++){
        weights_ex.push_back(w_exp[i]);
    }

    face_ptr->setNewIdentityAndExpression(w_id,w_exp);
    //unecessary it seems 
    face_ptr->setAverageDepth(average_depth);



    ZU.release();    
    Z_ex.release();
    A_ex.release();
    f.release();
    delete[] w_id;
    delete[] w_exp;
}
Beispiel #11
0
void NNLSOptimizer::estimateModelParameters(const vector<Point2f> &featurePoints,
                                 const Mat &cameraMatrix, const Mat& lensDist,
                                 Face* face_ptr,const vector<int> &point_indices,
                                 const Mat &rotation, const Mat &translation,
                                 vector<double> &weights_id, vector<double> &weights_ex)
{
    Mat_<double> weakCamera(2,3);
    //matrices for A_ex * w_ex = f and A_id * w_id = f
    Mat_<double> A_ex , A_id;
    //matrix Z = kron(weights, Identity_matrix)
    Mat_<double> Z_ex, Z_id;
    Mat_<double> ZU;
    Mat_<double> pr, Pt, PRM, prm;

    //core tensor rows
    Mat_<double> Mi;

    //featurePoints converted to 2x1 matrices
    Mat_<double> fi;
    Mat_<double> f;
    Mat_<double> O;

    int index = 0;
    const int exr_size = model->getExpSize();
    const int id_size = model->getIdSize();

    double *w_id = new double[id_size];
    double *w_exp = new double[exr_size];

    Mat_<double> rmatrix;
    Mat_<double> x_t(1,exr_size);
    Mat_<double> y_t(1,id_size);
    Mat_<double> x(exr_size,1);
    Mat_<double> y(id_size,1);
    //used for x_t*U_ex and y_t*U_id
    Mat_<double> lin_comb_x(exr_size,1);
    Mat_<double> lin_comb_y(id_size,1);

    double Z_avg;
    double average_depth;
    Point3f p;
    Mat_<double> proP;
    Mat_<double> pM(3,1);

    Rodrigues(rotation,rmatrix);

    //get weights from the current face instance
    face_ptr->getWeights(w_id,id_size,w_exp,exr_size);

    average_depth = face_ptr->getAverageDepth();
    pM(0,0) = 1;
    pM(1,0) = 1;
    pM(2,0) = average_depth;    
    proP = cameraMatrix*rmatrix*pM;
    proP = proP + cameraMatrix*translation;
    Z_avg = proP(0,2);

    /***************************************************************/
    /*create weak-perspecitve camera matrix from camera matrix*/
    /***************************************************************/
    for(int i=0;i<2;i++)
        for(int j=0;j<3;j++)
            weakCamera(i,j) = cameraMatrix.at<double>(i,j);

    f = Mat_<double>(featurePoints.size()*2,1);
    fi = Mat_<double>(2,1);

    //precompute translation
    Pt = (1./translation.at<double>(2,0)) * (weakCamera*translation);

    //preprocess points and core tensor rows
    //for points subtract translation too
    for(unsigned int i=0;i<featurePoints.size();i++)
    {        
        fi = Mat_<double>(2,1);
        fi(0,0) = featurePoints[i].x;
        fi(1,0) = featurePoints[i].y;

        fi = fi - Pt;
        f.row(2*i) = fi.row(0) + 0.0;
        f.row(2*i+1) = fi.row(1) + 0.0;
    }

    pr = (1.0/Z_avg)*weakCamera*rmatrix;
    PRM = Mat_<double>(2*featurePoints.size(),exr_size*id_size);
    prm = Mat_<double>(2,exr_size*id_size);

    for(unsigned int i=0;i<point_indices.size();++i)
    {
        index = point_indices[i];        
        prm = pr*M[index];
        PRM.row(2*i) = prm.row(0) + 0.0;
        PRM.row(2*i+1) = prm.row(1) + 0.0;
    }


    //do not use the guess in the first frame but do for every following frame
    for(int i=0;i<exr_size;i++)
        x_t(0,i) = x(i,0) = w_exp[i];
    for(int i=0;i<id_size;i++)
        y_t(0,i) = y(i,0) = w_id[i];

    A_ex = Mat_<double>::zeros(2*featurePoints.size(),exr_size);
    A_id = Mat_<double>::zeros(2*featurePoints.size(),id_size);

    for(int count = 0;count < max_iterations; count++)
    {
        //put the guesses into matrix y and x
        y_t = y.t();

        lin_comb_y = (y_t*u_id).t();

        Z_ex = Matrix::kronecker(lin_comb_y,Mat_<double>::eye(exr_size,exr_size));
        ZU = Z_ex*(u_ex.t());


        A_ex = PRM*ZU;


        this->scannls(A_ex,f,x);

        x_t = x.t();
        lin_comb_x = (x_t*u_ex).t();

        Z_id = Matrix::kronecker(Mat_<double>::eye(id_size,id_size),lin_comb_x);
        ZU = Z_id*(u_id.t());

        /* compute the formula :
         * Sum( U*Z'*Mi'*R'*PW'*PW*R*Mi*Z*U' )*y = Sum( U*Z'*Mi'*R'*PW'*fi - (1/tz)*U*Z'*Mi'*R'*PW'*PW'*t )
         */


        A_id = PRM*ZU;

//        Mat_<double> temp = (A_id*y - f);
//        Mat_<double> e = temp.t()*temp;
//        cout << "id, error before " << e(0,0)<<endl;
        this->scannls(A_id,f,y);
//        temp = (A_id*y - f);
//        e = temp.t()*temp;
//        cout << "id, error after " << e(0,0)<<endl;
    }


    for(int i=0;i<exr_size;i++){
        w_exp[i] = x(i,0);
    }
    for(int i=0;i<id_size;i++){
        w_id[i] = y(i,0);
    }

    //we cannot normalize because then we lose 1/z_avg which is the scale
//    Vector3::normalize(w_exp, exr_size);
//    Vector3::normalize(w_id, id_size);

    for(int i=0;i<exr_size;i++){
        weights_ex.push_back(w_exp[i]);    
    }


    for(int i=0;i<id_size;i++){
        weights_id.push_back(w_id[i]);        
    }

    face_ptr->setNewIdentityAndExpression(w_id,w_exp);    
    face_ptr->setAverageDepth(average_depth);


    prm.release();
    PRM.release();
    ZU.release();
    Z_id.release();
    A_id.release();
    Z_ex.release();
    A_ex.release();
    f.release();
    delete[] w_id;
    delete[] w_exp;
}
Beispiel #12
0
void NNLSOptimizer::estimateIdentityParameters(const vector<vector<Point2f> >&featurePointsVector,
                                 const Mat &cameraMatrix, const Mat& lensDist,
                                 Face* face_ptr,const vector<vector<int> >&point_indices_vector,
                                 const vector<Mat> &rotation, const vector<Mat> &translation,
                                 const vector<vector<double> > &weights_ex,
                                 vector<double> &weights_id)
{
    Mat_<double> weakCamera(2,3);
    //matrices for A_id * w_id = f
    Mat_<double> A_id, seg_A_id;
    //matrix Z = kron(weights, Identity_matrix)
    Mat_<double> Z_id;
    Mat_<double> ZU;
    Mat_<double> pr, Pt;

    //featurePoints converted to 2x1 matrices
    Mat_<double> fi;
    Mat_<double> f;

    int index = 0;
    //total number of feature points
    int featurePointNum = 0;

    const int exr_size = model->getExpSize();
    const int id_size = model->getIdSize();

    double *w_id = new double[id_size];
    double *w_exp = new double[exr_size];

    Mat_<double> rmatrix;
    Mat_<double> x_t(1,exr_size);
    Mat_<double> y_t(1,id_size);
    Mat_<double> x(exr_size,1);
    Mat_<double> y(id_size,1);
    //used for x_t*U_ex and y_t*U_id
    Mat_<double> lin_comb_x(exr_size,1);

    double Z_avg;
    double average_depth;    
    Mat_<double> proP;
    Mat_<double> pM(3,1);


    //get weights from the current face instance
    face_ptr->getWeights(w_id,id_size,w_exp,exr_size);


    /***************************************************************/
    /*create weak-perspecitve camera matrix from camera matrix*/
    /***************************************************************/
    for(int i=0;i<2;i++)
        for(int j=0;j<3;j++)
            weakCamera(i,j) = cameraMatrix.at<double>(i,j);

    for(unsigned int i=0;i<featurePointsVector.size();i++)
        featurePointNum += featurePointsVector[i].size();

    f = Mat_<double>(featurePointNum*2,1);
    fi = Mat_<double>(2,1);


    //preprocess points and core tensor rows
    //for points subtract translation too
    for(unsigned int i=0, count=0;i<featurePointsVector.size();i++)
    {
        //precompute translation
        Pt = (1./translation[i].at<double>(2,0)) * (weakCamera*translation[i]);

        for(unsigned int j=0;j<featurePointsVector[i].size();j++)
        {
            fi = Mat_<double>(2,1);
            fi(0,0) = featurePointsVector[i][j].x;
            fi(1,0) = featurePointsVector[i][j].y;

            fi = fi - Pt;
            f.row(count + 2*j) = fi.row(0) + 0.0;
            f.row(count + 2*j+1) = fi.row(1) + 0.0;
        }
        count += 2*featurePointsVector[i].size();
    }

    A_id = Mat_<double>(2*featurePointNum,id_size);
    seg_A_id = Mat_<double>(2,id_size);

    average_depth = face_ptr->getAverageDepth();

    pM(0,0) = 1;
    pM(1,0) = 1;
    pM(2,0) = average_depth;


    for(unsigned int i=0, count = 0;i<point_indices_vector.size();++i)
    {
        Rodrigues(rotation[i],rmatrix);
        proP = cameraMatrix*rmatrix*pM;
        proP = proP + cameraMatrix*translation[i];
        Z_avg = proP(0,2);

        pr = (1.0/Z_avg)*weakCamera*rmatrix;


        //load the appropriate weights for the i-th frame
        for(unsigned int k=0;k<weights_ex[i].size();k++)
            x_t(0,k) = weights_ex[i][k];
        lin_comb_x = (x_t*u_ex).t();
        Z_id = Matrix::kronecker(Mat_<double>::eye(id_size,id_size),lin_comb_x);
        ZU = Z_id*(u_id.t());

        for(unsigned int j=0;j<point_indices_vector[i].size();++j)
        {
            index = point_indices_vector[i][j];

            seg_A_id = pr*M[index]*ZU;
            A_id.row(count + 2*j) = seg_A_id.row(0) + 0.0;
            A_id.row(count + 2*j+1) = seg_A_id.row(1) + 0.0;
        }
        count += 2*point_indices_vector[i].size();
    }


    //do not use the guess in the first frame but do for every following frame
    for(int i=0;i<id_size;i++)
        y_t(0,i) = y(i,0) = w_id[i];

//    Mat_<double> temp = (A_id*y - f);
//    Mat_<double> e = temp.t()*temp;
//    cout << "id, error before " << e(0,0)<<endl;
    //optimize using sequential coordinate descent
    this->scannls(A_id,f,y);    
//    temp = (A_id*y - f);
//    e = temp.t()*temp;
//    cout << "id, error after " << e(0,0)<<endl;


    for(int i=0;i<id_size;i++){
        w_id[i] = y(i,0);
    }
    for(int i=0;i<exr_size;i++){
        w_exp[i] = weights_ex[0][i];
    }

    for(int i=0;i<id_size;i++){
        weights_id.push_back(w_id[i]);
    }

    face_ptr->setNewIdentityAndExpression(w_id,w_exp);
    face_ptr->setAverageDepth(average_depth);

    ZU.release();
    Z_id.release();
    A_id.release();
    f.release();
    delete[] w_id;
    delete[] w_exp;
}