Eigen::SparseMatrix<double> Condi2Joint(Eigen::SparseMatrix<double> Condi, Eigen::SparseVector<double> Pa)
{ // second dimension of Condi is the parent
Eigen::SparseMatrix<double> Joint;
Joint.resize(Condi.rows(), Condi.cols());
for (int cols = 0; cols < Condi.cols(); cols++)
{
Eigen::SparseVector<double> tmp_vec = Condi.block(0, cols, Condi.rows(), 1)*Pa.coeff(cols);
for (int id_rows = 0; id_rows < tmp_vec.size(); id_rows++)
{
Joint.coeffRef(id_rows, cols) = tmp_vec.coeff(id_rows);
}
}
Joint.prune(TOLERANCE);
return Joint;
}
void dart::NoCameraMovementPrior::computeContribution(Eigen::SparseMatrix<float> & JTJ,
Eigen::VectorXf & JTe,
const int * modelOffsets,
const int priorParamOffset,
const std::vector<MirroredModel *> & models,
const std::vector<Pose> & poses,
const OptimizationOptions & opts)
{
// get offsets for selected model in the full parameter space
const Pose & srcPose = poses[_srcModelID];
const int srcDims = srcPose.getReducedDimensions();
const int srcOffset = modelOffsets[_srcModelID];
// get step in parameter space at current optimization state
Eigen::VectorXf paramUpdate = JTJ.block(srcOffset,srcOffset,srcDims,srcDims).triangularView<Eigen::Upper>().solve(JTe.segment(srcOffset,srcDims));
// set camera to model transformation (first 6 parameters) to zero
paramUpdate.head<6>() = Eigen::VectorXf::Zero(6);
// compute jacobian and error to achieve no movement of camera frame
JTe.segment(srcOffset,srcDims) = JTJ.block(srcOffset,srcOffset,srcDims,srcDims) * paramUpdate;
}
Eigen::SparseMatrix<double> normProbMatrix(Eigen::SparseMatrix<double> P)
{
// each column is a probability simplex
Eigen::SparseMatrix<double> P_norm;
P_norm.resize(P.rows(), P.cols());
for (int col = 0; col < P.cols(); col++)
{
//SparseVector<double> A_col_sparse = A_sparse.block(0, i, A_sparse.rows(),1);
SparseVector<double> P_vec = P.block(0, col, P.rows(), 1);
SparseVector<double> P_vec_norm;
P_vec_norm.resize(P_vec.size());
P_vec_norm = normProbVector(P_vec);
for (int id_row = 0; id_row < P.rows(); id_row++)
{
P_norm.coeffRef(id_row, col) = P_vec_norm.coeff(id_row);
}
}
P_norm.makeCompressed();
P_norm.prune(TOLERANCE);
return P_norm;
}
Eigen::SparseMatrix<double> joint2conditional(Eigen::SparseMatrix<double> edgePot)// pa is the second dimension
{ // second dimension of edgePot is the parent
Eigen::SparseMatrix<double> Conditional;
Conditional.resize(edgePot.rows(), edgePot.cols());
Eigen::SparseVector<double> Parent_Marginal;
Parent_Marginal.resize(edgePot.cols());
for (int id_col = 0; id_col < edgePot.cols(); id_col++)
{
Eigen::SparseVector<double> tmp_vec = edgePot.block(0, id_col, edgePot.rows(), 1);
Parent_Marginal.coeffRef(id_col) = tmp_vec.sum();
if (Parent_Marginal.coeff(id_col)>TOLERANCE)
for (int id_row = 0; id_row < edgePot.rows(); id_row++)
{
Conditional.coeffRef(id_row, id_col) = edgePot.coeff(id_row, id_col) / Parent_Marginal.coeff(id_col);
}
}
Conditional.makeCompressed();
Conditional.prune(TOLERANCE);
return Conditional;
}