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