Skip to content

Commit bebb85f

Browse files
jlblancocjlblancoc
committed
Add k-d tree baking for IncrementalPointCloud + mm-ipc-bake-kdtree tool
Serializes the incremental k-d tree index alongside an IncrementalPointCloud layer's points (TCreationOptions::serialize_kdtree), so it does not have to be rebuilt (an O(N log M) bulk build) on every load. Unlike KeyframePointCloudMap's baked static trees, nanoflann's incremental index had no save/load support at all; this depends on saveIndex()/loadIndex() added upstream (nanoflann >= 1.11.0, see the companion nanoflann PR), gated behind MOLA_METRIC_MAPS_HAS_INCREMENTAL_KDTREE_BAKE so older builds keep working with the option as a documented no-op. Serialization always writes/reads the compacted (tombstone-free) point order, so baking builds a throwaway index over that exact order rather than reusing the live index (whose slots may not match after tombstones/slot recycling). Adds the mm-ipc-bake-kdtree CLI tool (analogous to mm-kf-bake-kdtrees) and a shared mm_cli_utils.h generic layer-iteration helper for it. Unit tests cover: bake/load round-trip through memory and through a real temporary file, k-d tree parameters differing between bake and load time, clearing and re-inserting into a loaded (baked) map, further insertions/trims on a loaded map, and serialize_kdtree=false remaining a no-op -- all independent of whether this build's nanoflann actually supports baking.
1 parent 42750f6 commit bebb85f

11 files changed

Lines changed: 711 additions & 7 deletions

File tree

‎agents.md‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -184,6 +184,18 @@ Tests: `mola_yaml/tests/test-yaml-parser.cpp`
184184
such registered CMetricMap class". `MOLA_METRIC_MAPS_HAS_INCREMENTAL_POINT_CLOUD`
185185
is defined (PUBLIC) only when the feature is functional; the header is always
186186
usable either way.
187+
`TCreationOptions::serialize_kdtree` (default `false`): bakes the incremental
188+
k-d tree index into the `.mm` on save, so it does not have to be rebuilt (an
189+
O(N log N) bulk build) on load. Unlike `KeyframePointCloudMap`'s baked static
190+
trees, nanoflann's incremental index (`KDTreeSingleIndexIncrementalAdaptor`)
191+
had no save/load support at all until `saveIndex()`/`loadIndex()` were added
192+
upstream (nanoflann >= 1.11.0); gated via `MOLA_METRIC_MAPS_HAS_INCREMENTAL_KDTREE_BAKE`
193+
(PUBLIC, set by CMake when `nanoflann_VERSION >= 1.11`), a no-op on write and
194+
a skipped blob on read otherwise. Serialization always writes/reads the
195+
*compacted* (tombstone-free) point order (see `serializeTo()`), so baking
196+
builds a throwaway index over that exact order rather than reusing the live
197+
`index_` (whose slots may not match after tombstones/recycling). CLI tool
198+
`mm-ipc-bake-kdtree` (in `apps/`), analogous to `mm-kf-bake-kdtrees`.
187199
**nanoflann is included by `src/IncrementalKDTree.cpp` alone, by absolute path
188200
(`MOLA_NANOFLANN_HEADER`, set by CMake) and with its namespace renamed**,
189201
because MRPT bundles its own, usually older, copy of the same header and

‎mola_metric_maps/CMakeLists.txt‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,17 @@ else()
5353
"built as a stub that throws at runtime; everything else is unaffected.")
5454
endif()
5555

56+
# mola::IncrementalPointCloud's k-d tree *baking* (TCreationOptions::serialize_kdtree,
57+
# the mm-ipc-bake-kdtree CLI tool) additionally needs nanoflann's
58+
# KDTreeSingleIndexIncrementalAdaptor::saveIndex()/loadIndex(), added in 1.11.0.
59+
# Without it, everything else in this library works as usual: the option is a
60+
# no-op on write and a loaded .mm's baked blob (if any) is skipped on read.
61+
if(MOLA_HAS_INCREMENTAL_POINT_CLOUD AND nanoflann_VERSION VERSION_GREATER_EQUAL 1.11)
62+
set(MOLA_HAS_INCREMENTAL_POINT_CLOUD_KDTREE_BAKE ON)
63+
message(STATUS "mola_metric_maps: nanoflann ${nanoflann_VERSION} >= 1.11, enabling IncrementalPointCloud k-d tree baking")
64+
else()
65+
set(MOLA_HAS_INCREMENTAL_POINT_CLOUD_KDTREE_BAKE OFF)
66+
endif()
5667

