Teaching a CUDA SLAM system to speak numpy

I needed to drive SemanticFusion frame-by-frame from Python: feed it RGB and depth, get a camera pose and a labelled point cloud back, so a config-driven evaluation harness (post 33) could run it next to three other algorithms without a bespoke runner for each one. SemanticFusion is a C++/CUDA program whose state lives in GPU textures and Caffe blobs. Getting there meant fourteen SWIG .i files, a handful of small C++ shims, and enough fighting with CMake’s SWIG support that the top-level CMakeLists.txt still carries the scar tissue as comments. None of that is interesting as an algorithm. It’s interesting as an interface, and interfaces are mostly about who owns which piece of memory.

Passing arrays in

The easy direction first. ProcessFrameNumpy takes a flat uint8 RGB array and a flat uint16 depth array, plus an optional (4,4) pose:

// python/map_interface/ElasticFusionInterface.i
%numpy_typemaps(unsigned short             , NPY_USHORT    , int)
%numpy_typemaps(unsigned char             , NPY_UBYTE    , int)
%apply (unsigned char* IN_ARRAY1, int DIM1){(ImagePtr rgb_arr, int n_rgb)};
%apply (unsigned short* IN_ARRAY1, int DIM1){(DepthPtr depth_arr, int n_depth)};
%apply (float* IN_ARRAY2, int DIM1, int DIM2){(float * pose, int n_pose_x, int n_pose_y)};

The lines doing real work are the three %apply ones underneath: they bind those typemaps (already registered, redundantly or not) onto this file’s specific parameter names. Once that’s in place, a C++ signature that takes (pointer, length) pairs collapses into a single Python argument: call ProcessFrameNumpy(rgb, depth, timestamp) with three plain numpy arrays and SWIG unpacks each one into the pointer-and-length pair the C++ side actually wants. IN_ARRAY1/IN_ARRAY2 are the cheapest typemap in the whole file: numpy already owns a contiguous buffer, so SWIG just hands the C++ side a raw pointer into it for the duration of the call. Nothing is allocated, nothing is copied, nothing needs to be freed on either side.

Getting arrays out: the part that bites

Passing an array in is a loan: numpy keeps the buffer, C++ borrows it, nothing to clean up. Getting one out is different, because now something has to allocate a buffer on the C++ side and something has to eventually free it, and if those two somethings disagree about how, you get memory corruption instead of an array.

GetGlobalMap downloads the confidence-filtered surfel map and hands back three numpy arrays: positions+normals, RGB, and per-surfel class probabilities:

// python/semantic_fusion/SemanticFusionInterface.i
%apply (float** ARGOUTVIEWM_FARRAY2, int *DIM1, int *DIM2) {(float** xyz, int* v1s1, int* v1s2)}
%apply (float** ARGOUTVIEWM_FARRAY2, int *DIM1, int *DIM2) {(float** pr, int* v3s1, int* v3s2)}
%apply (unsigned char** ARGOUTVIEWM_FARRAY2, int *DIM1, int *DIM2) {(unsigned char** rgb, int* v2s1, int* v2s2)}

ARGOUTVIEWM_FARRAY2 reads as four separate decisions bolted together, and every one of them matters:

• ARGOUT: this isn’t an input, it’s a return value smuggled through out-parameters, the C convention SemanticFusionInterface.cpp actually uses (float** xyz, int* v1s1, int* v1s2 rather than returning a struct).
• VIEW: construct the numpy array directly over the memory the pointer refers to. No second copy.
• M: managed. Numpy takes ownership. Drop the M (ARGOUTVIEW, not ARGOUTVIEWM) and numpy views the same memory forever without ever freeing it.
• FARRAY2: Fortran-ordered, 2-D. numpy.i’s argout code literally calls require_fortran() on the array it builds.

That M is doing real work. Looking inside numpy.i, the generated argout code wraps the malloc’d pointer in a capsule: PyCObject_FromVoidPtr((void*)(*$1), free) on old Python, PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap) on newer, and free_cap is four lines earlier in the same file: pull the pointer back out of the capsule and call free() on it. Either path ends at the same C library free(). When the numpy array is garbage-collected, that’s what runs, on exactly the pointer GetGlobalMap handed back. That is why the C++ side has to use malloc, not new:

