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;
}