5768
# 3rdparty deps:
5869
add_subdirectory(3rdparty)
@@ -117,6 +128,11 @@ if(MOLA_HAS_INCREMENTAL_POINT_CLOUD)
117128
target_compile_definitions(${PROJECT_NAME}
118129
PUBLIC MOLA_METRIC_MAPS_HAS_INCREMENTAL_POINT_CLOUD)
119130

131+
if(MOLA_HAS_INCREMENTAL_POINT_CLOUD_KDTREE_BAKE)
132+
target_compile_definitions(${PROJECT_NAME}
133+
PUBLIC MOLA_METRIC_MAPS_HAS_INCREMENTAL_KDTREE_BAKE)
134+
endif()
135+
120136
# nanoflann is deliberately NOT linked as a target, and its header is NOT
121137
# reached through the include path at all: src/IncrementalKDTree.cpp includes
122138
# it by ABSOLUTE path via the macro below.

‎mola_metric_maps/apps/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,3 +3,4 @@ add_subdirectory(mm2ini)
33
add_subdirectory(ini2mm)
44
add_subdirectory(mm-kf-regroup)
55
add_subdirectory(mm-kf-bake-kdtrees)
6+
add_subdirectory(mm-ipc-bake-kdtree)

‎mola_metric_maps/apps/mm-ipc-bake-kdtree/CMakeLists.txt‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
project(mm-ipc-bake-kdtree)
2+
3+
find_package(CLI11 REQUIRED)
4+
5+
mola_add_executable(
6+
TARGET ${PROJECT_NAME}
7+
SOURCES
8+
main.cpp
9+
LINK_LIBRARIES
10+
mola_metric_maps
11+
CLI11::CLI11
12+
)

‎mola_metric_maps/apps/mm-ipc-bake-kdtree/main.cpp‎

Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
1+
/* _
2+
_ __ ___ ___ | | __ _
3+
| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
4+
| | | | | | (_) | | (_| | Localization and mApping (MOLA)
5+
|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola
6+
7+
Copyright (C) 2018-2026 Jose Luis Blanco, University of Almeria,
8+
and individual contributors.
9+
SPDX-License-Identifier: GPL-3.0
10+
See LICENSE for full license information.
11+
*/
12+
13+
/**
14+
* @file mm-ipc-bake-kdtree/main.cpp
15+
* @brief CLI tool to bake the k-d tree index of IncrementalPointCloud layers
16+
* into a .mm file so it does not have to be rebuilt (an O(N log N)
17+
* bulk build) every time the map is loaded.
18+
* @author Jose Luis Blanco Claraco
19+
* @date Jul 31, 2026
20+
*/
21+
22+
#include <mola_metric_maps/IncrementalPointCloud.h>
23+
#include <mp2p_icp/metricmap.h>
24+
25+
#include <CLI/CLI.hpp>
26+
#include <iostream>
27+
#include <stdexcept>
28+
29+
#include "../mm_cli_utils.h"
30+
31+
namespace
32+
{
33+
struct Args
34+
{
35+
std::string input;
36+
std::string output;
37+
std::string layer; // empty: process all IncrementalPointCloud layers
38+
std::string plugins;
39+
bool disable = false; // if set, strip the cached k-d tree instead of adding it
40+
bool kdtree = true; // bake the k-d tree index
41+
};
42+
43+
void run_bake(const Args& args)
44+
{
45+
#if !defined(MOLA_METRIC_MAPS_HAS_INCREMENTAL_KDTREE_BAKE)
46+
if (!args.disable && args.kdtree)
47+
{
48+
std::cout << "[mm-ipc-bake-kdtree] WARNING: this build's nanoflann lacks the incremental "
49+
"index's save/load API (requires nanoflann >= 1.11.0); k-d tree baking is a "
50+
"no-op and no k-d tree data will be written."
51+
<< std::endl;
52+
}
53+
#endif
54+
55+
mp2p_icp::metric_map_t mm = mola::mm_cli::loadMap(args.input, args.plugins, "mm-ipc-bake-kdtree");
56+
57+
const size_t numProcessed = mola::mm_cli::forEachMapLayerOfType<mola::IncrementalPointCloud>(
58+
mm, args.layer, "mola::IncrementalPointCloud",
59+
[&](const std::string& layerName, const mola::IncrementalPointCloud::Ptr& ipcMap,
60+
mrpt::maps::CMetricMap::Ptr& /*layerSlot*/)
61+
{
62+
ipcMap->creationOptions.serialize_kdtree = !args.disable && args.kdtree;
63+
std::cout << "[mm-ipc-bake-kdtree] Layer '" << layerName << "': serialize_kdtree -> "
64+
<< (ipcMap->creationOptions.serialize_kdtree ? "true" : "false")
65+
<< " (cached data is (re)built on save only if not already current)."
66+
<< std::endl;
67+
});
68+
69+
std::cout << "[mm-ipc-bake-kdtree] Writing output map to: '" << args.output << "'..."
70+
<< std::endl;
71+
if (!mm.save_to_file(args.output))
72+
{
73+
throw std::runtime_error("Error writing output map file: '" + args.output + "'");
74+
}
75+
76+
std::cout << "[mm-ipc-bake-kdtree] Done. Processed " << numProcessed << " layer(s)." << std::endl;
77+
}
78+
} // namespace
79+
80+
int main(int argc, char** argv)
81+
{
82+
try
83+
{
84+
CLI::App cli{
85+
"mm-ipc-bake-kdtree: cache the IncrementalPointCloud k-d tree index inside a .mm file so "
86+
"it is not rebuilt on every load"};
87+
88+
Args args;
89+
90+
cli.add_option("-i,--input", args.input, "Input metric map file (*.mm)")
91+
->required()
92+
->check(CLI::ExistingFile);
93+
94+
cli.add_option("-o,--output", args.output, "Output metric map file (*.mm)")->required();
95+
96+
cli.add_option(
97+
"--layer", args.layer,
98+
"Name of the IncrementalPointCloud layer to process. If omitted, all such layers are "
99+
"processed.");
100+
101+
cli.add_flag(
102+
"--disable", args.disable,
103+
"Instead of baking, strip any cached data (sets serialize_kdtree=false).");
104+
105+
cli.add_flag(
106+
"--kdtree,!--no-kdtree", args.kdtree,
107+
"Bake the k-d tree index (default: on). Requires a nanoflann build with the incremental "
108+
"index's save/load API.");
109+
110+
cli.add_option(
111+
"-l,--load-plugins", args.plugins,
112+
"One or more (comma separated) *.so files to load as plugins");
113+
114+
try
115+
{
116+
cli.parse(argc, argv);
117+
}
118+
catch (const CLI::ParseError& e)
119+
{
120+
return cli.exit(e);
121+
}
122+
123+
run_bake(args);
124+
}
125+
catch (const std::exception& e)
126+
{
127+
std::cerr << e.what() << std::endl;
128+
return 1;
129+
}
130+
return 0;
131+
}