// src/semantic_fusion/SemanticFusionInterface.cpp
auto xyz_hold = (float *)malloc(sizeof(float) * 6 * validCount);
auto rgb_hold = (unsigned char *)malloc(sizeof(unsigned char) * 3 * validCount);
auto pr_hold  = (float *)malloc(sizeof(float) * num_classes * validCount);

Use new[] here instead and the code still compiles, still runs, and still looks correct in a five-minute test, right up until numpy calls free() on memory new[] allocated, corrupts the heap’s bookkeeping, and the interpreter crashes minutes later with a double free at shutdown, on a completely unrelated line. Drop the M instead and there’s no crash at all, just a leak the size of the entire surfel map on every single GetGlobalMap() call. Neither failure points back at the call site that caused it.

getCurrentPose and the numpy-returning overload of PredictAndUpdateProbabilities use the same ARGOUTVIEWM pattern, at FARRAY2 and FARRAY4 respectively. The widget below has all four side by side with their ownership traced out.

Why Fortran order, specifically

The F in ARGOUTVIEWM_FARRAY2 isn’t cosmetic. GetGlobalMap’s fill loop writes one surfel at a time, but it writes across columns, not along a row: six fixed strides of validCount apart, one per output field:

unsigned int offset_1 = hold_end + validCount;   // y
unsigned int offset_2 = offset_1 + validCount;   // z
// ...three more offsets for nx, ny, nz

So the buffer in memory is really [all N x-values][all N y-values][all N z-values]…, which is exactly what a Fortran-ordered (N, 6) array’s strides expect, and exactly not what a C-ordered one would mean by the same bytes. Ask for ARGOUTVIEWM_ARRAY2 (C order) over this memory and numpy would still build an (N, 6) array. It would just be silently, plausibly wrong, every column shuffled into a different one. getCurrentPose gets the same Fortran treatment for a different reason, covered below.

Six lines that turn an Eigen API into a numpy API

ProcessFrameNumpy also takes an optional camera pose as a (4,4) float array. numpy stores that row-major (C order); ElasticFusion’s ProcessFrame wants an Eigen::Matrix4f*, and Eigen’s default storage is column-major. Read the same sixteen floats under the wrong assumption and you don’t get an error: you get a transposed matrix that still looks like a valid rotation, just the wrong one. The shim exists entirely to make that mismatch explicit:

// src/map_interface/ElasticFusionInterface.h
Eigen::Matrix4f *inPose = nullptr;
if (n_pose_x == 4 && n_pose_y == 4) {
    inPose = new Eigen::Matrix4f(
        Eigen::Map<Eigen::Matrix<float,4,4, Eigen::RowMajor>>(pose));
} else if (n_pose_x != 0 || n_pose_y != 0) {
    std::cout << "Pose should be a 4x4 array skipping the provided pose!!" << std::endl;
}
auto ret = ProcessFrame(rgb_arr, depth_arr, timestamp, inPose, weightMultiplier, bootstrap);
delete inPose;
return ret;

Eigen::Map<Matrix<float,4,4,RowMajor>>(pose) reinterprets numpy’s raw floats row-major, matching how numpy actually laid them out; wrapping that in Eigen::Matrix4f(...) copies it into a normal, column-major Eigen matrix that the rest of the (Imperial-authored) pipeline already expects. Six lines, and the boundary between “numpy’s idea of a matrix” and “Eigen’s idea of a matrix” is closed. The pose the caller passed in is never touched (pose is still an IN_ARRAY2, a borrowed view), and the Eigen::Matrix4f this shim allocates with new is deleted before the function returns, so nothing here crosses back over the boundary at all.

getCurrentPose runs the same trade in reverse, and needs none of this:

void getCurrentPose(float ** out_pose, int * d_0, int * d_1){
    auto curr_pose = elastic_fusion_->getCurrPose();
    *out_pose = (float *)malloc(4 * 4 * sizeof(float));
    assert (*out_pose != NULL);
    *d_0 = 4;
    *d_1 = 4;
    memcpy(*out_pose, curr_pose.data(), 4 * 4 * sizeof(float));
}

