bool PointCloudFunctions::saveVmtAsCloud(const cv::SparseMat &vmt, std::string fileName)
{
if (vmt.nzcount() <= 0)
{
std::cout << "NO POINTS FOR " << fileName << ", save aborted." << std::endl;
return false;
}
//TRANSFORM VMT INTO Point Clout
PointCloud<PointXYZI> cloud;
cloud.resize(vmt.nzcount());
int i = 0;
int rows = vmt.size()[1];
int maxZ = vmt.size()[2];
for (cv::SparseMatConstIterator it = vmt.begin(); it != vmt.end(); ++it)
{
const cv::SparseMat::Node* n = it.node();
uchar val = it.value<uchar>();
cloud.points[i].x = n->idx[0];
cloud.points[i].y = rows - n->idx[1];
cloud.points[i].z = maxZ - n->idx[2];
cloud.points[i].intensity = static_cast<float>(val);
i++;
}
if (pcl::io::savePCDFileASCII (fileName, cloud) >= 0) //FIXME: what is the return value? it's not mentioned in http://docs.pointclouds.org/1.6.0/group__io.html#ga5e406a5854fa8ad026cad85158fef266
return true;
else
return false;
}
cv::SparseMat PointCloudFunctions::statisticalOutlierRemoval(const cv::SparseMat &vmt, int meanK, double stdDevMulThreshold)
{
PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);
// Create the filtering object
StatisticalOutlierRemoval<PointXYZI> sor;
sor.setInputCloud (cloud);
sor.setMeanK (meanK);
sor.setStddevMulThresh (stdDevMulThreshold);
sor.filter (*cloud_filtered);
cloud->clear();
cloud.reset();
return convertToSparseMat(cloud_filtered, vmt.dims(), vmt.size());
}
MxArray::MxArray(const cv::SparseMat& mat)
{
if (mat.dims() != 2)
mexErrMsgIdAndTxt("mexopencv:error", "cv::Mat is not 2D");
if (mat.type() != CV_32FC1)
mexErrMsgIdAndTxt("mexopencv:error", "cv::Mat is not float");
// Create a sparse array.
int m = mat.size(0), n = mat.size(1), nnz = mat.nzcount();
p_ = mxCreateSparse(m, n, nnz, mxREAL);
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
mwIndex *ir = mxGetIr(p_);
mwIndex *jc = mxGetJc(p_);
if (ir == NULL || jc == NULL)
mexErrMsgIdAndTxt("mexopencv:error", "Unknown error");
// Sort nodes before we put elems into mxArray.
std::vector<const cv::SparseMat::Node*> nodes;
nodes.reserve(nnz);
for (cv::SparseMatConstIterator it = mat.begin(); it != mat.end(); ++it)
nodes.push_back(it.node());
std::sort(nodes.begin(), nodes.end(), CompareSparseMatNode());
// Copy data.
double *pr = mxGetPr(p_);
int i = 0;
jc[0] = 0;
for (std::vector<const cv::SparseMat::Node*>::const_iterator
it = nodes.begin(); it != nodes.end(); ++it)
{
mwIndex row = (*it)->idx[0], col = (*it)->idx[1];
ir[i] = row;
jc[col+1] = i+1;
pr[i] = static_cast<double>(mat.value<float>(*it));
++i;
}
}
PointCloud<PointXYZI>::Ptr PointCloudFunctions::convertToPointCloud(const cv::SparseMat &vmt)
{
PointCloud<PointXYZI>::Ptr cloud(new PointCloud<PointXYZI> ());
cloud->resize(vmt.nzcount());
int i = 0;
int rows = vmt.size()[1];
int maxZ = vmt.size()[2];
for (cv::SparseMatConstIterator it=vmt.begin(); it != vmt.end(); ++it)
{
const cv::SparseMat::Node* n = it.node();
uchar val = it.value<uchar>();
cloud->points[i].x = n->idx[0];
cloud->points[i].y = rows - n->idx[1];
cloud->points[i].z = maxZ - n->idx[2];
cloud->points[i].intensity = static_cast<float>(val);
i++;
}
return cloud;
}