‎mola_metric_maps/apps/mm_cli_utils.h‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
/* _
2+
_ __ ___ ___ | | __ _
3+
| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
4+
| | | | | | (_) | | (_| | Localization and mApping (MOLA)
5+
|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola
6+
7+
Copyright (C) 2018-2026 Jose Luis Blanco, University of Almeria,
8+
and individual contributors.
9+
SPDX-License-Identifier: GPL-3.0
10+
See LICENSE for full license information.
11+
*/
12+
13+
/**
14+
* @file mm_cli_utils.h
15+
* @brief Generic helpers shared by the .mm-file CLI tools: plugin loading,
16+
* map loading, and iteration over layers of a given map type.
17+
* @author Jose Luis Blanco Claraco
18+
* @date Jul 31, 2026
19+
*/
20+
#pragma once
21+
22+
#include <mp2p_icp/metricmap.h>
23+
#include <mrpt/maps/CMetricMap.h>
24+
#include <mrpt/system/filesystem.h>
25+
#include <mrpt/system/os.h>
26+
27+
#include <functional>
28+
#include <iostream>
29+
#include <stdexcept>
30+
#include <string>
31+
32+
namespace mola::mm_cli
33+
{
34+
/// Loads one or more comma-separated *.so plugin modules (no-op if empty).
35+
inline void loadPlugins(const std::string& plugins)
36+
{
37+
if (plugins.empty())
38+
{
39+
return;
40+
}
41+
std::string errMsg;
42+
std::cout << "Loading plugin(s): " << plugins << std::endl;
43+
if (!mrpt::system::loadPluginModules(plugins, errMsg))
44+
{
45+
throw std::runtime_error(errMsg);
46+
}
47+
}
48+
49+
/// Loads plugins (if any) and reads a `.mm` metric map from disk. `tag` is a
50+
/// short tool name used to prefix log lines.
51+
inline mp2p_icp::metric_map_t loadMap(
52+
const std::string& input, const std::string& plugins, const std::string& tag)
53+
{
54+
loadPlugins(plugins);
55+
56+
ASSERT_FILE_EXISTS_(input);
57+
58+
std::cout << "[" << tag << "] Reading input map from: '" << input << "'..." << std::endl;
59+
60+
mp2p_icp::metric_map_t mm;
61+
mm.load_from_file(input);
62+
return mm;
63+
}
64+
65+
/// Invokes `fn(layerName, typedLayer, layerSlot)` for every layer of `mm`
66+
/// whose runtime type is `MapT`. `layerSlot` is a mutable reference to the
67+
/// layer pointer in `mm.layers`, so callers can replace the layer in place if
68+
/// needed.
69+
///
70+
/// If `layerName` is non-empty, only that layer is processed and it is an
71+
/// error (throws) if it exists but is not a `MapT`. If `layerName` is empty,
72+
/// non-matching layers are silently skipped. `typeName` is used only for
73+
/// error messages (e.g. "mola::IncrementalPointCloud").
74+
///
75+
/// Returns the number of layers processed.
76+
template <typename MapT>
77+
inline size_t forEachMapLayerOfType(
78+
mp2p_icp::metric_map_t& mm, const std::string& layerName, const std::string& typeName,
79+
const std::function<
80+
void(const std::string&, const typename MapT::Ptr&, mrpt::maps::CMetricMap::Ptr&)>& fn)
81+
{
82+
// Distinguish "requested layer absent" from "no layers of this type at all".
83+
if (!layerName.empty() && mm.layers.find(layerName) == mm.layers.end())
84+
{
85+
throw std::runtime_error("Layer '" + layerName + "' not found in the input map.");
86+
}
87+
88+
size_t numProcessed = 0;
89+
for (auto& [name, layer] : mm.layers)
90+
{
91+
if (!layer)
92+
{
93+
continue;
94+
}
95+
if (!layerName.empty() && name != layerName)
96+
{
97+
continue;
98+
}
99+
100+
auto typedLayer = std::dynamic_pointer_cast<MapT>(layer);
101+
if (!typedLayer)
102+
{
103+
if (!layerName.empty())
104+
{
105+
throw std::runtime_error(
106+
"Layer '" + name + "' is not a " + typeName + " (it is a '" +
107+
layer->GetRuntimeClass()->className + "').");
108+
}
109+
continue; // silently skip non-matching layers when processing all
110+
}
111+
112+
fn(name, typedLayer, layer);
113+
numProcessed++;
114+
}
115+
116+
if (numProcessed == 0)
117+
{
118+
throw std::runtime_error("No " + typeName + " layer was found/processed in the input map.");
119+
}
120+
return numProcessed;
121+
}
122+
} // namespace mola::mm_cli