A bare memcpy, no Eigen::Map, no reordering, because curr_pose is a normal column-major Eigen::Matrix4f, and the array coming back is ARGOUTVIEWM_FARRAY2, Fortran-ordered. The two layouts already agree, so there’s nothing to shim. It’s the same lesson as ProcessFrameNumpy, read from the other direction: matching layouts need zero lines of code, and mismatched ones need exactly the right six.

Unpacking the surfel map

GetGlobalMap is the payoff of the whole binding: it’s how a labelled point cloud gets out of GPU memory and into something open3d can write to disk. The full function is about eighty lines; the shape of it is: download, filter, count, allocate, unpack.

// src/semantic_fusion/SemanticFusionInterface.cpp
Eigen::Vector4f * mapData = globalModel->downloadMap();
int validCount = 0;
for (unsigned int i = 0; i < globalModel->lastCount(); i++) {
    Eigen::Vector4f pos = mapData[(i * 3) + 0];
    if (pos[3] > confidenceThreshold) validCount++;
}
auto xyz_hold = (float *)malloc(sizeof(float) * 6 * validCount);
auto rgb_hold = (unsigned char *)malloc(sizeof(unsigned char) * 3 * validCount);
auto pr_hold  = (float *)malloc(sizeof(float) * num_classes * validCount);

The map is walked twice: once to count survivors above the confidence threshold, a second time to actually fill the buffers, precisely so the malloc calls above can be sized exactly, no over-allocation and no std::vector growth doubling. Every surfel’s RGB is packed into a single float, three bytes wide, and has to be unpacked byte-by-byte on the way out:

unsigned char r = int(col[0]) >> 16 & 0xFF;
unsigned char g = int(col[0]) >> 8  & 0xFF;
unsigned char b = int(col[0]) & 0xFF;

and the surface normal is stored flipped relative to what the caller wants, so the shim negates it in place (nor[0] *= -1; and so on) before writing it out. None of that is SWIG’s business: it’s ordinary C++ sitting between the download and the ARGOUTVIEWM return, but it’s exactly the kind of small, undocumented translation step that a binding layer accumulates and a caller never has to think about again.

The build: the 80% that never gets written down

The .i files are the part people write posts about; the CMake is the part that actually took the time. The top-level CMakeLists.txt still has three abandoned attempts at wiring SWIG into the build, left in as comments: a fossil record of fighting swig_add_library before this project’s CMake (3.8, with CMP0078 and CMP0086 explicitly opted into “NEW”) had settled behaviour for it. The # attempt N labels below are mine, added to the actually-commented-out lines for this post. The file itself just has them one after another, no headings:

# attempt 1: the pre-CMP0078 macros
#SWIG_ADD_MODULE(SemanticFusion python ${swig_files})
#SWIG_LINK_LIBRARIES(SemanticFusion ${PYTHON_LIBRARIES})

# attempt 2: a full swig_add_library, with explicit OUTPUT_DIR and
#   per-platform (APPLE / UNIX) RPATH and link-flag handling
##swig_add_library(pySemanticFusionInterface
##        LANGUAGE python
##        OUTPUT_DIR ${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}/semantic_fusion
##        SOURCES SemanticFusionInterface.i)
## ...(target_include_directories, then a macOS/Linux RPATH if/elseif block)...

# attempt 3: a stripped-down probe target, "TestPy", including a
#   tried-and-abandoned SWIG_FLAGS "-includeall"
#swig_add_library(TestPy LANGUAGE python SOURCES src/python/SemanticFusionInterface.i)

None of it shipped. What shipped instead lives one directory down, in python/CMakeLists.txt, as a single reusable function:

function(add_swig_item swig_file_name swig_folder_name)
    set_property(SOURCE ${swig_folder_name}/${swig_file_name}.i PROPERTY CPLUSPLUS ON)
    set_property(SOURCE ${swig_folder_name}/${swig_file_name}.i PROPERTY SWIG_MODULE_NAME py${swig_file_name})
    swig_add_library(py${swig_file_name}
            LANGUAGE python
            SOURCES ${swig_folder_name}/${swig_file_name}.i)
    set_property(TARGET py${swig_file_name} PROPERTY SWIG_USE_TARGET_INCLUDE_DIRECTORIES ON)
    target_include_directories(py${swig_file_name} PRIVATE
            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
            ${Python_INCLUDE_DIR}
            ${NUMPY_INCLUDES})
    target_link_libraries(py${swig_file_name}
            ${CMAKE_PROJECT_NAME} ${PYTHON_LIBRARIES} ${NUMPY_LIBRARIES})
    # ...install rules for the .so and the generated .py
