in visualization/FitAdam/src/HandFastCost.cpp [5:430]
bool HandFastCost::Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const
{
using namespace Eigen;
typedef double T;
const double* t = parameters[0];
const double* p_euler = parameters[1];
const double* c = parameters[2];
MatrixXdr outJ(smpl::HandModel::NUM_JOINTS + (fit_surface ? surface_constraints.cols() : 0), 3);
MatrixXdr dJdP(smpl::HandModel::NUM_JOINTS * 3 + 3 * (fit_surface ? surface_constraints.cols() : 0), smpl::HandModel::NUM_JOINTS * 3);
MatrixXdr dJdc(smpl::HandModel::NUM_JOINTS * 3 + 3 * (fit_surface ? surface_constraints.cols() : 0), smpl::HandModel::NUM_JOINTS * 3);
Map< const Vector3d > t_vec(t);
// First Step: Compute Current Joint;
ForwardKinematics(p_euler, c, outJ.data(), jacobians? dJdP.data():nullptr, jacobians? dJdc.data():nullptr);
outJ.rowwise() += t_vec.transpose();
MatrixXdr outProj(outJ.rows(), 3);
const Eigen::Map<const Eigen::Matrix<double, 3, 3>> Km(K_); // map to a Column Major matrix (equivalent to transpose already)
if (fit2d_) outProj = (SMPL_VIS_SCALING * outJ) * Km;
// Second step: Set the residual
const double* ptarget = HandJoints_.data();
const double* psurface = (fit_surface ? surface_constraints.data() : nullptr);
Map< VectorXd > res(residuals, m_nResiduals);
for (int i = 0; i < smpl::HandModel::NUM_JOINTS; i++)
{
const int idj = handm_.m_jointmap_pm2model(i);
if (fit3d_)
{
if(ptarget[5 * i + 0] == 0.0 && ptarget[5 * i + 1] == 0.0 && ptarget[5 * i + 2] == 0.0)
{
residuals[res_dim * idj + 0] = 0.0;
residuals[res_dim * idj + 1] = 0.0;
residuals[res_dim * idj + 2] = 0.0;
}
else
{
residuals[res_dim * idj + 0] = weight_joints[i] * (outJ(idj, 0) - ptarget[5 * i + 0]);
residuals[res_dim * idj + 1] = weight_joints[i] * (outJ(idj, 1) - ptarget[5 * i + 1]);
residuals[res_dim * idj + 2] = weight_joints[i] * (outJ(idj, 2) - ptarget[5 * i + 2]);
}
}
if (fit2d_)
{
if(ptarget[5 * i + 3] == 0.0 && ptarget[5 * i + 4] == 0.0)
{
residuals[res_dim * idj + start_2d_dim + 0] = 0.0;
residuals[res_dim * idj + start_2d_dim + 1] = 0.0;
}
else
{
// residuals[res_dim * idj + start_2d_dim + 0] = weight_2d * weight_joints[i] * ((outProj(idj, 0) / outProj(idj, 2) - ptarget[5 * i + 3]));
// residuals[res_dim * idj + start_2d_dim + 1] = weight_2d * weight_joints[i] * ((outProj(idj, 1) / outProj(idj, 2) - ptarget[5 * i + 4]));
residuals[res_dim * idj + start_2d_dim + 0] = weight_2d[0] * weight_joints[i] * ((outProj(idj, 0) / outProj(idj, 2) - ptarget[5 * i + 3]));
residuals[res_dim * idj + start_2d_dim + 1] = weight_2d[1] * weight_joints[i] * ((outProj(idj, 1) / outProj(idj, 2) - ptarget[5 * i + 4]));
}
}
}
if (fit_surface)
{
// std::cout << surface_constraints << std::endl;
// std::cout << outJ << std::endl;
// exit(0);
for (int i = 0; i < surface_constraints.cols(); i++)
{
if (fit3d_)
{
if (psurface[6 * i + 0] == 0.0 && psurface[6 * i + 1] == 0.0 && psurface[6 * i + 2] == 0.0)
{
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + 0] = 0.0;
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + 1] = 0.0;
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + 2] = 0.0;
}
else
{
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + 0] = weight_vertex.at(i) * (outJ(smpl::HandModel::NUM_JOINTS + i, 0) - psurface[6 * i + 0]);
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + 1] = weight_vertex.at(i) * (outJ(smpl::HandModel::NUM_JOINTS + i, 1) - psurface[6 * i + 1]);
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + 2] = weight_vertex.at(i) * (outJ(smpl::HandModel::NUM_JOINTS + i, 2) - psurface[6 * i + 2]);
}
}
if (fit2d_)
{
if (psurface[6 * i + 3] == 0.0 && psurface[6 * i + 4] == 0.0)
{
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + start_2d_dim + 0] = 0.0;
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + start_2d_dim + 1] = 0.0;
}
else
{
// residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + start_2d_dim + 0] = weight_2d * weight_vertex.at(i) * ((outProj(i + smpl::HandModel::NUM_JOINTS, 0) / outProj(i + smpl::HandModel::NUM_JOINTS, 2) - psurface[6 * i + 3]));
// residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + start_2d_dim + 1] = weight_2d * weight_vertex.at(i) * ((outProj(i + smpl::HandModel::NUM_JOINTS, 1) / outProj(i + smpl::HandModel::NUM_JOINTS, 2) - psurface[6 * i + 4]));
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + start_2d_dim + 0] = weight_2d[0] * weight_vertex.at(i) * ((outProj(i + smpl::HandModel::NUM_JOINTS, 0) / outProj(i + smpl::HandModel::NUM_JOINTS, 2) - psurface[6 * i + 3]));
residuals[res_dim * (i + smpl::HandModel::NUM_JOINTS) + start_2d_dim + 1] = weight_2d[1] * weight_vertex.at(i) * ((outProj(i + smpl::HandModel::NUM_JOINTS, 1) / outProj(i + smpl::HandModel::NUM_JOINTS, 2) - psurface[6 * i + 4]));
}
}
}
}
if (fitPAF_)
{
const int offset = start_PAF;
for (auto i = 0; i < num_PAF_constraint; i++)
{
if (PAF_.col(i).isZero(0))
{
residuals[offset + 3 * i + 0] = residuals[offset + 3 * i + 1] = residuals[offset + 3 * i + 2] = 0.0;
continue;
}
const std::array<double, 3> AB{{
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 0] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 0],
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 1] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 1],
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 2] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 2],
}};
const auto length = sqrt(AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2]);
residuals[offset + 3 * i + 0] = weight_PAF * (AB[0] / length - PAF_(0, i));
residuals[offset + 3 * i + 1] = weight_PAF * (AB[1] / length - PAF_(1, i));
residuals[offset + 3 * i + 2] = weight_PAF * (AB[2] / length - PAF_(2, i));
}
}
if (jacobians)
{
if (jacobians[0])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > drdt(jacobians[0], m_nResiduals, 3);
Matrix<double, Dynamic, Dynamic, RowMajor> dJdt(3, 3);
dJdt.setIdentity();
for (int i = 0; i < smpl::HandModel::NUM_JOINTS; i++)
{
const int idj = handm_.m_jointmap_pm2model(i);
if (fit3d_)
{
if(ptarget[5 * i + 0] == 0.0 && ptarget[5 * i + 1] == 0.0 && ptarget[5 * i + 2] == 0.0) drdt.block(res_dim * idj, 0, 3, 3).setZero();
else
{
drdt.block(res_dim * idj, 0, 3, 3) = weight_joints[i] * Eigen::Matrix<double, 3, 3, RowMajor>::Identity();
}
}
if (fit2d_)
{
if(ptarget[5 * i + 3] == 0.0 && ptarget[5 * i + 4] == 0.0) drdt.block(res_dim * idj + start_2d_dim, 0, 2, 3).setZero();
// else projection_Derivative(drdt.data(), dJdt.data(), drdt.cols(), (double*)(outJ.data() + 3 * i), K_, res_dim * idj + start_2d_dim, 0, weight_2d * weight_joints[i]);
else
{
projection_Derivative(drdt.data(), dJdt.data(), drdt.cols(), (double*)(outJ.data() + 3 * i), K_, res_dim * idj + start_2d_dim, 0, weight_joints[i]);
drdt.row(res_dim * idj + start_2d_dim + 0) *= weight_2d[0];
drdt.row(res_dim * idj + start_2d_dim + 1) *= weight_2d[1];
}
}
}
if (fit_surface)
{
for (int i = 0; i < surface_constraints.cols(); i++)
{
if (fit3d_)
{
if (psurface[6 * i + 0] == 0.0 && psurface[6 * i + 1] == 0.0 && psurface[6 * i + 2] == 0.0)
drdt.block(res_dim * (smpl::HandModel::NUM_JOINTS + i), 0, 3, 3).setZero();
else
drdt.block(res_dim * (smpl::HandModel::NUM_JOINTS + i), 0, 3, 3) = weight_vertex[i] * Eigen::Matrix<double, 3, 3, RowMajor>::Identity();
}
if (fit2d_)
{
if (psurface[6 * i + 3] == 0.0 && psurface[6 * i + 4] == 0.0)
drdt.block(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, 2, 3).setZero();
else
{
// projection_Derivative(drdt.data(), dJdt.data(), drdt.cols(), (double*)(outJ.data() + 3 * (smpl::HandModel::NUM_JOINTS + i)), K_, res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, weight_2d * weight_vertex[i]);
projection_Derivative(drdt.data(), dJdt.data(), drdt.cols(), (double*)(outJ.data() + 3 * (smpl::HandModel::NUM_JOINTS + i)), K_, res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, weight_vertex[i]);
drdt.row(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim + 0) *= weight_2d[0];
drdt.row(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim + 1) *= weight_2d[1];
}
}
}
}
if (fitPAF_)
{
std::fill(jacobians[0] + 3 * start_PAF, jacobians[0] + 3 * start_PAF + 9 * num_PAF_constraint, 0);
}
}
if (jacobians[1])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dPose(jacobians[1], m_nResiduals, smpl::HandModel::NUM_JOINTS * 3);
for (int i = 0; i < smpl::HandModel::NUM_JOINTS; i++)
{
const int idj = handm_.m_jointmap_pm2model(i);
if (fit3d_)
{
if(ptarget[5 * i + 0] == 0.0 && ptarget[5 * i + 1] == 0.0 && ptarget[5 * i + 2] == 0.0) dr_dPose.block(res_dim * idj, 0, 3, smpl::HandModel::NUM_JOINTS * 3).setZero();
else
{
dr_dPose.block(res_dim * idj, 0, 3, smpl::HandModel::NUM_JOINTS * 3) = weight_joints[i] * dJdP.block(3 * idj, 0, 3, smpl::HandModel::NUM_JOINTS * 3);
}
}
if (fit2d_)
{
if(ptarget[5 * i + 3] == 0.0 && ptarget[5 * i + 4] == 0.0) dr_dPose.block(res_dim * idj + start_2d_dim, 0, 2, 3 * smpl::HandModel::NUM_JOINTS).setZero();
// else projection_Derivative(dr_dPose.data(), dJdP.data(), dr_dPose.cols(), (double*)(outJ.data() + 3 * idj), K_, res_dim * idj + start_2d_dim, 3 * idj, weight_2d * weight_joints[i]);
else
{
projection_Derivative(dr_dPose.data(), dJdP.data(), dr_dPose.cols(), (double*)(outJ.data() + 3 * idj), K_, res_dim * idj + start_2d_dim, 3 * idj, weight_joints[i]);
dr_dPose.row(res_dim * idj + start_2d_dim + 0) *= weight_2d[0];
dr_dPose.row(res_dim * idj + start_2d_dim + 1) *= weight_2d[1];
}
}
}
if (fit_surface)
{
for (int i = 0; i < surface_constraints.cols(); i++)
{
if (fit3d_)
{
if (psurface[6 * i + 0] == 0.0 && psurface[6 * i + 1] == 0.0 && psurface[6 * i + 2] == 0.0)
dr_dPose.block(res_dim * (smpl::HandModel::NUM_JOINTS + i), 0, 3, smpl::HandModel::NUM_JOINTS * 3).setZero();
else
dr_dPose.block(res_dim * (smpl::HandModel::NUM_JOINTS + i), 0, 3, smpl::HandModel::NUM_JOINTS * 3) = weight_vertex[i] * dJdP.block(3 * (smpl::HandModel::NUM_JOINTS + i), 0, 3, smpl::HandModel::NUM_JOINTS * 3);
}
if (fit2d_)
{
if (psurface[6 * i + 3] == 0.0 && psurface[6 * i + 4] == 0.0)
dr_dPose.block(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, 2, smpl::HandModel::NUM_JOINTS * 3).setZero();
else
{
// projection_Derivative(dr_dPose.data(), dJdP.data(), dr_dPose.cols(), (double*)(outJ.data() + 3 * (smpl::HandModel::NUM_JOINTS + i)), K_, res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, weight_2d * weight_vertex[i]);
projection_Derivative(dr_dPose.data(), dJdP.data(), dr_dPose.cols(), (double*)(outJ.data() + 3 * (smpl::HandModel::NUM_JOINTS + i)), K_, res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, weight_vertex[i]);
dr_dPose.row(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim + 0) *= weight_2d[0];
dr_dPose.row(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim + 1) *= weight_2d[1];
}
}
}
}
if (fitPAF_)
{
const int offset = start_PAF;
for (auto i = 0; i < num_PAF_constraint; i++)
{
if (PAF_.col(i).isZero(0))
{
std::fill(dr_dPose.data() + (offset + 3 * i) * smpl::HandModel::NUM_POSE_PARAMETERS,
dr_dPose.data() + (offset + 3 * i + 3) * smpl::HandModel::NUM_POSE_PARAMETERS, 0);
continue;
}
const std::array<double, 3> AB{{
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 0] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 0],
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 1] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 1],
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 2] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 2],
}};
const auto length2 = AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2];
const auto length = sqrt(AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2]);
const Eigen::Map< const Matrix<double, 3, 1> > AB_vec(AB.data());
const Eigen::Matrix<double, 3, 3, RowMajor> dudJ = Eigen::Matrix<double, 3, 3>::Identity() / length - AB_vec * AB_vec.transpose() / length2 / length;
std::fill(dr_dPose.data() + (offset + 3 * i) * smpl::HandModel::NUM_POSE_PARAMETERS, dr_dPose.data() + (offset + 3 * i + 3) * smpl::HandModel::NUM_POSE_PARAMETERS, 0);
const double* dudJ_data = dudJ.data();
double* drdp_row0 = dr_dPose.data() + (offset + 3 * i) * smpl::HandModel::NUM_POSE_PARAMETERS;
double* drdp_row1 = dr_dPose.data() + (offset + 3 * i + 1) * smpl::HandModel::NUM_POSE_PARAMETERS;
double* drdp_row2 = dr_dPose.data() + (offset + 3 * i + 2) * smpl::HandModel::NUM_POSE_PARAMETERS;
{
const double* dJdP_row0 = dJdP.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3])) * smpl::HandModel::NUM_POSE_PARAMETERS;
const double* dJdP_row1 = dJdP.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 1) * smpl::HandModel::NUM_POSE_PARAMETERS;
const double* dJdP_row2 = dJdP.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 2) * smpl::HandModel::NUM_POSE_PARAMETERS;
for(auto& ipar: parentIndexes[handm_.m_jointmap_pm2model(PAF_connection[i][3])])
{
drdp_row0[3 * ipar + 0] += weight_PAF * (dudJ_data[0] * dJdP_row0[3 * ipar + 0] + dudJ_data[1] * dJdP_row1[3 * ipar + 0] + dudJ_data[2] * dJdP_row2[3 * ipar + 0]);
drdp_row0[3 * ipar + 1] += weight_PAF * (dudJ_data[0] * dJdP_row0[3 * ipar + 1] + dudJ_data[1] * dJdP_row1[3 * ipar + 1] + dudJ_data[2] * dJdP_row2[3 * ipar + 1]);
drdp_row0[3 * ipar + 2] += weight_PAF * (dudJ_data[0] * dJdP_row0[3 * ipar + 2] + dudJ_data[1] * dJdP_row1[3 * ipar + 2] + dudJ_data[2] * dJdP_row2[3 * ipar + 2]);
drdp_row1[3 * ipar + 0] += weight_PAF * (dudJ_data[3] * dJdP_row0[3 * ipar + 0] + dudJ_data[4] * dJdP_row1[3 * ipar + 0] + dudJ_data[5] * dJdP_row2[3 * ipar + 0]);
drdp_row1[3 * ipar + 1] += weight_PAF * (dudJ_data[3] * dJdP_row0[3 * ipar + 1] + dudJ_data[4] * dJdP_row1[3 * ipar + 1] + dudJ_data[5] * dJdP_row2[3 * ipar + 1]);
drdp_row1[3 * ipar + 2] += weight_PAF * (dudJ_data[3] * dJdP_row0[3 * ipar + 2] + dudJ_data[4] * dJdP_row1[3 * ipar + 2] + dudJ_data[5] * dJdP_row2[3 * ipar + 2]);
drdp_row2[3 * ipar + 0] += weight_PAF * (dudJ_data[6] * dJdP_row0[3 * ipar + 0] + dudJ_data[7] * dJdP_row1[3 * ipar + 0] + dudJ_data[8] * dJdP_row2[3 * ipar + 0]);
drdp_row2[3 * ipar + 1] += weight_PAF * (dudJ_data[6] * dJdP_row0[3 * ipar + 1] + dudJ_data[7] * dJdP_row1[3 * ipar + 1] + dudJ_data[8] * dJdP_row2[3 * ipar + 1]);
drdp_row2[3 * ipar + 2] += weight_PAF * (dudJ_data[6] * dJdP_row0[3 * ipar + 2] + dudJ_data[7] * dJdP_row1[3 * ipar + 2] + dudJ_data[8] * dJdP_row2[3 * ipar + 2]);
}
}
{
const double* dJdP_row0 = dJdP.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][1])) * smpl::HandModel::NUM_POSE_PARAMETERS;
const double* dJdP_row1 = dJdP.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 1) * smpl::HandModel::NUM_POSE_PARAMETERS;
const double* dJdP_row2 = dJdP.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 2) * smpl::HandModel::NUM_POSE_PARAMETERS;
for(auto& ipar: parentIndexes[handm_.m_jointmap_pm2model(PAF_connection[i][1])])
{
drdp_row0[3 * ipar + 0] -= weight_PAF * (dudJ_data[0] * dJdP_row0[3 * ipar + 0] + dudJ_data[1] * dJdP_row1[3 * ipar + 0] + dudJ_data[2] * dJdP_row2[3 * ipar + 0]);
drdp_row0[3 * ipar + 1] -= weight_PAF * (dudJ_data[0] * dJdP_row0[3 * ipar + 1] + dudJ_data[1] * dJdP_row1[3 * ipar + 1] + dudJ_data[2] * dJdP_row2[3 * ipar + 1]);
drdp_row0[3 * ipar + 2] -= weight_PAF * (dudJ_data[0] * dJdP_row0[3 * ipar + 2] + dudJ_data[1] * dJdP_row1[3 * ipar + 2] + dudJ_data[2] * dJdP_row2[3 * ipar + 2]);
drdp_row1[3 * ipar + 0] -= weight_PAF * (dudJ_data[3] * dJdP_row0[3 * ipar + 0] + dudJ_data[4] * dJdP_row1[3 * ipar + 0] + dudJ_data[5] * dJdP_row2[3 * ipar + 0]);
drdp_row1[3 * ipar + 1] -= weight_PAF * (dudJ_data[3] * dJdP_row0[3 * ipar + 1] + dudJ_data[4] * dJdP_row1[3 * ipar + 1] + dudJ_data[5] * dJdP_row2[3 * ipar + 1]);
drdp_row1[3 * ipar + 2] -= weight_PAF * (dudJ_data[3] * dJdP_row0[3 * ipar + 2] + dudJ_data[4] * dJdP_row1[3 * ipar + 2] + dudJ_data[5] * dJdP_row2[3 * ipar + 2]);
drdp_row2[3 * ipar + 0] -= weight_PAF * (dudJ_data[6] * dJdP_row0[3 * ipar + 0] + dudJ_data[7] * dJdP_row1[3 * ipar + 0] + dudJ_data[8] * dJdP_row2[3 * ipar + 0]);
drdp_row2[3 * ipar + 1] -= weight_PAF * (dudJ_data[6] * dJdP_row0[3 * ipar + 1] + dudJ_data[7] * dJdP_row1[3 * ipar + 1] + dudJ_data[8] * dJdP_row2[3 * ipar + 1]);
drdp_row2[3 * ipar + 2] -= weight_PAF * (dudJ_data[6] * dJdP_row0[3 * ipar + 2] + dudJ_data[7] * dJdP_row1[3 * ipar + 2] + dudJ_data[8] * dJdP_row2[3 * ipar + 2]);
}
}
}
}
}
if (jacobians[2])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dCoeff(jacobians[2], m_nResiduals, smpl::HandModel::NUM_JOINTS * 3);
for (int i = 0; i < smpl::HandModel::NUM_JOINTS; i++)
{
const int idj = handm_.m_jointmap_pm2model(i);
if (fit3d_)
{
if(ptarget[5 * i + 0] == 0.0 && ptarget[5 * i + 1] == 0.0 && ptarget[5 * i + 2] == 0.0) dr_dCoeff.block(res_dim * idj, 0, 3, smpl::HandModel::NUM_JOINTS * 3).setZero();
else
{
dr_dCoeff.block(res_dim * idj, 0, 3, smpl::HandModel::NUM_JOINTS * 3) = weight_joints[i] * dJdc.block(3 * idj, 0, 3, smpl::HandModel::NUM_JOINTS * 3);
}
}
if (fit2d_)
{
if(ptarget[5 * i + 3] == 0.0 && ptarget[5 * i + 4] == 0.0) dr_dCoeff.block(res_dim * idj + start_2d_dim, 0, 2, 3 * smpl::HandModel::NUM_JOINTS).setZero();
// else projection_Derivative(dr_dCoeff.data(), dJdc.data(), dr_dCoeff.cols(), (double*)(outJ.data() + 3 * idj), K_, res_dim * idj + start_2d_dim, 3 * idj, weight_2d * weight_joints[i]);
else
{
projection_Derivative(dr_dCoeff.data(), dJdc.data(), dr_dCoeff.cols(), (double*)(outJ.data() + 3 * idj), K_, res_dim * idj + start_2d_dim, 3 * idj, weight_joints[i]);
dr_dCoeff.row(res_dim * idj + start_2d_dim + 0) *= weight_2d[0];
dr_dCoeff.row(res_dim * idj + start_2d_dim + 1) *= weight_2d[1];
}
}
}
if (fit_surface)
{
for (int i = 0; i < surface_constraints.cols(); i++)
{
if (fit3d_)
{
if (psurface[6 * i + 0] == 0.0 && psurface[6 * i + 1] == 0.0 && psurface[6 * i + 2] == 0.0)
dr_dCoeff.block(res_dim * (smpl::HandModel::NUM_JOINTS + i), 0, 3, smpl::HandModel::NUM_JOINTS * 3).setZero();
else
dr_dCoeff.block(res_dim * (smpl::HandModel::NUM_JOINTS + i), 0, 3, smpl::HandModel::NUM_JOINTS * 3) = weight_vertex[i] * dJdc.block(3 * (smpl::HandModel::NUM_JOINTS + i), 0, 3, smpl::HandModel::NUM_JOINTS * 3);
}
if (fit2d_)
{
if (psurface[6 * i + 3] == 0.0 && psurface[6 * i + 4] == 0.0)
dr_dCoeff.block(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, 2, smpl::HandModel::NUM_JOINTS * 3).setZero();
else
{
// projection_Derivative(dr_dCoeff.data(), dJdc.data(), dr_dCoeff.cols(), (double*)(outJ.data() + 3 * (smpl::HandModel::NUM_JOINTS + i)), K_, res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, weight_2d * weight_vertex[i]);
projection_Derivative(dr_dCoeff.data(), dJdc.data(), dr_dCoeff.cols(), (double*)(outJ.data() + 3 * (smpl::HandModel::NUM_JOINTS + i)), K_, res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim, 0, weight_vertex[i]);
dr_dCoeff.row(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim + 0) *= weight_2d[0];
dr_dCoeff.row(res_dim * (smpl::HandModel::NUM_JOINTS + i) + start_2d_dim + 0) *= weight_2d[1];
}
}
}
}
if (fitPAF_)
{
const int offset = start_PAF;
for (auto i = 0; i < num_PAF_constraint; i++)
{
if (PAF_.col(i).isZero(0))
{
std::fill(dr_dCoeff.data() + (offset + 3 * i) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS,
dr_dCoeff.data() + (offset + 3 * i + 3) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS, 0);
continue;
}
const std::array<double, 3> AB{{
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 0] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 0],
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 1] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 1],
outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 2] - outJ.data()[3 * handm_.m_jointmap_pm2model(PAF_connection[i][1]) + 2],
}};
const auto length2 = AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2];
const auto length = sqrt(AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2]);
const Eigen::Map< const Matrix<double, 3, 1> > AB_vec(AB.data());
const Eigen::Matrix<double, 3, 3, RowMajor> dudJ = Eigen::Matrix<double, 3, 3>::Identity() / length - AB_vec * AB_vec.transpose() / length2 / length;
std::fill(dr_dCoeff.data() + (offset + 3 * i) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS, dr_dCoeff.data() + (offset + 3 * i + 3) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS, 0);
const double* dudJ_data = dudJ.data();
double* drdc_row0 = dr_dCoeff.data() + (offset + 3 * i) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
double* drdc_row1 = dr_dCoeff.data() + (offset + 3 * i + 1) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
double* drdc_row2 = dr_dCoeff.data() + (offset + 3 * i + 2) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
{
const double* dJdc_row0 = dJdc.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3])) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
const double* dJdc_row1 = dJdc.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 1) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
const double* dJdc_row2 = dJdc.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 2) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
for(auto& ipar: parentIndexes[handm_.m_jointmap_pm2model(PAF_connection[i][3])])
{
drdc_row0[3 * ipar + 0] += weight_PAF * (dudJ_data[0] * dJdc_row0[3 * ipar + 0] + dudJ_data[1] * dJdc_row1[3 * ipar + 0] + dudJ_data[2] * dJdc_row2[3 * ipar + 0]);
drdc_row0[3 * ipar + 1] += weight_PAF * (dudJ_data[0] * dJdc_row0[3 * ipar + 1] + dudJ_data[1] * dJdc_row1[3 * ipar + 1] + dudJ_data[2] * dJdc_row2[3 * ipar + 1]);
drdc_row0[3 * ipar + 2] += weight_PAF * (dudJ_data[0] * dJdc_row0[3 * ipar + 2] + dudJ_data[1] * dJdc_row1[3 * ipar + 2] + dudJ_data[2] * dJdc_row2[3 * ipar + 2]);
drdc_row1[3 * ipar + 0] += weight_PAF * (dudJ_data[3] * dJdc_row0[3 * ipar + 0] + dudJ_data[4] * dJdc_row1[3 * ipar + 0] + dudJ_data[5] * dJdc_row2[3 * ipar + 0]);
drdc_row1[3 * ipar + 1] += weight_PAF * (dudJ_data[3] * dJdc_row0[3 * ipar + 1] + dudJ_data[4] * dJdc_row1[3 * ipar + 1] + dudJ_data[5] * dJdc_row2[3 * ipar + 1]);
drdc_row1[3 * ipar + 2] += weight_PAF * (dudJ_data[3] * dJdc_row0[3 * ipar + 2] + dudJ_data[4] * dJdc_row1[3 * ipar + 2] + dudJ_data[5] * dJdc_row2[3 * ipar + 2]);
drdc_row2[3 * ipar + 0] += weight_PAF * (dudJ_data[6] * dJdc_row0[3 * ipar + 0] + dudJ_data[7] * dJdc_row1[3 * ipar + 0] + dudJ_data[8] * dJdc_row2[3 * ipar + 0]);
drdc_row2[3 * ipar + 1] += weight_PAF * (dudJ_data[6] * dJdc_row0[3 * ipar + 1] + dudJ_data[7] * dJdc_row1[3 * ipar + 1] + dudJ_data[8] * dJdc_row2[3 * ipar + 1]);
drdc_row2[3 * ipar + 2] += weight_PAF * (dudJ_data[6] * dJdc_row0[3 * ipar + 2] + dudJ_data[7] * dJdc_row1[3 * ipar + 2] + dudJ_data[8] * dJdc_row2[3 * ipar + 2]);
}
}
{
const double* dJdc_row0 = dJdc.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3])) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
const double* dJdc_row1 = dJdc.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 1) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
const double* dJdc_row2 = dJdc.data() + (3 * handm_.m_jointmap_pm2model(PAF_connection[i][3]) + 2) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS;
for(auto& ipar: parentIndexes[handm_.m_jointmap_pm2model(PAF_connection[i][1])])
{
drdc_row0[3 * ipar + 0] -= weight_PAF * (dudJ_data[0] * dJdc_row0[3 * ipar + 0] + dudJ_data[1] * dJdc_row1[3 * ipar + 0] + dudJ_data[2] * dJdc_row2[3 * ipar + 0]);
drdc_row0[3 * ipar + 1] -= weight_PAF * (dudJ_data[0] * dJdc_row0[3 * ipar + 1] + dudJ_data[1] * dJdc_row1[3 * ipar + 1] + dudJ_data[2] * dJdc_row2[3 * ipar + 1]);
drdc_row0[3 * ipar + 2] -= weight_PAF * (dudJ_data[0] * dJdc_row0[3 * ipar + 2] + dudJ_data[1] * dJdc_row1[3 * ipar + 2] + dudJ_data[2] * dJdc_row2[3 * ipar + 2]);
drdc_row1[3 * ipar + 0] -= weight_PAF * (dudJ_data[3] * dJdc_row0[3 * ipar + 0] + dudJ_data[4] * dJdc_row1[3 * ipar + 0] + dudJ_data[5] * dJdc_row2[3 * ipar + 0]);
drdc_row1[3 * ipar + 1] -= weight_PAF * (dudJ_data[3] * dJdc_row0[3 * ipar + 1] + dudJ_data[4] * dJdc_row1[3 * ipar + 1] + dudJ_data[5] * dJdc_row2[3 * ipar + 1]);
drdc_row1[3 * ipar + 2] -= weight_PAF * (dudJ_data[3] * dJdc_row0[3 * ipar + 2] + dudJ_data[4] * dJdc_row1[3 * ipar + 2] + dudJ_data[5] * dJdc_row2[3 * ipar + 2]);
drdc_row2[3 * ipar + 0] -= weight_PAF * (dudJ_data[6] * dJdc_row0[3 * ipar + 0] + dudJ_data[7] * dJdc_row1[3 * ipar + 0] + dudJ_data[8] * dJdc_row2[3 * ipar + 0]);
drdc_row2[3 * ipar + 1] -= weight_PAF * (dudJ_data[6] * dJdc_row0[3 * ipar + 1] + dudJ_data[7] * dJdc_row1[3 * ipar + 1] + dudJ_data[8] * dJdc_row2[3 * ipar + 1]);
drdc_row2[3 * ipar + 2] -= weight_PAF * (dudJ_data[6] * dJdc_row0[3 * ipar + 2] + dudJ_data[7] * dJdc_row1[3 * ipar + 2] + dudJ_data[8] * dJdc_row2[3 * ipar + 2]);
}
}
}
}
}
}
return true;
}