GLIM in OpenKAI

Build against the installed GLIM CMake package (the source checkout is not compiled into OpenKAI):

cmake -S . -B build-glim -DWITH_SLAM=ON -DUSE_GLIM=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo
cmake --build build-glim -j4

Use -DCMAKE_PREFIX_PATH=/your/install/prefix for a nonstandard installation. glim::glim supplies GTSAM, gtsam_points, Eigen, OpenMP, spdlog and Boost dependencies. GLIM is only required when both WITH_SLAM and USE_GLIM are enabled. SLAM also builds its shared navigation, geometry, and DataObject dependencies when their wider module groups are disabled.

Include jsonCfg/_GLIM.json using the application's APP.vInclude, or copy its SLAM module and DataObject declarations into your camera configuration. Connect the camera's PCLframeOut output to slamPoints and its IMU output to slamIMU (IMUstreamOut for RGBD cameras and Livox). SLAM reads them through PCLframeIn and IMUstreamIn. The example declares input streams; it needs a sensor producer to populate them. Paths are relative to the process working directory. See Geometry DataObjects for the complete configuration mapping.

jsonCfg/glim_orbbec contains CPU configuration templates adapted from GLIM 1.2.2, with its MIT license. You can also point configPath at a copy of /home/kai/dev/glim/config. In that copy, select the CPU odometry, sub-mapping and global-mapping JSON files in config.json; the upstream configuration selects GPU modules by default. Every _GLIM instance in one process must use the same configuration directory because GLIM configuration is global. Restart the process after editing those profile files. The supported web controls use per-instance constructor parameters and apply on the next session without replacing global configuration.

Before using a real sensor, set T_lidar_imu in config_sensors.json to its calibrated IMU-to-point-cloud transform. The glim_orbbec profile now contains the attached Gemini 335's factory IMU-to-native-depth transform: translation [0.000246, 0.000065, -0.016948] meters and identity rotation. This is device/frame-specific; RGB-aligned clouds require IMU-to-color calibration instead. Point coordinates must be in meters; acceleration in m/s² (including gravity); angular velocity in rad/s. Point-cloud and IMU timestamps must increase in nanoseconds and share one capture clock. _Orbbec uses device timestamps and separate 200 Hz IMU callbacks, avoiding sample loss and USB-arrival jitter from video framesets. Keep the input point cloud in a fixed sensor frame, and calibrate extrinsics against that frame. Do not feed the estimated pose back into the producer of its input PCLframe.

Run build/OpenKAI jsonCfg/GLIM_realSense.json for a RealSense D455 with its embedded IMU. Enable WITH_VISION, USE_OPENCV, USE_REALSENSE, WITH_SLAM, and USE_GLIM when building. The independent jsonCfg/glim_realSense profile uses the same CPU odometry and IMU-enabled mapping modules as Orbbec. Open http://localhost:8080/ and press Start; keep the camera stationary during initialization. Parameter saves update the GLIM.parameters object in the launch JSON; PLY exports go to data/glim_realSense.

The RealSense launch captures native depth at 640×480/30 Hz, acceleration at 250 Hz and gyro at 200 Hz, matching the attached D455's supported profiles. Other models may require different accelFPS/gyroFPS; zero lets the SDK choose a supported rate. Color, alignment, and local image windows are disabled. RS2_OPTION_GLOBAL_TIME_ENABLED is disabled on the depth and motion sensors so both use the hardware capture clock. Do not enable it for only one sensor. The driver publishes point clouds in native depth optical coordinates (X right, Y down, Z forward), independently of color alignment.

glim_realSense/config_sensors.json contains the attached D455's factory IMU-to-depth transform, queried through librealsense: translation [0.03022, -0.0074, -0.01602] meters and identity quaternion. Both acceleration and gyro profiles reported this transform. Verify T_lidar_imu against the SDK calibration when using another device; this is a D455 profile, not a universal RealSense transform. The raw SDK motion vectors already have the depth optical axis orientation; retain the IMU origin offset in GLIM rather than rotating the vectors again.

