in visualization/FitAdam/include/FitCost.h [687:1398]
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const
{
using namespace Eigen;
typedef double T;
const T * t = parameters[0];
const T * p_eulers = parameters[1];
const T * c = parameters[2];
const T * c_faceEx = parameters[3]; //Facial expression
MatrixXdr V(TotalModel::NUM_VERTICES, 3);
Map< VectorXd > V_vec(V.data(), TotalModel::NUM_VERTICES * 3);
Map< const Vector3d > t_vec(t);
Map< const VectorXd > pose_vec(p_eulers, TotalModel::NUM_JOINTS * 3);
Map< const VectorXd > coeffs_vec(c, TotalModel::NUM_SHAPE_COEFFICIENTS);
Map< const VectorXd > coeffs_vec_faceEx(c_faceEx, TotalModel::NUM_EXP_BASIS_COEFFICIENTS);
Matrix<double, Dynamic, Dynamic, RowMajor> dVdPfr(TotalModel::NUM_VERTICES * 3,
TotalModel::NUM_JOINTS * 3);
Matrix<double, Dynamic, Dynamic, RowMajor> dVdcfr(TotalModel::NUM_VERTICES * 3,
TotalModel::NUM_SHAPE_COEFFICIENTS);
Matrix<double, Dynamic, Dynamic, RowMajor> dTJdPfr((TotalModel::NUM_JOINTS) * 3, 3 * TotalModel::NUM_JOINTS);
Matrix<double, Dynamic, Dynamic, RowMajor> dTJdcfr((TotalModel::NUM_JOINTS) * 3, 3 * TotalModel::NUM_SHAPE_COEFFICIENTS);
Matrix<double, Dynamic, Dynamic, RowMajor> dVdfc(TotalModel::NUM_VERTICES * 3, TotalModel::NUM_EXP_BASIS_COEFFICIENTS);
dVdfc.setZero();
Eigen::VectorXd outJointv = adam_reconstruct_withDerivative_eulers(m_adam, c, p_eulers, c_faceEx, V.data(), dVdcfr, dVdPfr, dVdfc, dTJdcfr, dTJdPfr, joint_only, fit_face_);
// adam_reconstruct_withDerivative_eulers(m_adam, m_facem, c, p_eulers, c_faceEx, V.data(), dVdcfr, dVdPfr, dVdfc);
V_vec += m_dVdt * t_vec; //Translation
Map< VectorXd > res(residuals, m_nResiduals);
res.setZero();
// Joint Constraints //////////////////////
VectorXd tempJoints(m_mat_vert2joints.rows());
Eigen::Map< Matrix<T, TotalModel::NUM_JOINTS, 3, RowMajor> > outJoint(outJointv.data());
if (this->joint_only)
{
// std::cout << outT << std::endl;
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
tempJoints.block(3*i, 0, 3, 1) = outJoint.row(m_adam.m_indices_jointConst_adamIdx(i)).transpose() + t_vec;
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
tempJoints.block(3*(i + offset), 0, 3, 1) = outJoint.row(m_adam.m_correspond_adam2lHand_adamIdx(i)).transpose() + t_vec;
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
tempJoints.block(3*(i + offset), 0, 3, 1) = outJoint.row(m_adam.m_correspond_adam2rHand_adamIdx(i)).transpose() + t_vec;
}
}
else
tempJoints = m_mat_vert2joints * V_vec;
int vertexCons_res_startIdx = m_nCorrespond_adam2joints * res_dim;
if (projection_ == 0u)
// Fit 3D joints
{
for(int i = 0; i < m_nCorrespond_adam2joints; i++)
{
if (m_targetPts.block(5 * i, 0, 3, 1).isZero(0)) continue;
res.block(3 * i, 0, 3, 1) << tempJoints.block(3 * i, 0, 3, 1) - m_targetPts.block(5 * i, 0, 3, 1);
}
for (int j = 0; j < 3 * m_nCorrespond_adam2joints; ++j)
{
residuals[j] *= m_targetPts_weight[j];
}
// Vertex Constraints //////////////////////
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[0] == 0.0 && corres_vertex2targetpt[i].second[1] == 0.0 && corres_vertex2targetpt[i].second[2] == 0.0) continue;
res[vertexCons_res_startIdx + 3 * i + 0] = WEAK_WEIGHT * (V_vec(3 * corres_vertex2targetpt[i].first + 0) - corres_vertex2targetpt[i].second[0]); //- m_targetPts(res_offset + 3 * i + 0));
res[vertexCons_res_startIdx + 3 * i + 1] = WEAK_WEIGHT * (V_vec(3 * corres_vertex2targetpt[i].first + 1) - corres_vertex2targetpt[i].second[1]); //- m_targetPts(res_offset + 3 * i + 1));
res[vertexCons_res_startIdx + 3 * i + 2] = WEAK_WEIGHT * (V_vec(3 * corres_vertex2targetpt[i].first + 2) - corres_vertex2targetpt[i].second[2]); //- m_targetPts(res_offset + 3 * i + 2));
}
}
else res.block(vertexCons_res_startIdx, 0, 3 * corres_vertex2targetpt.size(), 1).setZero();
// compute jacobian
if (jacobians)
{
if (jacobians[0])
{
// Construct full system.
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > drdt(jacobians[0], m_nResiduals, 3);
drdt.block(0, 0, 3 * m_nCorrespond_adam2joints, 3) = m_mat_vert2joints * m_dVdt;
for (int j = 0; j < m_nCorrespond_adam2joints; j++)
{
if (m_targetPts.block(5 * j, 0, 3, 1).isZero(0)) drdt.block(3 * j, 0, 3, 3).setZero();
}
for (int j = 0; j < 3 * m_nCorrespond_adam2joints; ++j)
{
drdt.row(j) *= m_targetPts_weight[j];
}
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[0] == 0.0 && corres_vertex2targetpt[i].second[1] == 0.0 && corres_vertex2targetpt[i].second[2] == 0.0) continue;
drdt.row(vertexCons_res_startIdx + 3 * i + 0) = WEAK_WEIGHT * m_dVdt.row(3 * corres_vertex2targetpt[i].first + 0);
drdt.row(vertexCons_res_startIdx + 3 * i + 1) = WEAK_WEIGHT * m_dVdt.row(3 * corres_vertex2targetpt[i].first + 1);
drdt.row(vertexCons_res_startIdx + 3 * i + 2) = WEAK_WEIGHT * m_dVdt.row(3 * corres_vertex2targetpt[i].first + 2);
}
}
else
{
drdt.block(vertexCons_res_startIdx, 0, 3 * corres_vertex2targetpt.size(), 3).setZero();
}
}
if (jacobians[1])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dPose(jacobians[1],
m_nResiduals,
TotalModel::NUM_JOINTS * 3);
dr_dPose.setZero();
if (this->joint_only)
{
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
if (m_targetPts.block(5 * i, 0, 3, 1).isZero(0)) continue;
dr_dPose.block(3 * i, 0, 3, TotalModel::NUM_POSE_PARAMETERS) = dTJdPfr.block(3 * m_adam.m_indices_jointConst_adamIdx(i), 0, 3, TotalModel::NUM_POSE_PARAMETERS);
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
if (m_targetPts.block(5 * (i + offset), 0, 3, 1).isZero(0)) continue;
dr_dPose.block(3*(i + offset), 0, 3, TotalModel::NUM_POSE_PARAMETERS) = dTJdPfr.block(3 * m_adam.m_correspond_adam2lHand_adamIdx(i), 0, 3, TotalModel::NUM_POSE_PARAMETERS);
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
if (m_targetPts.block(5 * (i + offset), 0, 3, 1).isZero(0)) continue;
dr_dPose.block(3*(i + offset), 0, 3, TotalModel::NUM_POSE_PARAMETERS) = dTJdPfr.block(3 * m_adam.m_correspond_adam2rHand_adamIdx(i), 0, 3, TotalModel::NUM_POSE_PARAMETERS);
}
}
else
dr_dPose.block(0, 0, m_mat_vert2joints.rows(), TotalModel::NUM_JOINTS * 3) = m_mat_vert2joints*dVdPfr;
for (int j = 0; j < 3 * m_nCorrespond_adam2joints; ++j)
{
dr_dPose.row(j) *= m_targetPts_weight[j];
}
if(!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[0] == 0.0 && corres_vertex2targetpt[i].second[1] == 0.0 && corres_vertex2targetpt[i].second[2] == 0.0) continue;
dr_dPose.row(vertexCons_res_startIdx + 3 * i + 0) = WEAK_WEIGHT* dVdPfr.row(3 * corres_vertex2targetpt[i].first + 0);
dr_dPose.row(vertexCons_res_startIdx + 3 * i + 1) = WEAK_WEIGHT* dVdPfr.row(3 * corres_vertex2targetpt[i].first + 1);
dr_dPose.row(vertexCons_res_startIdx + 3 * i + 2) = WEAK_WEIGHT* dVdPfr.row(3 * corres_vertex2targetpt[i].first + 2);
}
}
else
{
dr_dPose.block(vertexCons_res_startIdx, 0, 3 * corres_vertex2targetpt.size(), TotalModel::NUM_POSE_PARAMETERS).setZero();
}
}
if (jacobians[2])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dCoeff(jacobians[2],
m_nResiduals,
TotalModel::NUM_SHAPE_COEFFICIENTS);
dr_dCoeff.setZero();
if (this->joint_only)
{
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
if (m_targetPts.block(5 * i, 0, 3, 1).isZero(0)) continue;
dr_dCoeff.block(3 * i, 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dTJdcfr.block(3 * m_adam.m_indices_jointConst_adamIdx(i), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
if (m_targetPts.block(5 * (i + offset), 0, 3, 1).isZero(0)) continue;
dr_dCoeff.block(3*(i + offset), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dTJdcfr.block(3 * m_adam.m_correspond_adam2lHand_adamIdx(i), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
if (m_targetPts.block(5 * (i + offset), 0, 3, 1).isZero(0)) continue;
dr_dCoeff.block(3*(i + offset), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dTJdcfr.block(3 * m_adam.m_correspond_adam2rHand_adamIdx(i), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
}
}
else dr_dCoeff.block(0, 0, m_mat_vert2joints.rows(), TotalModel::NUM_SHAPE_COEFFICIENTS) = m_mat_vert2joints*dVdcfr;
for (int j = 0; j < 3 * m_nCorrespond_adam2joints; ++j)
{
dr_dCoeff.row(j) *= m_targetPts_weight[j];
}
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[0] == 0.0 && corres_vertex2targetpt[i].second[1] == 0.0 && corres_vertex2targetpt[i].second[2] == 0.0) continue;
dr_dCoeff.row(vertexCons_res_startIdx + 3 * i + 0) = WEAK_WEIGHT* dVdcfr.row(3 * corres_vertex2targetpt[i].first + 0);
dr_dCoeff.row(vertexCons_res_startIdx + 3 * i + 1) = WEAK_WEIGHT* dVdcfr.row(3 * corres_vertex2targetpt[i].first + 1);
dr_dCoeff.row(vertexCons_res_startIdx + 3 * i + 2) = WEAK_WEIGHT* dVdcfr.row(3 * corres_vertex2targetpt[i].first + 2);
}
}
else
{
int vertexCons_res_startIdx = m_nCorrespond_adam2joints * 3;
dr_dCoeff.block(vertexCons_res_startIdx, 0, 3 * corres_vertex2targetpt.size(), TotalModel::NUM_SHAPE_COEFFICIENTS).setZero();
}
}
if (this->fit_face_ && jacobians[3]) //face
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dFaceCoef(jacobians[3], m_nResiduals, TotalModel::NUM_EXP_BASIS_COEFFICIENTS);
dr_dFaceCoef.setZero();
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[0] == 0.0 && corres_vertex2targetpt[i].second[1] == 0.0 && corres_vertex2targetpt[i].second[2] == 0.0) continue;
dr_dFaceCoef.row(vertexCons_res_startIdx + 3 * i + 0) = WEAK_WEIGHT * dVdfc.row(3 * corres_vertex2targetpt[i].first + 0);
dr_dFaceCoef.row(vertexCons_res_startIdx + 3 * i + 1) = WEAK_WEIGHT * dVdfc.row(3 * corres_vertex2targetpt[i].first + 1);
dr_dFaceCoef.row(vertexCons_res_startIdx + 3 * i + 2) = WEAK_WEIGHT * dVdfc.row(3 * corres_vertex2targetpt[i].first + 2);
}
}
}
}
}
else if(projection_ == 1u)
// fit projection of 3D joints
{
assert(this->pK_);
Eigen::Map< Matrix<double, Dynamic, 3, RowMajor> > jointArray(tempJoints.data(), m_nCorrespond_adam2joints, 3);
Eigen::Map< Matrix<double, 3, 3, RowMajor> > K(pK_);
Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> jointProjection = jointArray * K.transpose();
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
if (m_targetPts(5 * i + 3) == 0.0)
residuals[res_dim * i] = residuals[res_dim * i + 1] = 0.0;
else
{
residuals[2*i+0] = jointProjection(i, 0) / jointProjection(i, 2) - m_targetPts(5 * i + 3);
residuals[2*i+1] = jointProjection(i, 1) / jointProjection(i, 2) - m_targetPts(5 * i + 4);
}
}
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
residuals[j] *= m_targetPts_weight[j];
}
if (!this->joint_only)
{
double x, y, X, Y, Z;
for (auto i = 0u; i < corres_vertex2targetpt.size(); i++)
{
if (corres_vertex2targetpt[i].second[3] == 0.0)
res.block(vertexCons_res_startIdx + 2 * i, 0, 2, 1).setZero();
else
{
X = V_vec(3 * corres_vertex2targetpt[i].first + 0);
Y = V_vec(3 * corres_vertex2targetpt[i].first + 1);
Z = V_vec(3 * corres_vertex2targetpt[i].first + 2);
x = (K(0, 0) * X + K(0, 1) * Y) / Z + K(0, 2);
y = K(1, 1) * Y / Z + K(1, 2);
res[vertexCons_res_startIdx + res_dim * i + 0] = WEAK_WEIGHT * (x - corres_vertex2targetpt[i].second[3]);
res[vertexCons_res_startIdx + res_dim * i + 1] = WEAK_WEIGHT * (y - corres_vertex2targetpt[i].second[4]);
}
// std::cout << i << " " << x << " " << corres_vertex2targetpt[i].second.x << " " << y << " " << corres_vertex2targetpt[i].second.y << std::endl;
}
}
else res.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), 1).setZero();
if (jacobians)
{
if (jacobians[0])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > drdt(jacobians[0], m_nResiduals, 3);
drdt.setZero();
Matrix<double, Dynamic, Dynamic, RowMajor> dJdt = m_mat_vert2joints * m_dVdt;
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(drdt, dJdt, XYZ, pK_, res_dim * i, 3 * i);
}
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
drdt.row(j) *= m_targetPts_weight[j];
}
// set the vertex gradient
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(drdt, m_dVdt, XYZ, pK_, vertexCons_res_startIdx + res_dim * i, 3 * corres_vertex2targetpt[i].first, WEAK_WEIGHT);
}
}
else
{
drdt.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), 3).setZero();
}
}
if (jacobians[1])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dPose(jacobians[1],
m_nResiduals,
TotalModel::NUM_JOINTS * 3);
dr_dPose.setZero();
if (this->joint_only)
{
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dPose, dTJdPfr, XYZ, pK_, res_dim * i, 3 * m_adam.m_indices_jointConst_adamIdx(i));
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dPose, dTJdPfr, XYZ, pK_, res_dim * (i + offset), 3 * m_adam.m_correspond_adam2lHand_adamIdx(i));
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dPose, dTJdPfr, XYZ, pK_, res_dim * (i + offset), 3 * m_adam.m_correspond_adam2rHand_adamIdx(i));
}
}
else
{
Matrix<double, Dynamic, Dynamic, RowMajor> dJdP = m_mat_vert2joints * dVdPfr;
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dPose, dJdP, XYZ, pK_, res_dim * i, 3 * i);
}
}
// multiply by weight
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
dr_dPose.row(j) *= m_targetPts_weight[j];
}
if(!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(dr_dPose, dVdPfr, XYZ, pK_, vertexCons_res_startIdx + res_dim * i, 3 * corres_vertex2targetpt[i].first, WEAK_WEIGHT);
}
}
else
{
dr_dPose.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), TotalModel::NUM_POSE_PARAMETERS).setZero();
}
}
if (jacobians[2])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dCoeff(jacobians[2],
m_nResiduals,
TotalModel::NUM_SHAPE_COEFFICIENTS);
dr_dCoeff.setZero();
if (this->joint_only)
{
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dCoeff, dTJdcfr, XYZ, pK_, res_dim * i, 3 * m_adam.m_indices_jointConst_adamIdx(i));
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dCoeff, dTJdcfr, XYZ, pK_, res_dim * (i + offset), 3 * m_adam.m_correspond_adam2lHand_adamIdx(i));
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dCoeff, dTJdcfr, XYZ, pK_, res_dim * (i + offset), 3 * m_adam.m_correspond_adam2rHand_adamIdx(i));
}
}
else
{
Matrix<double, Dynamic, Dynamic, RowMajor> dJdC = m_mat_vert2joints * dVdcfr;
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dCoeff, dJdC, XYZ, pK_, res_dim * i, 3 * i);
}
}
// multiply by weight
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
dr_dCoeff.row(j) *= m_targetPts_weight[j];
}
if(!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(dr_dCoeff, dVdcfr, XYZ, pK_, vertexCons_res_startIdx + res_dim * i, 3 * corres_vertex2targetpt[i].first, WEAK_WEIGHT);
}
}
else
{
dr_dCoeff.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), TotalModel::NUM_SHAPE_COEFFICIENTS).setZero();
}
}
if (this->fit_face_ && jacobians[3])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dFaceCoef(jacobians[3], m_nResiduals, TotalModel::NUM_EXP_BASIS_COEFFICIENTS);
dr_dFaceCoef.setZero();
// only related to vertex weights
if(!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(dr_dFaceCoef, dVdfc, XYZ, pK_, vertexCons_res_startIdx + res_dim * i, 3 * corres_vertex2targetpt[i].first, WEAK_WEIGHT);
}
}
}
}
}
else
{
assert(projection_ == 2u);
assert(pK_);
int root_index = 12; // last target point, neck
Eigen::Map< Matrix<double, Dynamic, 3, RowMajor> > jointArray(tempJoints.data(), m_nCorrespond_adam2joints, 3);
Eigen::Map< Matrix<double, 3, 3, RowMajor> > K(pK_);
Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> jointProjection = jointArray * K.transpose();
for(int i = 0; i < m_nCorrespond_adam2joints; i++)
{
res.block(5 * i, 0, 3, 1) = tempJoints.block(3 * i, 0, 3, 1) - tempJoints.block(3 * root_index, 0, 3, 1);
res.block(5 * i, 0, 3, 1) += (- m_targetPts.block(5 * i, 0, 3, 1) + m_targetPts.block(5 * root_index, 0, 3, 1));
if (m_targetPts(5 * i + 3) != 0.0)
{
residuals[5*i+3] = weight2d * (jointProjection(i, 0) / jointProjection(i, 2) - m_targetPts(5 * i + 3));
residuals[5*i+4] = weight2d * (jointProjection(i, 1) / jointProjection(i, 2) - m_targetPts(5 * i + 4));
}
}
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
residuals[j] *= m_targetPts_weight[j];
}
// Vertex Constraints //////////////////////
if (!this->joint_only)
{
double X, Y, Z, x, y;
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
res[vertexCons_res_startIdx + res_dim * i + 0] = WEAK_WEIGHT * (V_vec(3 * corres_vertex2targetpt[i].first + 0) - tempJoints(3 * root_index + 0) - corres_vertex2targetpt[i].second[0] + m_targetPts(5 * root_index)); //- m_targetPts(res_offset + 3 * i + 0));
res[vertexCons_res_startIdx + res_dim * i + 1] = WEAK_WEIGHT * (V_vec(3 * corres_vertex2targetpt[i].first + 1) - tempJoints(3 * root_index + 1) - corres_vertex2targetpt[i].second[1] + m_targetPts(5 * root_index + 1)); //- m_targetPts(res_offset + 3 * i + 1));
res[vertexCons_res_startIdx + res_dim * i + 2] = WEAK_WEIGHT * (V_vec(3 * corres_vertex2targetpt[i].first + 2) - tempJoints(3 * root_index + 2) - corres_vertex2targetpt[i].second[2] + m_targetPts(5 * root_index + 2)); //- m_targetPts(res_offset + 3 * i + 2));
if (corres_vertex2targetpt[i].second[3] != 0.0)
{
X = V_vec(3 * corres_vertex2targetpt[i].first + 0);
Y = V_vec(3 * corres_vertex2targetpt[i].first + 1);
Z = V_vec(3 * corres_vertex2targetpt[i].first + 2);
x = (K(0, 0) * X + K(0, 1) * Y) / Z + K(0, 2);
y = K(1, 1) * Y / Z + K(1, 2);
res[vertexCons_res_startIdx + res_dim * i + 3] = WEAK_WEIGHT * weight2d * (x - corres_vertex2targetpt[i].second[3]);
res[vertexCons_res_startIdx + res_dim * i + 4] = WEAK_WEIGHT * weight2d * (y - corres_vertex2targetpt[i].second[4]);
}
}
}
else res.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), 1).setZero();
// compute jacobian
if (jacobians)
{
if (jacobians[0])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > drdt(jacobians[0], m_nResiduals, 3);
drdt.setZero();
// no jacobian from 3D residual
Matrix<double, Dynamic, Dynamic, RowMajor> dJdt = m_mat_vert2joints * m_dVdt;
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(drdt, dJdt, XYZ, pK_, res_dim * i + 3, 3 * i, weight2d);
}
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
drdt.row(j) *= m_targetPts_weight[j];
}
// No jacobian from 3D vertex
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(drdt, m_dVdt, XYZ, pK_, vertexCons_res_startIdx + res_dim * i + 3, 3 * corres_vertex2targetpt[i].first, weight2d * WEAK_WEIGHT);
}
}
}
if (jacobians[1])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dPose(jacobians[1],
m_nResiduals,
TotalModel::NUM_JOINTS * 3);
dr_dPose.setZero();
Matrix<double, Dynamic, Dynamic, RowMajor> dJdP = m_mat_vert2joints * dVdPfr; // used to compute jacobian when joint_only is false
if (this->joint_only)
{
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
// tempJoints(0, 3*i, 3, 1) = outT(3*m_adam.m_indices_jointConst_adamIdx(i), 3, 3, 1);
dr_dPose.block(res_dim * i, 0, 3, TotalModel::NUM_POSE_PARAMETERS) = dTJdPfr.block(3 * m_adam.m_indices_jointConst_adamIdx(i), 0, 3, TotalModel::NUM_POSE_PARAMETERS) -
dTJdPfr.block(3 * m_adam.m_indices_jointConst_adamIdx(root_index), 0, 3, TotalModel::NUM_POSE_PARAMETERS);
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dPose, dTJdPfr, XYZ, pK_, res_dim * i + 3, 3 * m_adam.m_indices_jointConst_adamIdx(i), weight2d);
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
dr_dPose.block(res_dim * (i + offset), 0, 3, TotalModel::NUM_POSE_PARAMETERS) = dTJdPfr.block(3 * m_adam.m_correspond_adam2lHand_adamIdx(i), 0, 3, TotalModel::NUM_POSE_PARAMETERS)
- dTJdPfr.block(3 * m_adam.m_indices_jointConst_adamIdx(root_index), 0, 3, TotalModel::NUM_POSE_PARAMETERS);
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dPose, dTJdPfr, XYZ, pK_, res_dim * (i + offset) + 3, 3 * m_adam.m_correspond_adam2lHand_adamIdx(i), weight2d);
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
dr_dPose.block(res_dim * (i + offset), 0, 3, TotalModel::NUM_POSE_PARAMETERS) = dTJdPfr.block(3 * m_adam.m_correspond_adam2rHand_adamIdx(i), 0, 3, TotalModel::NUM_POSE_PARAMETERS)
- dTJdPfr.block(3 * m_adam.m_indices_jointConst_adamIdx(root_index), 0, 3, TotalModel::NUM_POSE_PARAMETERS);
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dPose, dTJdPfr, XYZ, pK_, res_dim * (i + offset) + 3, 3 * m_adam.m_correspond_adam2rHand_adamIdx(i), weight2d);
}
}
else
{
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
dr_dPose.block(res_dim * i, 0, 3, TotalModel::NUM_JOINTS * 3) = dJdP.block(3 * i, 0, 3, TotalModel::NUM_JOINTS * 3)
- dJdP.block(3 * root_index, 0, 3, TotalModel::NUM_JOINTS * 3);
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dPose, dJdP, XYZ, pK_, res_dim * i + 3, 3 * i, weight2d);
}
}
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
dr_dPose.row(j) *= m_targetPts_weight[j];
}
if(!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
dr_dPose.row(vertexCons_res_startIdx + res_dim * i + 0) = WEAK_WEIGHT * (dVdPfr.row(3 * corres_vertex2targetpt[i].first + 0) - dJdP.row(3 * root_index + 0));
dr_dPose.row(vertexCons_res_startIdx + res_dim * i + 1) = WEAK_WEIGHT * (dVdPfr.row(3 * corres_vertex2targetpt[i].first + 1) - dJdP.row(3 * root_index + 1));
dr_dPose.row(vertexCons_res_startIdx + res_dim * i + 2) = WEAK_WEIGHT * (dVdPfr.row(3 * corres_vertex2targetpt[i].first + 2) - dJdP.row(3 * root_index + 2));
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(dr_dPose, dVdPfr, XYZ, pK_, vertexCons_res_startIdx + res_dim * i + 3, 3 * corres_vertex2targetpt[i].first, weight2d * WEAK_WEIGHT);
}
}
else
{
dr_dPose.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), TotalModel::NUM_POSE_PARAMETERS).setZero();
}
}
if (jacobians[2])
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dCoeff(jacobians[2],
m_nResiduals,
TotalModel::NUM_SHAPE_COEFFICIENTS);
dr_dCoeff.setZero();
Matrix<double, Dynamic, Dynamic, RowMajor> dJdC = m_mat_vert2joints*dVdcfr;
if (this->joint_only)
{
for (int i = 0; i < m_adam.m_indices_jointConst_adamIdx.rows(); i++)
{
// tempJoints(0, 3*i, 3, 1) = outT(3*m_adam.m_indices_jointConst_adamIdx(i), 3, 3, 1);
dr_dCoeff.block(res_dim * i, 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dTJdcfr.block(3 * m_adam.m_indices_jointConst_adamIdx(i), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS)
- dTJdcfr.block(3 * m_adam.m_indices_jointConst_adamIdx(root_index), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dCoeff, dTJdcfr, XYZ, pK_, res_dim * i + 3, 3 * m_adam.m_indices_jointConst_adamIdx(i), weight2d);
}
int offset = m_adam.m_indices_jointConst_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2lHand_adamIdx.rows(); i++)
{
dr_dCoeff.block(res_dim*(i + offset), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dTJdcfr.block(3 * m_adam.m_correspond_adam2lHand_adamIdx(i), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS)
- dTJdcfr.block(3 * m_adam.m_indices_jointConst_adamIdx(root_index), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dCoeff, dTJdcfr, XYZ, pK_, res_dim * (i + offset) + 3, 3 * m_adam.m_correspond_adam2lHand_adamIdx(i), weight2d);
}
offset += m_adam.m_correspond_adam2lHand_adamIdx.rows();
for (int i = 0; i < m_adam.m_correspond_adam2rHand_adamIdx.rows(); i++)
{
dr_dCoeff.block(res_dim * (i + offset), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dTJdcfr.block(3 * m_adam.m_correspond_adam2rHand_adamIdx(i), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS)
- dTJdcfr.block(3 * m_adam.m_indices_jointConst_adamIdx(root_index), 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
if (m_targetPts(5 * (i + offset) + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i + offset, 0), jointArray(i + offset, 1), jointArray(i + offset, 2)};
projection_jacobian(dr_dCoeff, dTJdcfr, XYZ, pK_, res_dim * (i + offset) + 3, 3 * m_adam.m_correspond_adam2rHand_adamIdx(i), weight2d);
}
}
else
{
for (int i = 0; i < m_nCorrespond_adam2joints; i++)
{
dr_dCoeff.block(res_dim * i, 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS) = dJdC.block(3 * i, 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS)
- dJdC.block(3 * root_index, 0, 3, TotalModel::NUM_SHAPE_COEFFICIENTS);
if (m_targetPts(5 * i + 3) == 0.0) continue;
double XYZ[3] = {jointArray(i, 0), jointArray(i, 1), jointArray(i, 2)};
projection_jacobian(dr_dCoeff, dJdC, XYZ, pK_, res_dim * i + 3, 3 * i, weight2d);
}
}
for (int j = 0; j < res_dim * m_nCorrespond_adam2joints; ++j)
{
dr_dCoeff.row(j) *= m_targetPts_weight[j];
}
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
// face coeffs do not influence body vertex at all
dr_dCoeff.row(vertexCons_res_startIdx + res_dim * i + 0) = WEAK_WEIGHT * (dVdcfr.row(3 * corres_vertex2targetpt[i].first + 0) - dJdC.row(3 * root_index + 0));
dr_dCoeff.row(vertexCons_res_startIdx + res_dim * i + 1) = WEAK_WEIGHT * (dVdcfr.row(3 * corres_vertex2targetpt[i].first + 1) - dJdC.row(3 * root_index + 1));
dr_dCoeff.row(vertexCons_res_startIdx + res_dim * i + 2) = WEAK_WEIGHT * (dVdcfr.row(3 * corres_vertex2targetpt[i].first + 2) - dJdC.row(3 * root_index + 2));
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(dr_dCoeff, dVdcfr, XYZ, pK_, vertexCons_res_startIdx + res_dim * i, 3 * corres_vertex2targetpt[i].first, weight2d * WEAK_WEIGHT);
}
}
else
{
dr_dCoeff.block(vertexCons_res_startIdx, 0, res_dim * corres_vertex2targetpt.size(), TotalModel::NUM_SHAPE_COEFFICIENTS).setZero();
}
}
if (this->fit_face_ && jacobians[3]) //face
{
Map< Matrix<double, Dynamic, Dynamic, RowMajor> > dr_dFaceCoef(jacobians[3], m_nResiduals, TotalModel::NUM_EXP_BASIS_COEFFICIENTS);
dr_dFaceCoef.setZero();
if (!this->joint_only)
{
for (auto i = 0u; i < corres_vertex2targetpt.size(); ++i)
{
// face coeff has no influence on the root joint location
dr_dFaceCoef.row(vertexCons_res_startIdx + res_dim * i + 0) = WEAK_WEIGHT * dVdfc.row(3 * corres_vertex2targetpt[i].first + 0);
dr_dFaceCoef.row(vertexCons_res_startIdx + res_dim * i + 1) = WEAK_WEIGHT * dVdfc.row(3 * corres_vertex2targetpt[i].first + 1);
dr_dFaceCoef.row(vertexCons_res_startIdx + res_dim * i + 2) = WEAK_WEIGHT * dVdfc.row(3 * corres_vertex2targetpt[i].first + 2);
if (corres_vertex2targetpt[i].second[3] == 0.0) continue;
double XYZ[3] = {V_vec(3 * corres_vertex2targetpt[i].first + 0), V_vec(3 * corres_vertex2targetpt[i].first + 1), V_vec(3 * corres_vertex2targetpt[i].first + 2)};
projection_jacobian(dr_dFaceCoef, dVdfc, XYZ, pK_, vertexCons_res_startIdx + res_dim * i + 3, 3 * corres_vertex2targetpt[i].first, weight2d * WEAK_WEIGHT);
}
}
}
}
}
return true;
}