RowVectorXd DataPackage::cutData(const RowVectorXd &originalData, int cutFront, int cutBack)
{
if(originalData.cols()-cutFront-cutBack < 0 || cutFront>originalData.cols()) {
qDebug()<<"DataPackage::cutData - cutFront or cutBack do not fit. Aborting mapping and returning original data.";
RowVectorXd returnVec = originalData;
return returnVec;
}
//Cut original data using segment
return (RowVectorXd)originalData.segment(cutFront, originalData.cols()-cutFront-cutBack);
}
virtual void Transform(_Out_ frame_view target_frame, _In_ const_frame_view source_frame, _In_ const_frame_view last_frame, float frame_time) override
{
//if (!g_UseVelocity)
//{
// Transform(target_frame, source_frame);
// return;
//}
const auto& blocks = *pBlockArmature;
int pvDim = inputExtractor.GetDimension(*blocks[0]);
RowVectorXd X(g_UseVelocity ? pvDim * 2 : pvDim), Y;
double semga = 1000;
RowVectorXf yf;
std::vector<RowVectorXd> Xabs;
for (auto& block : blocks)
{
//X[0] *= 13;
if (block->Index > 0 && block->ActiveActions.size() > 0)
{
auto& sik = pController->GetStylizedIK(block->Index);
auto& gpr = sik.Gplvm();
auto& joints = block->Joints;
RowVectorXf xf = inputExtractor.Get(*block, source_frame);
RowVectorXf xfl = inputExtractor.Get(*block, last_frame);
yf = outputExtractor.Get(*block, target_frame);
auto xyf = inputExtractor.Get(*block, target_frame);
auto pDecoder = sik.getDecoder();
auto baseRot = target_frame[block->parent()->Joints.back()->ID].GblRotation;
sik.setBaseRotation(baseRot);
sik.setChain(block->Joints, target_frame);
//sik.SetGplvmWeight(block->Wx.cast<double>());
//std::vector<DirectX::Quaternion, XMAllocator> corrrots(joints.size());
//std::vector<DirectX::Quaternion, XMAllocator> rots(joints.size());
//for (int i = 0; i < joints.size(); i++)
//{
// corrrots[i] = target_frame[joints[i]->ID].LclRotation;
//}
//(*pDecoder)(rots.data(), yf.cast<double>());
////outputExtractor.Set(*block, target_frame, yf);
//auto ep = sik.EndPosition(reinterpret_cast<XMFLOAT4A*>(rots.data()));
X.segment(0, pvDim) = xf.cast<double>();
auto Xd = X.segment(0, pvDim);
auto uXd = gpr.uX.segment(0, pvDim);
//auto uXv = block->PdGpr.uX.segment<3>(3);
//Xv = (Xv - uXv).array() * g_NoiseInterpolation.array() + uXv.array();
Xd = (Xd - uXd).array();
double varZ = (Xd.array() * (g_NoiseInterpolation.array() - 1.0)).cwiseAbs2().sum();
// if no noise
varZ = std::max(varZ, 1e-5);
Xd = Xd.array() * g_NoiseInterpolation.array() + uXd.array();
RowVector3d Xld = (xfl.cast<double>() - uXd).array() * g_NoiseInterpolation.array() + uXd.array();
if (g_UseVelocity)
{
auto Xv = X.segment(pvDim, pvDim);
Xv = (Xd - Xld) / (frame_time * g_FrameTimeScaleFactor);
}
xf = X.cast<float>();
SetVisualizeHandle(block, xf);
//m_Xs.row(block->Index) = X;
Xabs.emplace_back(X);
// Beyesian majarnlize over X
//size_t detail = 3;
//MatrixXd Xs(detail*2+1,g_PvDimension), Ys;
//Xs = gaussian_sample(X, X, detail);
//VectorXd Pxs = (Xs - X.replicate(detail * 2 + 1, 1)).cwiseAbs2().rowwise().sum();
//Pxs = (-Pxs.array() / semga).exp();
//VectorXd Py_xs = block->PdGpr.get_expectation_and_likelihood(Xs, &Ys);
//Py_xs = (-Py_xs.array()).exp() * Pxs.array();
//Py_xs /= Py_xs.sum();
//Y = (Ys.array() * Py_xs.replicate(1, Ys.cols()).array()).colwise().sum();
MatrixXd covObsr(g_PvDimension, g_PvDimension);
covObsr.setZero();
covObsr.diagonal() = g_NoiseInterpolation.replicate(1, g_PvDimension / 3).transpose() * varZ;
//block->PdGpr.get_expectation_from_observation(X, covObsr, &Y);
//block->PdGpr.get_expectation(X, &Y);
//auto yc = yf;
//yf = Y.cast<float>();
//yf.array() *= block->Wx.cwiseInverse().array().transpose();
//block->PdStyleIk.SetHint();
if (!g_UseVelocity)
Y = sik.apply(X.transpose(), baseRot).cast<double>();
else
Y = sik.apply(X.segment(0, pvDim).transpose(), Vector3d(X.segment(pvDim, pvDim).transpose()), baseRot).cast<double>();
//block->PdStyleIk.SetGoal(X.leftCols<3>());
//auto scoref = block->PdStyleIk.objective(X, yf.cast<double>());
//auto scorec = block->PdStyleIk.objective(X, yc.cast<double>());
//std::cout << "Gpr score : " << scoref << " ; Cannonical score : " << scorec << endl;
//auto ep = sik.EndPosition(yf.cast<double>());
//Y = yf.cast<double>();
outputExtractor.Set(*block, target_frame, Y.cast<float>());
for (int i = 0; i < block->Joints.size(); i++)
{
target_frame[block->Joints[i]->ID].UpdateGlobalData(target_frame[block->Joints[i]->parent()->ID]);
}
auto ep2 = target_frame[block->Joints.back()->ID].GblTranslation -
target_frame[block->Joints[0]->parent()->ID].GblTranslation;
//break;
}
}
// Fill Xabpv
if (g_EnableDependentControl)
{
RowVectorXd Xabpv;
Xabpv.resize(pController->uXabpv.size());
int i = 0;
for (const auto& xab : Xabs)
{
auto Yi = Xabpv.segment(i, xab.size());
Yi = xab;
i += xab.size();
}
auto _x = (Xabpv.cast<float>() - pController->uXabpv) * pController->XabpvT;
auto _xd = _x.cast<double>().eval();
for (auto& block : blocks)
{
if (block->ActiveActions.size() == 0 && block->SubActiveActions.size() > 0)
{
auto& sik = pController->GetStylizedIK(block->Index);
auto& gpr = sik.Gplvm();
auto lk = gpr.get_expectation_and_likelihood(_xd, &Y);
yf = Y.cast<float>();
//yf *= block->Wx.cwiseInverse().asDiagonal();
outputExtractor.Set(*block, target_frame, yf);
}
}
}
target_frame[0].LclTranslation = source_frame[0].LclTranslation;
target_frame[0].GblTranslation = source_frame[0].GblTranslation;
FrameRebuildGlobal(*m_tArmature, target_frame);
}
