KPConv-PyTorch/cpp_wrappers/cpp_subsampling/wrapper.cpp

566 lines
17 KiB
C++
Raw Normal View History

2020-03-31 19:42:35 +00:00
#include <Python.h>
#include <numpy/arrayobject.h>
#include "grid_subsampling/grid_subsampling.h"
#include <string>
// docstrings for our module
// *************************
static char module_docstring[] = "This module provides an interface for the subsampling of a batch of stacked pointclouds";
static char subsample_docstring[] = "function subsampling a pointcloud";
static char subsample_batch_docstring[] = "function subsampling a batch of stacked pointclouds";
// Declare the functions
// *********************
static PyObject *cloud_subsampling(PyObject* self, PyObject* args, PyObject* keywds);
static PyObject *batch_subsampling(PyObject *self, PyObject *args, PyObject *keywds);
// Specify the members of the module
// *********************************
static PyMethodDef module_methods[] =
{
{ "subsample", (PyCFunction)cloud_subsampling, METH_VARARGS | METH_KEYWORDS, subsample_docstring },
{ "subsample_batch", (PyCFunction)batch_subsampling, METH_VARARGS | METH_KEYWORDS, subsample_batch_docstring },
{NULL, NULL, 0, NULL}
};
// Initialize the module
// *********************
static struct PyModuleDef moduledef =
{
PyModuleDef_HEAD_INIT,
"grid_subsampling", // m_name
module_docstring, // m_doc
-1, // m_size
module_methods, // m_methods
NULL, // m_reload
NULL, // m_traverse
NULL, // m_clear
NULL, // m_free
};
PyMODINIT_FUNC PyInit_grid_subsampling(void)
{
import_array();
return PyModule_Create(&moduledef);
}
// Definition of the batch_subsample method
// **********************************
static PyObject* batch_subsampling(PyObject* self, PyObject* args, PyObject* keywds)
{
// Manage inputs
// *************
// Args containers
PyObject* points_obj = NULL;
PyObject* features_obj = NULL;
PyObject* classes_obj = NULL;
PyObject* batches_obj = NULL;
// Keywords containers
static char* kwlist[] = { "points", "batches", "features", "classes", "sampleDl", "method", "max_p", "verbose", NULL };
float sampleDl = 0.1;
const char* method_buffer = "barycenters";
int verbose = 0;
int max_p = 0;
// Parse the input
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|$OOfsii", kwlist, &points_obj, &batches_obj, &features_obj, &classes_obj, &sampleDl, &method_buffer, &max_p, &verbose))
{
PyErr_SetString(PyExc_RuntimeError, "Error parsing arguments");
return NULL;
}
// Get the method argument
string method(method_buffer);
// Interpret method
if (method.compare("barycenters") && method.compare("voxelcenters"))
{
PyErr_SetString(PyExc_RuntimeError, "Error parsing method. Valid method names are \"barycenters\" and \"voxelcenters\" ");
return NULL;
}
// Check if using features or classes
bool use_feature = true, use_classes = true;
if (features_obj == NULL)
use_feature = false;
if (classes_obj == NULL)
use_classes = false;
// Interpret the input objects as numpy arrays.
PyObject* points_array = PyArray_FROM_OTF(points_obj, NPY_FLOAT, NPY_IN_ARRAY);
PyObject* batches_array = PyArray_FROM_OTF(batches_obj, NPY_INT, NPY_IN_ARRAY);
PyObject* features_array = NULL;
PyObject* classes_array = NULL;
if (use_feature)
features_array = PyArray_FROM_OTF(features_obj, NPY_FLOAT, NPY_IN_ARRAY);
if (use_classes)
classes_array = PyArray_FROM_OTF(classes_obj, NPY_INT, NPY_IN_ARRAY);
// Verify data was load correctly.
if (points_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input points to numpy arrays of type float32");
return NULL;
}
if (batches_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input batches to numpy arrays of type int32");
return NULL;
}
if (use_feature && features_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input features to numpy arrays of type float32");
return NULL;
}
if (use_classes && classes_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input classes to numpy arrays of type int32");
return NULL;
}
// Check that the input array respect the dims
if ((int)PyArray_NDIM(points_array) != 2 || (int)PyArray_DIM(points_array, 1) != 3)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : points.shape is not (N, 3)");
return NULL;
}
if ((int)PyArray_NDIM(batches_array) > 1)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : batches.shape is not (B,) ");
return NULL;
}
if (use_feature && ((int)PyArray_NDIM(features_array) != 2))
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : features.shape is not (N, d)");
return NULL;
}
if (use_classes && (int)PyArray_NDIM(classes_array) > 2)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : classes.shape is not (N,) or (N, d)");
return NULL;
}
// Number of points
int N = (int)PyArray_DIM(points_array, 0);
// Number of batches
int Nb = (int)PyArray_DIM(batches_array, 0);
// Dimension of the features
int fdim = 0;
if (use_feature)
fdim = (int)PyArray_DIM(features_array, 1);
//Dimension of labels
int ldim = 1;
if (use_classes && (int)PyArray_NDIM(classes_array) == 2)
ldim = (int)PyArray_DIM(classes_array, 1);
// Check that the input array respect the number of points
if (use_feature && (int)PyArray_DIM(features_array, 0) != N)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : features.shape is not (N, d)");
return NULL;
}
if (use_classes && (int)PyArray_DIM(classes_array, 0) != N)
{
Py_XDECREF(points_array);
Py_XDECREF(batches_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : classes.shape is not (N,) or (N, d)");
return NULL;
}
// Call the C++ function
// *********************
// Create pyramid
if (verbose > 0)
cout << "Computing cloud pyramid with support points: " << endl;
// Convert PyArray to Cloud C++ class
vector<PointXYZ> original_points;
vector<int> original_batches;
vector<float> original_features;
vector<int> original_classes;
original_points = vector<PointXYZ>((PointXYZ*)PyArray_DATA(points_array), (PointXYZ*)PyArray_DATA(points_array) + N);
original_batches = vector<int>((int*)PyArray_DATA(batches_array), (int*)PyArray_DATA(batches_array) + Nb);
if (use_feature)
original_features = vector<float>((float*)PyArray_DATA(features_array), (float*)PyArray_DATA(features_array) + N * fdim);
if (use_classes)
original_classes = vector<int>((int*)PyArray_DATA(classes_array), (int*)PyArray_DATA(classes_array) + N * ldim);
// Subsample
vector<PointXYZ> subsampled_points;
vector<float> subsampled_features;
vector<int> subsampled_classes;
vector<int> subsampled_batches;
batch_grid_subsampling(original_points,
subsampled_points,
original_features,
subsampled_features,
original_classes,
subsampled_classes,
original_batches,
subsampled_batches,
sampleDl,
max_p);
// Check result
if (subsampled_points.size() < 1)
{
PyErr_SetString(PyExc_RuntimeError, "Error");
return NULL;
}
// Manage outputs
// **************
// Dimension of input containers
npy_intp* point_dims = new npy_intp[2];
point_dims[0] = subsampled_points.size();
point_dims[1] = 3;
npy_intp* feature_dims = new npy_intp[2];
feature_dims[0] = subsampled_points.size();
feature_dims[1] = fdim;
npy_intp* classes_dims = new npy_intp[2];
classes_dims[0] = subsampled_points.size();
classes_dims[1] = ldim;
npy_intp* batches_dims = new npy_intp[1];
batches_dims[0] = Nb;
// Create output array
PyObject* res_points_obj = PyArray_SimpleNew(2, point_dims, NPY_FLOAT);
PyObject* res_batches_obj = PyArray_SimpleNew(1, batches_dims, NPY_INT);
PyObject* res_features_obj = NULL;
PyObject* res_classes_obj = NULL;
PyObject* ret = NULL;
// Fill output array with values
size_t size_in_bytes = subsampled_points.size() * 3 * sizeof(float);
memcpy(PyArray_DATA(res_points_obj), subsampled_points.data(), size_in_bytes);
size_in_bytes = Nb * sizeof(int);
memcpy(PyArray_DATA(res_batches_obj), subsampled_batches.data(), size_in_bytes);
if (use_feature)
{
size_in_bytes = subsampled_points.size() * fdim * sizeof(float);
res_features_obj = PyArray_SimpleNew(2, feature_dims, NPY_FLOAT);
memcpy(PyArray_DATA(res_features_obj), subsampled_features.data(), size_in_bytes);
}
if (use_classes)
{
size_in_bytes = subsampled_points.size() * ldim * sizeof(int);
res_classes_obj = PyArray_SimpleNew(2, classes_dims, NPY_INT);
memcpy(PyArray_DATA(res_classes_obj), subsampled_classes.data(), size_in_bytes);
}
// Merge results
if (use_feature && use_classes)
ret = Py_BuildValue("NNNN", res_points_obj, res_batches_obj, res_features_obj, res_classes_obj);
else if (use_feature)
ret = Py_BuildValue("NNN", res_points_obj, res_batches_obj, res_features_obj);
else if (use_classes)
ret = Py_BuildValue("NNN", res_points_obj, res_batches_obj, res_classes_obj);
else
ret = Py_BuildValue("NN", res_points_obj, res_batches_obj);
// Clean up
// ********
Py_DECREF(points_array);
Py_DECREF(batches_array);
Py_XDECREF(features_array);
Py_XDECREF(classes_array);
return ret;
}
// Definition of the subsample method
// ****************************************
static PyObject* cloud_subsampling(PyObject* self, PyObject* args, PyObject* keywds)
{
// Manage inputs
// *************
// Args containers
PyObject* points_obj = NULL;
PyObject* features_obj = NULL;
PyObject* classes_obj = NULL;
// Keywords containers
static char* kwlist[] = { "points", "features", "classes", "sampleDl", "method", "verbose", NULL };
float sampleDl = 0.1;
const char* method_buffer = "barycenters";
int verbose = 0;
// Parse the input
if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|$OOfsi", kwlist, &points_obj, &features_obj, &classes_obj, &sampleDl, &method_buffer, &verbose))
{
PyErr_SetString(PyExc_RuntimeError, "Error parsing arguments");
return NULL;
}
// Get the method argument
string method(method_buffer);
// Interpret method
if (method.compare("barycenters") && method.compare("voxelcenters"))
{
PyErr_SetString(PyExc_RuntimeError, "Error parsing method. Valid method names are \"barycenters\" and \"voxelcenters\" ");
return NULL;
}
// Check if using features or classes
bool use_feature = true, use_classes = true;
if (features_obj == NULL)
use_feature = false;
if (classes_obj == NULL)
use_classes = false;
// Interpret the input objects as numpy arrays.
PyObject* points_array = PyArray_FROM_OTF(points_obj, NPY_FLOAT, NPY_IN_ARRAY);
PyObject* features_array = NULL;
PyObject* classes_array = NULL;
if (use_feature)
features_array = PyArray_FROM_OTF(features_obj, NPY_FLOAT, NPY_IN_ARRAY);
if (use_classes)
classes_array = PyArray_FROM_OTF(classes_obj, NPY_INT, NPY_IN_ARRAY);
// Verify data was load correctly.
if (points_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input points to numpy arrays of type float32");
return NULL;
}
if (use_feature && features_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input features to numpy arrays of type float32");
return NULL;
}
if (use_classes && classes_array == NULL)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Error converting input classes to numpy arrays of type int32");
return NULL;
}
// Check that the input array respect the dims
if ((int)PyArray_NDIM(points_array) != 2 || (int)PyArray_DIM(points_array, 1) != 3)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : points.shape is not (N, 3)");
return NULL;
}
if (use_feature && ((int)PyArray_NDIM(features_array) != 2))
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : features.shape is not (N, d)");
return NULL;
}
if (use_classes && (int)PyArray_NDIM(classes_array) > 2)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : classes.shape is not (N,) or (N, d)");
return NULL;
}
// Number of points
int N = (int)PyArray_DIM(points_array, 0);
// Dimension of the features
int fdim = 0;
if (use_feature)
fdim = (int)PyArray_DIM(features_array, 1);
//Dimension of labels
int ldim = 1;
if (use_classes && (int)PyArray_NDIM(classes_array) == 2)
ldim = (int)PyArray_DIM(classes_array, 1);
// Check that the input array respect the number of points
if (use_feature && (int)PyArray_DIM(features_array, 0) != N)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : features.shape is not (N, d)");
return NULL;
}
if (use_classes && (int)PyArray_DIM(classes_array, 0) != N)
{
Py_XDECREF(points_array);
Py_XDECREF(classes_array);
Py_XDECREF(features_array);
PyErr_SetString(PyExc_RuntimeError, "Wrong dimensions : classes.shape is not (N,) or (N, d)");
return NULL;
}
// Call the C++ function
// *********************
// Create pyramid
if (verbose > 0)
cout << "Computing cloud pyramid with support points: " << endl;
// Convert PyArray to Cloud C++ class
vector<PointXYZ> original_points;
vector<float> original_features;
vector<int> original_classes;
original_points = vector<PointXYZ>((PointXYZ*)PyArray_DATA(points_array), (PointXYZ*)PyArray_DATA(points_array) + N);
if (use_feature)
original_features = vector<float>((float*)PyArray_DATA(features_array), (float*)PyArray_DATA(features_array) + N * fdim);
if (use_classes)
original_classes = vector<int>((int*)PyArray_DATA(classes_array), (int*)PyArray_DATA(classes_array) + N * ldim);
// Subsample
vector<PointXYZ> subsampled_points;
vector<float> subsampled_features;
vector<int> subsampled_classes;
grid_subsampling(original_points,
subsampled_points,
original_features,
subsampled_features,
original_classes,
subsampled_classes,
sampleDl,
verbose);
// Check result
if (subsampled_points.size() < 1)
{
PyErr_SetString(PyExc_RuntimeError, "Error");
return NULL;
}
// Manage outputs
// **************
// Dimension of input containers
npy_intp* point_dims = new npy_intp[2];
point_dims[0] = subsampled_points.size();
point_dims[1] = 3;
npy_intp* feature_dims = new npy_intp[2];
feature_dims[0] = subsampled_points.size();
feature_dims[1] = fdim;
npy_intp* classes_dims = new npy_intp[2];
classes_dims[0] = subsampled_points.size();
classes_dims[1] = ldim;
// Create output array
PyObject* res_points_obj = PyArray_SimpleNew(2, point_dims, NPY_FLOAT);
PyObject* res_features_obj = NULL;
PyObject* res_classes_obj = NULL;
PyObject* ret = NULL;
// Fill output array with values
size_t size_in_bytes = subsampled_points.size() * 3 * sizeof(float);
memcpy(PyArray_DATA(res_points_obj), subsampled_points.data(), size_in_bytes);
if (use_feature)
{
size_in_bytes = subsampled_points.size() * fdim * sizeof(float);
res_features_obj = PyArray_SimpleNew(2, feature_dims, NPY_FLOAT);
memcpy(PyArray_DATA(res_features_obj), subsampled_features.data(), size_in_bytes);
}
if (use_classes)
{
size_in_bytes = subsampled_points.size() * ldim * sizeof(int);
res_classes_obj = PyArray_SimpleNew(2, classes_dims, NPY_INT);
memcpy(PyArray_DATA(res_classes_obj), subsampled_classes.data(), size_in_bytes);
}
// Merge results
if (use_feature && use_classes)
ret = Py_BuildValue("NNN", res_points_obj, res_features_obj, res_classes_obj);
else if (use_feature)
ret = Py_BuildValue("NN", res_points_obj, res_features_obj);
else if (use_classes)
ret = Py_BuildValue("NN", res_points_obj, res_classes_obj);
else
ret = Py_BuildValue("N", res_points_obj);
// Clean up
// ********
Py_DECREF(points_array);
Py_XDECREF(features_array);
Py_XDECREF(classes_array);
return ret;
}