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 place::scanToSparse(const cv::Mat &scan,
Eigen::SparseMatrix<double> &sparse) {
std::vector<Eigen::Triplet<double>> tripletList;
for (int i = 0; i < scan.rows; ++i) {
const uchar *src = scan.ptr<uchar>(i);
for (int j = 0; j < scan.cols; ++j) {
if (src[j] == 255)
continue;
double confidence = 1.0 - (double)src[j] / 255.0;
tripletList.push_back(Eigen::Triplet<double>(i, j, confidence));
}
}
sparse = Eigen::SparseMatrix<double>(scan.rows, scan.cols);
sparse.setFromTriplets(tripletList.begin(), tripletList.end());
sparse.makeCompressed();
sparse.prune(1.0);
}
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;
}