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