‎mola_metric_maps/include/mola_metric_maps/IncrementalPointCloud.h‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -298,6 +298,22 @@ class IncrementalPointCloud : public mrpt::maps::CGenericPointsMap,
298298

299299
/** Maximum distance [m] to search neighbors for the covariance estimate. */
300300
double max_distance_for_cov = 1.0;
301+
302+
/** If `true`, the k-d tree index is serialized alongside the points (see
303+
* `IncrementalPointCloud` serialization), so it does NOT have to be
304+
* rebuilt (an O(N log N) bulk build) when the map is loaded. Requires an
305+
* MRPT/nanoflann build providing the incremental index's save/load API
306+
* (feature-detected at compile time via
307+
* `MOLA_METRIC_MAPS_HAS_INCREMENTAL_KDTREE_BAKE`); when unavailable this
308+
* option is silently a no-op on write, and a reader without the feature
309+
* skips any baked blob found in the file. Default `false`. Typically
310+
* enabled offline by the `mm-ipc-bake-kdtree` tool: the map is always
311+
* serialized compacted (no tombstoned slots, see
312+
* `IncrementalPointCloud::serializeTo()`), so baking rebuilds the index
313+
* once over that compacted point order at save time instead of leaving
314+
* the O(N log N) build for every subsequent load.
315+
*/
316+
bool serialize_kdtree = false;
301317
};
302318
TCreationOptions creationOptions;
303319

@@ -346,6 +362,15 @@ class IncrementalPointCloud : public mrpt::maps::CGenericPointsMap,
346362
/// Used for getAsSimplePointsMap() only.
347363
mutable mrpt::maps::CSimplePointsMap::Ptr cachedPoints_;
348364

365+
/** Creates a fresh, empty index_ from `creationOptions`, sized/reserved as
366+
* configured, and points it at the current coordinate buffers. Leaves
367+
* indexed_up_to_ at 0: the caller is responsible for indexing the current
368+
* storage afterwards (either the bulk `addPoints()` done by resetIndex(),
369+
* or `internal::IncrementalKDTree::loadIndex()` when reconstructing a
370+
* baked index from a serialized stream).
371+
*/
372+
void createEmptyIndex();
373+
349374
/** (Re)creates the index from `creationOptions` and bulk-loads **every**
350375
* storage slot into it. Only valid when no slot is tombstoned, i.e. right
351376
* after the storage was cleared, replaced wholesale, or compacted;

0 commit comments

Comments
 (0)