void HandFastCost::ForwardKinematics()

in visualization/FitAdam/src/HandFastCost.cpp [432:694]

• 163 lines of code
• 20 McCabe index (conditional complexity)

void HandFastCost::ForwardKinematics(double const* p_euler, double const* c, double* J_data, double* dJdP_data, double* dJdc_data) const
{
	using namespace Eigen;
	Eigen::Map< Matrix<double, smpl::HandModel::NUM_JOINTS, 3, RowMajor> > outJ(J_data);
	Map< Matrix<double, smpl::HandModel::NUM_JOINTS * 3, smpl::HandModel::NUM_JOINTS * 3, RowMajor> > dJdP(dJdP_data);
	Map< Matrix<double, smpl::HandModel::NUM_JOINTS * 3, smpl::HandModel::NUM_JOINTS * 3, RowMajor> > dJdc(dJdc_data);
	Matrix<double, 3, 3, RowMajor> R1; // Interface with ceres (Angle Axis Output)
	Matrix<double, 3, 3, RowMajor> S; // Scaling diagonal
	Matrix<double, 3, 3, RowMajor> R2; // R1 * S
	Matrix<double, 3, 3, RowMajor> R3; // m_M_l2pl
	Matrix<double, 3, 3, RowMajor> R4; // R3 * R2;

	// some buffer
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR1dP(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR1dc(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR2dP(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR2dc(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR3dP(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR3dc(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR4dP(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dR4dc(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dSdP(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, RowMajor> dSdc(9, 3 * smpl::HandModel::NUM_JOINTS);
    Matrix<double, 3, 1> offset; // a buffer for 3D vector
    Matrix<double, 3, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor> dtdP(3, 3 * smpl::HandModel::NUM_JOINTS); // a buffer for the derivative
    Matrix<double, 3, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor> dtdc(3, 3 * smpl::HandModel::NUM_JOINTS); // a buffer for the derivative

    std::vector<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> MR(smpl::HandModel::NUM_JOINTS, Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(3, 3));
    std::vector<Eigen::Matrix<double, 3, 1>> Mt(smpl::HandModel::NUM_JOINTS, Eigen::Matrix<double, 3, 1>(3, 1));

    std::vector<Eigen::Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>> dMRdP(smpl::HandModel::NUM_JOINTS, Eigen::Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>(9, 3 * smpl::HandModel::NUM_JOINTS));
    std::vector<Eigen::Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>> dMRdc(smpl::HandModel::NUM_JOINTS, Eigen::Matrix<double, 9, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>(9, 3 * smpl::HandModel::NUM_JOINTS));
    std::vector<Eigen::Matrix<double, 3, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>> dMtdP(smpl::HandModel::NUM_JOINTS, Eigen::Matrix<double, 3, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>(3, 3 * smpl::HandModel::NUM_JOINTS));
    std::vector<Eigen::Matrix<double, 3, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>> dMtdc(smpl::HandModel::NUM_JOINTS, Eigen::Matrix<double, 3, 3 * smpl::HandModel::NUM_JOINTS, Eigen::RowMajor>(3, 3 * smpl::HandModel::NUM_JOINTS));

    int idj = handm_.update_inds_(0);
    ceres::AngleAxisToRotationMatrix(p_euler + idj * 3, R1.data());
    // S = Eigen::DiagonalMatrix<double, 3>(exp(c[3 * idj]), exp(c[3 * idj]), exp(c[3 * idj]));
    S = Eigen::DiagonalMatrix<double, 3>(c[3 * idj], c[3 * idj], c[3 * idj]);
    R2 = R1 * S;
    R3 = handm_.m_M_l2pl.block(idj * 4, 0, 3, 3);
    offset = handm_.m_M_l2pl.block(idj * 4, 3, 3, 1);
    MR.at(idj) = R3 * R2;  // no parent
    Mt.at(idj) = offset;
    outJ.row(idj) = Mt.at(idj).transpose();

    if (dJdP_data && dJdc_data)
    {
	    AngleAxisToRotationMatrix_Derivative(p_euler + 3 * idj, dR1dP.data(), idj, 3 * smpl::HandModel::NUM_JOINTS);
	    std::fill(dR1dc.data(), dR1dc.data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);
	    std::fill(dSdP.data(), dSdP.data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);
	    std::fill(dSdc.data(), dSdc.data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);
	    // dSdc.data()[3 * idj] = exp(c[3 * idj]);
	    // dSdc.data()[3 * idj + 4 * 3 * smpl::HandModel::NUM_JOINTS] = exp(c[3 * idj]);
	    // dSdc.data()[3 * idj + 8 * 3 * smpl::HandModel::NUM_JOINTS] = exp(c[3 * idj]);
	    dSdc.data()[3 * idj] = 1;
	    dSdc.data()[3 * idj + 4 * 3 * smpl::HandModel::NUM_JOINTS] = 1;
	    dSdc.data()[3 * idj + 8 * 3 * smpl::HandModel::NUM_JOINTS] = 1;
	    SparseProductDerivative(R1.data(), dR1dP.data(), S.data(), dSdP.data(), idj, parentIndexes[idj], dR2dP.data(), 3 * smpl::HandModel::NUM_JOINTS);
	    SparseProductDerivative(R1.data(), dR1dc.data(), S.data(), dSdc.data(), idj, parentIndexes[idj], dR2dc.data(), 3 * smpl::HandModel::NUM_JOINTS);
	    std::fill(dR3dP.data(), dR3dP.data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);  // R3 is precomputed
	    std::fill(dR3dc.data(), dR3dc.data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);  // R3 is precomputed
	    SparseProductDerivative(R3.data(), dR3dP.data(), R2.data(), dR2dP.data(), idj, parentIndexes[idj], dMRdP[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
	    SparseProductDerivative(R3.data(), dR3dc.data(), R2.data(), dR2dc.data(), idj, parentIndexes[idj], dMRdc[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
	    std::fill(dMtdP[idj].data(), dMtdP[idj].data() + 3 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);
	    std::fill(dMtdc[idj].data(), dMtdc[idj].data() + 3 * 3 * smpl::HandModel::NUM_JOINTS, 0.0);
	    std::copy(dMtdP[idj].data(), dMtdP[idj].data() + 3 * 3 * smpl::HandModel::NUM_JOINTS, dJdP_data + 3 * smpl::HandModel::NUM_JOINTS * 3 * idj);
	    std::copy(dMtdc[idj].data(), dMtdc[idj].data() + 3 * 3 * smpl::HandModel::NUM_JOINTS, dJdc_data + 3 * smpl::HandModel::NUM_JOINTS * 3 * idj);
	    // set back the dSdc value (no need to reset)
	    dSdc.data()[3 * idj] = 0;
	    dSdc.data()[3 * idj + 4 * 3 * smpl::HandModel::NUM_JOINTS] = 0;
	    dSdc.data()[3 * idj + 8 * 3 * smpl::HandModel::NUM_JOINTS] = 0;
	}

	for (int idji = 1; idji < handm_.NUM_JOINTS; idji++)
	{
		idj = handm_.update_inds_(idji);
		int ipar = handm_.parents_(idj);
		double angles[3];
		// if (idj == 4 || idj == 6 || idj == 8 || idj == 10)
		// {
		// 	angles[0] = 0;
		// 	angles[1] = p_euler[idj * 3 + 1];
		// 	angles[2] = p_euler[idj * 3 + 2];
		// }
		// else if (idj == 2 || idj == 12 || idj == 13 || idj == 14 || idj == 15 || idj == 16 || idj == 17 || idj == 18 || idj == 19)		//No twist, no side directional
		// {
		// 	angles[0] = 0;
		// 	angles[1] = 0;
		// 	angles[2] = p_euler[idj * 3 + 2];
		// }
		// else
		// {
		// 	angles[0] = p_euler[idj * 3 + 0];
		// 	angles[1] = p_euler[idj * 3 + 1];
		// 	angles[2] = p_euler[idj * 3 + 2];
		// }
		angles[0] = p_euler[idj * 3 + 0];
		angles[1] = p_euler[idj * 3 + 1];
		angles[2] = p_euler[idj * 3 + 2];
		if(euler_) ceres::EulerAnglesToRotationMatrix(angles, 3, R1.data());
		else ceres::AngleAxisToRotationMatrix(angles, R1.data());
		// S = Eigen::DiagonalMatrix<double, 3>(exp(c[3 * idj]), 1, 1);
		S = Eigen::DiagonalMatrix<double, 3>(c[3 * idj], 1, 1);
		R2 = R1 * S;
		R3 = handm_.m_M_l2pl.block(idj * 4, 0, 3, 3);
		offset = handm_.m_M_l2pl.block(idj * 4, 3, 3, 1);
		R4 = R3 * R2;
		MR.at(idj) = MR.at(ipar) * R4;
		Mt.at(idj) = MR.at(ipar) * offset + Mt.at(ipar);

	    if (dJdP_data && dJdc_data)
	    {
		    if(euler_) EulerAnglesToRotationMatrix_Derivative(angles, dR1dP.data(), idj, 3 * smpl::HandModel::NUM_JOINTS);
		    else AngleAxisToRotationMatrix_Derivative(angles, dR1dP.data(), idj, 3 * smpl::HandModel::NUM_JOINTS);
   //  		if (idj == 4 || idj == 6 || idj == 8 || idj == 10)
			// {
			// 	dR1dP.block(0, 3 * idj, 9, 1).setZero();
			// }
			// else if (idj == 2 || idj == 12 || idj == 13 || idj == 14 || idj == 15 || idj == 16 || idj == 17 || idj == 18 || idj == 19)		//No twist, no side directional
			// {
			// 	dR1dP.block(0, 3 * idj, 9, 2).setZero();
			// }
		    // no need to reset dR1dc
		    // no need to reset dSdP
		    // dSdc.data()[3 * idj] = exp(c[3 * idj]);
		    dSdc.data()[3 * idj] = 1;
		    SparseProductDerivative(R1.data(), dR1dP.data(), S.data(), dSdP.data(), idj, parentIndexes[idj], dR2dP.data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseProductDerivative(R1.data(), dR1dc.data(), S.data(), dSdc.data(), idj, parentIndexes[idj], dR2dc.data(), 3 * smpl::HandModel::NUM_JOINTS);
		    // no need to reset dR3dP
		    // no need to reset dR3dc
		    SparseProductDerivative(R3.data(), dR3dP.data(), R2.data(), dR2dP.data(), idj, parentIndexes[idj], dR4dP.data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseProductDerivative(R3.data(), dR3dc.data(), R2.data(), dR2dc.data(), idj, parentIndexes[idj], dR4dc.data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseProductDerivative(MR.at(ipar).data(), dMRdP[ipar].data(), R4.data(), dR4dP.data(), idj, parentIndexes[idj], dMRdP[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseProductDerivative(MR.at(ipar).data(), dMRdc[ipar].data(), R4.data(), dR4dc.data(), idj, parentIndexes[idj], dMRdc[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseProductDerivative(dMRdP[ipar].data(), offset.data(), parentIndexes[idj], dMtdP[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseProductDerivative(dMRdc[ipar].data(), offset.data(), parentIndexes[idj], dMtdc[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseAdd(dMtdP[ipar].data(), parentIndexes[idj], dMtdP[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
		    SparseAdd(dMtdc[ipar].data(), parentIndexes[idj], dMtdc[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
		    // reset dSdc
		    dSdc.data()[3 * idj] = 0;

// if (idji == 20)
// {
// std::cout << "(0, 0)\n" << MR[idj](0, 0) << std::endl;
// std::cout << "(0, 0)\n" << dMRdP[idj].row(0) << std::endl;
// std::cout << "(0, 0)\n" << dMRdc[idj].row(0) << std::endl;
// std::cout << "(0, 1)\n" << MR[idj](0, 1) << std::endl;
// std::cout << "(0, 1)\n" << dMRdP[idj].row(1) << std::endl;
// std::cout << "(0, 1)\n" << dMRdc[idj].row(1) << std::endl;
// std::cout << "(0, 2)\n" << MR[idj](0, 2) << std::endl;
// std::cout << "(0, 2)\n" << dMRdP[idj].row(2) << std::endl;
// std::cout << "(0, 2)\n" << dMRdc[idj].row(2) << std::endl;
// std::cout << "(1, 0)\n" << MR[idj](1, 0) << std::endl;
// std::cout << "(1, 0)\n" << dMRdP[idj].row(3) << std::endl;
// std::cout << "(1, 0)\n" << dMRdc[idj].row(3) << std::endl;
// std::cout << "(1, 1)\n" << MR[idj](1, 1) << std::endl;
// std::cout << "(1, 1)\n" << dMRdP[idj].row(4) << std::endl;
// std::cout << "(1, 1)\n" << dMRdc[idj].row(4) << std::endl;
// std::cout << "(1, 2)\n" << MR[idj](1, 2) << std::endl;
// std::cout << "(1, 2)\n" << dMRdP[idj].row(5) << std::endl;
// std::cout << "(1, 2)\n" << dMRdc[idj].row(5) << std::endl;
// std::cout << "(2, 0)\n" << MR[idj](2, 0) << std::endl;
// std::cout << "(2, 0)\n" << dMRdP[idj].row(6) << std::endl;
// std::cout << "(2, 0)\n" << dMRdc[idj].row(6) << std::endl;
// std::cout << "(2, 1)\n" << MR[idj](2, 1) << std::endl;
// std::cout << "(2, 1)\n" << dMRdP[idj].row(7) << std::endl;
// std::cout << "(2, 1)\n" << dMRdc[idj].row(7) << std::endl;
// std::cout << "(2, 2)\n" << MR[idj](2, 2) << std::endl;
// std::cout << "(2, 2)\n" << dMRdP[idj].row(8) << std::endl;
// std::cout << "(2, 2)\n" << dMRdc[idj].row(8) << std::endl;
// }
		}
	}

	for (int idji = 1; idji < handm_.NUM_JOINTS; idji++)
	{
		idj = handm_.m_jointmap_pm2model(idji);  // joint_order vs update_inds_ // joint_order(SMC -> idj)
		outJ.row(idj) = Mt.at(idj).transpose();

		if (dJdP_data && dJdc_data)
		{
		    std::copy(dMtdP[idj].data(), dMtdP[idj].data() + 3 * 3 * smpl::HandModel::NUM_JOINTS, dJdP_data + 3 * smpl::HandModel::NUM_JOINTS * 3 * idj);
		    std::copy(dMtdc[idj].data(), dMtdc[idj].data() + 3 * 3 * smpl::HandModel::NUM_JOINTS, dJdc_data + 3 * smpl::HandModel::NUM_JOINTS * 3 * idj);
		}
	}

	if (regressor_type == 1 || fit_surface)
	{
		// compute LBS for joints apply regressor regressor
		// First Compute output transform,
		// Original:
		// outT.block(idj * 3, 0, 3, 4) = Ms.block(idj * 4, 0, 3, 4)*mod_.m_M_w2l.block(idj * 4, 0, 4, 4).cast<T>();
		for (int idji = 0; idji < handm_.NUM_JOINTS; idji++)
		{
			const int idj = handm_.update_inds_(idji);
			offset = handm_.m_M_w2l.block(idj * 4, 3, 3, 1);
			R3 = handm_.m_M_w2l.block(idj * 4, 0, 3, 3); 
			// first compute the derivative, in case the original variable changes
			if (dJdP_data && dJdc_data)
			{
				std::copy(dMRdP[idj].data(), dMRdP[idj].data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, dR2dP.data());  // copy the jacobian to dR2dP
				std::copy(dMRdc[idj].data(), dMRdc[idj].data() + 9 * 3 * smpl::HandModel::NUM_JOINTS, dR2dc.data());  // copy the jacobian to dR2dc
				// dR3dP, dR3dc has always been zero
			    SparseProductDerivative(dMRdP[idj].data(), offset.data(), parentIndexes[idj], dtdP.data(), 3 * smpl::HandModel::NUM_JOINTS);
			    SparseProductDerivative(dMRdc[idj].data(), offset.data(), parentIndexes[idj], dtdc.data(), 3 * smpl::HandModel::NUM_JOINTS);
			    SparseAdd(dtdP.data(), parentIndexes[idj], dMtdP[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
			    SparseAdd(dtdc.data(), parentIndexes[idj], dMtdc[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
				SparseProductDerivative(MR.at(idj).data(), dR2dP.data(), R3.data(), dR3dP.data(), idj, parentIndexes[idj], dMRdP[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
				SparseProductDerivative(MR.at(idj).data(), dR2dc.data(), R3.data(), dR3dc.data(), idj, parentIndexes[idj], dMRdc[idj].data(), 3 * smpl::HandModel::NUM_JOINTS);
			}
			Mt.at(idj) = MR.at(idj) * offset + Mt.at(idj);
			MR.at(idj) = MR.at(idj) * R3;
		}
// std::cout << Mt[1] << "\n";
// std::cout << "dMtdP[1]\n" << dMtdP[1].row(0) << "\n";
// std::cout << "dMtdc[1]\n" << dMtdc[1].row(0) << "\n";

		// Second Perform LBS given output transformation
		MatrixXdr outVert(total_vertex.size(), 3);
		MatrixXdr dVdP(total_vertex.size() * 3, smpl::HandModel::NUM_POSE_PARAMETERS);
		MatrixXdr dVdc(total_vertex.size() * 3, smpl::HandModel::NUM_SHAPE_COEFFICIENTS);
		select_lbs(MR, Mt, dMRdP, dMRdc, dMtdP, dMtdc, outVert, dVdP.data(), dVdc.data());

		if (regressor_type == 1)
		{
			// Third, perform Sparse Regression
			// The STB regressor only contains the wrist
			// MatrixXdr outJ(1, 3);
			// MatrixXdr dJdP(3, smpl::HandModel::NUM_POSE_PARAMETERS);
			// MatrixXdr dJdc(3, smpl::HandModel::NUM_SHAPE_COEFFICIENTS);
			const int idj = handm_.update_inds_(0);
			SparseRegress(handm_.STB_wrist_reg,
						  outVert.data(),
						  dJdP_data? dVdP.data() : nullptr,
						  dJdP_data? dVdc.data() : nullptr,
						  outJ.data() + 3 * idj,
						  dJdP_data? dJdP_data + 3 * idj * smpl::HandModel::NUM_POSE_PARAMETERS : nullptr,
						  dJdP_data? dJdc_data + 3 * idj * smpl::HandModel::NUM_SHAPE_COEFFICIENTS : nullptr);
		}

		if (fit_surface)
		{
			const int offset = handm_.NUM_JOINTS;
			// copy the vertex
			for (auto i = 0u; i < correspondence_vertex_constraints.size(); i++)
			{
				const int iv = correspondence_vertex_constraints[i];
				J_data[(offset + i) * 3 + 0] = outVert.data()[3 * iv + 0];
				J_data[(offset + i) * 3 + 1] = outVert.data()[3 * iv + 1];
				J_data[(offset + i) * 3 + 2] = outVert.data()[3 * iv + 2];
				if (dJdP_data)
				{
					// also need to copy the jacobian
					std::copy(dVdP.data() + 3 * iv * smpl::HandModel::NUM_POSE_PARAMETERS, dVdP.data() + (3 * iv + 3) * smpl::HandModel::NUM_POSE_PARAMETERS,
    						  dJdP_data + 3 * (offset + i) * smpl::HandModel::NUM_POSE_PARAMETERS);
					std::copy(dVdc.data() + 3 * iv * smpl::HandModel::NUM_SHAPE_COEFFICIENTS, dVdc.data() + (3 * iv + 3) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS,
    						  dJdc_data + 3 * (offset + i) * smpl::HandModel::NUM_SHAPE_COEFFICIENTS);
				}
			}
		}
	}
}