Exemple #1
0
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;
}
Exemple #2
0
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;
    }
}
Exemple #3
0
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;
}