in visualization/FitAdam/src/FitToBody.cpp [154:352]
void FitToProjectionCeres(
smpl::HandModel &hand_model,
Eigen::MatrixXd &Joints2d,
const double* K,
Eigen::MatrixXd &PAF,
Eigen::MatrixXd &surface_constraint,
Eigen::Vector3d& parm_handl_t,
Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>& parm_hand_pose,
Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>& parm_hand_coeffs,
int regressor_type,
bool fit_surface,
bool euler,
const double prior_weight,
const int mode
)
{
using namespace Eigen;
ceres::Problem problem_init;
ceres::Solver::Options options_init;
// define the reprojection error
HandFastCost* fit_cost_analytic_ha_init = new HandFastCost(hand_model, Joints2d, PAF, surface_constraint, false, false, true, false, nullptr, regressor_type, euler);
problem_init.AddResidualBlock(fit_cost_analytic_ha_init,
NULL,
parm_handl_t.data(),
parm_hand_pose.data(),
parm_hand_coeffs.data());
assert(mode >= 0 && mode <= 3);
// Regularization
if (euler)
{
ceres::CostFunction *hand_pose_reg_init = new ceres::AutoDiffCostFunction
<HandPoseParameterNorm,
smpl::HandModel::NUM_JOINTS * 3,
smpl::HandModel::NUM_JOINTS * 3>(new HandPoseParameterNorm(smpl::HandModel::NUM_JOINTS * 3, hand_model));
ceres::LossFunction* hand_pose_reg_loss_init = new ceres::ScaledLoss(NULL,
1e-5,
ceres::TAKE_OWNERSHIP);
problem_init.AddResidualBlock(hand_pose_reg_init,
hand_pose_reg_loss_init,
parm_hand_pose.data());
}
else
{
Eigen::MatrixXd hand_prior_mu(3 * smpl::HandModel::NUM_JOINTS, 1); hand_prior_mu.setZero();
hand_prior_mu.block<60, 1>(0, 0) = hand_model.pose_prior_mu_;
Eigen::MatrixXd hand_prior_A(60, 3 * smpl::HandModel::NUM_JOINTS); hand_prior_A.setZero();
hand_prior_A.block<60, 60>(0, 0) = hand_model.pose_prior_A_;
ceres::CostFunction* hand_pose_reg_init = new ceres::NormalPrior(hand_prior_A, hand_prior_mu);
ceres::LossFunction* hand_pose_reg_loss_init = new ceres::ScaledLoss(NULL,
prior_weight,
ceres::TAKE_OWNERSHIP);
problem_init.AddResidualBlock(hand_pose_reg_init,
hand_pose_reg_loss_init,
parm_hand_pose.data());
}
Eigen::MatrixXd A(smpl::HandModel::NUM_JOINTS * 3, smpl::HandModel::NUM_JOINTS * 3);
A.setIdentity();
Eigen::VectorXd b(smpl::HandModel::NUM_JOINTS * 3);
b.setOnes();
// ceres::CostFunction *hand_coeff_reg_init = new ceres::NormalPrior(A, b);
// ceres::LossFunction* hand_coeff_reg_loss_init = new ceres::ScaledLoss(NULL,
// 1000,
// ceres::TAKE_OWNERSHIP);
// problem_init.AddResidualBlock(hand_coeff_reg_init,
// hand_coeff_reg_loss_init,
// parm_hand_coeffs.data());
for (int i = 0; i < smpl::HandModel::NUM_JOINTS - 1; ++i)
{
problem_init.SetParameterLowerBound(parm_hand_coeffs.data(), i * 3 + 0, 0.5);
problem_init.SetParameterLowerBound(parm_hand_coeffs.data(), i * 3 + 1, 0.5);
problem_init.SetParameterLowerBound(parm_hand_coeffs.data(), i * 3 + 2, 0.5);
problem_init.SetParameterUpperBound(parm_hand_coeffs.data(), i * 3 + 0, 2);
problem_init.SetParameterUpperBound(parm_hand_coeffs.data(), i * 3 + 1, 2);
problem_init.SetParameterUpperBound(parm_hand_coeffs.data(), i * 3 + 2, 2);
}
SetSolverOptions(&options_init);
options_init.function_tolerance = 1e-8;
options_init.max_num_iterations = 30;
options_init.use_nonmonotonic_steps = true;
options_init.num_linear_solver_threads = 1;
options_init.minimizer_progress_to_stdout = true;
options_init.parameter_tolerance = 1e-12;
if (mode == 0)
problem_init.SetParameterBlockConstant(parm_hand_coeffs.data());
else if (mode == 1)
{
problem_init.SetParameterBlockConstant(parm_hand_coeffs.data());
problem_init.SetParameterBlockConstant(parm_hand_pose.data());
}
else if (mode == 2)
{
problem_init.SetParameterBlockConstant(parm_hand_pose.data());
}
CHECK(StringToLinearSolverType("sparse_normal_cholesky",
&options_init.linear_solver_type));
ceres::Solver::Summary summary_init;
ceres::Solve(options_init, &problem_init, &summary_init);
std::cout << summary_init.FullReport() << "\n";
// problem_init.SetParameterBlockVariable(parm_hand_coeffs.data());
ceres::Problem problem;
ceres::Solver::Options options;
HandFastCost* fit_cost_analytic_ha = new HandFastCost(hand_model, Joints2d, PAF, surface_constraint, false, true, true, fit_surface, K, regressor_type, euler);
fit_cost_analytic_ha->weight_PAF = 50.0f;
problem.AddResidualBlock(fit_cost_analytic_ha,
NULL,
parm_handl_t.data(),
parm_hand_pose.data(),
parm_hand_coeffs.data());
// Regularization
if (euler)
{
ceres::CostFunction *hand_pose_reg = new ceres::AutoDiffCostFunction
<HandPoseParameterNorm,
smpl::HandModel::NUM_JOINTS * 3,
smpl::HandModel::NUM_JOINTS * 3>(new HandPoseParameterNorm(smpl::HandModel::NUM_JOINTS * 3, hand_model));
ceres::LossFunction* hand_pose_reg_loss = new ceres::ScaledLoss(NULL,
1e-5,
ceres::TAKE_OWNERSHIP);
problem.AddResidualBlock(hand_pose_reg,
hand_pose_reg_loss,
parm_hand_pose.data());
}
else
{
Eigen::MatrixXd hand_prior_mu(3 * smpl::HandModel::NUM_JOINTS, 1); hand_prior_mu.setZero();
hand_prior_mu.block<60, 1>(0, 0) = hand_model.pose_prior_mu_;
Eigen::MatrixXd hand_prior_A(60, 3 * smpl::HandModel::NUM_JOINTS); hand_prior_A.setZero();
hand_prior_A.block<60, 60>(0, 0) = hand_model.pose_prior_A_;
ceres::CostFunction* hand_pose_reg = new ceres::NormalPrior(hand_prior_A, hand_prior_mu);
ceres::LossFunction* hand_pose_reg_loss = new ceres::ScaledLoss(NULL,
prior_weight,
ceres::TAKE_OWNERSHIP);
problem.AddResidualBlock(hand_pose_reg,
hand_pose_reg_loss,
parm_hand_pose.data());
}
ceres::CostFunction *hand_coeff_reg = new ceres::NormalPrior(A, b);
ceres::LossFunction* hand_coeff_reg_loss = new ceres::ScaledLoss(NULL,
10,
ceres::TAKE_OWNERSHIP);
problem.AddResidualBlock(hand_coeff_reg,
hand_coeff_reg_loss,
parm_hand_coeffs.data());
for (int i = 0; i < smpl::HandModel::NUM_JOINTS; ++i)
{
problem.SetParameterLowerBound(parm_hand_coeffs.data(), i * 3 + 0, 0.5);
problem.SetParameterLowerBound(parm_hand_coeffs.data(), i * 3 + 1, 0.5);
problem.SetParameterLowerBound(parm_hand_coeffs.data(), i * 3 + 2, 0.5);
problem.SetParameterUpperBound(parm_hand_coeffs.data(), i * 3 + 0, 2);
problem.SetParameterUpperBound(parm_hand_coeffs.data(), i * 3 + 1, 2);
problem.SetParameterUpperBound(parm_hand_coeffs.data(), i * 3 + 2, 2);
}
SetSolverOptions(&options);
options.function_tolerance = 1e-8;
options.max_num_iterations = 30;
options.use_nonmonotonic_steps = true;
options.num_linear_solver_threads = 1;
options.minimizer_progress_to_stdout = true;
options.parameter_tolerance = 1e-12;
CHECK(StringToLinearSolverType("sparse_normal_cholesky",
&options.linear_solver_type));
ceres::Solver::Summary summary;
if (mode == 0)
problem.SetParameterBlockConstant(parm_hand_coeffs.data());
else if (mode == 1)
{
problem.SetParameterBlockConstant(parm_hand_coeffs.data());
problem.SetParameterBlockConstant(parm_hand_pose.data());
// fit_cost_analytic_ha->weight_2d[0] = 0.0;
}
else if (mode == 2)
{
problem.SetParameterBlockConstant(parm_hand_pose.data());
}
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
// std::cout << "Fit shape: coeff" << parm_hand_coeffs << std::endl;
// std::cout << "Fit shape: pose" << parm_hand_pose << std::endl;
// std::cout << "Fit shape: trans" << parm_handl_t << std::endl;
// problem.SetParameterBlockVariable(parm_hand_coeffs.data());
// ceres::Solve(options, &problem, &summary);
std::cout << "After: coeff" << parm_hand_coeffs << std::endl;
std::cout << "After: pose" << parm_hand_pose << std::endl;
std::cout << "After: trans" << parm_handl_t << std::endl;
printf("FitToProjectionCeres: Done\n");
}