The GLIM adapter treats each cloud as a single exposure, with zero per-point time offsets. It suits depth-camera clouds; it does not deskew a scanning LiDAR. Producers copy complete vectors into PCLframe::set(points, sensorTimestampNs). Consumers use get(points) to copy a cloud and its matching timestamp. getTstamp() allows idle polls to skip that copy. There are no shared DataObject snapshots or revision counters; updates with the same timestamp are indistinguishable.

IMUstream accepts addGyro(value, timestampNs) and addAcc(value, timestampNs). get(gyro, acc) copies the bounded histories and returns their update timestamp. SLAM owns its interpolation cursor; several readers consume independent copies of the same history. Publishing appends in constant time; each get copies the bounded history. SLAM checks the two histories independently because gyro and acceleration can arrive separately with the same timestamp. Acceleration is linearly interpolated at each gyro timestamp, preserving the gyro rate when the sensor rates differ. An unmatched gyro sample waits for a later acceleration sample; SLAM does not extrapolate or bridge acceleration outages longer than 100 ms.

The Orbbec CPU odometry example requires an IMU. Run build/OpenKAI jsonCfg/GLIM_scepter.json for Scepter cameras without an IMU. Its isolated glim_scepter profile selects CT odometry, disables enable_imu in both mapping configs, and uses identity sensor extrinsics. The map starts in the first camera's optical frame, without gravity alignment. No IMU module or synthetic inertial samples are needed. The shared frontend, WSconsole controls, submap stream and PLY export work with both profiles. The adapter checks the selected estimator's IMU requirement at startup.

Scepter and Orbbec SDKs can be enabled in the same build. GLIM's spdlog must precede the Scepter SDK in the link order because the SDK exports an incompatible bundled spdlog; CMake declares the explicit dependency to preserve that order. Camera/SLAM saved controls for Scepter live under jsonCfg/glim_scepter, and its PLY output defaults to data/glim_scepter. The Orbbec settings remain separate. Sensor rates and preprocessing parameters still need tuning for the scene and camera.

_SLAMbase owns DataObject linking, timestamp tracking, the worker, pose publication and the session lifecycle. _GLIM owns preprocessing, odometry, submaps and global mapping. Processing is sequential on the OpenKAI worker; if processing falls behind, it takes the latest completed cloud. Reading a new cloud copies its points into the consumer. SLAM rejects empty clouds and non-increasing capture timestamps without advancing the estimator clock. IMU-required odometry waits for a paired IMU sample later than the cloud timestamp.

• start() starts the worker and, by default, a session. Set bAutoStart: false to start tracking explicitly.
• startTracking() creates a new session, or returns success if one is already active. Initialization may need several seconds of IMU and point-cloud data.
• stopTracking() finishes mapping and retains the last pose and map. Confidence becomes zero.
• saveMap(path) saves the finished GLIM map after stopTracking(). The saved map includes global optimization; the navigation pose is continuous local odometry in GLIM's world frame, expressed as T_world_lidar for the input cloud frame.
• reset() stops tracking, discards the map, and resets position and orientation. Call startTracking() to begin again.

_NavBase::setConfidence() uses a 0–100 scale and optionally expires stale updates. SLAM defaults to a one-second tConfidenceTimeoutNs; zero disables expiry. GLIM supplies no scalar tracking-quality score, so this adapter reports 100 for an available finite pose and 0 for initialization, insufficient points, stopped tracking or expired updates. It is a pose-availability signal, not an accuracy estimate. bTracking() reports whether the session is active, including initialization.

