void plotNodesAsSpheres(const Eigen::MatrixXf nodes, const Eigen::VectorXf& pVis, const Eigen::MatrixXf& stdev, PlotSpheres::Ptr spheres) {
assert(nodes.rows() == pVis.rows());
assert(nodes.rows() == stdev.rows());
assert(nodes.cols() == stdev.cols());
MatrixXf centers = nodes.leftCols(3);
MatrixXf colors(nodes.rows(), 4);
if (nodes.cols() >= 6)
//colors << nodes.middleCols(3,3).rowwise().reverse(), 0.25*VectorXf::Ones(nodes.rows());
colors << nodes.middleCols(3,3).rowwise().reverse(), pVis;
//colors << nodes.middleCols(3,3).rowwise().reverse(), nodes.col(6);
else
colors = Vector4f(1,1,1,1).transpose().replicate(nodes.rows(), 1);
VectorXf sizes = (stdev.leftCols(3)/4.0).rowwise().mean();
spheres->plot(util::toVec3Array(centers), util::toVec4Array(colors), toVec(sizes));
}
void plotObs(const Eigen::MatrixXf cloud, PointCloudPlot::Ptr plot) {
MatrixXf centers = cloud.leftCols(3);
MatrixXf colors(cloud.rows(), 4);
if (cloud.cols() >= 6)
colors << cloud.middleCols(3,3).rowwise().reverse(), 0.5*VectorXf::Ones(cloud.rows());
else
colors = Vector4f(1,1,1,1).transpose().replicate(cloud.rows(), 1);
plot->setPoints(util::toVec3Array(centers), util::toVec4Array(colors));
}