PCL-python调用科学计算包(scripy库)

前言

考虑到点云数据最后可以转化成一个三维矩阵[x,y,z],而python在机器学习领域里面有着莫大的优势，python作为一门胶水语言，已经集成并调用其他科学领域的库。如scripy库，底层采用用fortran语言编写的，所以执行起来比C++快。因此本文将采用学习如何从C++调用相关python的模块以及库。

1. 准备

安装python3.9，在mac下可以直接通过brew进行安装:

brew install python@3.9

找到相应的安装目录:/usr/local/Cellar/python@3.9,
并且按照相应的科学计算包通过:

pip3 install scipy

修改CMakeList.txt 引入相关python的头，以及numpy相关的头以及库函数。如下:

#包含头文件以及链接路径
include_directories(/usr/local/Cellar/python@3.9/3.9.12/Frameworks/Python.framework/Headers)
link_directories(/usr/local/Cellar/python@3.9/3.9.12/Frameworks/Python.framework/Versions/Current/lib)

include_directories(/usr/local/Cellar/numpy/1.22.3_1/lib/python3.9/site-packages/numpy/core/include)
link_directories(/usr/local/Cellar/numpy/1.22.3_1/lib/python3.9/site-packages/numpy/core/lib)

# 链接numpy库，以及python的库
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} libgtest.a glog::glog libnpymath.a python3.9)

修改相关的CMakeList.txt文件，引入相关的头文件以及进行链接

2.使用

新建一个C++类，如下

template <typename PointT>
class PythonGridModule{
public:
    using Ptr = std::shared_ptr<PythonGridModule<PointT>>;
    PythonGridModule();
    ~PythonGridModule();
    void exec_mesh_grid_py(typename pcl::PointCloud<PointT>::Ptr pointPtr, typename pcl::PointCloud<PointT>::Ptr out,
    const GripStep &step);
private:
    PyObject *pModule;
    PyObject *hGridDataFunc;
};

定义一个模板类，用于调用python相关的库，其中class里面有两个属性pModule和hGridDataFunc,两个属性，指向python的两个模块以及对象。在构造函数的时候对持有对象进行初始化，并且在析构函数的时候，释放python的对象，如下所示:

long init_numpy() {
    import_array();
}
template<typename Point>
PythonGridModule<Point>::PythonGridModule(){
    setenv("PYTHONPATH", ".", 1);
    Py_Initialize();
    init_numpy();
    this->pModule = PyImport_ImportModule("grid");
    this->hGridDataFunc = PyObject_GetAttrString(this->pModule, "griddata");
}
template<typename Point>
PythonGridModule<Point>::~PythonGridModule() {
    Py_Finalize();
}

上述代码，在构造函数里面初始化python相关的运行环境，然后在初始类里面的属性，其中pModule是python的模块，hGridDataFunc是模块里面的function,其中python文件为:

import numpy as np
from scipy.interpolate import griddata
import matplotlib.pyplot as plt
import matplotlib

def griddata(cloud, x_step, x_start, x_stop, y_step, y_start, y_stop):
    points = cloud[:, 0:2]
    values = cloud[:, 2]
    x = np.arange(x_start, x_stop + x_step, x_step,dtype =np.float64)
    y = np.arange(y_start, y_stop + y_step, y_step,dtype =np.float64)
    grid_x, grid_y = np.meshgrid(x, y)
    grid = griddata(points, values, (grid_x, grid_y), method='cubic')
    return grid

这里实现了用scripy实现了一个曲面重建的算法，第一个参数为点云numpy数组，后面的是相关的步以及开始结束节点。

实现调用相关的python库函数如下:

template<typename PointT>
void PythonGridModule<PointT>::exec_mesh_grid_py(typename pcl::PointCloud<PointT>::Ptr pointPtr, typename pcl::PointCloud<PointT>::Ptr out,
                       const GripStep &step){
    auto *data = new float[pointPtr->size() * 3];
    auto j = 0;
    for (auto i = 0; i < pointPtr->size(); i++) {
        data[j] = pointPtr->points[i].x;
        j++;
        data[j] = pointPtr->points[i].y;
        j++;
        data[j] = pointPtr->points[i].z;
        j++;
    }

    auto *pArgs = PyTuple_New(7);
    npy_intp dim[2] = {(long) pointPtr->size(), 3};
    PyTuple_SetItem(pArgs, 0, PyArray_SimpleNewFromData(2, dim, NPY_FLOAT, data));
    PyTuple_SetItem(pArgs, 1, Py_BuildValue("f", step.xStep));
    PyTuple_SetItem(pArgs, 2, Py_BuildValue("f", step.xStart));
    PyTuple_SetItem(pArgs, 3, Py_BuildValue("f", step.xStop));
    PyTuple_SetItem(pArgs, 4, Py_BuildValue("f", step.yStep));
    PyTuple_SetItem(pArgs, 5, Py_BuildValue("f", step.yStart));
    PyTuple_SetItem(pArgs, 6, Py_BuildValue("f", step.yStop));

    auto *func = PyObject_CallObject(this->hGridDataFunc, pArgs);
    if (func) {
        auto *pyArray = (PyArrayObject *) func;
        npy_intp d1 = pyArray->dimensions[0];
        npy_intp d2 = pyArray->dimensions[1];

        out->width = d2;
        out->height = d1;
        out->resize(d1 * d2);
        float x_index ;
        float y_index = step.yStop;
        int index = 0;
        for (int i = 0; i < d1; i++) {
            x_index = step.xStart;
            for (int j = 0; j < d2; j++) {
                out->points[index].x = x_index;
                out->points[index].y = y_index;
                out->points[index].z = ((double *) pyArray->data)[index];
                if (out->is_dense && isnan(out->points[index].z))
                    out->is_dense = false;
                x_index += step.xStep;
                index++;
            }
            y_index += step.yStep;
        }
    } else {
        PyErr_Print();
    }

    delete[] data;
}

上述代码也简单，通过先将点云数据转化成Numpy数组，然后执行python的griddata函数，再将执行完成后的结果写入到点云对象out里面去，完成一个完整的调用逻辑。

3. 总结

本文完成相关C++调用python相关的内容，也就是说点云数据可以转化为3维矩阵，可以采用python里面相关的机器学习算法进行后续的建模 ，优化，寻找最小值等等优化内容。