Run the hardware-independent DataObject checks using the commands in Geometry DataObjects. The dedicated SLAM IMU regression checks unequal rates (including D455's 250/200 Hz), interpolation, callback delays, duplicate samples, independent readers, acceleration outages, and capture-clock resets:

cmake -S test/SLAM -B /tmp/openkai-slam-tests
cmake --build /tmp/openkai-slam-tests --parallel 4
ctest --test-dir /tmp/openkai-slam-tests --output-on-failure

These checks do not validate real GLIM estimation, live sensor calibration, or trajectory accuracy; those require a GLIM-enabled integration or hardware run.

Viewer and completed submaps

jsonCfg/GLIM_orbbec.json, jsonCfg/GLIM_realSense.json, and jsonCfg/GLIM_scepter.json connect camera → PCLframe → _GLIM → PCLmap → _WebGLIM. GLIM publishes submaps to its PCLmapOut stream; the viewer names that same stream with PCLmapIn. The viewer uses the shared HttpServer with its own /stream/glim protocol. _WSconsole carries controls, parameters and pose/status independently. The viewer reads only the DataObject and does not depend on the GLIM estimator class or SDK.

The backend retains a copied map of completed submaps, each with a stable session-local ID and current map pose. The browser accumulates those submaps and uploads each submap's geometry once. Graph optimization sends pose corrections without retransmitting unchanged point arrays. Copying a changed PCLmap still copies every included local point vector. There is no point cloud before the first completed submap; pose, FoV and the browser's recent trajectory update while tracking. Keyframe selection and smoother lag determine when submaps finish. Stop flushes remaining mapping work. With bMapping: false, the dedicated viewer displays pose and trajectory only.

The stream sends a versioned JSON hello, then flow-controlled reset/submap/pose metadata and bounded binary XYZ chunks. Every data message requires a next credit; each client has its own cursor. Reconnection replays all completed submaps, slow clients catch up, and a new session resets both complete and partial geometry. New map timestamps may update poses without changing IDs or point arrays. The dedicated layout and routes are separate from geometry point/line streams; see the wire contract.

PCLframeOut names an optional flat point-cloud output for other modules. With bPublishLiveMap: false (default), only completed-submap changes and finalization refresh this output, subject to tMapUpdateNs. nMapPoints caps this output cloud and does not limit the dedicated viewer's accumulated submaps. Set bPublishLiveMap: true to include the continuous recent-frame preview for other consumers. nLiveFrames bounds retained unfinished history; _WebGLIM never streams those individual frames. The flat output must differ from PCLframeIn.

The browser trajectory is a bounded history of fresh status poses, not GLIM's optimized historical trajectory. Completed-submap pose corrections update the map and latest sensor marker; they do not retrospectively solve the browser's old trajectory samples. See the viewer guide for controls, distance coloring, sensor setup and launch instructions.

Parameters and PLY export

The web parameter panel edits a validated JSON object exposed by getConfig. Common fields are bMapping, nMinPoints, and preprocess fields distanceNear, distanceFar, voxelResolution, targetPoints, kNeighbors, and threads. Recognized CPU and CT odometry add odometry controls; standard submapping/global mapping add their corresponding groups. Other selected backends still load through GLIM's module factory. Unexposed settings and sensor calibration continue to come from the selected profile.

Configuration changes require stopped tracking. setConfig merges and validates partial values in memory; saveConfig writes the current values into the module's parameters object in its original launch JSON file. getConfig retrieves current values. Startup loads profile defaults and the optional module parameters object through loadConfig(), including values saved previously. Values take effect on the next Start; Start applies unsaved UI edits first. Unknown fields, wrong types, invalid ranges and inconsistent point-count limits are rejected. The Save parameters button applies edits with setConfig before sending the no-argument saveConfig command.

Commands use the usual _WSconsole envelope and trailing EOJ:

{"module":"GLIM","cmd":"getConfig","requestId":"1"}
{"module":"GLIM","cmd":"setConfig","config":{"preprocess":{"distanceFar":5.0}},"requestId":"2"}
{"module":"GLIM","cmd":"saveConfig","requestId":"3"}
{"module":"GLIM","cmd":"savePointCloud","requestId":"5"}

Replies echo the command/module/request ID, report bSuccess and optional error, and normally include status. Parameter replies add config and configFile; point-cloud saves add path and points. session identifies the estimator lifecycle. mapTimestampNs and submapTimestampNs are decimal timestamps for the flat cloud and submap outputs.

savePointCloud uses _PCLfile::savePLY to write binary little-endian XYZ float32 and RGB uint8. It snapshots all completed submaps at their latest optimized poses plus retained unfinished frames, up to nLiveFrames of recent history. It bypasses nMapPoints and the optional display cloud; it still exports GLIM's processed map points, not every raw depth sample. Snapshot collection holds the estimator lock and file I/O occurs after releasing it. Saving is allowed during tracking; stop first when a finalized, optimized export is desired.

Without a path, the command creates exportPath (default data/glim) and a unique map-<timestamp>.ply. With an explicit .ply path, its parent must already exist. The reply names the file on the backend PC; the button does not download a file in the browser. This PLY export is distinct from saveMap(path), which saves GLIM's native finished map after tracking stops.

Measuring performance

Use the GLIM: build optimized VSCode task for live SLAM. It configures build as RelWithDebInfo, preserves the cached component switches, and builds build/OpenKAI. The GLIM Scepter (optimized) launch runs this task before starting GDB with jsonCfg/GLIM_scepter.json; enable the SDK first as described in the Scepter profile guide. The original Orbbec launch remains available. Debug (-O0) retains expensive Eigen and container operations in the point-cloud hot path; changing the thread's target FPS does not remove that work.

The dedicated viewer transfers completed submaps and small pose updates when needed. DataObject set and get each copy their payloads, including all local point vectors in a PCLmap. The optional flat PCLframe is copied too. Readers poll getTstamp() and copy only changed timestamps; repeated timestamps cannot signal a changed payload. The independent DataObject tests cover copy isolation, payload/timestamp coherence, replacement, and empty frames.

Measure the real camera with an active point-cloud stream and collect getStatus samples after a warmup period. Keep the camera still during initialization. Compare runs with the same sensor, scene, mapping settings and compiler settings. Run long enough to produce completed submaps before comparing sustained mapping and stream throughput; network traffic is event-driven rather than one point cloud per video frame.

getStatus exposes cumulative workMs for IMU ingestion, input copying/conversion, preprocessing, odometry, mapping and map publication. Subtract two readings and divide by the change in frames for amortized milliseconds per processed frame. These totals include polls that did not produce a pose; they exclude worker sleep, command-lock waiting and asynchronous HTTP/WebSocket serialization. publishMap measures the optional flat cloud output, not the dedicated stream. inputPoints, registrationPoints and updates show the workload. processingMs is the last frame's work time, so periodic samples of it are not a per-frame timing distribution.

With the dedicated submap stream, a stationary Gemini 335 run measured 29.98 SLAM frames/s over 20 seconds after 8 seconds of warmup. There was no repeated cloud transfer between submaps. Stop finalized one 4,345-point submap, delivered in one 52,196-byte binary chunk with no stream errors. This stationary run checks throughput and finalization; the synthetic motion and transport tests exercise multiple submaps, corrections, slow clients and resets during a transfer.

Historical measurements of the preceding _WebGeometry display pipeline on the attached Gemini 335 and Intel Core i9-14900KF, with 10 seconds of warmup followed by 30 seconds of sampling and an active geometry stream:

Build / implementation	SLAM frames/s	Input ms/frame	Map publication ms/frame
Original, Debug	1.49	52.04	498.16
Optimized hot paths, Debug	5.78	8.65	64.92
Optimized hot paths, RelWithDebInfo	29.94	1.54	0.38

These results predate the dedicated completed-submap stream and are retained to document the original bottleneck, not as a benchmark of the current transport. They are stage totals divided by processed frames, including the configured map-publication interval. Sensor/registration/map limits were unchanged; the runs processed approximately 176–198k valid input points, 5.8–6k registration points, and 338–360k displayed map points. The final run had no stream errors and a maximum IMU gap of 4.943 ms. This measures throughput in the camera's current scene; motion accuracy is a separate measurement. The subsequent hardware browser test also passed Start/Stop/Reset, retained submaps, map streaming, pose/FoV rendering and reconnect at 30.0 SLAM frames/s.

Installing the dependencies

sudo apt update
sudo apt install --no-install-recommends \
    libomp-dev libmetis-dev \
    libfmt-dev libspdlog-dev \
    libboost-serialization-dev \
    libboost-filesystem-dev \
    libboost-thread-dev \
    libboost-program-options-dev \
    libboost-date-time-dev \
    libboost-timer-dev \
    libboost-chrono-dev \
    libboost-regex-dev \
    libboost-random-dev \
    libboost-graph-dev

unset ROS_VERSION

git clone --depth 1 --branch 4.3a1 https://github.com/borglab/gtsam.git

Edit

nano gtsam/gtsam/geometry/SL4.cpp

add near the top

#include <cassert>

Edit

nano gtsam/gtsam/slam/FrobeniusFactor.h

Around lines 200–225, replace the evaluateError() implementation with this:

Vector evaluateError(const T& T1, const T& T2, OptionalMatrixType H1,
                     OptionalMatrixType H2) const override {
  const bool computeJacobians = H1 || H2;

  // predict T2*T1
  typename T::Jacobian H_T21_T2;
  const T hatT21 =
      traits<T>::Between(T2, T1,
                         computeJacobians ? &H_T21_T2 : nullptr);

  // Calculate \hat T21 * T12_, which is predicted to be I_NxN
  typename T::Jacobian H_pred_hat = T::Jacobian::Zero();
  const T pred =
      traits<T>::Compose(hatT21, T12_,
                         computeJacobians ? &H_pred_hat : nullptr);

  // Move to constructor
  const MatrixN I = MatrixN::Identity();
  const VectorD vecI = Eigen::Map<const VectorD>(I.data());

  // Calculate error
  Eigen::Matrix<double, Dim, T::dimension> H_vec_pred;
  Vector error =
      traits<T>::Vec(pred, computeJacobians ? &H_vec_pred : nullptr) - vecI;

  // Do chain rule only when Jacobians were actually requested
  if (computeJacobians) {
    const auto H_error_hat21 = H_vec_pred * H_pred_hat;
    if (H1) *H1 = H_error_hat21;
    if (H2) *H2 = H_error_hat21 * H_T21_T2;
  }

  return error;
}

Build

cd gtsam
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release \
      -DCMAKE_INSTALL_PREFIX=/usr/local \
      -DCMAKE_INSTALL_LIBDIR=lib \
      -DGTSAM_BUILD_TESTS=OFF \
      -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
      -DGTSAM_BUILD_TIMING_ALWAYS=OFF \
      -DGTSAM_BUILD_UNSTABLE=ON \
      -DGTSAM_BUILD_PYTHON=OFF \
      -DGTSAM_INSTALL_MATLAB_TOOLBOX=OFF \
      -DGTSAM_WITH_TBB=OFF \
      -DGTSAM_USE_SYSTEM_EIGEN=ON \
      -DEigen3_DIR=/usr/local/share/eigen3/cmake \
      -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ../

make -j$(nproc)
sudo make install
sudo ldconfig

gtsam_points

git clone --depth 1 --branch v1.2.2 https://github.com/koide3/gtsam_points.git
cd gtsam_points
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release \
      -DCMAKE_INSTALL_PREFIX=/usr/local \
      -DCMAKE_INSTALL_LIBDIR=lib \
      -DCMAKE_PREFIX_PATH=/usr/local \
      -DEigen3_DIR=/usr/local/share/eigen3/cmake \
      -DBUILD_WITH_CUDA=OFF \
      -DBUILD_WITH_TBB=OFF \
      -DBUILD_WITH_OPENMP=ON \
      -DBUILD_WITH_MARCH_NATIVE=OFF \
      -DBUILD_DEMO=OFF \
      -DBUILD_EXAMPLE=OFF \
      -DBUILD_TOOLS=OFF \
      -DBUILD_TESTS=OFF \
      -DBUILD_TESTS_PCL=OFF ../

make -j$(nproc)
sudo make install
sudo ldconfig

GLIM

git clone --depth 1 --branch v1.2.2 https://github.com/koide3/glim.git
cd glim
mkdir build
cd build

unset ROS_VERSION
cmake -DCMAKE_BUILD_TYPE=Release \
      -DCMAKE_INSTALL_PREFIX=/usr/local \
      -DCMAKE_INSTALL_LIBDIR=lib \
      -DCMAKE_PREFIX_PATH=/usr/local \
      -DEigen3_DIR=/usr/local/share/eigen3/cmake \
      -DBUILD_WITH_CUDA=OFF \
      -DBUILD_WITH_VIEWER=OFF \
      -DBUILD_WITH_OPENCV=OFF \
      -DBUILD_WITH_MARCH_NATIVE=OFF ../

make -j$(nproc)
sudo make install
sudo ldconfig