endfunction(add_swig_item)

fourteen calls follow: add_swig_item(SemanticFusionInterface semantic_fusion), add_swig_item(ElasticFusionInterface map_interface), and so on, one Python extension module per header, each with its own install rule. That’s the entire lesson of the graveyard above: the working version isn’t cleverer than the abandoned ones, it’s a function, called fourteen times, instead of fourteen hand-copied blocks each slightly different from the last.

FindNumPy.cmake (vendored, not written for this project) does the unglamorous thing CMake has no built-in for: asking the Python interpreter where its own numpy headers live, by literally invoking Python and parsing what comes back (import numpy; print(numpy.get_include())). And two lines further down the top-level CMakeLists.txt:

set(CUDA_ARCH_BIN "30 35 50 52 61" CACHE STRING "...")
set(CUDA_HOST_COMPILER gcc-5)

is a list of Kepler-through-Pascal compute capabilities and a pin to GCC 5, already four years old by the time this was written, because nvcc in 2019 could not yet be pointed at anything newer. None of this is interesting to read. All of it is why the thing built at all.

What I actually changed

git log --name-status on this repo is short enough to read end to end, and it draws a clean line between “vendored” and “mine”:

Commit	Date	What it touched
612945f Initial commit	2019-09-23	The whole python/ SWIG tree (14 .i files, python/CMakeLists.txt, vendored numpy.i) checked in alongside Imperial’s src/ and the top-level CMakeLists.txt
3ef16aa Added deps	2019-09-23	deps/ElasticFusionCore and deps/caffe_semanticfusion vendored wholesale: 531 files, none of it mine
e596cf7 work	2019-09-25	First real edits: CMakeLists.txt, python/CMakeLists.txt, SemanticFusionInterface.i, both interface .h/.cpp pairs, and the one-line offscreen addition to Gui
3b542ea Mostly working	2019-09-25	Both .i files and both interface .cpp files again: getting the typemaps to actually compile and run
1b8c806 more work done	2019-10-16	ElasticFusionInterface.i / .h / .cpp only: the last code commit, and this post’s date
869e334, ddfceee	2019-12-16	README.md added and edited: the licence notice, no code

Everything else in the repo (every CUDA kernel, the CRF, the GUI internals, the original ElasticFusionInterface/SemanticFusionInterface class skeletons these commits extend) predates all of the above and stays Imperial’s.

The payoff

The reason any of this mattered: pythontest/main.py in this repo is a 120-line manual driver: load a colour scheme, set Resolution/Intrinsics, open a PNGLogReader, loop frames, run the CNN every cnn_skip_frames-th frame, optionally CRF, render. It proves the binding works. But the actual point was that the same calls (ProcessFrameNumpy, PredictAndUpdateProbabilities, GetGlobalMap, getCurrentPose) could be driven from somewhere else entirely: a 167-line algorithm wrapper in the sibling 3D-scene-tester-lib repo (segtester/algs/semanticfusion/semanticfusion.py), called by a YAML-configured harness that also runs three other, unrelated 3D algorithms over the same scenes. The whole SemanticFusion half of post 33’s benchmark ran through the four calls in the widget below, and none of that harness code had to know anything about CUDA, Caffe, or surfel maps, only that it could hand over a (4,4) numpy array and get one back.

The boundary explorer

Four real signatures from this repo. Pick one to see the %apply directives that bind it, the Python signature and call site those directives produce, and where the buffer that crosses the boundary was allocated, who owns it, and who frees it, with a toggle for what happens if you get that detail wrong. The second tab is a timeline of which pipeline stage fires on which frame, driven by the same cnn_skip_frames / crf_skip_frames knobs the evaluation harness’s config exposes (cnn_skip_frames: 10 in its ScanNet config).

With JavaScript enabled this becomes a two-tab widget: a click-through explorer of four SWIG-bound C++ signatures with their typemaps and ownership diagrams, and an SVG timeline of ElasticFusion / CNN+fusion / CRF firing per frame, driven by cnn_skip_frames and crf_skip_frames sliders.