// 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);
}
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;
}
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);
}
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);
}
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());
}
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());
}
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());
}
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 ;
}
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;
}
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;
}
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;
}