diff --git a/data/.lfs/go2dds_data1.tar.gz b/data/.lfs/go2dds_data1.tar.gz new file mode 100644 index 0000000000..f44d478ad5 --- /dev/null +++ b/data/.lfs/go2dds_data1.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dfd237ce49cef1f96074ba02576b9a8094db6d6e0dc9ca24b63e5fdaa84acaf2 +size 224684671 diff --git a/data/.lfs/go2dds_data2.tar.gz b/data/.lfs/go2dds_data2.tar.gz new file mode 100644 index 0000000000..1cfc61c3ee --- /dev/null +++ b/data/.lfs/go2dds_data2.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:26dd5e60cb618f8e65fc25f878e6eb4ce23fbc486207ccb6c3b6cafdcfb33f44 +size 482604872 diff --git a/data/.lfs/go2dds_data3.tar.gz b/data/.lfs/go2dds_data3.tar.gz new file mode 100644 index 0000000000..15f597da62 --- /dev/null +++ b/data/.lfs/go2dds_data3.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f58b2035be3c0d81d045c3162e93dff2150980d8ecf22721c797c443f9a3a81 +size 2940817005 diff --git a/dimos/mapping/research/go2/.gitignore b/dimos/mapping/research/go2/.gitignore new file mode 100644 index 0000000000..8bb5cb9a39 --- /dev/null +++ b/dimos/mapping/research/go2/.gitignore @@ -0,0 +1,36 @@ +# Python-generated files +__pycache__/ +*.py[oc] +dist/ +wheels/ +*.egg-info + +# Virtual environments +.venv +worktrees/ +queue/ + +# Agent prompt files (generated per-session by launchers) +CLAUDE.md +AGENTS.md + +# Experimental code/artifacts +dev/ + +# Results file (leave untracked) +results.tsv + +# experiment artifacts +run.log + +# large human-debug recordings (kept on disk, not in git; derivable from the mcap) +human-debug/*.mcap +human-debug/*.rrd + +# point_lio build + per-run outputs (regenerated each run) +point_lio/build/ +point_lio/Log/* +!point_lio/Log/.gitkeep +point_lio/PCD/* +!point_lio/PCD/do_not_delete_this_file.txt +point_lio/config/_active.yaml diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/.gitignore b/dimos/mapping/research/go2/autoresearch/lio-1/.gitignore new file mode 100644 index 0000000000..52936d78d2 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/.gitignore @@ -0,0 +1,40 @@ +# Python-generated files +__pycache__/ +*.py[oc] +dist/ +wheels/ +*.egg-info + +# Virtual environments +.venv +worktrees/ +queue/ + +# Agent prompt files (generated per-session by launchers) +CLAUDE.md +AGENTS.md + +# Experimental code/artifacts +dev/ + +# Results / search artifacts (leave untracked) +results.tsv +search_log.tsv +best_config.json +optuna_study.db + +# experiment artifacts +run.log +logs/ + +# large human-debug recordings (kept on disk, not in git; derivable from the mcap) +human-debug/*.mcap +human-debug/*.rrd + +# point_lio build + per-run outputs (regenerated each run) +point_lio/build/ +point_lio/Log/* +!point_lio/Log/.gitkeep +point_lio/PCD/* +!point_lio/PCD/do_not_delete_this_file.txt +point_lio/config/_*.yaml diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/README.md new file mode 100644 index 0000000000..bf7d0914c0 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/README.md @@ -0,0 +1,80 @@ +# lio-autoresearch + +An [autoresearch](https://github.com/karpathy/autoresearch)-style autonomous +experiment loop, adapted from LLM pretraining to **LiDAR-inertial odometry +tuning**. Give an AI agent a real Point-LIO pipeline and a recorded Go2 +LiDAR+IMU stream, and let it experiment autonomously: modify the config, run +the LIO, check whether the trajectory agrees better with the robot's onboard +odometry, keep or discard, repeat. + +## How it works + +Three files matter (same idiom as the original autoresearch): + +- **`evaluate.py`** — fixed data paths, the Point-LIO substrate location, and the + evaluation (`evaluate`, the ground-truth metric). Not modified. +- **`algo.py`** — the single file the agent edits. Holds the Point-LIO `CONFIG` + and the run/eval driver. **Edited and iterated on by the agent.** +- **`program.md`** — baseline instructions for the agent. **Edited by the human.** + +The agent does NOT touch the Point-LIO C++ engine (`point_lio/`) — that's the +fixed substrate, built once by `setup.sh`, analogous to a fixed model framework. +The agent tunes its configuration. + +The metric is **`val_ate_xy`**: the 2D (xy) absolute trajectory error (RMSE, +meters) of the rigid-aligned LIO trajectory against `robot_odom`, the robot's +leg-inertial odometry, which loop-closes to ~0.47 m over a 16.5 m path and is +our rough ground truth. Lower is better. It is **2D only** today; 3D criteria +(measured step heights from a stairs recording) come later. + +## The experiment + +The Go2's L1 lidar is sparse (18 lines) and Point-LIO tracks well on straight +motion but loses lock during in-place rotation (the loop turn-around). The +research question this harness lets an agent explore: how far can configuration +tuning alone close the gap to `robot_odom`? + +## Quick start + +This package runs in the **dimos environment** — it carries no venv of its own. +The C++ build deps (cmake, eigen, pcl, yaml-cpp, boost) come from the Nix dev +shell defined in `../flake.nix`; the python deps (numpy, matplotlib) and +`dimos.get_data` come from the dimos `.venv`. The LIO input + ground truth are +pulled from the dimos LFS data store (`go2dds_data1`) by `get_data` on first run. + +```bash +# 1. Enter the dev shell (C++ build deps layered on the dimos dev shell) +cd .. # -> dimos/mapping/research/go2/autoresearch +nix develop + +# 2. Build the substrate + sanity check (pulls the data via get_data) +cd lio-1 +./setup.sh + +# 3. Run a single baseline experiment (~1-3 min) +python algo.py +``` + +If those work, you're ready for autonomous mode: point your agent at +`program.md`. + +## Project structure + +``` +evaluate.py — data paths (via get_data), substrate location, evaluation (do not modify) +algo.py — Point-LIO CONFIG + run/eval driver (agent modifies this) +program.md — agent instructions +setup.sh — environment prep (build point_lio; checks the dimos venv) +config/v2_imu.yaml— the baseline config, for reference +point_lio/ — the fixed Point-LIO C++ engine (built by setup.sh) +human-debug/ — convert script, raw .mcap, .rrd rerun — HUMAN USE ONLY, not in the loop +``` + +Data lives in the dimos LFS store, not here: `data/go2dds_data1/` holds +`20260529-185959.mcap` (LIO input — Point-LIO reads it directly) + +`gt_robot_odom.tsv` (ground truth), fetched via `get_data("go2dds_data1/...")`. + +## License + +The `point_lio/` engine is GPL (see `point_lio/LICENSE`); it derives from +unitreerobotics/point_lio_unilidar. The harness files are MIT. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/algo.py b/dimos/mapping/research/go2/autoresearch/lio-1/algo.py new file mode 100644 index 0000000000..f4c97b878d --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/algo.py @@ -0,0 +1,229 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The single file the agent edits. It defines the Point-LIO configuration +(CONFIG), runs the offline LIO on the recorded Go2 stream to produce an +odometry trajectory, scores it against robot_odom ground truth via the frozen +harness in evaluate.py, and prints a summary block. + +The Point-LIO C++ substrate (point_lio/) and the evaluation (evaluate.py) are +fixed. You tune CONFIG (and, if you want, the Python pre/post-processing in +this file). The goal: minimize `val_ate_xy` — the 2D trajectory error vs the +robot's leg-inertial odometry ground truth. + +Baseline CONFIG below = the "v2_imu" config: IMU-coupled, with the corrections +that matter for this Go2-L1 unit — measured acc_norm 10.434, imu_time_inte +0.004 (~250 Hz IMU), and extrinsic_R = Rx(pi) = diag(1,-1,-1) for the IMU's +180-deg roll vs the lidar. Run this as-is first to establish the baseline. +""" + +import os +import subprocess +import time + +import evaluate + +# A run must overlap at least this fraction of the GT *duration* to count as a +# real result; below it the trajectory is a stub from an early death (see run()). +MIN_COVERAGE_FRAC = 0.8 + +# ---------------------------------------------------------------------------- +# CONFIG — the knobs you tune. These map 1:1 to the Point-LIO yaml. +# ---------------------------------------------------------------------------- +CONFIG = { + # preprocess + "lidar_type": 5, # 5 = unilidar L1 + "scan_line": 18, + "timestamp_unit": 0, # per-point time is in SECONDS (validated) + "blind": 0.5, # ignore returns closer than this (m) + # mapping / EKF + "imu_en": True, + "imu_time_inte": 0.004, # ~250 Hz IMU integration dt + "acc_norm": 10.434, # measured static |g| for this Go2-L1 IMU + "lidar_meas_cov": 0.01, + "plane_thr": 0.1, + "match_s": 81, + "filter_size_surf": 0.1, # downsample voxel size (surf), m + "filter_size_map": 0.1, # downsample voxel size (map), m + "fov_degree": 180, + "det_range": 100.0, + "gravity_align": True, + "start_in_aggressive_motion": False, + "extrinsic_est_en": False, + # IMU process / measurement covariances + "satu_acc": 30.0, + "satu_gyro": 35, + "acc_cov_output": 500, + "gyr_cov_output": 1000, + "b_acc_cov": 0.0001, + "b_gyr_cov": 0.0001, + "imu_meas_acc_cov": 0.1, + "imu_meas_omg_cov": 0.1, + "gyr_cov_input": 0.01, + "acc_cov_input": 0.1, + # extrinsics (imu -> lidar). Rx(pi) handles the L1 IMU's 180-deg roll. + "extrinsic_T": [0, 0, 0], + "extrinsic_R": [1, 0, 0, 0, -1, 0, 0, 0, -1], +} + + +def write_yaml(cfg, path): + R = cfg["extrinsic_R"] + T = cfg["extrinsic_T"] + txt = f"""# auto-generated by algo.py — do not hand-edit; edit CONFIG in algo.py +common: + lid_topic: "/unilidar/cloud" + imu_topic: "/unilidar/imu" + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 +preprocess: + lidar_type: {cfg["lidar_type"]} + scan_line: {cfg["scan_line"]} + timestamp_unit: {cfg["timestamp_unit"]} + blind: {cfg["blind"]} +mapping: + imu_en: {str(cfg["imu_en"]).lower()} + start_in_aggressive_motion: {str(cfg["start_in_aggressive_motion"]).lower()} + extrinsic_est_en: {str(cfg["extrinsic_est_en"]).lower()} + imu_time_inte: {cfg["imu_time_inte"]} + filter_size_surf: {cfg["filter_size_surf"]} + filter_size_map: {cfg["filter_size_map"]} + satu_acc: {cfg["satu_acc"]} + satu_gyro: {cfg["satu_gyro"]} + acc_norm: {cfg["acc_norm"]} + lidar_meas_cov: {cfg["lidar_meas_cov"]} + acc_cov_output: {cfg["acc_cov_output"]} + gyr_cov_output: {cfg["gyr_cov_output"]} + b_acc_cov: {cfg["b_acc_cov"]} + b_gyr_cov: {cfg["b_gyr_cov"]} + imu_meas_acc_cov: {cfg["imu_meas_acc_cov"]} + imu_meas_omg_cov: {cfg["imu_meas_omg_cov"]} + gyr_cov_input: {cfg["gyr_cov_input"]} + acc_cov_input: {cfg["acc_cov_input"]} + plane_thr: {cfg["plane_thr"]} + match_s: {cfg["match_s"]} + fov_degree: {cfg["fov_degree"]} + det_range: {cfg["det_range"]} + gravity_align: {str(cfg["gravity_align"]).lower()} + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [ {T[0]}, {T[1]}, {T[2]} ] + extrinsic_R: [ {R[0]}, {R[1]}, {R[2]}, {R[3]}, {R[4]}, {R[5]}, {R[6]}, {R[7]}, {R[8]} ] +odometry: + publish_odometry_without_downsample: enable +publish: + path_en: true + scan_publish_en: true + scan_bodyframe_pub_en: false +pcd_save: + pcd_save_en: false + interval: -1 +""" + with open(path, "w") as f: + f.write(txt) + + +def run(overrides=None, *, yaml_path=None, out_path=None, render=False, timeout=None): + """Run Point-LIO once with CONFIG (optionally overridden) and score it. + + `overrides` is a dict merged over CONFIG — this is the importable entry point + a hyperparameter search drives (see search.py / search_optuna.py). Pass + distinct yaml_path/out_path to run trials concurrently (parallel search); + they default to the shared paths, which is correct for sequential use. + `timeout` (seconds) caps the binary; pathological configs (tiny filter sizes + → huge map → very slow ICP) get killed and surfaced as TimeoutExpired so the + caller can penalize them. Defaults to evaluate.RUN_TIMEOUT. + Returns the evaluate() metrics dict with run_seconds added. Raises on failure + (TimeoutExpired, or a ValueError from evaluate() on a missing/malformed + trajectory); the caller decides how to treat it. + """ + if not os.path.exists(evaluate.POINTLIO_BIN): + raise SystemExit("pointlio binary not built — run ./setup.sh first.") + yaml_path = yaml_path or evaluate.ACTIVE_YAML + out_path = out_path or evaluate.TRAJ_PATH + cfg = {**CONFIG, **(overrides or {})} + write_yaml(cfg, yaml_path) + if os.path.exists(out_path): + os.remove(out_path) + + t0 = time.time() + subprocess.run( + [ + evaluate.POINTLIO_BIN, + "--yaml", + yaml_path, + "--mcap", + evaluate.MCAP_PATH, + "--out", + out_path, + ], + cwd=evaluate.POINTLIO_DIR, + timeout=timeout or evaluate.RUN_TIMEOUT, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + check=True, # nonzero exit = the binary aborted/diverged -> CalledProcessError + ) + m = evaluate.evaluate(out_path) # raises if trajectory missing/malformed (= crash) + m["run_seconds"] = time.time() - t0 + + # Coverage guard: a config that makes Point-LIO die early still writes a short + # trajectory stub, and evaluate() only scores the LIO/GT *time overlap* — so a + # stub near the start aligns near-perfectly and yields a deceptively tiny ATE + # (the spurious 0.42 outlier). Guard on TIME coverage, not path length: every + # config (good or bad) integrates a huge path_len from ~4500 poses/s of micro- + # jitter, so path_len can't distinguish a stub, but a short trajectory's + # overlap_s is genuinely a fraction of the GT duration. + gt_t, _ = evaluate.load_gt() + gt_dur = float(gt_t[-1] - gt_t[0]) + if m["overlap_s"] < MIN_COVERAGE_FRAC * gt_dur: + raise ValueError( + f"trajectory overlaps GT for only {m['overlap_s']:.1f}s of {gt_dur:.1f}s " + f"(< {MIN_COVERAGE_FRAC:.0%}) — Point-LIO diverged/died early; not a real result" + ) + + if render: + # Render viz.png + downsampled traj_ds.tsv (non-fatal: a plotting failure + # must never fail the run). + try: + import visualize + + visualize.render() + except Exception as e: + print(f"(visualize skipped: {e})") + return m + + +def main(): + try: + m = run(render=True) + except subprocess.TimeoutExpired: + raise SystemExit(f"run exceeded {evaluate.RUN_TIMEOUT}s — treat as failure") + + print("---") + print(f"val_ate_xy: {m['val_ate_xy']:.6f}") + print(f"final_err_xy: {m['final_err_xy']:.3f}") + print(f"loop_close_xy: {m['loop_close_xy']:.3f}") + print(f"gt_loop_xy: {m['gt_loop_xy']:.3f}") + print(f"path_len: {m['path_len']:.2f}") + print(f"gt_path_len: {m['gt_path_len']:.2f}") + print(f"num_poses: {m['num_poses']}") + print(f"overlap_s: {m['overlap_s']:.1f}") + print(f"run_seconds: {m['run_seconds']:.1f}") + + +if __name__ == "__main__": + main() diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/config/v2_imu.yaml b/dimos/mapping/research/go2/autoresearch/lio-1/config/v2_imu.yaml new file mode 100644 index 0000000000..e68e61f882 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/config/v2_imu.yaml @@ -0,0 +1,52 @@ +###### v2_imu — IMU-coupled, corrected for this Go2-L1: measured acc_norm 10.434, +###### imu_time_inte 0.004 (~223-250 Hz), and extrinsic_R = Rx(pi)=diag(1,-1,-1) for the +###### IMU's 180-deg roll vs the lidar (doc 20260529-06: imu2lidar = [0,0,0,pi,0,0]). +###### extrinsic_T ~ 0 (L1 IMU is co-located inside the lidar). Tight lidar cov + match_s 81. +common: + lid_topic: "/unilidar/cloud" + imu_topic: "/unilidar/imu" + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 +preprocess: + lidar_type: 5 + scan_line: 18 + timestamp_unit: 0 + blind: 0.5 +mapping: + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.004 + satu_acc: 30.0 + satu_gyro: 35 + acc_norm: 10.434 + lidar_meas_cov: 0.01 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.1 + imu_meas_omg_cov: 0.1 + gyr_cov_input: 0.01 + acc_cov_input: 0.1 + plane_thr: 0.1 + match_s: 81 + fov_degree: 180 + det_range: 100.0 + gravity_align: true + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [ 0, 0, 0 ] + extrinsic_R: [ 1, 0, 0, 0, -1, 0, 0, 0, -1 ] +odometry: + publish_odometry_without_downsample: enable +publish: + path_en: true + scan_publish_en: true + scan_bodyframe_pub_en: false +pcd_save: + pcd_save_en: true + interval: -1 diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/eval3d.py b/dimos/mapping/research/go2/autoresearch/lio-1/eval3d.py new file mode 100644 index 0000000000..f37fba0b39 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/eval3d.py @@ -0,0 +1,443 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""3D criteria for the LIO autoresearch eval, scored from an odometry trajectory +against a recording's annotations.json. Three complementary signals: + + C1 tag_spread — detect AprilTags (36h11, 10 cm) in the recording, project each + to the world via the INPUT trajectory's pose, group same-id + detections into per-visit tracks (>15 s gap = new visit), and + measure the spread of a tag's tracks across visits. A drift-free + odom puts the same physical tag at the same world point every + visit → spread → 0. (The mentor's TOTAL_SPREAD idea.) + C2 z_floor — per floor-occupancy window, mean |z - floor_level| (z anchored + to the first known floor). Penalizes off-level AND non-flat. + C3 z_ramp — per stair transition, |net Δz - (h_to - h_from)| + a monotonicity + penalty. A bump-and-reset or random z scores badly. + +Settings below are the Go2-L1 current settings (720p intrinsics scaled to the +recorded 1080p; equidistant/fisheye distortion; base->cam extrinsic chain). +""" + +from __future__ import annotations + +from itertools import combinations +import json +import os +import struct + +import numpy as np + +HERE = os.path.dirname(os.path.abspath(__file__)) + +# --- current settings ------------------------------------------------------- +TAG_SIZE_M = 0.10 +_S = 1.5 # recorded frames are 1080p = 1.5x the 720p calibration +K = np.array([[797.4756 * _S, 0, 643.5352 * _S], [0, 796.4872 * _S, 349.2784 * _S], [0, 0, 1.0]]) +DIST = np.array( + [-0.07309428880537933, -0.02341140740909078, -0.0069305931780026956, 0.009238684474464793] +) # fisheye k1..k4 +ARUCO_DICT = "DICT_APRILTAG_36h11" +R_OPT2LINK = np.array([[0, 0, 1.0], [-1, 0, 0], [0, -1, 0]]) # camera_optical -> camera_link +T_LINK2BASE = np.array([0.3, 0.0, 0.0]) # camera_link -> base_link +VISIT_GAP_S = 15.0 # time gap that splits a tag's detections into separate visits +TRIM_S = 3.0 # trim each floor window's edges (annotation times are approximate) + +# Total |Δz| the robot really travels in a recording = Σ |floor-gap| over its stair +# transitions, from the tape-measured floor levels. Hardcoded per dataset on purpose +# (do not read annotations for it). Keyed by a substring of the mcap path. +Z_TRAVEL_M = { + "go2dds_data2": 2.090, # 2.5F<->2F, descend + ascend (1.045 x2) + "go2dds_data3": 23.090, # 1.045 x2 + 3.5 x6 across 1F / 2F / 2.5F / 3F +} + + +# --- minimal CDR (matches go2-station/scripts/go2_cdr.py) ------------------- +class _Cur: + def __init__(self, b): + self.b = b + self.p = 4 # skip 4-byte encapsulation header + + def _al(self, n): + m = (self.p - 4) % n + if m: + self.p += n - m + + def i32(self): + self._al(4) + v = struct.unpack_from(" (pos[3], quat xyzw[4]) + c = _Cur(data) + c.stamp_ns() + c.s() + c.s() + pos = c.f64n(3) + quat = c.f64n(4) + return pos, quat + + +# --- pose helpers ----------------------------------------------------------- +def quat_to_R(x, y, z, w): + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ] + ) + + +def euler_deg_to_R(rpy_deg): + """ZYX (yaw·pitch·roll) from mat_out's SO3ToEuler (degrees, pitch=asin(2(wy-zx))). + Vectorized: rpy_deg (3,) -> (3,3); (N,3) -> (N,3,3).""" + a = np.radians(np.atleast_2d(np.asarray(rpy_deg, float))) + r, p, y = a[:, 0], a[:, 1], a[:, 2] + cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p), np.sin(p), np.cos(y), np.sin(y) + R = np.empty((len(r), 3, 3)) + R[:, 0, 0] = cy * cp + R[:, 0, 1] = cy * sp * sr - sy * cr + R[:, 0, 2] = cy * sp * cr + sy * sr + R[:, 1, 0] = sy * cp + R[:, 1, 1] = sy * sp * sr + cy * cr + R[:, 1, 2] = sy * sp * cr - cy * sr + R[:, 2, 0] = -sp + R[:, 2, 1] = cp * sr + R[:, 2, 2] = cp * cr + return R[0] if np.ndim(rpy_deg) == 1 else R + + +# --- trajectory loaders ----------------------------------------------------- +def load_mat_out(path): + """Point-LIO Log/mat_out.txt -> (t_rel[N], pos[N,3], R[N,3,3]). Harness output.""" + M = np.loadtxt( + path, usecols=(0, 1, 2, 3, 4, 5, 6) + ) # t, euler(rpy deg), pos(xyz) — skip the rest + t, eul, pos = M[:, 0], M[:, 1:4], M[:, 4:7] + R = euler_deg_to_R(eul) # vectorized -> (N,3,3) + return t, pos, R + + +def load_robot_odom(mcap_path): + """robot_odom from the recording -> (t_rel, pos, R, first_lidar_pub_ns). The + gt-ish leg-inertial backbone; handy as the reference 'input odom' for testing.""" + from mcap.reader import make_reader + + t, pos, R, flp = [], [], [], None + with open(mcap_path, "rb") as f: + for _, ch, m in make_reader(f).iter_messages( + topics=["rt/utlidar/robot_odom", "rt/utlidar/cloud"] + ): + if ch.topic == "rt/utlidar/cloud": + if flp is None: + flp = m.publish_time + else: + p, q = _decode_odom(m.data) + t.append(m.publish_time) + pos.append(p) + R.append(quat_to_R(*q)) + t = np.array(t) + o = np.argsort(t) + return (t[o] - flp) / 1e9, np.array(pos)[o], np.array(R)[o], flp + + +# --- annotation helpers ----------------------------------------------------- +def _anchor_z(t, z, ann): + """Offset z so the first known-level floor window sits at its level.""" + for ph in ann["phases"]: + lvl = ph.get("level") + h = ann["levels"].get(lvl) if lvl else None + if h is None: + continue + m = (t >= ph["t0"] + TRIM_S) & (t <= ph["t1"] - TRIM_S) + if m.sum() > 2: + return z + (h - z[m].mean()) + return z + + +def _floor_before_after(ann, idx): + """Levels of the nearest level-phase before/after transition phase idx.""" + before = after = None + for j in range(idx - 1, -1, -1): + if "level" in ann["phases"][j]: + before = ann["phases"][j]["level"] + break + for j in range(idx + 1, len(ann["phases"])): + if "level" in ann["phases"][j]: + after = ann["phases"][j]["level"] + break + return before, after + + +def _path_len(P): + P = np.asarray(P) + return float(np.sum(np.linalg.norm(np.diff(P, axis=0), axis=1))) + + +def _dataset_key(mcap_path): + s = str(mcap_path) + for k in Z_TRAVEL_M: + if k in s: + return k + return None + + +def _dataset_z_travel(mcap_path): + k = _dataset_key(mcap_path) + return Z_TRAVEL_M.get(k) if k else None + + +def _predetect_path(mcap_path): + """Cache produced by predetect.py: {first_lidar_pub_ns, gt_xy_path_m, detections}.""" + k = _dataset_key(mcap_path) + return os.path.join(HERE, "predetect", k + ".json") if k else None + + +# --- C2: floor flatness/level ---------------------------------------------- +def c2_z_floor(t, z, ann): + per, _errs = {}, [] + for ph in ann["phases"]: + lvl = ph.get("level") + h = ann["levels"].get(lvl) if lvl else None + if h is None: + continue + m = (t >= ph["t0"] + TRIM_S) & (t <= ph["t1"] - TRIM_S) + if m.sum() < 3: + continue + e = np.abs(z[m] - h) + per.setdefault(lvl, []).extend(e.tolist()) + summary = {k: float(np.mean(v)) for k, v in per.items()} + allerr = [e for v in per.values() for e in v] + return { + "z_floor_err_m": float(np.mean(allerr)) if allerr else None, + "z_floor_by_level": summary, + } + + +# --- C3: transition ramp ---------------------------------------------------- +def c3_z_ramp(t, z, ann): + per, errs = [], [] + for i, ph in enumerate(ann["phases"]): + if "name" not in ph or ph["name"] not in ("ascend", "descend"): + continue + fr, to = _floor_before_after(ann, i) + hf, ht = ann["levels"].get(fr), ann["levels"].get(to) + if hf is None or ht is None: + continue + gap = ht - hf + seg = (t >= ph["t0"]) & (t <= ph["t1"]) + if seg.sum() < 3: + continue + zs = z[seg] + net = zs[-3:].mean() - zs[:3].mean() + mono = float(np.mean(np.sign(np.diff(zs)) == np.sign(gap))) + err = abs(net - gap) + per.append( + { + "phase": f"{fr}->{to}", + "net_dz": float(net), + "gap": float(gap), + "err_m": float(err), + "monotonic_frac": mono, + } + ) + errs.append(err) + return {"z_ramp_err_m": float(np.mean(errs)) if errs else None, "z_ramp_by_transition": per} + + +# --- C1: AprilTag 3D spread ------------------------------------------------- +def _detect_tags(mcap_path): + """EXPENSIVE, trajectory-independent: read the video, detect 36h11, solvePnP -> + list of (t_ns, tag_id, tvec_cam[3]) (tag centre in the camera-optical frame). + Used by predetect.py to build the cache; also the no-cache fallback.""" + import cv2 + from mcap.reader import make_reader + + det = cv2.aruco.ArucoDetector( + cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, ARUCO_DICT)), + cv2.aruco.DetectorParameters(), + ) + s = TAG_SIZE_M + objp = np.array( + [[-s / 2, s / 2, 0], [s / 2, s / 2, 0], [s / 2, -s / 2, 0], [-s / 2, -s / 2, 0]] + ) + out = [] + with open(mcap_path, "rb") as f: + for _, _ch, m in make_reader(f).iter_messages(topics=["rt/frontvideo"]): + img = cv2.imdecode( + np.frombuffer(_decode_jpeg_bytes(m.data), np.uint8), cv2.IMREAD_GRAYSCALE + ) + corners, ids, _ = det.detectMarkers(img) + if ids is None: + continue + for cn, idv in zip(corners, ids.flatten(), strict=False): + und = cv2.fisheye.undistortPoints(cn.reshape(-1, 1, 2).astype(np.float64), K, DIST) + ok, _rvec, tvec = cv2.solvePnP( + objp, und, np.eye(3), None, flags=cv2.SOLVEPNP_IPPE_SQUARE + ) + if ok: + out.append((int(m.publish_time), int(idv), [float(x) for x in tvec.ravel()])) + return out + + +def c1_tag_spread(t_rel, pos, R, first_lidar_pub_ns, mcap_path, dets=None): + """Project pre-detected tag poses (camera frame) to world via the INPUT trajectory, + group same-id detections into per-visit tracks (>15 s gap), measure their spread. + `dets` = [(t_ns, id, tvec_cam)]; loaded from the predetect cache when None (no mcap/cv2).""" + if dets is None: + cache = _predetect_path(mcap_path) + if cache and os.path.exists(cache): + dets = [(d["t_ns"], d["id"], d["tvec"]) for d in json.load(open(cache))["detections"]] + else: + dets = _detect_tags(mcap_path) + traj_abs = first_lidar_pub_ns + (t_rel * 1e9) # video on the same clock (≈; latency « drift) + byid = {} # id -> list of (t_ns, world_xyz) + for t_ns, idv, tvec in dets: + i = int(np.clip(np.searchsorted(traj_abs, t_ns), 0, len(traj_abs) - 1)) + p_base = R_OPT2LINK @ np.asarray(tvec) + T_LINK2BASE + byid.setdefault(int(idv), []).append((t_ns, R[i] @ p_base + pos[i])) + per, spreads = {}, [] + for idv, lst in byid.items(): + lst.sort() + ts = np.array([d[0] for d in lst]) + W = np.array([d[1] for d in lst]) + groups = np.split(np.arange(len(lst)), np.where(np.diff(ts) / 1e9 > VISIT_GAP_S)[0] + 1) + cents = np.array([W[g].mean(0) for g in groups]) + sp = ( + float( + np.mean( + [ + np.linalg.norm(cents[i] - cents[j]) + for i, j in combinations(range(len(cents)), 2) + ] + ) + ) + if len(cents) > 1 + else None + ) + per[idv] = {"detections": len(lst), "visits": len(cents), "spread_m": sp} + if sp is not None: + spreads.append(sp) + return {"tag_spread_m": float(np.mean(spreads)) if spreads else None, "tag_by_id": per} + + +# --- top-level -------------------------------------------------------------- +def score_3d(t_rel, pos, R, first_lidar_pub_ns, mcap_path, ann, gt_xy_path, with_tags=True): + z = _anchor_z(t_rel, pos[:, 2].copy(), ann) + out = {} + out.update(c2_z_floor(t_rel, z, ann)) + out.update(c3_z_ramp(t_rel, z, ann)) + if with_tags: + try: + out.update(c1_tag_spread(t_rel, pos, R, first_lidar_pub_ns, mcap_path)) + except Exception as e: # cv2/mcap missing or no tags — keep z criteria usable + out["tag_spread_m"] = None + out["tag_error"] = repr(e) + + # --- two separate path-length scores: horizontal jitter vs vertical travel --- + # xy: horizontal path length vs the gt horizontal path (over-travel = jitter). + # z : total floor change actually traversed = Σ per-transition |net Δz| (bob-robust; + # NOT total-variation, which is dominated by body-bob noise) vs the measured total. + lio_xy = _path_len(pos[:, :2]) + climbed = float(sum(abs(tr["net_dz"]) for tr in out.get("z_ramp_by_transition", []))) + exp_z = _dataset_z_travel(mcap_path) + xy_path_score = max(0.0, lio_xy / gt_xy_path - 1.0) if gt_xy_path else None + z_path_score = abs(climbed / exp_z - 1.0) if exp_z else None + out["xy_path_score"] = xy_path_score + out["z_path_score"] = z_path_score + out["path"] = { + "lio_xy_m": lio_xy, + "gt_xy_m": gt_xy_path, + "climbed_z_m": climbed, + "expected_z_m": exp_z, + } + + # --- combined score (lower = better; None terms drop to 0) --- + out["score"] = float( + (out.get("tag_spread_m") or 0.0) + + (out.get("z_floor_err_m") or 0.0) + + (out.get("z_ramp_err_m") or 0.0) + + (xy_path_score or 0.0) + + (z_path_score or 0.0) + ) + return out + + +if __name__ == "__main__": + import argparse + import json + + ap = argparse.ArgumentParser(description="3D eval (tag spread + z floor + z ramp)") + ap.add_argument("--mcap", required=True, help="recording (.mcap) for tag frames + clock") + ap.add_argument("--ann", required=True, help="annotations.json") + ap.add_argument("--traj", help="mat_out.txt to score; omitted -> use robot_odom from --mcap") + ap.add_argument("--no-tags", action="store_true") + ap.add_argument( + "--allow-fallback", + action="store_true", + help="allow the slow no-cache path (read mcap + detect). Default off: require a " + "predetect cache; if absent, print empty JSON and exit (run predetect.py first).", + ) + a = ap.parse_args() + cache = _predetect_path(a.mcap) + cdat = json.load(open(cache)) if (cache and os.path.exists(cache)) else None + if cdat is None and not a.allow_fallback: + print(json.dumps({})) # no predetect cache and fallback disabled — nothing to score + raise SystemExit(0) + ann = json.load(open(a.ann)) + if a.traj: + t, pos, R = load_mat_out(a.traj) + if cdat: # FAST path: constants from the predetect cache; no robot_odom mcap pass + flp, gt_xy_path = cdat["first_lidar_pub_ns"], cdat["gt_xy_path_m"] + else: # un-preprocessed fallback + _, rt_pos, _, flp = load_robot_odom(a.mcap) + gt_xy_path = _path_len(rt_pos[:, :2]) + else: # scoring robot_odom itself (baseline) — needs the odom pass + t, pos, R, flp = load_robot_odom(a.mcap) + gt_xy_path = _path_len(pos[:, :2]) + res = score_3d(t, pos, R, flp, a.mcap, ann, gt_xy_path, with_tags=not a.no_tags) + print(json.dumps(res, indent=2)) diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py b/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py new file mode 100644 index 0000000000..844e0e8691 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py @@ -0,0 +1,173 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Fixed constants, data paths, and the evaluation harness for the LIO +autoresearch experiment. This is the analog of nanochat-autoresearch's +prepare.py: it is READ-ONLY. It defines where the input data and ground truth +live, how to build the Point-LIO substrate, and — most importantly — the +ground-truth metric `evaluate()`. Do not modify this file; tuning happens in +algo.py. + +The experiment: run Point-LIO (offline, on a recorded Go2 L1 lidar+IMU stream) +to produce an odometry trajectory, and score how well it agrees with the +robot's onboard leg-inertial odometry (`robot_odom`), which loop-closes to +~0.47 m over a 16.5 m path and is our rough ground truth. + +METRIC (current): 2D (xy) absolute trajectory error vs ground truth, after a +rigid (rotation+translation, no scale) Umeyama alignment of the LIO frame onto +the odom frame. Lower is better. z is intentionally ignored for now: on this +flat single-story recording robot_odom's z is near-constant and uninformative. +When the stairs ("ankle killer") recording lands we will add 3D criteria +(measured height/range change at specific timesteps) — see evaluate_3d() below. +""" + +import os + +import numpy as np + +from dimos.utils.data import get_data + +HERE = os.path.dirname(os.path.abspath(__file__)) + +# --- fixed data inputs, pulled from the dimos LFS data store. get_data resolves +# /data/go2dds_data1/, downloading + decompressing the LFS archive on first +# use. Point-LIO reads the mcap directly (rt/utlidar/cloud + rt/utlidar/imu). --- +MCAP_PATH = str(get_data("go2dds_data1/20260529-185959.mcap")) # lidar+imu input +GT_PATH = str(get_data("go2dds_data1/gt_robot_odom.tsv")) # robot_odom ground truth + +# --- Point-LIO substrate (fixed; built once by setup.sh) --- +POINTLIO_DIR = os.path.join(HERE, "point_lio") +POINTLIO_BIN = os.path.join(POINTLIO_DIR, "build", "pointlio_mapping") +TRAJ_PATH = os.path.join(POINTLIO_DIR, "Log", "mat_out.txt") # written by each run +ACTIVE_YAML = os.path.join(POINTLIO_DIR, "config", "_active.yaml") # algo.py writes this + +# --- run limits --- +RUN_TIMEOUT = 600 # seconds; a healthy run is ~1-3 min, kill at 10 min + +# mat_out.txt columns: t_rel euler(r p y) pos(x y z) vel(x y z) ... +TRAJ_T, TRAJ_XYZ = 0, slice(4, 7) + + +def check_data(): + """Setup-step sanity check: input mcap, GT, and built binary all present.""" + ok = True + for label, p in [ + ("input mcap", MCAP_PATH), + ("ground truth", GT_PATH), + ("pointlio binary", POINTLIO_BIN), + ]: + present = os.path.exists(p) + print(f" [{'ok' if present else 'MISSING'}] {label}: {p}") + ok = ok and present + if not os.path.exists(POINTLIO_BIN): + print(" -> pointlio binary missing; run ./setup.sh to build it.") + return ok + + +def load_gt(): + """Ground truth robot_odom. Returns (t_rel[N], xyz[N,3]).""" + a = np.loadtxt(GT_PATH) + return a[:, 0], a[:, 1:4] + + +def load_traj(path=TRAJ_PATH): + """LIO trajectory from a Point-LIO mat_out.txt. Returns (t_rel[N], xyz[N,3]).""" + a = np.loadtxt(path) + if a.ndim != 2 or a.shape[0] < 5: + raise ValueError(f"trajectory too short / malformed: {path} shape={a.shape}") + return a[:, TRAJ_T], a[:, TRAJ_XYZ] + + +def _umeyama_2d(src, dst): + """Rigid (R,t), no scale, minimizing |R src + t - dst| for Nx2 point sets.""" + ms, md = src.mean(0), dst.mean(0) + H = (src - ms).T @ (dst - md) + U, _, Vt = np.linalg.svd(H) + d = np.sign(np.linalg.det(Vt.T @ U.T)) + R = Vt.T @ np.diag([1.0, d]) @ U.T + return R, md - R @ ms + + +def _path_len(P): + return float(np.sum(np.linalg.norm(np.diff(P, axis=0), axis=1))) + + +def evaluate(traj_path=TRAJ_PATH): + """THE METRIC. Score a LIO trajectory against robot_odom ground truth in 2D. + + Returns a dict; the primary number to minimize is `val_ate_xy` (meters): + RMSE of the rigid-aligned LIO xy trajectory vs GT, sampled at the LIO times + over the overlapping interval. + """ + gt_t, gt_p = load_gt() + t, P = load_traj(traj_path) + gt_xy = gt_p[:, :2] + xy = P[:, :2] + + # GT interpolated onto LIO timestamps over the overlap + lo, hi = max(t[0], gt_t[0]), min(t[-1], gt_t[-1]) + m = (t >= lo) & (t <= hi) + tv, xyv = t[m], xy[m] + gti = np.column_stack([np.interp(tv, gt_t, gt_xy[:, k]) for k in range(2)]) + + R, tr = _umeyama_2d(xyv, gti) + aligned = (R @ xyv.T).T + tr + ate = float(np.sqrt(np.mean(np.sum((aligned - gti) ** 2, axis=1)))) + final_err = float(np.linalg.norm(aligned[-1] - gti[-1])) + + return { + "val_ate_xy": ate, # PRIMARY (minimize), meters + "final_err_xy": final_err, # aligned end-pose error, m + "loop_close_xy": float(np.linalg.norm(xy[-1] - xy[0])), # LIO |end-start|, m + "gt_loop_xy": float(np.linalg.norm(gt_xy[-1] - gt_xy[0])), # GT |end-start| (~0.47) + "path_len": _path_len(xy), # LIO xy path length, m + "gt_path_len": _path_len(gt_xy), # GT xy path length (~16.5) + "num_poses": int(P.shape[0]), + "overlap_s": float(hi - lo), + } + + +def evaluate_3d(traj_path=TRAJ_PATH): + """PLACEHOLDER for future 3D criteria. Not part of the metric yet. + + Once the stairs ("ankle killer") recording is captured with tape-measured + step heights, this will check actual range / z-height change at specific + timesteps (e.g. total descent over the ~5 steps) against the LIO z. Today's + flat recording has near-constant robot_odom z, so there is nothing to score + in 3D — the metric is 2D only (see evaluate()). + """ + return {} + + +if __name__ == "__main__": + # Score the latest algo.py run against ground truth. Also a setup check: + # if the data/binary or a trajectory is missing, it says what to do. + print("LIO autoresearch — evaluate") + print(f" HERE: {HERE}") + if not check_data(): + raise SystemExit("Setup incomplete — run ./setup.sh.") + + gt_t, gt_p = load_gt() + print( + f" GT: {len(gt_t)} poses, t {gt_t[0]:.1f}..{gt_t[-1]:.1f}s, " + f"xy path {_path_len(gt_p[:, :2]):.2f}m, " + f"xy loop {np.linalg.norm(gt_p[-1, :2] - gt_p[0, :2]):.3f}m" + ) + + if not os.path.exists(TRAJ_PATH): + raise SystemExit(f"No trajectory at {TRAJ_PATH} yet — run `python algo.py` first.") + + print(f" scoring {TRAJ_PATH}") + for k, v in evaluate().items(): + print(f" {k:14} {v}") diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/human-debug/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/human-debug/README.md new file mode 100644 index 0000000000..ecd5df2e6a --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/human-debug/README.md @@ -0,0 +1,11 @@ +# human-debug — NOT part of the experiment + +These files are for manual inspection by a human only. The autoresearch loop +(program.md) must NOT use them. The experiment's input (`data/go2-185959.bin`) +and ground truth (`data/gt_robot_odom.tsv`) are already prepared. + +- `mcap_to_plnr1.py` — converts a go2-station `.mcap` recording into the PLNR1 + binary that Point-LIO reads. Already run; output is `data/go2-185959.bin`. + Usage (human): `uv run --with mcap --with numpy python mcap_to_plnr1.py ` +- `20260529-185959.mcap` — the raw recording (lidar/imu/odom/video/telemetry). +- `20260529-185959.rrd` — the recording converted for Rerun visualization. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/human-debug/mcap_to_plnr1.py b/dimos/mapping/research/go2/autoresearch/lio-1/human-debug/mcap_to_plnr1.py new file mode 100644 index 0000000000..d257ed87d3 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/human-debug/mcap_to_plnr1.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""MCAP (go2-station) -> PLNR1 binary for point_lio_noros. + +Unlike the old extract_to_bin.py (SQLite + SYNTHESIZED ring/time), our MCAP +records the raw rt/utlidar/cloud whose data blob IS the 32-byte +unilidar_ros::Point layout (x@0,y@4,z@8,intensity@16,ring@20,time@24) with the +REAL per-point ring + sweep time — so we write it verbatim. Lidar frame ts + +imu ts = the onboard device stamp (MCAP publish_time). Records are time-sorted. + +PLNR1 (see extract_to_bin.py): + header 16B: b"PLNR1\\0"*pad + lidar: +""" + +import struct +import sys + +sys.path.insert(0, "/Users/p2o5/Desktop/2026.5/dim/go2-station/scripts") +import go2_cdr as cdr +from mcap.reader import make_reader + +MAGIC = b"PLNR1\0\0\0\0\0\0\0\0\0\0\0" # 16 B +inp, out = sys.argv[1], sys.argv[2] + +recs = [] # (ts_ns, payload_bytes) +nl = ni = 0 +with open(inp, "rb") as f: + for _sch, ch, msg in make_reader(f).iter_messages( + topics=["rt/utlidar/cloud", "rt/utlidar/imu"] + ): + ts = msg.publish_time # onboard stamp (ns) + if ch.topic == "rt/utlidar/imu": + m = cdr.decode_imu(msg.data) + g, a = m["ang_vel"], m["lin_acc"] + payload = struct.pack(" {out}") diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/.gitignore b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/.gitignore new file mode 100755 index 0000000000..a2a524a83c --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/.gitignore @@ -0,0 +1,2 @@ +.vscode +./PCD/scans.pcd diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt new file mode 100755 index 0000000000..5ca9706c69 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.10) +project(point_lio_noros LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") + +# MP_PROC_NUM controls Point-LIO's internal OpenMP fanout; the original +# CMakeLists scaled it to N-2 cores. Match that here. +include(ProcessorCount) +ProcessorCount(N) +if(N GREATER 5) + add_definitions(-DMP_EN -DMP_PROC_NUM=4) +elseif(N GREATER 3) + math(EXPR PROC_NUM "${N} - 2") + add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM}) +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +find_package(OpenMP QUIET) +if(OpenMP_CXX_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +find_package(Eigen3 REQUIRED) + +# PCL's CMake config drags in all of VTK, including VTK::mpi which +# requires MPI::MPI_C. Skip find_package(PCL) entirely and link only the +# components we use directly. PCL headers themselves are header-only for +# our purposes (point types + voxel filter inlines); the .so's for +# common/io/filters/kdtree provide the heavy bits. +find_path(PCL_INCLUDE_DIR + NAMES pcl/point_cloud.h + HINTS /usr/include/pcl-1.15 /usr/include/pcl-1.14 /usr/include/pcl-1.12 /usr/include/pcl +) +if(NOT PCL_INCLUDE_DIR) + message(FATAL_ERROR "pcl headers not found; try `apt install libpcl-dev`") +endif() +find_library(PCL_COMMON_LIB NAMES pcl_common REQUIRED) +find_library(PCL_IO_LIB NAMES pcl_io REQUIRED) +find_library(PCL_FILTERS_LIB NAMES pcl_filters REQUIRED) +find_library(PCL_KDTREE_LIB NAMES pcl_kdtree REQUIRED) + +# Boost.System is header-only in modern Boost (≥1.69) and ships no compiled +# component/CMake config in nixpkgs, so request only filesystem. Point-LIO's +# own sources use no Boost directly; this is here for PCL's header needs. +find_package(Boost REQUIRED COMPONENTS filesystem) + +find_package(yaml-cpp REQUIRED) + +# zstd: the vendored MCAP C++ reader (include/mcap/) decompresses our zstd +# chunks. lz4 is disabled via MCAP_COMPRESSION_NO_LZ4 so we need only zstd. +find_path(ZSTD_INCLUDE_DIR NAMES zstd.h REQUIRED) +find_library(ZSTD_LIB NAMES zstd REQUIRED) + +# noros shim headers (include/ros/ros.h etc.) live in include/ and +# shadow real ROS headers. Put include/ first so they win even if a +# system ROS is installed. +include_directories(BEFORE include) +include_directories(${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIR} ${ZSTD_INCLUDE_DIR}) + +add_executable(pointlio_mapping + src/laserMapping.cpp + src/mcap_source.cpp + include/ikd-Tree/ikd_Tree.cpp + src/parameters.cpp + src/preprocess.cpp + src/Estimator.cpp +) +target_compile_definitions(pointlio_mapping PRIVATE MCAP_COMPRESSION_NO_LZ4) +target_link_libraries(pointlio_mapping + ${PCL_COMMON_LIB} ${PCL_IO_LIB} ${PCL_FILTERS_LIB} ${PCL_KDTREE_LIB} + Boost::filesystem + yaml-cpp + ${ZSTD_LIB} +) +if(OpenMP_CXX_FOUND) + target_link_libraries(pointlio_mapping OpenMP::OpenMP_CXX) +endif() diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/LICENSE b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/LICENSE new file mode 100644 index 0000000000..d159169d10 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/Log/.gitkeep b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/Log/.gitkeep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/PCD/do_not_delete_this_file.txt b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/PCD/do_not_delete_this_file.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/config/v2_imu.yaml b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/config/v2_imu.yaml new file mode 100644 index 0000000000..e68e61f882 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/config/v2_imu.yaml @@ -0,0 +1,52 @@ +###### v2_imu — IMU-coupled, corrected for this Go2-L1: measured acc_norm 10.434, +###### imu_time_inte 0.004 (~223-250 Hz), and extrinsic_R = Rx(pi)=diag(1,-1,-1) for the +###### IMU's 180-deg roll vs the lidar (doc 20260529-06: imu2lidar = [0,0,0,pi,0,0]). +###### extrinsic_T ~ 0 (L1 IMU is co-located inside the lidar). Tight lidar cov + match_s 81. +common: + lid_topic: "/unilidar/cloud" + imu_topic: "/unilidar/imu" + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 +preprocess: + lidar_type: 5 + scan_line: 18 + timestamp_unit: 0 + blind: 0.5 +mapping: + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.004 + satu_acc: 30.0 + satu_gyro: 35 + acc_norm: 10.434 + lidar_meas_cov: 0.01 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.1 + imu_meas_omg_cov: 0.1 + gyr_cov_input: 0.01 + acc_cov_input: 0.1 + plane_thr: 0.1 + match_s: 81 + fov_degree: 180 + det_range: 100.0 + gravity_align: true + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [ 0, 0, 0 ] + extrinsic_R: [ 1, 0, 0, 0, -1, 0, 0, 0, -1 ] +odometry: + publish_odometry_without_downsample: enable +publish: + path_en: true + scan_publish_en: true + scan_bodyframe_pub_en: false +pcd_save: + pcd_save_en: true + interval: -1 diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/flake.lock b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/flake.lock new file mode 100644 index 0000000000..a3058241a4 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/flake.lock @@ -0,0 +1,61 @@ +{ + "nodes": { + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1778869304, + "narHash": "sha256-30sZNZoA1cqF5JNO9fVX+wgiQYjB7HJqqJ4ztCDeBZE=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "d233902339c02a9c334e7e593de68855ad26c4cb", + "type": "github" + }, + "original": { + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "d233902339c02a9c334e7e593de68855ad26c4cb", + "type": "github" + } + }, + "root": { + "inputs": { + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/flake.nix b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/flake.nix new file mode 100644 index 0000000000..8205e010c8 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/flake.nix @@ -0,0 +1,48 @@ +{ + description = "Point-LIO C++ build toolchain for the LIO autoresearch experiment (cmake, eigen, pcl, yaml-cpp, boost, zstd)."; + + # STANDALONE on purpose. This flake does NOT reference the root dimos flake. + # The dimos worktree is ~100 G and usually dirty, and a flake that lives inside + # that repo (or takes it as an input) makes `nix develop` copy the whole tree + # into the store. By keeping this independent — only nixpkgs as an input — and + # invoking it with `path:` (see ../setup.sh), `nix develop` copies just this + # point_lio directory, not the repo. A bare `nix develop` (no `path:`) would + # still trigger the repo copy, so always use the `path:` form. + # + # Scope is the C++ build only. Python (numpy, matplotlib, the dimos package for + # get_data) comes from the dimos .venv, which runs fine without any nix env — + # so after building, you "drop into" the dimos venv to run evaluate.py/algo.py. + inputs = { + # Pinned to the same rev as the root dimos flake, so pcl/boost/etc. resolve + # to store paths the dimos dev shell already realized (no extra downloads). + nixpkgs.url = "github:NixOS/nixpkgs/d233902339c02a9c334e7e593de68855ad26c4cb"; + flake-utils.url = "github:numtide/flake-utils"; + }; + + outputs = { self, nixpkgs, flake-utils, ... }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = import nixpkgs { inherit system; }; + in { + devShells.default = pkgs.mkShell { + nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; + buildInputs = [ + pkgs.eigen + pkgs.pcl + pkgs.yaml-cpp + pkgs.boost + pkgs.zstd + ]; + # nixpkgs lays PCL headers under include/pcl-/, which + # point_lio's hand-rolled find_path (HINTS only /usr/include/pcl*) + # can't see. Put that dir on CMAKE_INCLUDE_PATH, which find_path + # honors — keeps the vendored substrate untouched. Other deps resolve + # via the cmake setup hook (they're buildInputs). + shellHook = '' + for d in ${pkgs.pcl}/include/pcl-*; do + export CMAKE_INCLUDE_PATH="$d''${CMAKE_INCLUDE_PATH:+:$CMAKE_INCLUDE_PATH}" + done + ''; + }; + }); +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.cpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.cpp new file mode 100755 index 0000000000..8b734dbfc3 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.cpp @@ -0,0 +1,471 @@ +#include "FOV_Checker.h" + +FOV_Checker::FOV_Checker(){ + // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","w"); + // fprintf(fp,"cur_pose_x,cur_pose_y,cur_pose_z,axis_x,axis_y,axis_z,theta,depth\n"); + // fclose(fp); +} + +FOV_Checker::~FOV_Checker(){ + +} + +void FOV_Checker::Set_Env(BoxPointType env_param){ + env = env_param; +} + +void FOV_Checker::Set_BoxLength(double box_len_param){ + box_length = box_len_param; +} + +void round_v3d(Eigen::Vector3d &vec, int decimal){ + double tmp; + int t; + for (int i = 0; i < 3; i++){ + t = pow(10,decimal); + tmp = round(vec(i)*t); + vec(i) = tmp/t; + } + return; +} + +void FOV_Checker::check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes){ + round_v3d(cur_pose,4); + round_v3d(axis,3); + axis = axis/axis.norm(); + // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","a"); + // fprintf(fp,"%f,%f,%f,%f,%f,%f,%0.4f,%0.1f,",cur_pose(0),cur_pose(1),cur_pose(2),axis(0),axis(1),axis(2),theta,depth); + // fclose(fp); + // cout << "cur_pose: " << cur_pose.transpose() << endl; + // cout<< "axis: " << axis.transpose() << endl; + // cout<< "theta: " << theta << " depth: " << depth << endl; + // cout<< "env: " << env.vertex_min[0] << " " << env.vertex_max[0] << endl; + double axis_angle[6], min_angle, gap, plane_u_min, plane_u_max; + Eigen::Vector3d plane_w, plane_u, plane_v, center_point, start_point, box_p; + Eigen::Vector3d box_p_min, box_p_max; + int i, j, k, index, maxn, start_i, max_uN, max_vN, max_ulogN, u_min, u_max; + bool flag = false, box_found = false; + boxes.clear(); + BoxPointType box; + axis_angle[0] = acos(axis(0)); + axis_angle[1] = acos(axis(1)); + axis_angle[2] = acos(axis(2)); + axis_angle[3] = acos(-axis(0)); + axis_angle[4] = acos(-axis(1)); + axis_angle[5] = acos(-axis(2)); + index = 1; + min_angle = axis_angle[0]; + for (i=1;i<6;i++){ + if (axis_angle[i]=0){ + box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length; + box.vertex_min[0] = box_p_min(0); + box.vertex_min[1] = box_p_min(1); + box.vertex_min[2] = box_p_min(2); + box.vertex_max[0] = box_p(0); + box.vertex_max[1] = box_p(1); + box.vertex_max[2] = box_p(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k); + k = k-1; + } + k = max_ulogN; + u_max = 0; + while (k>=0){ + box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p_max(0); + box.vertex_max[1] = box_p_max(1); + box.vertex_max[2] = box_p_max(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_max = u_max + pow(2,k); + + k = k-1; + } + u_max = max(0, max_uN - u_max - 1); + box_found = false; + //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max); + for (k = u_min; k <= u_max; k++){ + box_p = box_p_min + plane_u * box_length * k; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p(0) + box_length; + box.vertex_max[1] = box_p(1) + box_length; + box.vertex_max[2] = box_p(2) + box_length; + if (check_box_in_env(box)){ + //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + box_found = true; + boxes.push_back(box); + } + } + if (box_found) { + flag = true; + } else { + if (j>1) break; + } + } + for (j = 1; j <= max_vN; j++){ + k = max_ulogN; + u_min = 0; + box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min - plane_v * box_length * j; + box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) - plane_v * box_length * (j-1) + plane_w * box_length; + //printf("---- DOWNSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]); + + while (k>=0){ + box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length; + box.vertex_min[0] = box_p_min(0); + box.vertex_min[1] = box_p_min(1); + box.vertex_min[2] = box_p_min(2); + box.vertex_max[0] = box_p(0); + box.vertex_max[1] = box_p(1); + box.vertex_max[2] = box_p(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k); + k = k-1; + } + k = max_ulogN; + u_max = 0; + while (k>=0){ + box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p_max(0); + box.vertex_max[1] = box_p_max(1); + box.vertex_max[2] = box_p_max(2); + if (!check_box(cur_pose, axis, theta, depth, box)) { + u_max = u_max + pow(2,k); + // printf("-------- Not Included: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + } + + k = k-1; + } + u_max = max(0, max_uN - u_max - 1); + //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max); + box_found = 0; + for (k = u_min; k <= u_max; k++){ + box_p = box_p_min + plane_u * box_length * k; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p(0) + box_length; + box.vertex_max[1] = box_p(1) + box_length; + box.vertex_max[2] = box_p(2) + box_length; + if (check_box_in_env(box)){ + //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + box_found = 1; + boxes.push_back(box); + } + } + if (box_found) { + flag = true; + } else { + if (j>1) break; + } + } + if (!flag && i>0) break; + } +} + +bool FOV_Checker::check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box){ + Eigen::Vector3d vertex[8]; + bool s; + vertex[0] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_min[2]); + vertex[1] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_max[2]); + vertex[2] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_min[2]); + vertex[3] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_max[2]); + vertex[4] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_min[2]); + vertex[5] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_max[2]); + vertex[6] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_min[2]); + vertex[7] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_max[2]); + for (int i = 0; i < 8; i++){ + if (check_point(cur_pose, axis, theta, depth, vertex[i])){ + return true; + } + } + Eigen::Vector3d center_point = (vertex[7]+vertex[0])/2.0; + if (check_point(cur_pose, axis, theta, depth, center_point)){ + return true; + } + PlaneType plane[6]; + plane[0].p[0] = vertex[0]; + plane[0].p[1] = vertex[2]; + plane[0].p[2] = vertex[1]; + plane[0].p[3] = vertex[3]; + + plane[1].p[0] = vertex[0]; + plane[1].p[1] = vertex[4]; + plane[1].p[2] = vertex[2]; + plane[1].p[3] = vertex[6]; + + plane[2].p[0] = vertex[0]; + plane[2].p[1] = vertex[4]; + plane[2].p[2] = vertex[1]; + plane[2].p[3] = vertex[5]; + + plane[3].p[0] = vertex[4]; + plane[3].p[1] = vertex[6]; + plane[3].p[2] = vertex[5]; + plane[3].p[3] = vertex[7]; + + plane[4].p[0] = vertex[2]; + plane[4].p[1] = vertex[6]; + plane[4].p[2] = vertex[3]; + plane[4].p[3] = vertex[7]; + + plane[5].p[0] = vertex[1]; + plane[5].p[1] = vertex[5]; + plane[5].p[2] = vertex[3]; + plane[5].p[3] = vertex[7]; + if (check_surface(cur_pose, axis, theta, depth, plane[0]) || check_surface(cur_pose, axis, theta, depth, plane[1]) || check_surface(cur_pose, axis, theta, depth, plane[2]) || check_surface(cur_pose, axis, theta, depth, plane[3]) || check_surface(cur_pose, axis, theta, depth, plane[4]) || check_surface(cur_pose, axis, theta, depth, plane[5])) + s = 1; + else + s = 0; + return s; +} + +bool FOV_Checker::check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane){ + Eigen::Vector3d plane_p, plane_u, plane_v, plane_w, pc, p, vec; + bool s; + double t, vec_dot_u, vec_dot_v; + plane_p = plane.p[0]; + plane_u = plane.p[1] - plane_p; + plane_v = plane.p[2] - plane_p; + if (check_line(cur_pose, axis, theta, depth, plane_p, plane_u) || check_line(cur_pose, axis, theta, depth, plane_p, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_u, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_v, plane_u)){ + s = 1; + return s; + } + pc = plane_p + (plane.p[3]-plane.p[0])/2; + if (check_point(cur_pose, axis, theta, depth, pc)){ + s = 1; + return s; + } + plane_w = plane_u.cross(plane_v); + p = plane_p - cur_pose; + t = (p.dot(plane_w))/(axis.dot(plane_w)); + vec = cur_pose + t * axis - plane_p; + vec_dot_u = vec.dot(plane_u)/plane_u.norm(); + vec_dot_v = vec.dot(plane_v)/plane_v.norm(); + if (t>=-eps_value && t<=depth && vec_dot_u>=-eps_value && vec_dot_u<=plane_u.norm() && vec_dot_v>=-eps_value && vec_dot_v <= plane_v.norm()) + s = 1; + else + s = 0; + return s; +} + +bool FOV_Checker::check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec){ + Eigen::Vector3d p, vec_1, vec_2; + double xl, yl, zl, xn, yn, zn, dot_1, dot_2, ln, pn, pl, l2, p2; + double A, B, C, delta, t1, t2; + bool s; + p = line_p - cur_pose; + xl = line_vec(0); yl = line_vec(1); zl = line_vec(2); + xn = axis(0); yn = axis(1); zn = axis(2); + vec_1 = line_p - cur_pose; + vec_2 = line_p + line_vec - cur_pose; + dot_1 = vec_1.dot(axis); + dot_2 = vec_2.dot(axis); + //printf("xl yl zl: %0.4f, %0.4f, %0.4f\n", xl, yl, zl); + //printf("xn yn zn: %0.4f, %0.4f, %0.4f\n", xn, yn, zn); + //printf("dot_1, dot_2, %0.4f, %0.4f\n",dot_1, dot_2); + if ((dot_1<0 && dot_2<0) || (dot_1>depth && dot_2>depth)){ + s = false; + return s; + } + ln = xl*xn+yl*yn+zl*zn; + pn = p(0)*xn+p(1)*yn+p(2)*zn; + pl = p(0)*xl+p(1)*yl+p(2)*zl; + l2 = xl*xl+yl*yl+zl*zl; + p2 = p.norm()*p.norm(); + //printf("ln: %0.4f\n",ln); + //printf("pn:%0.4f\n",pn); + //printf("pl:%0.4f\n",pl); + //printf("l2:%0.4f\n",l2); + //printf("p2:%0.4f\n",p2); + //printf("theta, cos(theta):%0.4f %0.4f\n",theta,cos(theta)); + A = ln * ln - l2 * cos(theta) * cos(theta); + B = 2 * pn * ln - 2 * cos(theta) * cos(theta)*pl; + C = pn * pn - p2 * cos(theta) * cos(theta); + //printf("A:%0.4f, B:%0.4f, C:%0.4f\n", A,B,C); + if (!(fabs(A)<=eps_value)){ + delta = B*B - 4*A*C; + //printf("delta: %0.4f\n",delta); + if (delta <= eps_value){ + if (A < -eps_value){ + s = false; + return s; + } + else{ + s = true; + return s; + } + } else { + double sqrt_delta = sqrt(delta); + t1 = (-B - sqrt_delta)/(2*A); + t2 = (-B + sqrt_delta)/(2*A); + if (t1>t2) swap(t1,t2); + //printf("t1,t2: %0.4f,%0.4f\n",t1,t2); + // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)); + if ((t1>=-eps_value && t1<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)){ + s = true; + return s; + } + // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)); + if ((t2>=-eps_value && t2<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)){ + s = true; + return s; + } + if (A>-eps_value && (t21-eps_value)){ + s = true; + return s; + } + if (A1-eps_value){ + s = true; + return s; + } + s = false; + } + } else{ + if (!(fabs(B)<=eps_value)){ + s = (B>-eps_value && -C/B<=1+eps_value) || (B=-eps_value); + return s; + } + else { + s = C>=-eps_value; + return s; + } + } + return false; +} + +bool FOV_Checker::check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point){ + Eigen::Vector3d vec; + double proj_len; + bool s; + vec = point-cur_pose; + if (vec.transpose()*vec < 0.4 * box_length * box_length){ + return true; + } + proj_len = vec.dot(axis); + if (proj_len > depth){ + s = false; + return s; + } + //printf("acos: %0.4f\n",acos(proj_len/vec.norm())); + if (fabs(vec.norm()) <= 1e-4 || acos(proj_len/vec.norm()) <= theta + 0.0175) + s = true; + else + s = false; + return s; +} + +bool FOV_Checker::check_box_in_env(BoxPointType box){ + if (box.vertex_min[0] >= env.vertex_min[0]-eps_value && box.vertex_min[1] >= env.vertex_min[1]-eps_value && box.vertex_min[2] >= env.vertex_min[2]-eps_value && box.vertex_max[0]<= env.vertex_max[0]+eps_value && box.vertex_max[1]<= env.vertex_max[1]+eps_value && box.vertex_max[2]<= env.vertex_max[2]+eps_value){ + return true; + } else { + return false; + } +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.h new file mode 100755 index 0000000000..dc3b4fa8ea --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.h @@ -0,0 +1,32 @@ +// Include Files +#pragma once +#include +#include +#include "ikd-Tree/ikd_Tree.h" +#include +#include + +#define eps_value 1e-6 + +struct PlaneType{ + Eigen::Vector3d p[4]; +}; + +class FOV_Checker{ +public: + FOV_Checker(); + ~FOV_Checker(); + void Set_Env(BoxPointType env_param); + void Set_BoxLength(double box_len_param); + void check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes); + bool check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box); + bool check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec); + bool check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane); + bool check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point); + bool check_box_in_env(BoxPointType box); +private: + BoxPointType env; + double box_length; + FILE *fp; + +}; diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/.gitignore b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/.gitignore new file mode 100644 index 0000000000..259148fa18 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/.gitignore @@ -0,0 +1,32 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp new file mode 100755 index 0000000000..5e84c20b79 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp @@ -0,0 +1,390 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ESEKFOM_EKF_HPP +#define ESEKFOM_EKF_HPP + + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../mtk/types/vect.hpp" +#include "../mtk/types/SOn.hpp" +#include "../mtk/types/S2.hpp" +#include "../mtk/types/SEn.hpp" +#include "../mtk/startIdx.hpp" +#include "../mtk/build_manifold.hpp" +#include "util.hpp" + +namespace esekfom { + +using namespace Eigen; + +template +struct dyn_share_modified +{ + bool valid; + bool converge; + T M_Noise; + Eigen::Matrix z; + Eigen::Matrix h_x; + Eigen::Matrix z_IMU; + Eigen::Matrix R_IMU; + bool satu_check[6]; +}; + +template +class esekf{ + + typedef esekf self; + enum{ + n = state::DOF, m = state::DIM, l = measurement::DOF + }; + +public: + + typedef typename state::scalar scalar_type; + typedef Matrix cov; + typedef Matrix cov_; + typedef SparseMatrix spMt; + typedef Matrix vectorized_state; + typedef Matrix flatted_state; + typedef flatted_state processModel(state &, const input &); + typedef Eigen::Matrix processMatrix1(state &, const input &); + typedef Eigen::Matrix processMatrix2(state &, const input &); + typedef Eigen::Matrix processnoisecovariance; + + typedef void measurementModel_dyn_share_modified(state &, dyn_share_modified &); + typedef Eigen::Matrix measurementMatrix1(state &); + typedef Eigen::Matrix measurementMatrix1_dyn(state &); + typedef Eigen::Matrix measurementMatrix2(state &); + typedef Eigen::Matrix measurementMatrix2_dyn(state &); + typedef Eigen::Matrix measurementnoisecovariance; + typedef Eigen::Matrix measurementnoisecovariance_dyn; + + esekf(const state &x = state(), + const cov &P = cov::Identity()): x_(x), P_(P){}; + + void init_dyn_share_modified(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in) + { + f = f_in; + f_x = f_x_in; + // f_w = f_w_in; + h_dyn_share_modified_1 = h_dyn_share_in; + maximum_iter = 1; + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + + void init_dyn_share_modified_2h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in1, measurementModel_dyn_share_modified h_dyn_share_in2) + { + f = f_in; + f_x = f_x_in; + // f_w = f_w_in; + h_dyn_share_modified_1 = h_dyn_share_in1; + h_dyn_share_modified_2 = h_dyn_share_in2; + maximum_iter = 1; + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + + // iterated error state EKF propogation + void predict(double &dt, processnoisecovariance &Q, const input &i_in, bool predict_state, bool prop_cov){ + if (predict_state) + { + flatted_state f_ = f(x_, i_in); + x_.oplus(f_, dt); + } + + if (prop_cov) + { + flatted_state f_ = f(x_, i_in); + // state x_before = x_; + + cov_ f_x_ = f_x(x_, i_in); + cov f_x_final; + F_x1 = cov::Identity(); + for (std::vector, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) { + int idx = (*it).first.first; + int dim = (*it).first.second; + int dof = (*it).second; + for(int i = 0; i < n; i++){ + for(int j=0; j res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = -1 * f_(dim + i) * dt; + } + MTK::SO3 res; + res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1/2)); + F_x1.template block<3, 3>(idx, idx) = res.normalized().toRotationMatrix(); + res_temp_SO3 = MTK::A_matrix(seg_SO3); + for(int i = 0; i < n; i++){ + f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i)); + } + } + + F_x1 += f_x_final * dt; + P_ = F_x1 * P_ * (F_x1).transpose() + Q * (dt * dt); + } + } + + bool update_iterated_dyn_share_modified() { + dyn_share_modified dyn_share; + state x_propagated = x_; + int dof_Measurement; + double m_noise; + for(int i=0; i z = dyn_share.z; + // Matrix R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + // Matrix h_v = dyn_share.h_v; + dof_Measurement = h_x.rows(); + m_noise = dyn_share.M_Noise; + // dof_Measurement_noise = dyn_share.R.rows(); + // vectorized_state dx, dx_new; + // x_.boxminus(dx, x_propagated); + // dx_new = dx; + // P_ = P_propagated; + + Matrix PHT; + Matrix HPHT; + Matrix K_; + // if(n > dof_Measurement) + { + PHT = P_. template block(0, 0) * h_x.transpose(); + HPHT = h_x * PHT.topRows(12); + for (int m = 0; m < dof_Measurement; m++) + { + HPHT(m, m) += m_noise; + } + K_= PHT*HPHT.inverse(); + } + Matrix dx_ = K_ * z; // - h) + (K_x - Matrix::Identity()) * dx_new; + // state x_before = x_; + + x_.boxplus(dx_); + dyn_share.converge = true; + + // L_ = P_; + // Matrix res_temp_SO3; + // MTK::vect<3, scalar_type> seg_SO3; + // for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + // int idx = (*it).first; + // for(int i = 0; i < 3; i++){ + // seg_SO3(i) = dx_(i + idx); + // } + // res_temp_SO3 = A_matrix(seg_SO3).transpose(); + // for(int i = 0; i < n; i++){ + // L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + // } + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + // } + // } + // for(int i = 0; i < n; i++){ + // L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // } + // for(int i = 0; i < n; i++){ + // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // } + // } + // Matrix res_temp_S2; + // MTK::vect<2, scalar_type> seg_S2; + // for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + // int idx = (*it).first; + + // for(int i = 0; i < 2; i++){ + // seg_S2(i) = dx_(i + idx); + // } + + // Eigen::Matrix Nx; + // Eigen::Matrix Mx; + // x_.S2_Nx_yy(Nx, idx); + // x_propagated.S2_Mx(Mx, seg_S2, idx); + // res_temp_S2 = Nx * Mx; + + // for(int i = 0; i < n; i++){ + // L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + // } + + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + // } + // } + // for(int i = 0; i < n; i++){ + // L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + // } + // for(int i = 0; i < n; i++){ + // P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + // } + // } + // if(n > dof_Measurement) + { + P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0); + } + } + return true; + } + + void update_iterated_dyn_share_IMU() { + + dyn_share_modified dyn_share; + for(int i=0; i z = dyn_share.z_IMU; + + Matrix PHT; + Matrix HP; + Matrix HPHT; + PHT.setZero(); + HP.setZero(); + HPHT.setZero(); + for (int l_ = 0; l_ < 6; l_++) + { + if (!dyn_share.satu_check[l_]) + { + PHT.col(l_) = P_.col(15+l_) + P_.col(24+l_); + HP.row(l_) = P_.row(15+l_) + P_.row(24+l_); + } + } + for (int l_ = 0; l_ < 6; l_++) + { + if (!dyn_share.satu_check[l_]) + { + HPHT.col(l_) = HP.col(15+l_) + HP.col(24+l_); + } + HPHT(l_, l_) += dyn_share.R_IMU(l_); //, l); + } + Eigen::Matrix K = PHT * HPHT.inverse(); + + Matrix dx_ = K * z; + + P_ -= K * HP; + x_.boxplus(dx_); + } + return; + } + + void change_x(state &input_state) + { + x_ = input_state; + + if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())&&(!x_.SEN_state.size())) + { + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + } + + void change_P(cov &input_cov) + { + P_ = input_cov; + } + + const state& get_x() const { + return x_; + } + const cov& get_P() const { + return P_; + } + state x_; +private: + measurement m_; + cov P_; + spMt l_; + spMt f_x_1; + spMt f_x_2; + cov F_x1 = cov::Identity(); + cov F_x2 = cov::Identity(); + cov L_ = cov::Identity(); + + processModel *f; + processMatrix1 *f_x; + processMatrix2 *f_w; + + measurementMatrix1 *h_x; + measurementMatrix2 *h_v; + + measurementMatrix1_dyn *h_x_dyn; + measurementMatrix2_dyn *h_v_dyn; + + measurementModel_dyn_share_modified *h_dyn_share_modified_1; + + measurementModel_dyn_share_modified *h_dyn_share_modified_2; + + int maximum_iter = 0; + scalar_type limit[n]; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace esekfom + +#endif // ESEKFOM_EKF_HPP diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp new file mode 100755 index 0000000000..de6e0b7bf7 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __MEKFOM_UTIL_HPP__ +#define __MEKFOM_UTIL_HPP__ + +#include +#include "../mtk/src/mtkmath.hpp" +namespace esekfom { + +template +class is_same { +public: + operator bool() { + return false; + } +}; +template +class is_same { +public: + operator bool() { + return true; + } +}; + +template +class is_double { +public: + operator bool() { + return false; + } +}; + +template<> +class is_double { +public: + operator bool() { + return true; + } +}; + +template +static T +id(const T &x) +{ + return x; +} + +} // namespace esekfom + +#endif // __MEKFOM_UTIL_HPP__ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp new file mode 100755 index 0000000000..f5a4869067 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp @@ -0,0 +1,248 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/build_manifold.hpp + * @brief Macro to automatically construct compound manifolds. + * + */ +#ifndef MTK_AUTOCONSTRUCT_HPP_ +#define MTK_AUTOCONSTRUCT_HPP_ + +#include + +#include +#include +#include + +#include "src/SubManifold.hpp" +#include "startIdx.hpp" + +#ifndef PARSED_BY_DOXYGEN +//////// internals ////// + +#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple + +#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) + +#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) + +#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() +#define MTK_CONSTRUCTOR_COPY( type, id) id(id) +#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); +#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); +#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); +#define MTK_HAT( type, id) if(id.IDX == idx){id.hat(vec, res);} +#define MTK_JACOB_RIGHT_INV( type, id) if(id.IDX == idx){id.Jacob_right_inv(vec, res);} +#define MTK_JACOB_RIGHT( type, id) if(id.IDX == idx){id.Jacob_right(vec, res);} +#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} +#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} +#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} +#define MTK_OSTREAM( type, id) << __var.id << " " +#define MTK_ISTREAM( type, id) >> __var.id +#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} +#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} +#define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} +#define MTK_SEN_state( type, id) if(id.TYP == 4){(SEN_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} + +#define MTK_SUBVARLIST(seq, S2state, SO3state, SENstate) \ +BOOST_PP_FOR_1( \ + ( \ + BOOST_PP_SEQ_SIZE(seq), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq) (~), \ + 0,\ + 0,\ + S2state,\ + SO3state,\ + SENstate ),\ + MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) + +#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \ + MTK::SubManifold id; +#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \ + MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \ + enum {DOF = type::DOF + dof}; \ + enum {DIM = type::DIM+dim}; \ + typedef type::scalar scalar; + +#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state +#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate) \ + MTK_APPLY_MACRO_ON_TUPLE(~, \ + BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ + ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)) + +#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state + +//! this used to be BOOST_PP_TUPLE_ELEM_4_0: +#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g, h) a + +#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state +#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \ + BOOST_PP_DEC(len), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq), \ + dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ + dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ + S2state,\ + SO3state,\ + SENstate ) + +#endif /* not PARSED_BY_DOXYGEN */ + + +/** + * Construct a manifold. + * @param name is the class-name of the manifold, + * @param entries is the list of sub manifolds + * + * Entries must be given in a list like this: + * @code + * typedef MTK::trafo > Pose; + * typedef MTK::vect Vec3; + * MTK_BUILD_MANIFOLD(imu_state, + * ((Pose, pose)) + * ((Vec3, vel)) + * ((Vec3, acc_bias)) + * ) + * @endcode + * Whitespace is optional, but the double parentheses are necessary. + * Construction is done entirely in preprocessor. + * After construction @a name is also a manifold. Its members can be + * accessed by names given in @a entries. + * + * @note Variable types are not allowed to have commas, thus types like + * @c vect need to be typedef'ed ahead. + */ +#define MTK_BUILD_MANIFOLD(name, entries) \ +struct name { \ + typedef name self; \ + std::vector > S2_state;\ + std::vector > SO3_state;\ + std::vector, int> > vect_state;\ + std::vector, int> > SEN_state;\ + MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_state) \ + name ( \ + MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ + ) : \ + MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ + int getDOF() const { return DOF; } \ + void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_BOXPLUS, entries)\ + } \ + void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_OPLUS, entries)\ + } \ + void boxminus(MTK::vectview __res, const name& __oth) const { \ + MTK_TRANSFORM(MTK_BOXMINUS, entries)\ + } \ + friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ + return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ + } \ + void build_S2_state(){\ + MTK_TRANSFORM(MTK_S2_state, entries)\ + }\ + void build_vect_state(){\ + MTK_TRANSFORM(MTK_vect_state, entries)\ + }\ + void build_SO3_state(){\ + MTK_TRANSFORM(MTK_SO3_state, entries)\ + }\ + void build_SEN_state(){\ + MTK_TRANSFORM(MTK_SEN_state, entries)\ + }\ + void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\ + MTK_TRANSFORM(MTK_HAT, entries)\ + }\ + void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\ + MTK_TRANSFORM(MTK_JACOB_RIGHT_INV, entries)\ + }\ + void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\ + MTK_TRANSFORM(MTK_JACOB_RIGHT, entries)\ + }\ + void S2_hat(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_hat, entries)\ + }\ + void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ + }\ + void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ + MTK_TRANSFORM(MTK_S2_Mx, entries)\ + }\ + friend std::istream& operator>>(std::istream& __is, name& __var){ \ + return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ + } \ +}; + + + +#endif /*MTK_AUTOCONSTRUCT_HPP_*/ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp new file mode 100755 index 0000000000..284e2263b8 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp @@ -0,0 +1,123 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/SubManifold.hpp + * @brief Defines the SubManifold class + */ + + +#ifndef SUBMANIFOLD_HPP_ +#define SUBMANIFOLD_HPP_ + + +#include "vectview.hpp" + + +namespace MTK { + +/** + * @ingroup SubManifolds + * Helper class for compound manifolds. + * This class wraps a manifold T and provides an enum IDX refering to the + * index of the SubManifold within the compound manifold. + * + * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". + * + * @tparam T The manifold type of the sub-type + * @tparam idx The index of the sub-type within the compound manifold + */ +template +struct SubManifold : public T +{ + enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; + //! manifold type + typedef T type; + + //! Construct from derived type + template + explicit + SubManifold(const X& t) : T(t) {}; + + //! Construct from internal type + //explicit + SubManifold(const T& t) : T(t) {}; + + //! inherit assignment operator + using T::operator=; + +}; + +} // namespace MTK + + +#endif /* SUBMANIFOLD_HPP_ */ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp new file mode 100755 index 0000000000..636aec20b7 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp @@ -0,0 +1,294 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/mtkmath.hpp + * @brief several math utility functions. + */ + +#ifndef MTKMATH_H_ +#define MTKMATH_H_ + +#include + +#include + +#include "../types/vect.hpp" + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + + +namespace MTK { + +namespace internal { + +template +struct traits { + typedef typename Manifold::scalar scalar; + enum {DOF = Manifold::DOF}; + typedef vect vectorized_type; + typedef Eigen::Matrix matrix_type; +}; + +template<> +struct traits : traits > {}; +template<> +struct traits : traits > {}; + +} // namespace internal + +/** + * \defgroup MTKMath Mathematical helper functions + */ +//@{ + +//! constant @f$ \pi @f$ +const double pi = M_PI; + +template inline scalar tolerance(); + +template<> inline float tolerance() { return 1e-5f; } +template<> inline double tolerance() { return 1e-11; } + + +/** + * normalize @a x to @f$[-bound, bound] @f$. + * + * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. + */ +template +inline scalar normalize(scalar x, scalar bound){ //not used + if(std::fabs(x) <= bound) return x; + int r = (int)(x *(scalar(1.0)/ bound)); + return x - ((r + (r>>31) + 1) & ~1)*bound; +} + +/** + * Calculate cosine and sinc of sqrt(x2). + * @param x2 the squared angle must be non-negative + * @return a pair containing cos and sinc of sqrt(x2) + */ +template +std::pair cos_sinc_sqrt(const scalar &x2){ + using std::sqrt; + using std::cos; + using std::sin; + static scalar const taylor_0_bound = boost::math::tools::epsilon(); + static scalar const taylor_2_bound = sqrt(taylor_0_bound); + static scalar const taylor_n_bound = sqrt(taylor_2_bound); + + assert(x2>=0 && "argument must be non-negative and must not be nan/-nan"); + + // FIXME check if bigger bounds are possible + if(x2>=taylor_n_bound) { + // slow fall-back solution + scalar x = sqrt(x2); + return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. + } + + // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) + // TODO Find optimal coefficients using Remez algorithm + static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; + scalar cosi = 1., sinc=1; + scalar term = -1/2. * x2; + for(int i=0; i<3; ++i) { + cosi += term; + term *= inv[2*i]; + sinc += term; + term *= -inv[2*i+1] * x2; + } + + return std::make_pair(cosi, sinc); + +} + +template +Eigen::Matrix hat(const Base& v) { + Eigen::Matrix res; + res << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return res; +} + +template +Eigen::Matrix A_inv_trans(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix A_inv(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) + { + Eigen::Matrix res; + scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Zero(); + res(0, 1) = 1; + res(1, 2) = 1; + res /= length; + } + else{ + res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, + -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); + res /= length; + } + } + +template +Eigen::Matrix A_matrix(const Base & v){ + Eigen::Matrix res; + double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + double norm = std::sqrt(squaredNorm); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); + } + return res; +} + +template +scalar exp(vectview result, vectview vec, const scalar& scale = 1) { + scalar norm2 = vec.squaredNorm(); + std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); + scalar mult = cos_sinc.second * scale; + result = mult * vec; + return cos_sinc.first; +} + + +/** + * Inverse function to @c exp. + * + * @param result @c vectview to the result + * @param w scalar part of input + * @param vec vector part of input + * @param scale scale result by this value + * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result + */ +template +void log(vectview result, + const scalar &w, const vectview vec, + const scalar &scale, bool plus_minus_periodicity) +{ + // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division + scalar nv = vec.norm(); + if(nv < tolerance()) { + if(!plus_minus_periodicity && w < 0) { + // find the maximal entry: + int i; + nv = vec.cwiseAbs().maxCoeff(&i); + result = scale * std::atan2(nv, w) * vect::Unit(i); + return; + } + nv = tolerance(); + } + scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); + + result = s * vec; +} + + +} // namespace MTK + + +#endif /* MTKMATH_H_ */ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp new file mode 100755 index 0000000000..3278f9ff08 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp @@ -0,0 +1,168 @@ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/vectview.hpp + * @brief Wrapper class around a pointer used as interface for plain vectors. + */ + +#ifndef VECTVIEW_HPP_ +#define VECTVIEW_HPP_ + +#include + +namespace MTK { + +/** + * A view to a vector. + * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. + * The dimension of the vector is given as template parameter and type-checked when used in expressions. + * Data has to be modifiable. + * + * @tparam scalar Scalar type of the vector. + * @tparam dim Dimension of the vector. + * + * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct + */ +namespace internal { + template + struct CovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct CrossCovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CrossCovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct VectviewBase { + typedef Eigen::Matrix matrix_type; + typedef typename matrix_type::MapType Type; + typedef typename matrix_type::ConstMapType ConstType; + }; + + template + struct UnalignedType { + typedef T type; + }; +} + +template +class vectview : public internal::VectviewBase::Type { + typedef internal::VectviewBase VectviewBase; +public: + //! plain matrix type + typedef typename VectviewBase::matrix_type matrix_type; + //! base type + typedef typename VectviewBase::Type base; + //! construct from pointer + explicit + vectview(scalar* data, int dim_=dim) : base(data, dim_) {} + //! construct from plain matrix + vectview(matrix_type& m) : base(m.data(), m.size()) {} + //! construct from another @c vectview + vectview(const vectview &v) : base(v) {} + //! construct from Eigen::Block: + template + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} + + //! inherit assignment operator + using base::operator=; + //! data pointer + scalar* data() {return const_cast(base::data());} +}; + +/** + * @c const version of @c vectview. + * Compared to @c Eigen::Map this implementation is const correct, i.e., + * data will not be modifiable using this view. + * + * @tparam scalar Scalar type of the vector. + * @tparam dim Dimension of the vector. + * + * @sa vectview + */ +template +class vectview : public internal::VectviewBase::ConstType { + typedef internal::VectviewBase VectviewBase; +public: + //! plain matrix type + typedef typename VectviewBase::matrix_type matrix_type; + //! base type + typedef typename VectviewBase::ConstType base; + //! construct from const pointer + explicit + vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} + //! construct from column vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from row vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from another @c vectview + vectview(vectview x) : base(x.data()) {} + //! construct from base + vectview(const base &x) : base(x) {} + /** + * Construct from Block + * @todo adapt this, when Block gets const-correct + */ + template + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} + +}; + + +} // namespace MTK + +#endif /* VECTVIEW_HPP_ */ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp new file mode 100755 index 0000000000..59547434a2 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp @@ -0,0 +1,328 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/startIdx.hpp + * @brief Tools to access sub-elements of compound manifolds. + */ +#ifndef GET_START_INDEX_H_ +#define GET_START_INDEX_H_ + +#include + +#include "src/SubManifold.hpp" +#include "src/vectview.hpp" + +namespace MTK { + + +/** + * \defgroup SubManifolds Accessing Submanifolds + * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers + * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. + * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers + * @c &pose::orient and @c &pose::trans give all required information and are still + * valid if the base type gets extended or the actual types of @a orient and @a trans + * change (e.g. from 2D to 3D). + * + * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. + */ +//@{ + +/** + * Determine the index of a sub-variable within a compound variable. + */ +template +int getStartIdx( MTK::SubManifold Base::*) +{ + return idx; +} + +template +int getStartIdx_( MTK::SubManifold Base::*) +{ + return dim; +} + +/** + * Determine the degrees of freedom of a sub-variable within a compound variable. + */ +template +int getDof( MTK::SubManifold Base::*) +{ + return T::DOF; +} +template +int getDim( MTK::SubManifold Base::*) +{ + return T::DIM; +} + +/** + * set the diagonal elements of a covariance matrix corresponding to a sub-variable + */ +template +void setDiagonal(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(idx).setConstant(val); +} + +template +void setDiagonal_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(dim).setConstant(val); +} + +/** + * Get the subblock of corresponding to two members, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; + * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); + * \endcode + * lets you modify mixed covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(dim1, dim2); +} + +template +typename MTK::internal::CrossCovBlock::Type +subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CrossCovBlock_::Type +subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(dim1, dim2); +} +/** + * Get the subblock of corresponding to a member, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient) = some_expression; + * \endcode + * lets you modify covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(dim, dim); +} + +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(idx, idx); +} + +template +class get_cov { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cov_ { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cross_cov { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + +template +class get_cross_cov_ { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + + +template +vectview +subvector_impl_(vectview vec, SubManifold Base::*) +{ + return vec.template segment(dim); +} + +template +vectview +subvector_impl(vectview vec, SubManifold Base::*) +{ + return vec.template segment(idx); +} + +/** + * Get the subvector corresponding to a sub-manifold from a bigger vector. + */ + template +vectview +subvector_(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vec, ptr); +} + +template +vectview +subvector(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl(vec, ptr); +} + +/** + * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + +template +vectview +subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + + +/** + * const version of subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector_impl(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(idx); +} + +template +vectview +subvector_impl_(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(dim); +} + +template +vectview +subvector(const vectview cvec, SubManifold Base::* ptr) +{ + return subvector_impl(cvec, ptr); +} + + +} // namespace MTK + +#endif // GET_START_INDEX_H_ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp new file mode 100755 index 0000000000..e4075db0b6 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp @@ -0,0 +1,326 @@ +// This is a NEW implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/S2.hpp + * @brief Unit vectors on the sphere, or directions in 3D. + */ +#ifndef S2_H_ +#define S2_H_ + + +#include "vect.hpp" + +#include "SOn.hpp" +#include "../src/mtkmath.hpp" + + + + +namespace MTK { + +/** + * Manifold representation of @f$ S^2 @f$. + * Used for unit vectors on the sphere or directions in 3D. + * + * @todo add conversions from/to polar angles? + */ +template +struct S2 { + + typedef _scalar scalar; + typedef vect<3, scalar> vect_type; + typedef SO3 SO3_type; + typedef typename vect_type::base vec3; + scalar length = scalar(den)/scalar(num); + enum {DOF=2, TYP = 1, DIM = 3}; + +//private: + /** + * Unit vector on the sphere, or vector pointing in a direction + */ + vect_type vec; + +public: + S2() { + if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); + if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); + if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); + } + S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { + vec.normalize(); + vec = vec * length; + } + + S2(const vect_type &_vec) : vec(_vec) { + vec.normalize(); + vec = vec * length; + } + + void oplus(MTK::vectview delta, scalar scale = 1) + { + SO3_type res; + res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); + vec = res.normalized().toRotationMatrix() * vec; + } + + void boxplus(MTK::vectview delta, scalar scale=1) { + Eigen::Matrix Bx; + S2_Bx(Bx); + vect_type Bu = Bx*delta;SO3_type res; + res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); + vec = res.normalized().toRotationMatrix() * vec; + } + + void boxminus(MTK::vectview res, const S2& other) const { + scalar v_sin = (MTK::hat(vec)*other.vec).norm(); + scalar v_cos = vec.transpose() * other.vec; + scalar theta = std::atan2(v_sin, v_cos); + if(v_sin < MTK::tolerance()) + { + if(std::fabs(theta) > MTK::tolerance() ) + { + res[0] = 3.1415926; + res[1] = 0; + } + else{ + res[0] = 0; + res[1] = 0; + } + } + else + { + S2 other_copy = other; + Eigen::MatrixBx; + other_copy.S2_Bx(Bx); + res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; + } + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + Eigen::Matrix skew_vec; + skew_vec << scalar(0), -vec[2], vec[1], + vec[2], scalar(0), -vec[0], + -vec[1], vec[0], scalar(0); + res = skew_vec; + } + + + void S2_Bx(Eigen::Matrix &res) + { + if(S2_typ == 3) + { + if(vec[2] + length > tolerance()) + { + + res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), + -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), + -vec[0], -vec[1]; + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else if(S2_typ == 2) + { + if(vec[1] + length > tolerance()) + { + + res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), + -vec[0], -vec[2], + -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else + { + if(vec[0] + length > tolerance()) + { + + res << -vec[1], -vec[2], + length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), + -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + } + + void S2_Nx(Eigen::Matrix &res, S2& subtrahend) + { + if((vec+subtrahend.vec).norm() > tolerance()) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if((vec-subtrahend.vec).norm() > tolerance()) + { + scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); + scalar v_cos = vec.transpose() * subtrahend.vec; + + res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); + } + else + { + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + } + else + { + std::cerr << "No N(x, y) for x=-y" << std::endl; + std::exit(100); + } + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if(delta.norm() < tolerance()) + { + res = -MTK::hat(vec)*Bx; + } + else{ + vect_type Bu = Bx*delta; + SO3_type exp_delta; + exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); + res = -exp_delta.normalized().toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; + } + } + + operator const vect_type&() const{ + return vec; + } + + const vect_type& get_vect() const { + return vec; + } + + friend S2 operator*(const SO3& rot, const S2& dir) + { + S2 ret; + ret.vec = rot.normalized() * dir.vec; + return ret; + } + + scalar operator[](int idx) const {return vec[idx]; } + + friend std::ostream& operator<<(std::ostream &os, const S2& vec){ + return os << vec.vec.transpose() << " "; + } + friend std::istream& operator>>(std::istream &is, S2& vec){ + for(int i=0; i<3; ++i) + is >> vec.vec[i]; + vec.vec.normalize(); + vec.vec = vec.vec * vec.length; + return is; + + } +}; + + +} // namespace MTK + + +#endif /*S2_H_*/ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp new file mode 100755 index 0000000000..8edad93f7f --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp @@ -0,0 +1,333 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/SEn.hpp + * @brief Standard Orthogonal Groups i.e.\ rotatation groups. + */ +#ifndef SEN_H_ +#define SEN_H_ + +#include + +#include "SOn.hpp" +#include "vect.hpp" +#include "../src/mtkmath.hpp" + + +namespace MTK { + + +/** + * Three-dimensional orientations represented as Quaternion. + * It is assumed that the internal Quaternion always stays normalized, + * should this not be the case, call inherited member function @c normalize(). + */ +template +struct SEN { + enum {DOF = num_of_vec_plus1, DIM = num_of_vec_plus1, TYP = 4}; + typedef _scalar scalar; + typedef Eigen::Matrix base; + typedef SO3 SO3_type; + // typedef Eigen::Quaternion base; + // typedef Eigen::Quaternion Quaternion; + typedef vect vect_type; + SO3_type SO3_data; + base mat; + + /** + * Construct from real part and three imaginary parts. + * Quaternion is normalized after construction. + */ + // SEN(const base& src) : mat(src) { + // // base::normalize(); + // } + + /** + * Construct from Eigen::Quaternion. + * @note Non-normalized input may result result in spurious behavior. + */ + SEN(const base& src = base::Identity()) : mat(src) {} + + /** + * Construct from rotation matrix. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + // template + // SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + // template + // SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + void boxplus(MTK::vectview vec, scalar scale=1) { + SEN delta = exp(vec, scale); // ? + mat = mat * delta.mat; + } + void boxminus(MTK::vectview res, const SEN& other) const { + base error_mat = other.mat.inverse() * mat; + res = log(error_mat); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SEN delta = exp(vec, scale); + mat = mat * delta.mat; + } + + // void hat(MTK::vectview& v, Eigen::Matrix &res) { + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + res = Eigen::Matrix::Zero(); + Eigen::Matrix psi; + psi << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + res.block<3, 3>(0, 0) = psi; + for(int i = 3; i < v.size() / 3 + 2; i++) + { + res.block<3, 1>(0, i) = v.segment<3>(i + (i-3)*3); + } + // return res; + } + + // void Jacob_right_inv(MTK::vectview vec, Eigen::Matrix & res){ + void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + res = Eigen::Matrix::Zero(); + Eigen::Matrix M_v; + Eigen::VectorXd vec_psi, vec_ro; + Eigen::MatrixXd jac_v; + Eigen::MatrixXd hat_v, hat_ro; + vec_psi = vec.segment<3>(0); + // Eigen::Matrix ; + SO3_data.hat(vec_psi, hat_v); + SO3_data.Jacob_right_inv(vec_psi, jac_v); + double norm = vec_psi.norm(); + for(int i = 0; i < vec.size() / 3; i++) + { + res.block<3, 3>(i*3, i*3) = jac_v; + } + for(int i = 1; i < vec.size() / 3; i++) + { + vec_ro = vec.segment<3>(i * 3); + SO3_data.hat(vec_ro, hat_ro); + if(norm > MTK::tolerance()) + { + res.block<3,3>(i*3, 0) = 0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v; + } + else + { + res.block<3,3>(i*3, 0) = 0.5 * hat_ro; + } + + } + // return res; + } + + // void Jacob_right(MTK::vectview & vec, Eigen::Matrix &res){ + void Jacob_right(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + res = Eigen::Matrix::Zero(); + Eigen::MatrixXd hat_v, hat_ro; + Eigen::VectorXd vec_psi, vec_ro; + Eigen::MatrixXd jac_v; + vec_psi = vec.segment<3>(0); + // Eigen::Matrix ; + SO3_data.hat(vec_psi, hat_v); + SO3_data.Jacob_right(vec_psi, jac_v); + // double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + // double norm = std::sqrt(squaredNorm); + double norm = vec_psi.norm(); + for(int i = 0; i < vec.size() / 3; i++) + { + res.block<3, 3>(i*3, i*3) = jac_v; + } + for(int i = 1; i < vec.size() / 3; i++) + { + vec_ro = vec.segment<3>(i * 3); + SO3_data.hat(vec_ro, hat_ro); + if(norm > MTK::tolerance()) + { + res.block<3,3>(i*3, 0) = -1 * jac_v * (0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v) * jac_v; + } + else + { + res.block<3,3>(i*3, 0) = -0.5 * jac_v * hat_ro * jac_v; + } + + } + // return res; + } + + void S2_hat(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SEN& q){ + for(int i=0; i>(std::istream &is, SEN& q){ + // vect coeffs; + for(int i=0; i> q.mat(i, j); + } + } + // is >> q.mat; + // coeffs; + // q.coeffs() = coeffs.normalized(); + return is; + } + + //! @name Helper functions + //{ + /** + * Calculate the exponential map. In matrix terms this would correspond + * to the Rodrigues formula. + */ + // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround +// static SO3 exp(MTK::vectview dvec, scalar scale = 1){ + static SEN exp(const Eigen::Matrix& dvec, scalar scale = 1){ + SEN res; + res.mat = Eigen::Matrix::Identity(); + Eigen::Matrix exp_; //, jac; + Eigen::MatrixXd jac; + Eigen::Matrix psi; + Eigen::VectorXd minus_psi; + psi = dvec.template block<3,1>(0, 0); + minus_psi = -psi; + SO3_type SO3_temp; + exp_ = SO3_type::exp(psi); + SO3_temp.Jacob_right(minus_psi, jac); + res.mat.template block<3,3>(0, 0) = exp_; + for(int i = 3; i < DOF / 3 + 2; i++) + { + res.mat.template block<3, 1>(0, i) = jac * dvec.template block<3,1>(i + (i-3)*3,0); + } + return res; + } + /** + * Calculate the inverse of @c exp. + * Only guarantees that exp(log(x)) == x + */ + static Eigen::Matrix log(base &orient){ + Eigen::Matrix res; + Eigen::Matrix psi; + Eigen::VectorXd minus_psi; + Eigen::Matrix mat_psi; + Eigen::MatrixXd jac; + mat_psi = orient.template block<3, 3>(0, 0); + SO3_type SO3_temp; + SO3_type exp_psi(mat_psi); + psi = SO3_type::log(exp_psi); + minus_psi = -psi; + SO3_temp.Jacob_right_inv(minus_psi, jac); + for(int i = 3; i < dim_of_mat; i++) + { + res.template block<3,1>(i + (i-3)*3,0) = jac * orient.template block<3, 1>(0, i); + } + return res; + } +}; + + +} // namespace MTK + +#endif /*SON_H_*/ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp new file mode 100755 index 0000000000..2d612f1156 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp @@ -0,0 +1,364 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/SOn.hpp + * @brief Standard Orthogonal Groups i.e.\ rotatation groups. + */ +#ifndef SON_H_ +#define SON_H_ + +#include + +#include "vect.hpp" +#include "../src/mtkmath.hpp" + + +namespace MTK { + + +/** + * Two-dimensional orientations represented as scalar. + * There is no guarantee that the representing scalar is within any interval, + * but the result of boxminus will always have magnitude @f$\le\pi @f$. + */ +template +struct SO2 : public Eigen::Rotation2D<_scalar> { + enum {DOF = 1, DIM = 2, TYP = 3}; + + typedef _scalar scalar; + typedef Eigen::Rotation2D base; + typedef vect vect_type; + + //! Construct from angle + SO2(const scalar& angle = 0) : base(angle) { } + + //! Construct from Eigen::Rotation2D + SO2(const base& src) : base(src) {} + + /** + * Construct from 2D vector. + * Resulting orientation will rotate the first unit vector to point to vec. + */ + SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; + + + //! Calculate @c this->inverse() * @c r + SO2 operator%(const base &r) const { + return base::inverse() * r; + } + + //! Calculate @c this->inverse() * @c r + template + vect_type operator%(const Eigen::MatrixBase &vec) const { + return base::inverse() * vec; + } + + //! Calculate @c *this * @c r.inverse() + SO2 operator/(const SO2 &r) const { + return *this * r.inverse(); + } + + //! Gets the angle as scalar. + operator scalar() const { + return base::angle(); + } + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + //! @name Manifold requirements + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + void boxminus(MTK::vectview res, const SO2& other) const { + res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); + } + + friend std::istream& operator>>(std::istream &is, SO2& ang){ + return is >> ang.angle(); + } + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } +}; + + +/** + * Three-dimensional orientations represented as Quaternion. + * It is assumed that the internal Quaternion always stays normalized, + * should this not be the case, call inherited member function @c normalize(). + */ +template +struct SO3 : public Eigen::Quaternion<_scalar, Options> { + enum {DOF = 3, DIM = 3, TYP = 2}; + typedef _scalar scalar; + typedef Eigen::Quaternion base; + typedef Eigen::Quaternion Quaternion; + typedef vect vect_type; + + //! Calculate @c this->inverse() * @c r + template EIGEN_STRONG_INLINE + Quaternion operator%(const Eigen::QuaternionBase &r) const { + return base::conjugate() * r; + } + + //! Calculate @c this->inverse() * @c r + template + vect_type operator%(const Eigen::MatrixBase &vec) const { + return base::conjugate() * vec; + } + + //! Calculate @c this * @c r.conjugate() + template EIGEN_STRONG_INLINE + Quaternion operator/(const Eigen::QuaternionBase &r) const { + return *this * r.conjugate(); + } + + /** + * Construct from real part and three imaginary parts. + * Quaternion is normalized after construction. + */ + SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { + base::normalize(); + } + + /** + * Construct from Eigen::Quaternion. + * @note Non-normalized input may result result in spurious behavior. + */ + SO3(const base& src = base::Identity()) : base(src) {} + + /** + * Construct from rotation matrix. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + template + SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + template + SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + void boxplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + void boxminus(MTK::vectview res, const SO3& other) const { + res = SO3::log(other.conjugate() * *this); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + + // void hat(MTK::vectview& v, Eigen::Matrix &res) { + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + // Eigen::Matrix res; + res << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + // return res; + } + + // void Jacob_right_inv(MTK::vectview vec, Eigen::Matrix & res){ + void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + Eigen::MatrixXd hat_v; + hat(vec, hat_v); + if(vec.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() + 0.5 * hat_v + (1 - vec.norm() * std::cos(vec.norm() / 2) / 2 / std::sin(vec.norm() / 2)) * hat_v * hat_v / vec.squaredNorm(); + } + else + { + res = Eigen::Matrix::Identity(); + } + // return res; + } + + // void Jacob_right(MTK::vectview & v, Eigen::Matrix &res){ + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res){ + Eigen::MatrixXd hat_v; + hat(v, hat_v); + double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + double norm = std::sqrt(squaredNorm); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::Identity() - (1 - std::cos(norm)) / squaredNorm * hat_v + (1 - std::sin(norm) / norm) / squaredNorm * hat_v * hat_v; + } + // return res; + } + + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SO3& q){ + return os << q.coeffs().transpose() << " "; + } + + friend std::istream& operator>>(std::istream &is, SO3& q){ + vect<4,scalar> coeffs; + is >> coeffs; + q.coeffs() = coeffs.normalized(); + return is; + } + + //! @name Helper functions + //{ + /** + * Calculate the exponential map. In matrix terms this would correspond + * to the Rodrigues formula. + */ + // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround +// static SO3 exp(MTK::vectview dvec, scalar scale = 1){ + static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ + SO3 res; + res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); + return res; + } + /** + * Calculate the inverse of @c exp. + * Only guarantees that exp(log(x)) == x + */ + static typename base::Vector3 log(const SO3 &orient){ + typename base::Vector3 res; + MTK::log(res, orient.w(), orient.vec(), scalar(2), true); + return res; + } +}; + +namespace internal { +template +struct UnalignedType >{ + typedef SO2 type; +}; + +template +struct UnalignedType >{ + typedef SO3 type; +}; + +} // namespace internal + + +} // namespace MTK + +#endif /*SON_H_*/ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp new file mode 100755 index 0000000000..fa36ccaad5 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp @@ -0,0 +1,511 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/vect.hpp + * @brief Basic vectors interpreted as manifolds. + * + * This file also implements a simple wrapper for matrices, for arbitrary scalars + * and for positive scalars. + */ +#ifndef VECT_H_ +#define VECT_H_ + +#include +#include +#include + +#include "../src/vectview.hpp" + +namespace MTK { + +static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); + + +/** + * A simple vector class. + * Implementation is basically a wrapper around Eigen::Matrix with manifold + * requirements added. + */ +template +struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { + typedef Eigen::Matrix<_scalar, D, 1, _Options> base; + enum {DOF = D, DIM = D, TYP = 0}; + typedef _scalar scalar; + + //using base::operator=; + + /** Standard constructor. Sets all values to zero. */ + vect(const base &src = base::Zero()) : base(src) {} + + /** Constructor copying the value of the expression \a other */ + template + EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} + + /** Construct from memory. */ + vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } + + void boxplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + void boxminus(MTK::vectview res, const vect& other) const { + res = *this - other; + } + + void oplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const vect& v){ + // Eigen sometimes messes with the streams flags, so output manually: + for(int i=0; i>(std::istream &is, vect& v){ + char term=0; + is >> std::ws; // skip whitespace + switch(is.peek()) { + case '(': term=')'; is.ignore(1); break; + case '[': term=']'; is.ignore(1); break; + case '{': term='}'; is.ignore(1); break; + default: break; + } + if(D==Eigen::Dynamic) { + assert(term !=0 && "Dynamic vectors must be embraced"); + std::vector temp; + while(is.good() && is.peek() != term) { + scalar x; + is >> x; + temp.push_back(x); + if(is.peek()==',') is.ignore(1); + } + v = vect::Map(temp.data(), temp.size()); + } else + for(int i=0; i> v[i]; + if(is.peek()==',') { // ignore commas between values + is.ignore(1); + } + } + if(term!=0) { + char x; + is >> x; + if(x!=term) { + is.setstate(is.badbit); +// assert(x==term && "start and end bracket do not match!"); + } + } + return is; + } + + template + vectview tail(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview tail() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview head(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } + template + vectview head() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } +}; + + +/** + * A simple matrix class. + * Implementation is basically a wrapper around Eigen::Matrix with manifold + * requirements added, i.e., matrix is viewed as a plain vector for that. + */ +template::Options> +struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { + typedef Eigen::Matrix<_scalar, M, N, _Options> base; + enum {DOF = M * N, TYP = 4, DIM=0}; + typedef _scalar scalar; + + using base::operator=; + + /** Standard constructor. Sets all values to zero. */ + matrix() { + base::setZero(); + } + + /** Constructor copying the value of the expression \a other */ + template + EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} + + /** Construct from memory. */ + matrix(const scalar* src) : base(src) { } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + void boxminus(MTK::vectview res, const matrix& other) const { + base::Map(res.data()) = *this - other; + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ + for(int i=0; i>(std::istream &is, matrix& mat){ + for(int i=0; i> mat.data()[i]; + } + return is; + } +};// @todo What if M / N = Eigen::Dynamic? + + + +/** + * A simple scalar type. + */ +template +struct Scalar { + enum {DOF = 1, TYP = 5, DIM=0}; + typedef _scalar scalar; + + scalar value; + + Scalar(const scalar& value = scalar(0)) : value(value) {} + operator const scalar&() const { return value; } + operator scalar&() { return value; } + Scalar& operator=(const scalar& val) { value = val; return *this; } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + void boxminus(MTK::vectview res, const Scalar& other) const { + res[0] = *this - other; + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } +}; + +/** + * Positive scalars. + * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. + */ +template +struct PositiveScalar { + enum {DOF = 1, TYP = 6, DIM=0}; + typedef _scalar scalar; + + scalar value; + + PositiveScalar(const scalar& value = scalar(1)) : value(value) { + assert(value > scalar(0)); + } + operator const scalar&() const { return value; } + PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + void boxminus(MTK::vectview res, const PositiveScalar& other) const { + res[0] = std::log(*this / other); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + + friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ + is >> s.value; + assert(s.value > 0); + return is; + } +}; + +template +struct Complex : public std::complex<_scalar>{ + enum {DOF = 2, TYP = 7, DIM=0}; + typedef _scalar scalar; + + typedef std::complex Base; + + Complex(const Base& value) : Base(value) {} + Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} + Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} + template + Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} + + void boxplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + void boxminus(MTK::vectview res, const Complex& other) const { + Complex diff = *this - other; + res << diff.real(), diff.imag(); + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + scalar squaredNorm() const { + return std::pow(Base::real(),2) + std::pow(Base::imag(),2); + } + + const scalar& operator()(int i) const { + assert(0<=i && i<2 && "Index out of range"); + return i==0 ? Base::real() : Base::imag(); + } + scalar& operator()(int i){ + assert(0<=i && i<2 && "Index out of range"); + return i==0 ? Base::real() : Base::imag(); + } +}; + + +namespace internal { + +template +struct UnalignedType >{ + typedef vect type; +}; + +} // namespace internal + + +} // namespace MTK + + + + +#endif /*VECT_H_*/ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp new file mode 100755 index 0000000000..b46226bbc6 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH + * All rights reserved. + * + * Author: Rene Wagner + * Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the DFKI GmbH + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef WRAPPED_CV_MAT_HPP_ +#define WRAPPED_CV_MAT_HPP_ + +#include +#include + +namespace MTK { + +template +struct cv_f_type; + +template<> +struct cv_f_type +{ + enum {value = CV_64F}; +}; + +template<> +struct cv_f_type +{ + enum {value = CV_32F}; +}; + +/** + * cv_mat wraps a CvMat around an Eigen Matrix + */ +template +class cv_mat : public matrix +{ + typedef matrix base_type; + enum {type_ = cv_f_type::value}; + CvMat cv_mat_; + +public: + cv_mat() + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + cv_mat(const cv_mat& oth) : base_type(oth) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat(const Eigen::MatrixBase &value) : base_type(value) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat& operator=(const Eigen::MatrixBase &value) + { + base_type::operator=(value); + return *this; + } + + cv_mat& operator=(const cv_mat& value) + { + base_type::operator=(value); + return *this; + } + + // FIXME: Maybe overloading operator& is not a good idea ... + CvMat* operator&() + { + return &cv_mat_; + } + const CvMat* operator&() const + { + return &cv_mat_; + } +}; + +} // namespace MTK + +#endif /* WRAPPED_CV_MAT_HPP_ */ diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/LICENSE b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/LICENSE new file mode 100644 index 0000000000..d159169d10 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/README.md new file mode 100644 index 0000000000..208c6679c9 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/README.md @@ -0,0 +1,488 @@ +## IKFoM +**IKFoM** (Iterated Kalman Filters on Manifolds) is a computationally efficient and convenient toolkit for deploying iterated Kalman filters on various robotic systems, especially systems operating on high-dimension manifold. It implements a manifold-embedding Kalman filter which separates the manifold structures from system descriptions and is able to be used by only defining the system in a canonical form and calling the respective steps accordingly. The current implementation supports the full iterated Kalman filtering for systems on manifold and any of its sub-manifolds, and it is extendable to other types of manifold when necessary. + + +**Developers** + +[Dongjiao He](https://github.com/Joanna-HE) + +**Our related video**: https://youtu.be/sz_ZlDkl6fA + +## 1. Prerequisites + +### 1.1. **Eigen && Boost** +Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). + +Boost >= 1.65. + +## 2. Usage when the measurement is of constant dimension and type. +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` + +1. include the necessary head file: +``` +#include +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1 +``` +3. Build system state, input and measurement as compound manifolds which are composed of the primitive manifolds: +``` +MTK_BUILD_MANIFOLD(state, // name of compound manifold: state +((vect3, pos)) // ((primitive manifold type, name of variable)) +((vect3, vel)) +((SO3, rot)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +); +``` +4. Implement the vector field that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + return cov; +} +``` +Those functions would be called during the ekf state predict + +5. Implement the output equation and its differentiation , : +``` +measurement h(state &s, bool &valid) // the iteration stops before convergence whenever the user set valid as false +{ + if (condition){ valid = false; + } // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied. + measurement h_; + h_.position = s.pos; + return h_; +} +Eigen::Matrix dh_dx(state &s) {} +Eigen::Matrix dh_dv(state &s) {} +``` +Those functions would be called during the ekf state update + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**. + +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated(z, R); // measurement noise covariance: R, an Eigen matrix +``` +*Remarks(1):* +- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows. + +5. Implement the output equation and its differentiation , : +``` +measurement h_share(state &s, esekfom::share_datastruct &share_data) +{ + if(share_data.converge) {} // this value is true means iteration is converged + if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified + share_data.h_x = H_x; // H_x is the result matrix of the first differentiation + share_data.h_v = H_v; // H_v is the result matrix of the second differentiation + share_data.R = R; // R is the measurement noise covariance + share_data.z = z; // z is the obtained measurement + + measurement h_; + h_.position = s.pos; + return h_; +} +``` +This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated_share(); +``` + +*Remarks(2):* +- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**: +``` +state set_x; +kf.change_x(set_x); +esekfom::esekf::cov set_P; +kf.change_P(set_P); +``` + +## 3. Usage when the measurement is an Eigen vector of changing dimension. + +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` + +1. include the necessary head file: +``` +#include +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1 +``` +3. Build system state and input as compound manifolds which are composed of the primitive manifolds: +``` +MTK_BUILD_MANIFOLD(state, // name of compound manifold: state +((vect3, pos)) // ((primitive manifold type, name of variable)) +((vect3, vel)) +((SO3, rot)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +); +``` +4. Implement the vector field that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + return cov; +} +``` +Those functions would be called during ekf state predict + +5. Implement the output equation and its differentiation , : +``` +Eigen::Matrix h(state &s, bool &valid) //the iteration stops before convergence when valid is false { + if (condition){ valid = false; + } // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied. + Eigen::Matrix h_; + h_(0) = s.pos[0]; + return h_; +} +Eigen::Matrix dh_dx(state &s) {} +Eigen::Matrix dh_dv(state &s) {} +``` +Those functions would be called during ekf state update + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof** + +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated_dyn(z, R); // measurement noise covariance: R, an Eigen matrix +``` +*Remarks(1):* +- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows. +5. Implement the output equation and its differentiation , : +``` +Eigen::Matrix h_dyn_share(state &s, esekfom::dyn_share_datastruct &dyn_share_data) +{ + if(dyn_share_data.converge) {} // this value is true means iteration is converged + if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified + dyn_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation + dyn_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation + dyn_share_data.R = R; // R is the measurement noise covariance + dyn_share_data.z = z; // z is the obtained measurement + + Eigen::Matrix h_; + h_(0) = s.pos[0]; + return h_; +} +This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function +``` +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated_dyn_share(); +``` + +*Remarks(2):* +- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**: +``` +state set_x; +kf.change_x(set_x); +esekfom::esekf::cov set_P; +kf.change_P(set_P); +``` + +## 4. Usage when the measurement is a changing manifold during the run time. + +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` + +1. include the necessary head file: +``` +#include +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1 +``` +3. Build system state and input as compound manifolds which are composed of the primitive manifolds: +``` +MTK_BUILD_MANIFOLD(state, // name of compound manifold: state +((vect3, pos)) // ((primitive manifold type, name of variable)) +((vect3, vel)) +((SO3, rot)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +); +``` +4. Implement the vector field that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + return cov; +} +``` +Those functions would be called during ekf state predict + +5. Implement the differentiation of the output equation , : +``` +Eigen::Matrix dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false +Eigen::Matrix dh_dv(state &s, bool &valid) {} +``` +Those functions would be called during ekf state update + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) +``` +esekfom::esekf kf; +``` +Where **process_noise_dof** is the dimension of process noise, of type of std int + +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q +``` +9. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation : +``` +measurement h(state &s, bool &valid) //the iteration stops before convergence when valid is false +{ + if (condition) valid = false; // the update iteration could be stopped when the condition other than convergence is satisfied + measurement h_; + h_.pos = s.pos; + return h_; +} +``` +then an iterated update is executed: +``` +kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R, an Eigen matrix +``` +*Remarks(1):* +- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows. +5. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +6. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, epsi); +``` +7. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q. an Eigen matrix +``` +8. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation and its differentiation , : +``` +measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct &dyn_runtime_share_data) +{ + if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged + if(condition) dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false, if conditions other than convergence is satisfied + dyn_runtime_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation + dyn_runtime_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation + dyn_runtime_share_data.R = R; // R is the measurement noise covariance + + measurement h_; + h_.pos = s.pos; + return h_; +} +``` +This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function + +then an iterated update is executed: +``` +kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share); +``` + +*Remarks(2):* +- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**: +``` +state set_x; +kf.change_x(set_x); +esekfom::esekf::cov set_P; +kf.change_P(set_P); +``` + +## 5. Run the sample +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` +In the **Samples** file folder, there is the scource code that applys the **IKFoM** on the original source code from [FAST LIO](https://github.com/hku-mars/FAST_LIO). Please follow the README.md shown in that repository excepting the step **2. Build**, which is modified as: +``` +cd ~/catkin_ws/src +cp -r ~/IKFoM/Samples/FAST_LIO-stable FAST_LIO-stable +cd .. +catkin_make +source devel/setup.bash +``` + +## 6.Acknowledgments +Thanks for C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/common_lib.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/common_lib.h new file mode 100755 index 0000000000..0e6232c440 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/common_lib.h @@ -0,0 +1,180 @@ +#ifndef COMMON_LIB_H +#define COMMON_LIB_H + +#include +#include +#include +#include +#include +#include +#include +#include +using namespace std; +using namespace Eigen; + +#define PI_M (3.14159265358) +#define G_m_s2 (9.81) // Gravaty const in GuangDong/China +#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) +#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) +#define CUBE_LEN (6.0) +#define LIDAR_SP_LEN (2) +#define INIT_COV (0.0001) +#define NUM_MATCH_POINTS (5) +#define MAX_MEAS_DIM (10000) + +#define VEC_FROM_ARRAY(v) v[0],v[1],v[2] +#define VEC_FROM_ARRAY_SIX(v) v[0],v[1],v[2],v[3],v[4],v[5] +#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] +#define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) +#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) + +// 常用的PCL点类型、点云类型、点向量 +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointXYZRGB PointTypeRGB; +typedef pcl::PointCloud PointCloudXYZI; +typedef pcl::PointCloud PointCloudXYZRGB; +typedef vector> PointVector; + +// 常用的Eigen变量类型 +typedef Vector3d V3D; +typedef Matrix3d M3D; +typedef Vector3f V3F; +typedef Matrix3f M3F; + +#define MD(a,b) Matrix +#define VD(a) Matrix +#define MF(a,b) Matrix +#define VF(a) Matrix + +const M3D Eye3d(M3D::Identity()); +const M3F Eye3f(M3F::Identity()); +const V3D Zero3d(0, 0, 0); +const V3F Zero3f(0, 0, 0); + +struct MeasureGroup // Lidar data and imu dates for the curent process +{ + MeasureGroup() + { + lidar_beg_time = 0.0; + lidar_last_time = 0.0; + this->lidar.reset(new PointCloudXYZI()); + }; + + double lidar_beg_time; // 点云起始时间戳 + double lidar_last_time; // 点云结束时间戳，即最后一个点的时间戳 + PointCloudXYZI::Ptr lidar; // 当前帧点云 + deque imu; // IMU队列 +}; + +template +T calc_dist(PointType p1, PointType p2){ + T d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); + return d; +} + +template +T calc_dist(Eigen::Vector3d p1, PointType p2){ + T d = (p1(0) - p2.x) * (p1(0) - p2.x) + (p1(1) - p2.y) * (p1(1) - p2.y) + (p1(2) - p2.z) * (p1(2) - p2.z); + return d; +} + +template +std::vector time_compressing(const PointCloudXYZI::Ptr &point_cloud) +{ + int points_size = point_cloud->points.size(); + int j = 0; + std::vector time_seq; + // time_seq.clear(); + time_seq.reserve(points_size); + for(int i = 0; i < points_size - 1; i++) + { + j++; + if (point_cloud->points[i+1].curvature > point_cloud->points[i].curvature) + { + time_seq.emplace_back(j); + j = 0; + } + } + if (j == 0) + { + time_seq.emplace_back(1); + } + else + { + time_seq.emplace_back(j+1); + } + return time_seq; +} + +/* comment +plane equation: Ax + By + Cz + D = 0 +convert to: A/D*x + B/D*y + C/D*z = -1 +solve: A0*x0 = b0 +where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T +normvec: normalized x0 +*/ +template +bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) +{ + MatrixXf A(point_num, 3); + MatrixXf b(point_num, 1); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < point_num; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + normvec = A.colPivHouseholderQr().solve(b); + + for (int j = 0; j < point_num; j++) + { + if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) + { + return false; + } + } + + normvec.normalize(); + return true; +} + +template +bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) +{ + Matrix A; + Matrix b; + A.setZero(); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + + Matrix normvec = A.colPivHouseholderQr().solve(b); + + T n = normvec.norm(); + pca_result(0) = normvec(0) / n; + pca_result(1) = normvec(1) / n; + pca_result(2) = normvec(2) / n; + pca_result(3) = 1.0 / n; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) + { + return false; + } + } + return true; +} + +#endif diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/eigen_conversions/eigen_msg.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/eigen_conversions/eigen_msg.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/eigen_conversions/eigen_msg.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/geometry_msgs/Vector3.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/geometry_msgs/Vector3.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/geometry_msgs/Vector3.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/README.md new file mode 100644 index 0000000000..e113a91991 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/README.md @@ -0,0 +1,2 @@ +# ikd-Tree +ikd-Tree is an incremental k-d tree for robotic applications. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.cpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.cpp new file mode 100644 index 0000000000..8ea5c892f9 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.cpp @@ -0,0 +1,1727 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) +{ + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + + + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) +{ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE *root) +{ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->radius_sq = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock), NULL); +} + +template +int KD_TREE::size() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + return Root_Node->TreeSize; + } + else + { + return 0; + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range() +{ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } + else + { + memset(&range, 0, sizeof(range)); + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } + else + { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize - Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) +{ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } + else + { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread() +{ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread() +{ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) + pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void *KD_TREE::multi_thread_ptr(void *arg) +{ + KD_TREE *handle = (KD_TREE *)arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild() +{ + bool terminated = false; + KD_TREE_NODE *father_ptr, **new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated) + { + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr) + { + /* Traverse and copy */ + if (!Rebuild_Logger.empty()) + { + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) + { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE *new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0) + { + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()) + { + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter++; + if (tmp_counter % 10 == 0) + usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) + { + father_ptr->left_son_ptr = new_root_node; + } + else if (father_ptr->right_son_ptr == *Rebuild_Ptr) + { + father_ptr->right_son_ptr = new_root_node; + } + else + { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) + new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) + Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE *update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node) + { + update_root = update_root->father_ptr; + if (update_root->working_flag) + break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) + break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) + break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } + else + { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) +{ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) + (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) + (*root)->invalid_point_num = (*root)->TreeSize; + else + (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud) +{ + if (Root_Node != nullptr) + { + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) + return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist) +{ + MANUAL_HEAP q(2 * k_nearest); + q.clear(); + vector().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Search(Root_Node, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest, int(q.size())); + PointVector().swap(Nearest_Points); + vector().swap(Point_Distance); + for (int i = 0; i < k_found; i++) + { + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) +{ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i = 0; i < PointToAdd.size(); i++) + { + if (downsample_switch) + { + Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; + Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; + Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; + Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; + Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; + Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; + mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; + mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; + mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; + PointVector().swap(Downsample_Storage); + Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); + min_dist = calc_dist(PointToAdd[i], mid_point); + downsample_result = PointToAdd[i]; + for (int index = 0; index < Downsample_Storage.size(); index++) + { + tmp_dist = calc_dist(Downsample_Storage[index], mid_point); + if (tmp_dist < min_dist) + { + min_dist = tmp_dist; + downsample_result = Downsample_Storage[index]; + } + } + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter++; + } + } + else + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, false, true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter++; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) + Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector &BoxPoints) +{ + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_range(&Root_Node, BoxPoints[i], true); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node, BoxPoints[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector &PointToDel) +{ + for (int i = 0; i < PointToDel.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Delete_by_point(&Root_Node, PointToDel[i], true); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToDel[i]; + operation.op = DELETE_POINT; + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&Root_Node, PointToDel[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) +{ + int tmp_counter = 0; + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector &removed_points) +{ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size(); i++) + { + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size(); i++) + { + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) +{ + if (l > r) + return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l + r) >> 1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0, 0, 0}; + for (i = l; i <= r; i++) + { + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i = 0; i < 3; i++) + dim_range[i] = max_value[i] - min_value[i]; + for (i = 1; i < 3; i++) + if (dim_range[i] > dim_range[div_axis]) + div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); + break; + default: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; + BuildTree(&left_son, l, mid - 1, Storage); + BuildTree(&right_son, mid + 1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE **root) +{ + KD_TREE_NODE *father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) + { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) + { + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) + { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } + else + { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); + if (*root != nullptr) + (*root)->father_ptr = father_ptr; + if (*root == Root_Node) + STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample) + { + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) + (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) + delete_box_log.op = DOWNSAMPLE_DELETE; + else + delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) + { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) + (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) +{ + if ((*root) == nullptr) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) +{ + if (*root == nullptr) + { + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) +{ + if (root == nullptr || root->tree_deleted) + return; + float cur_dist = calc_box_dist(root, point); + float max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) + return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) + { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0) + { + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + else + { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted) + { + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) + { + if (q.size() >= k_nearest) + q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) + { + if (dist_left_node <= dist_right_node) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } + else + { + if (dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) + { + if (!root->point_deleted) + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){ + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) +{ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) + { + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num) / root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); + if (delete_evaluation > delete_criterion_param) + { + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) + { + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root) +{ + if (root == nullptr) + return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE *root) +{ + KD_TREE_NODE *left_son_ptr = root->left_son_ptr; + KD_TREE_NODE *right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (left_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (right_son_ptr != nullptr) + { + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else + { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted ? 1 : 0); + root->down_del_num = (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); + memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); + memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) + left_son_ptr->father_ptr = root; + if (right_son_ptr != nullptr) + right_son_ptr->father_ptr = root; + if (root == Root_Node && root->TreeSize > 3) + { + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); + root->alpha_del = float(root->invalid_point_num) / root->TreeSize; + root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) +{ + if (root == nullptr) + return; + Push_Down(root); + if (!root->point_deleted) + { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) +{ + if (*root == nullptr) + return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy(&(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b) +{ + return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) +{ + if (node == nullptr) + return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) + min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) + min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) + min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) + min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) + min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) + min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); + return min_dist; +} +template +bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } +template +bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } +template +bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } + +// Manual heap + + + +// manual queue + + +// Manual Instatiations +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.h new file mode 100644 index 0000000000..764e206014 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.h @@ -0,0 +1,344 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +// typedef pcl::PointXYZINormal PointType; +// typedef vector> PointVector; + +struct BoxPointType +{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set +{ + ADD_POINT, + DELETE_POINT, + DELETE_BOX, + ADD_BOX, + DOWNSAMPLE_DELETE, + PUSH_DOWN +}; + +enum delete_point_storage_set +{ + NOT_RECORD, + DELETE_POINTS_REC, + MULTI_THREAD_REC +}; + +template +class KD_TREE +{ + // using MANUAL_Q_ = MANUAL_Q; + // using PointVector = std::vector; + + // using MANUAL_Q_ = MANUAL_Q; +public: + using PointVector = std::vector>; + using Ptr = std::shared_ptr>; + + struct KD_TREE_NODE + { + PointType point; + int division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + float radius_sq; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type + { + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + // static const PointType zeroP; + + struct PointType_CMP + { + PointType point; + float dist = 0.0; + PointType_CMP(PointType p = PointType(), float d = INFINITY) + { + this->point = p; + this->dist = d; + }; + bool operator<(const PointType_CMP &a) const + { + if (fabs(dist - a.dist) < 1e-10) + return point.x < a.point.x; + else + return dist < a.dist; + } + }; + + class MANUAL_HEAP + { + + public: + MANUAL_HEAP(int max_capacity = 100) + + { + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP() + { + delete[] heap; + } + void pop() + { + if (heap_size == 0) + return; + heap[0] = heap[heap_size - 1]; + heap_size--; + MoveDown(0); + return; + } + PointType_CMP top() + { + return heap[0]; + } + void push(PointType_CMP point) + { + if (heap_size >= cap) + return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + int size() + { + return heap_size; + } + void clear() + { + heap_size = 0; + return; + } + + private: + PointType_CMP *heap; + void MoveDown(int heap_index) + { + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size) + { + if (l + 1 < heap_size && heap[l] < heap[l + 1]) + l++; + if (tmp < heap[l]) + { + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + void FloatUp(int heap_index) + { + int ancestor = (heap_index - 1) / 2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0) + { + if (heap[ancestor] < tmp) + { + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index - 1) / 2; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + int heap_size = 0; + int cap = 0; + }; + + class MANUAL_Q + { + private: + int head = 0, tail = 0, counter = 0; + Operation_Logger_Type q[Q_LEN]; + bool is_empty; + + public: + void pop() + { + if (counter == 0) + return; + head++; + head %= Q_LEN; + counter--; + if (counter == 0) + is_empty = true; + return; + } + Operation_Logger_Type front() + { + return q[head]; + } + Operation_Logger_Type back() + { + return q[tail]; + } + void clear() + { + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; + } + void push(Operation_Logger_Type op) + { + q[tail] = op; + counter++; + if (is_empty) + is_empty = false; + tail++; + tail %= Q_LEN; + } + bool empty() + { + return is_empty; + } + int size() + { + return counter; + } + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE **Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void *multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE *root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); + void Rebuild(KD_TREE_NODE **root); + int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE *root); + void Push_Down(KD_TREE_NODE *root); + void Update(KD_TREE_NODE *root); + void delete_tree_nodes(KD_TREE_NODE **root); + void downsample(KD_TREE_NODE **root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE *node, PointType point); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param) + { + delete_criterion_param = delete_param; + } + void Set_balance_criterion_param(float balance_param) + { + balance_criterion_param = balance_param; + } + void set_downsample_param(float downsample_param) + { + downsample_size = downsample_param; + } + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector &PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector &BoxPoints); + void Delete_Points(PointVector &PointToDel); + int Delete_Point_Boxes(vector &BoxPoints); + void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector &removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE *Root_Node = nullptr; + int max_queue_size = 0; +}; + +// template +// PointType KD_TREE::zeroP = PointType(0,0,0); diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/crc32.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/crc32.hpp new file mode 100644 index 0000000000..39a0fb5ce2 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/crc32.hpp @@ -0,0 +1,109 @@ +#include +#include +#include + +namespace mcap::internal { + +/** + * Compute CRC32 lookup tables as described at: + * https://github.com/komrad36/CRC#option-6-1-byte-tabular + * + * An iteration of CRC computation can be performed on 8 bits of input at once. By pre-computing a + * table of the values of CRC(?) for all 2^8 = 256 possible byte values, during the final + * computation we can replace a loop over 8 bits with a single lookup in the table. + * + * For further speedup, we can also pre-compute the values of CRC(?0) for all possible bytes when a + * zero byte is appended. Then we can process two bytes of input at once by computing CRC(AB) = + * CRC(A0) ^ CRC(B), using one lookup in the CRC(?0) table and one lookup in the CRC(?) table. + * + * The same technique applies for any number of bytes to be processed at once, although the speed + * improvements diminish. + * + * @param Polynomial The binary representation of the polynomial to use (reversed, i.e. most + * significant bit represents x^0). + * @param NumTables The number of bytes of input that will be processed at once. + */ +template +struct CRC32Table { +private: + std::array table = {}; + +public: + constexpr CRC32Table() { + for (uint32_t i = 0; i < 256; i++) { + uint32_t r = i; + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + r = ((r & 1) * Polynomial) ^ (r >> 1); + table[i] = r; + } + for (size_t i = 256; i < table.size(); i++) { + uint32_t value = table[i - 256]; + table[i] = table[value & 0xff] ^ (value >> 8); + } + } + + constexpr uint32_t operator[](size_t index) const { + return table[index]; + } +}; + +inline uint32_t getUint32LE(const std::byte* data) { + return (uint32_t(data[0]) << 0) | (uint32_t(data[1]) << 8) | (uint32_t(data[2]) << 16) | + (uint32_t(data[3]) << 24); +} + +static constexpr CRC32Table<0xedb88320, 8> CRC32_TABLE; + +/** + * Initialize a CRC32 to all 1 bits. + */ +static constexpr uint32_t CRC32_INIT = 0xffffffff; + +/** + * Update a streaming CRC32 calculation. + * + * For performance, this implementation processes the data 8 bytes at a time, using the algorithm + * presented at: https://github.com/komrad36/CRC#option-9-8-byte-tabular + */ +inline uint32_t crc32Update(const uint32_t prev, const std::byte* const data, const size_t length) { + uint32_t r = prev; + + // Handle small inputs byte-by-byte, avoiding the 8-byte bulk loop below. + if (length <= 8) { + for (size_t i = 0; i < length; i++) { + r = CRC32_TABLE[(r ^ uint8_t(data[i])) & 0xff] ^ (r >> 8); + } + return r; + } + + // Process 8 bytes (2 uint32s) at a time. + size_t offset = 0; + size_t remainingBytes = length; + for (; remainingBytes >= 8; offset += 8, remainingBytes -= 8) { + r ^= getUint32LE(data + offset); + uint32_t r2 = getUint32LE(data + offset + 4); + r = CRC32_TABLE[0 * 256 + ((r2 >> 24) & 0xff)] ^ CRC32_TABLE[1 * 256 + ((r2 >> 16) & 0xff)] ^ + CRC32_TABLE[2 * 256 + ((r2 >> 8) & 0xff)] ^ CRC32_TABLE[3 * 256 + ((r2 >> 0) & 0xff)] ^ + CRC32_TABLE[4 * 256 + ((r >> 24) & 0xff)] ^ CRC32_TABLE[5 * 256 + ((r >> 16) & 0xff)] ^ + CRC32_TABLE[6 * 256 + ((r >> 8) & 0xff)] ^ CRC32_TABLE[7 * 256 + ((r >> 0) & 0xff)]; + } + + // Process any remaining bytes one by one. + for (; offset < length; offset++) { + r = CRC32_TABLE[(r ^ uint8_t(data[offset])) & 0xff] ^ (r >> 8); + } + return r; +} + +/** Finalize a CRC32 by inverting the output value. */ +inline uint32_t crc32Final(uint32_t crc) { + return crc ^ 0xffffffff; +} + +} // namespace mcap::internal diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/errors.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/errors.hpp new file mode 100644 index 0000000000..8c61b8d60a --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/errors.hpp @@ -0,0 +1,120 @@ +#pragma once + +#include + +namespace mcap { + +/** + * @brief Status codes for MCAP readers and writers. + */ +enum class StatusCode { + Success = 0, + NotOpen, + InvalidSchemaId, + InvalidChannelId, + FileTooSmall, + ReadFailed, + MagicMismatch, + InvalidFile, + InvalidRecord, + InvalidOpCode, + InvalidChunkOffset, + InvalidFooter, + DecompressionFailed, + DecompressionSizeMismatch, + UnrecognizedCompression, + OpenFailed, + MissingStatistics, + InvalidMessageReadOptions, + NoMessageIndexesAvailable, + UnsupportedCompression, +}; + +/** + * @brief Wraps a status code and string message carrying additional context. + */ +struct [[nodiscard]] Status { + StatusCode code; + std::string message; + + Status() + : code(StatusCode::Success) {} + + Status(StatusCode _code) + : code(_code) { + switch (code) { + case StatusCode::Success: + break; + case StatusCode::NotOpen: + message = "not open"; + break; + case StatusCode::InvalidSchemaId: + message = "invalid schema id"; + break; + case StatusCode::InvalidChannelId: + message = "invalid channel id"; + break; + case StatusCode::FileTooSmall: + message = "file too small"; + break; + case StatusCode::ReadFailed: + message = "read failed"; + break; + case StatusCode::MagicMismatch: + message = "magic mismatch"; + break; + case StatusCode::InvalidFile: + message = "invalid file"; + break; + case StatusCode::InvalidRecord: + message = "invalid record"; + break; + case StatusCode::InvalidOpCode: + message = "invalid opcode"; + break; + case StatusCode::InvalidChunkOffset: + message = "invalid chunk offset"; + break; + case StatusCode::InvalidFooter: + message = "invalid footer"; + break; + case StatusCode::DecompressionFailed: + message = "decompression failed"; + break; + case StatusCode::DecompressionSizeMismatch: + message = "decompression size mismatch"; + break; + case StatusCode::UnrecognizedCompression: + message = "unrecognized compression"; + break; + case StatusCode::OpenFailed: + message = "open failed"; + break; + case StatusCode::MissingStatistics: + message = "missing statistics"; + break; + case StatusCode::InvalidMessageReadOptions: + message = "message read options conflict"; + break; + case StatusCode::NoMessageIndexesAvailable: + message = "file has no message indices"; + break; + case StatusCode::UnsupportedCompression: + message = "unsupported compression"; + break; + default: + message = "unknown"; + break; + } + } + + Status(StatusCode _code, const std::string& _message) + : code(_code) + , message(_message) {} + + bool ok() const { + return code == StatusCode::Success; + } +}; + +} // namespace mcap diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/internal.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/internal.hpp new file mode 100644 index 0000000000..69b1dd9d43 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/internal.hpp @@ -0,0 +1,193 @@ +#pragma once + +#include "types.hpp" +#include + +// Do not compile on systems with non-8-bit bytes +static_assert(std::numeric_limits::digits == 8); + +namespace mcap { + +namespace internal { + +constexpr uint64_t MinHeaderLength = /* magic bytes */ sizeof(Magic) + + /* opcode */ 1 + + /* record length */ 8 + + /* profile length */ 4 + + /* library length */ 4; +constexpr uint64_t FooterLength = /* opcode */ 1 + + /* record length */ 8 + + /* summary start */ 8 + + /* summary offset start */ 8 + + /* summary crc */ 4 + + /* magic bytes */ sizeof(Magic); + +inline std::string ToHex(uint8_t byte) { + std::string result{2, '\0'}; + result[0] = "0123456789ABCDEF"[(uint8_t(byte) >> 4) & 0x0F]; + result[1] = "0123456789ABCDEF"[uint8_t(byte) & 0x0F]; + return result; +} +inline std::string ToHex(std::byte byte) { + return ToHex(uint8_t(byte)); +} + +inline std::string to_string(const std::string& arg) { + return arg; +} +inline std::string to_string(std::string_view arg) { + return std::string(arg); +} +inline std::string to_string(const char* arg) { + return std::string(arg); +} +template +[[nodiscard]] inline std::string StrCat(T&&... args) { + using mcap::internal::to_string; + using std::to_string; + return ("" + ... + to_string(std::forward(args))); +} + +inline uint32_t KeyValueMapSize(const KeyValueMap& map) { + size_t size = 0; + for (const auto& [key, value] : map) { + size += 4 + key.size() + 4 + value.size(); + } + return (uint32_t)(size); +} + +inline const std::string CompressionString(Compression compression) { + switch (compression) { + case Compression::None: + default: + return std::string{}; + case Compression::Lz4: + return "lz4"; + case Compression::Zstd: + return "zstd"; + } +} + +inline uint16_t ParseUint16(const std::byte* data) { + return uint16_t(data[0]) | (uint16_t(data[1]) << 8); +} + +inline uint32_t ParseUint32(const std::byte* data) { + return uint32_t(data[0]) | (uint32_t(data[1]) << 8) | (uint32_t(data[2]) << 16) | + (uint32_t(data[3]) << 24); +} + +inline Status ParseUint32(const std::byte* data, uint64_t maxSize, uint32_t* output) { + if (maxSize < 4) { + const auto msg = StrCat("cannot read uint32 from ", maxSize, " bytes"); + return Status{StatusCode::InvalidRecord, msg}; + } + *output = ParseUint32(data); + return StatusCode::Success; +} + +inline uint64_t ParseUint64(const std::byte* data) { + return uint64_t(data[0]) | (uint64_t(data[1]) << 8) | (uint64_t(data[2]) << 16) | + (uint64_t(data[3]) << 24) | (uint64_t(data[4]) << 32) | (uint64_t(data[5]) << 40) | + (uint64_t(data[6]) << 48) | (uint64_t(data[7]) << 56); +} + +inline Status ParseUint64(const std::byte* data, uint64_t maxSize, uint64_t* output) { + if (maxSize < 8) { + const auto msg = StrCat("cannot read uint64 from ", maxSize, " bytes"); + return Status{StatusCode::InvalidRecord, msg}; + } + *output = ParseUint64(data); + return StatusCode::Success; +} + +inline Status ParseStringView(const std::byte* data, uint64_t maxSize, std::string_view* output) { + uint32_t size = 0; + if (auto status = ParseUint32(data, maxSize, &size); !status.ok()) { + const auto msg = StrCat("cannot read string size: ", status.message); + return Status{StatusCode::InvalidRecord, msg}; + } + if (uint64_t(size) > (maxSize - 4)) { + const auto msg = StrCat("string size ", size, " exceeds remaining bytes ", (maxSize - 4)); + return Status(StatusCode::InvalidRecord, msg); + } + *output = std::string_view(reinterpret_cast(data + 4), size); + return StatusCode::Success; +} + +inline Status ParseString(const std::byte* data, uint64_t maxSize, std::string* output) { + uint32_t size = 0; + if (auto status = ParseUint32(data, maxSize, &size); !status.ok()) { + return status; + } + if (uint64_t(size) > (maxSize - 4)) { + const auto msg = StrCat("string size ", size, " exceeds remaining bytes ", (maxSize - 4)); + return Status(StatusCode::InvalidRecord, msg); + } + *output = std::string(reinterpret_cast(data + 4), size); + return StatusCode::Success; +} + +inline Status ParseByteArray(const std::byte* data, uint64_t maxSize, ByteArray* output) { + uint32_t size = 0; + if (auto status = ParseUint32(data, maxSize, &size); !status.ok()) { + return status; + } + if (uint64_t(size) > (maxSize - 4)) { + const auto msg = StrCat("byte array size ", size, " exceeds remaining bytes ", (maxSize - 4)); + return Status(StatusCode::InvalidRecord, msg); + } + output->resize(size); + // output->data() may return nullptr if 'output' is empty, but memcpy() does not accept nullptr. + // 'output' will be empty only if the 'size' is equal to 0. + if (size > 0) { + std::memcpy(output->data(), data + 4, size); + } + return StatusCode::Success; +} + +inline Status ParseKeyValueMap(const std::byte* data, uint64_t maxSize, KeyValueMap* output) { + uint32_t sizeInBytes = 0; + if (auto status = ParseUint32(data, maxSize, &sizeInBytes); !status.ok()) { + return status; + } + if (sizeInBytes > (maxSize - 4)) { + const auto msg = + StrCat("key-value map size ", sizeInBytes, " exceeds remaining bytes ", (maxSize - 4)); + return Status(StatusCode::InvalidRecord, msg); + } + + // Account for the byte size prefix in sizeInBytes to make the bounds checking + // below simpler + sizeInBytes += 4; + + output->clear(); + uint64_t pos = 4; + while (pos < sizeInBytes) { + std::string_view key; + if (auto status = ParseStringView(data + pos, sizeInBytes - pos, &key); !status.ok()) { + const auto msg = StrCat("cannot read key-value map key at pos ", pos, ": ", status.message); + return Status{StatusCode::InvalidRecord, msg}; + } + pos += 4 + key.size(); + std::string_view value; + if (auto status = ParseStringView(data + pos, sizeInBytes - pos, &value); !status.ok()) { + const auto msg = StrCat("cannot read key-value map value for key \"", key, "\" at pos ", pos, + ": ", status.message); + return Status{StatusCode::InvalidRecord, msg}; + } + pos += 4 + value.size(); + output->emplace(key, value); + } + return StatusCode::Success; +} + +inline std::string MagicToHex(const std::byte* data) { + return internal::ToHex(data[0]) + internal::ToHex(data[1]) + internal::ToHex(data[2]) + + internal::ToHex(data[3]) + internal::ToHex(data[4]) + internal::ToHex(data[5]) + + internal::ToHex(data[6]) + internal::ToHex(data[7]); +} + +} // namespace internal + +} // namespace mcap diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/intervaltree.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/intervaltree.hpp new file mode 100644 index 0000000000..267eb7a45f --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/intervaltree.hpp @@ -0,0 +1,303 @@ +// Adapted from + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace mcap::internal { + +template +class Interval { +public: + Scalar start; + Scalar stop; + Value value; + Interval(const Scalar& s, const Scalar& e, const Value& v) + : start(std::min(s, e)) + , stop(std::max(s, e)) + , value(v) {} +}; + +template +Value intervalStart(const Interval& i) { + return i.start; +} + +template +Value intervalStop(const Interval& i) { + return i.stop; +} + +template +std::ostream& operator<<(std::ostream& out, const Interval& i) { + out << "Interval(" << i.start << ", " << i.stop << "): " << i.value; + return out; +} + +template +class IntervalTree { +public: + using interval = Interval; + using interval_vector = std::vector; + + struct IntervalStartCmp { + bool operator()(const interval& a, const interval& b) { + return a.start < b.start; + } + }; + + struct IntervalStopCmp { + bool operator()(const interval& a, const interval& b) { + return a.stop < b.stop; + } + }; + + IntervalTree() + : left(nullptr) + , right(nullptr) + , center(Scalar(0)) {} + + ~IntervalTree() = default; + + std::unique_ptr clone() const { + return std::unique_ptr(new IntervalTree(*this)); + } + + IntervalTree(const IntervalTree& other) + : intervals(other.intervals) + , left(other.left ? other.left->clone() : nullptr) + , right(other.right ? other.right->clone() : nullptr) + , center(other.center) {} + + IntervalTree& operator=(IntervalTree&&) = default; + IntervalTree(IntervalTree&&) = default; + + IntervalTree& operator=(const IntervalTree& other) { + center = other.center; + intervals = other.intervals; + left = other.left ? other.left->clone() : nullptr; + right = other.right ? other.right->clone() : nullptr; + return *this; + } + + IntervalTree(interval_vector&& ivals, std::size_t depth = 16, std::size_t minbucket = 64, + std::size_t maxbucket = 512, Scalar leftextent = 0, Scalar rightextent = 0) + : left(nullptr) + , right(nullptr) { + --depth; + const auto minmaxStop = std::minmax_element(ivals.begin(), ivals.end(), IntervalStopCmp()); + const auto minmaxStart = std::minmax_element(ivals.begin(), ivals.end(), IntervalStartCmp()); + if (!ivals.empty()) { + center = (minmaxStart.first->start + minmaxStop.second->stop) / 2; + } + if (leftextent == 0 && rightextent == 0) { + // sort intervals by start + std::sort(ivals.begin(), ivals.end(), IntervalStartCmp()); + } else { + assert(std::is_sorted(ivals.begin(), ivals.end(), IntervalStartCmp())); + } + if (depth == 0 || (ivals.size() < minbucket && ivals.size() < maxbucket)) { + std::sort(ivals.begin(), ivals.end(), IntervalStartCmp()); + intervals = std::move(ivals); + assert(is_valid().first); + return; + } else { + Scalar leftp = 0; + Scalar rightp = 0; + + if (leftextent || rightextent) { + leftp = leftextent; + rightp = rightextent; + } else { + leftp = ivals.front().start; + rightp = std::max_element(ivals.begin(), ivals.end(), IntervalStopCmp())->stop; + } + + interval_vector lefts; + interval_vector rights; + + for (typename interval_vector::const_iterator i = ivals.begin(); i != ivals.end(); ++i) { + const interval& cur = *i; + if (cur.stop < center) { + lefts.push_back(cur); + } else if (cur.start > center) { + rights.push_back(cur); + } else { + assert(cur.start <= center); + assert(center <= cur.stop); + intervals.push_back(cur); + } + } + + if (!lefts.empty()) { + left.reset(new IntervalTree(std::move(lefts), depth, minbucket, maxbucket, leftp, center)); + } + if (!rights.empty()) { + right.reset( + new IntervalTree(std::move(rights), depth, minbucket, maxbucket, center, rightp)); + } + } + assert(is_valid().first); + } + + // Call f on all intervals near the range [start, stop]: + template + void visit_near(const Scalar& start, const Scalar& stop, UnaryFunction f) const { + if (!intervals.empty() && !(stop < intervals.front().start)) { + for (auto& i : intervals) { + f(i); + } + } + if (left && start <= center) { + left->visit_near(start, stop, f); + } + if (right && stop >= center) { + right->visit_near(start, stop, f); + } + } + + // Call f on all intervals crossing pos + template + void visit_overlapping(const Scalar& pos, UnaryFunction f) const { + visit_overlapping(pos, pos, f); + } + + // Call f on all intervals overlapping [start, stop] + template + void visit_overlapping(const Scalar& start, const Scalar& stop, UnaryFunction f) const { + auto filterF = [&](const interval& cur) { + if (cur.stop >= start && cur.start <= stop) { + // Only apply f if overlapping + f(cur); + } + }; + visit_near(start, stop, filterF); + } + + // Call f on all intervals contained within [start, stop] + template + void visit_contained(const Scalar& start, const Scalar& stop, UnaryFunction f) const { + auto filterF = [&](const interval& cur) { + if (start <= cur.start && cur.stop <= stop) { + f(cur); + } + }; + visit_near(start, stop, filterF); + } + + interval_vector find_overlapping(const Scalar& start, const Scalar& stop) const { + interval_vector result; + visit_overlapping(start, stop, [&](const interval& cur) { + result.emplace_back(cur); + }); + return result; + } + + interval_vector find_contained(const Scalar& start, const Scalar& stop) const { + interval_vector result; + visit_contained(start, stop, [&](const interval& cur) { + result.push_back(cur); + }); + return result; + } + + bool empty() const { + if (left && !left->empty()) { + return false; + } + if (!intervals.empty()) { + return false; + } + if (right && !right->empty()) { + return false; + } + return true; + } + + template + void visit_all(UnaryFunction f) const { + if (left) { + left->visit_all(f); + } + std::for_each(intervals.begin(), intervals.end(), f); + if (right) { + right->visit_all(f); + } + } + + std::pair extent() const { + struct Extent { + std::pair x{std::numeric_limits::max(), + std::numeric_limits::min()}; + void operator()(const interval& cur) { + x.first = std::min(x.first, cur.start); + x.second = std::max(x.second, cur.stop); + } + }; + Extent extent; + + visit_all([&](const interval& cur) { + extent(cur); + }); + return extent.x; + } + + // Check all constraints. + // If first is false, second is invalid. + std::pair> is_valid() const { + const auto minmaxStop = + std::minmax_element(intervals.begin(), intervals.end(), IntervalStopCmp()); + const auto minmaxStart = + std::minmax_element(intervals.begin(), intervals.end(), IntervalStartCmp()); + + std::pair> result = { + true, {std::numeric_limits::max(), std::numeric_limits::min()}}; + if (!intervals.empty()) { + result.second.first = std::min(result.second.first, minmaxStart.first->start); + result.second.second = std::min(result.second.second, minmaxStop.second->stop); + } + if (left) { + auto valid = left->is_valid(); + result.first &= valid.first; + result.second.first = std::min(result.second.first, valid.second.first); + result.second.second = std::min(result.second.second, valid.second.second); + if (!result.first) { + return result; + } + if (valid.second.second >= center) { + result.first = false; + return result; + } + } + if (right) { + auto valid = right->is_valid(); + result.first &= valid.first; + result.second.first = std::min(result.second.first, valid.second.first); + result.second.second = std::min(result.second.second, valid.second.second); + if (!result.first) { + return result; + } + if (valid.second.first <= center) { + result.first = false; + return result; + } + } + if (!std::is_sorted(intervals.begin(), intervals.end(), IntervalStartCmp())) { + result.first = false; + } + return result; + } + +private: + interval_vector intervals; + std::unique_ptr left; + std::unique_ptr right; + Scalar center; +}; + +} // namespace mcap::internal diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/mcap.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/mcap.hpp new file mode 100644 index 0000000000..71f479cd83 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/mcap.hpp @@ -0,0 +1,4 @@ +#pragma once + +#include "reader.hpp" +#include "writer.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/read_job_queue.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/read_job_queue.hpp new file mode 100644 index 0000000000..7faf4468ad --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/read_job_queue.hpp @@ -0,0 +1,147 @@ +#pragma once + +#include "types.hpp" +#include +#include + +namespace mcap::internal { + +// Helper for writing compile-time exhaustive variant visitors. +template +inline constexpr bool always_false_v = false; + +/** + * @brief A job to read a specific message at offset `offset` from the decompressed chunk + * stored in `chunkReaderIndex`. A timestamp is provided to order this job relative to other jobs. + */ +struct ReadMessageJob { + Timestamp timestamp; + RecordOffset offset; + size_t chunkReaderIndex; +}; + +/** + * @brief A job to decompress the chunk starting at `chunkStartOffset`. The message indices + * starting directly after the chunk record and ending at `messageIndexEndOffset` will be used to + * find specific messages within the chunk. + */ +struct DecompressChunkJob { + Timestamp messageStartTime; + Timestamp messageEndTime; + ByteOffset chunkStartOffset; + ByteOffset messageIndexEndOffset; +}; + +/** + * @brief A union of jobs that an indexed MCAP reader executes. + */ +using ReadJob = std::variant; + +/** + * @brief A priority queue of jobs for an indexed MCAP reader to execute. + */ +struct ReadJobQueue { +private: + bool reverse_ = false; + std::vector heap_; + + /** + * @brief return the timestamp key that should be used to compare jobs. + */ + static Timestamp TimeComparisonKey(const ReadJob& job, bool reverse) { + Timestamp result = 0; + std::visit( + [&](auto&& arg) { + using T = std::decay_t; + if constexpr (std::is_same_v) { + result = arg.timestamp; + } else if constexpr (std::is_same_v) { + if (reverse) { + result = arg.messageEndTime; + } else { + result = arg.messageStartTime; + } + } else { + static_assert(always_false_v, "non-exhaustive visitor!"); + } + }, + job); + return result; + } + static RecordOffset PositionComparisonKey(const ReadJob& job, bool reverse) { + RecordOffset result; + std::visit( + [&](auto&& arg) { + using T = std::decay_t; + if constexpr (std::is_same_v) { + result = arg.offset; + } else if constexpr (std::is_same_v) { + if (reverse) { + result.offset = arg.messageIndexEndOffset; + } else { + result.offset = arg.chunkStartOffset; + } + } else { + static_assert(always_false_v, "non-exhaustive visitor!"); + } + }, + job); + return result; + } + + static bool CompareForward(const ReadJob& a, const ReadJob& b) { + auto aTimestamp = TimeComparisonKey(a, false); + auto bTimestamp = TimeComparisonKey(b, false); + if (aTimestamp == bTimestamp) { + return PositionComparisonKey(a, false) > PositionComparisonKey(b, false); + } + return aTimestamp > bTimestamp; + } + + static bool CompareReverse(const ReadJob& a, const ReadJob& b) { + auto aTimestamp = TimeComparisonKey(a, true); + auto bTimestamp = TimeComparisonKey(b, true); + if (aTimestamp == bTimestamp) { + return PositionComparisonKey(a, true) < PositionComparisonKey(b, true); + } + return aTimestamp < bTimestamp; + } + +public: + explicit ReadJobQueue(bool reverse) + : reverse_(reverse) {} + void push(DecompressChunkJob&& decompressChunkJob) { + heap_.emplace_back(std::move(decompressChunkJob)); + if (!reverse_) { + std::push_heap(heap_.begin(), heap_.end(), CompareForward); + } else { + std::push_heap(heap_.begin(), heap_.end(), CompareReverse); + } + } + + void push(ReadMessageJob&& readMessageJob) { + heap_.emplace_back(std::move(readMessageJob)); + if (!reverse_) { + std::push_heap(heap_.begin(), heap_.end(), CompareForward); + } else { + std::push_heap(heap_.begin(), heap_.end(), CompareReverse); + } + } + + ReadJob pop() { + if (!reverse_) { + std::pop_heap(heap_.begin(), heap_.end(), CompareForward); + } else { + std::pop_heap(heap_.begin(), heap_.end(), CompareReverse); + } + auto popped = heap_.back(); + heap_.pop_back(); + return popped; + } + + size_t len() const { + return heap_.size(); + } +}; + +} // namespace mcap::internal diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/reader.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/reader.hpp new file mode 100644 index 0000000000..38eae7f347 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/reader.hpp @@ -0,0 +1,743 @@ +#pragma once + +#include "intervaltree.hpp" +#include "read_job_queue.hpp" +#include "types.hpp" +#include "visibility.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mcap { + +enum struct ReadSummaryMethod { + /** + * @brief Parse the Summary section to produce seeking indexes and summary + * statistics. If the Summary section is not present or corrupt, a failure + * Status is returned and the seeking indexes and summary statistics are not + * populated. + */ + NoFallbackScan, + /** + * @brief If the Summary section is missing or incomplete, allow falling back + * to reading the file sequentially to produce seeking indexes and summary + * statistics. + */ + AllowFallbackScan, + /** + * @brief Read the file sequentially from Header to DataEnd to produce seeking + * indexes and summary statistics. + */ + ForceScan, +}; + +/** + * @brief An abstract interface for reading MCAP data. + */ +struct MCAP_PUBLIC IReadable { + virtual ~IReadable() = default; + + /** + * @brief Returns the size of the file in bytes. + * + * @return uint64_t The total number of bytes in the MCAP file. + */ + virtual uint64_t size() const = 0; + /** + * @brief This method is called by MCAP reader classes when they need to read + * a portion of the file. + * + * @param output A pointer to a pointer to the buffer to write to. This method + * is expected to either maintain an internal buffer, read data into it, and + * update this pointer to point at the internal buffer, or update this + * pointer to point directly at the source data if possible. The pointer and + * data must remain valid and unmodified until the next call to read(). + * @param offset The offset in bytes from the beginning of the file to read. + * @param size The number of bytes to read. + * @return uint64_t Number of bytes actually read. This may be less than the + * requested size if the end of the file is reached. The output pointer must + * be readable from `output` to `output + size`. If the read fails, this + * method should return 0. + */ + virtual uint64_t read(std::byte** output, uint64_t offset, uint64_t size) = 0; +}; + +/** + * @brief IReadable implementation wrapping a FILE* pointer created by fopen() + * and a read buffer. + */ +class MCAP_PUBLIC FileReader final : public IReadable { +public: + FileReader(std::FILE* file); + + uint64_t size() const override; + uint64_t read(std::byte** output, uint64_t offset, uint64_t size) override; + +private: + // Numeric type returned by the tell/seek operations. Necessary because long on Windows is 32 + // bits so the standard C library interfaces don't work for files larger than 2GiB. +#if defined _WIN32 || defined __CYGWIN__ + typedef __int64 offset_type; +#else + typedef long offset_type; +#endif + + static_assert((offset_type)(uint64_t)std::numeric_limits::max() == + std::numeric_limits::max(), + "offset_type should fit in uint64_t"); + + std::FILE* file_; + std::vector buffer_; + uint64_t size_; + uint64_t position_; +}; + +/** + * @brief IReadable implementation wrapping a std::ifstream input file stream. + */ +class MCAP_PUBLIC FileStreamReader final : public IReadable { +public: + FileStreamReader(std::ifstream& stream); + + uint64_t size() const override; + uint64_t read(std::byte** output, uint64_t offset, uint64_t size) override; + +private: + std::ifstream& stream_; + std::vector buffer_; + uint64_t size_; + uint64_t position_; +}; + +/** + * @brief An abstract interface for compressed readers. + */ +class MCAP_PUBLIC ICompressedReader : public IReadable { +public: + virtual ~ICompressedReader() override = default; + + /** + * @brief Reset the reader state, clearing any internal buffers and state, and + * initialize with new compressed data. + * + * @param data Compressed data to read from. + * @param size Size of the compressed data in bytes. + * @param uncompressedSize Size of the data in bytes after decompression. A + * buffer of this size will be allocated for the uncompressed data. + */ + virtual void reset(const std::byte* data, uint64_t size, uint64_t uncompressedSize) = 0; + /** + * @brief Report the current status of decompression. A StatusCode other than + * `StatusCode::Success` after `reset()` is called indicates the decompression + * was not successful and the reader is in an invalid state. + */ + virtual Status status() const = 0; +}; + +/** + * @brief A "null" compressed reader that directly passes through uncompressed + * data. No internal buffers are allocated. + */ +class MCAP_PUBLIC BufferReader final : public ICompressedReader { +public: + void reset(const std::byte* data, uint64_t size, uint64_t uncompressedSize) override; + uint64_t read(std::byte** output, uint64_t offset, uint64_t size) override; + uint64_t size() const override; + Status status() const override; + + BufferReader() = default; + BufferReader(const BufferReader&) = delete; + BufferReader& operator=(const BufferReader&) = delete; + BufferReader(BufferReader&&) = delete; + BufferReader& operator=(BufferReader&&) = delete; + +private: + const std::byte* data_; + uint64_t size_; +}; + +#ifndef MCAP_COMPRESSION_NO_ZSTD +/** + * @brief ICompressedReader implementation that decompresses Zstandard + * (https://facebook.github.io/zstd/) data. + */ +class MCAP_PUBLIC ZStdReader final : public ICompressedReader { +public: + void reset(const std::byte* data, uint64_t size, uint64_t uncompressedSize) override; + uint64_t read(std::byte** output, uint64_t offset, uint64_t size) override; + uint64_t size() const override; + Status status() const override; + + /** + * @brief Decompresses an entire Zstd-compressed chunk into `output`. + * + * @param data The Zstd-compressed input chunk. + * @param compressedSize The size of the Zstd-compressed input. + * @param uncompressedSize The size of the data once uncompressed. + * @param output The output vector. This will be resized to `uncompressedSize` to fit the data, + * or 0 if the decompression encountered an error. + * @return Status + */ + static Status DecompressAll(const std::byte* data, uint64_t compressedSize, + uint64_t uncompressedSize, ByteArray* output); + ZStdReader() = default; + ZStdReader(const ZStdReader&) = delete; + ZStdReader& operator=(const ZStdReader&) = delete; + ZStdReader(ZStdReader&&) = delete; + ZStdReader& operator=(ZStdReader&&) = delete; + +private: + Status status_; + ByteArray uncompressedData_; +}; +#endif + +#ifndef MCAP_COMPRESSION_NO_LZ4 +/** + * @brief ICompressedReader implementation that decompresses LZ4 + * (https://lz4.github.io/lz4/) data. + */ +class MCAP_PUBLIC LZ4Reader final : public ICompressedReader { +public: + void reset(const std::byte* data, uint64_t size, uint64_t uncompressedSize) override; + uint64_t read(std::byte** output, uint64_t offset, uint64_t size) override; + uint64_t size() const override; + Status status() const override; + + /** + * @brief Decompresses an entire LZ4-encoded chunk into `output`. + * + * @param data The LZ4-compressed input chunk. + * @param size The size of the LZ4-compressed input. + * @param uncompressedSize The size of the data once uncompressed. + * @param output The output vector. This will be resized to `uncompressedSize` to fit the data, + * or 0 if the decompression encountered an error. + * @return Status + */ + Status decompressAll(const std::byte* data, uint64_t size, uint64_t uncompressedSize, + ByteArray* output); + LZ4Reader(); + LZ4Reader(const LZ4Reader&) = delete; + LZ4Reader& operator=(const LZ4Reader&) = delete; + LZ4Reader(LZ4Reader&&) = delete; + LZ4Reader& operator=(LZ4Reader&&) = delete; + ~LZ4Reader() override; + +private: + void* decompressionContext_ = nullptr; // LZ4F_dctx* + Status status_; + const std::byte* compressedData_; + ByteArray uncompressedData_; + uint64_t compressedSize_; + uint64_t uncompressedSize_; +}; +#endif + +struct LinearMessageView; + +/** + * @brief Options for reading messages out of an MCAP file. + */ +struct MCAP_PUBLIC ReadMessageOptions { +public: + /** + * @brief Only messages with log timestamps greater or equal to startTime will be included. + */ + Timestamp startTime = 0; + /** + * @brief Only messages with log timestamps less than endTime will be included. + */ + Timestamp endTime = MaxTime; + /** + * @brief If provided, `topicFilter` is called on all topics found in the MCAP file. If + * `topicFilter` returns true for a given channel, messages from that channel will be included. + * if not provided, messages from all channels are provided. + */ + std::function topicFilter; + enum struct ReadOrder { FileOrder, LogTimeOrder, ReverseLogTimeOrder }; + /** + * @brief Set the expected order that messages should be returned in. + * if readOrder == FileOrder, messages will be returned in the order they appear in the MCAP file. + * if readOrder == LogTimeOrder, messages will be returned in ascending log time order. + * if readOrder == ReverseLogTimeOrder, messages will be returned in descending log time order. + */ + ReadOrder readOrder = ReadOrder::FileOrder; + + ReadMessageOptions(Timestamp start, Timestamp end) + : startTime(start) + , endTime(end) {} + + ReadMessageOptions() = default; + + /** + * @brief validate the configuration. + */ + Status validate() const; +}; + +/** + * @brief Provides a read interface to an MCAP file. + */ +class MCAP_PUBLIC McapReader final { +public: + ~McapReader(); + + /** + * @brief Opens an MCAP file for reading from an already constructed IReadable + * implementation. + * + * @param reader An implementation of the IReader interface that provides raw + * MCAP data. + * @return Status StatusCode::Success on success. If a non-success Status is + * returned, the data source is not considered open and McapReader is not + * usable until `open()` is called and a success response is returned. + */ + Status open(IReadable& reader); + /** + * @brief Opens an MCAP file for reading from a given filename. + * + * @param filename Filename to open. + * @return Status StatusCode::Success on success. If a non-success Status is + * returned, the data source is not considered open and McapReader is not + * usable until `open()` is called and a success response is returned. + */ + Status open(std::string_view filename); + /** + * @brief Opens an MCAP file for reading from a std::ifstream input file + * stream. + * + * @param stream Input file stream to read MCAP data from. + * @return Status StatusCode::Success on success. If a non-success Status is + * returned, the file is not considered open and McapReader is not usable + * until `open()` is called and a success response is returned. + */ + Status open(std::ifstream& stream); + + /** + * @brief Closes the MCAP file, clearing any internal data structures and + * state and dropping the data source reference. + * + */ + void close(); + + /** + * @brief Read and parse the Summary section at the end of the MCAP file, if + * available. This will populate internal indexes to allow for efficient + * summarization and random access. This method will automatically be called + * upon requesting summary data or first seek if Summary section parsing is + * allowed by the configuration options. + */ + Status readSummary( + ReadSummaryMethod method, const ProblemCallback& onProblem = [](const Status&) {}); + + /** + * @brief Returns an iterable view with `begin()` and `end()` methods for + * iterating Messages in the MCAP file. If a non-zero `startTime` is provided, + * this will first parse the Summary section (by calling `readSummary()`) if + * allowed by the configuration options and it has not been parsed yet. + * + * @param startTime Optional start time in nanoseconds. Messages before this + * time will not be returned. + * @param endTime Optional end time in nanoseconds. Messages equal to or after + * this time will not be returned. + */ + LinearMessageView readMessages(Timestamp startTime = 0, Timestamp endTime = MaxTime); + /** + * @brief Returns an iterable view with `begin()` and `end()` methods for + * iterating Messages in the MCAP file. If a non-zero `startTime` is provided, + * this will first parse the Summary section (by calling `readSummary()`) if + * allowed by the configuration options and it has not been parsed yet. + * + * @param onProblem A callback that will be called when a parsing error + * occurs. Problems can either be recoverable, indicating some data could + * not be read, or non-recoverable, stopping the iteration. + * @param startTime Optional start time in nanoseconds. Messages before this + * time will not be returned. + * @param endTime Optional end time in nanoseconds. Messages equal to or after + * this time will not be returned. + */ + LinearMessageView readMessages(const ProblemCallback& onProblem, Timestamp startTime = 0, + Timestamp endTime = MaxTime); + + /** + * @brief Returns an iterable view with `begin()` and `end()` methods for + * iterating Messages in the MCAP file. + * Uses the options from `options` to select the messages that are yielded. + */ + LinearMessageView readMessages(const ProblemCallback& onProblem, + const ReadMessageOptions& options); + + /** + * @brief Returns starting and ending byte offsets that must be read to + * iterate all messages in the given time range. If `readSummary()` has been + * successfully called and the recording contains Chunk records, this range + * will be narrowed to Chunk records that contain messages in the given time + * range. Otherwise, this range will be the entire Data section if the Data + * End record has been found or the entire file otherwise. + * + * This method is automatically used by `readMessages()`, and only needs to be + * called directly if the caller is manually constructing an iterator. + * + * @param startTime Start time in nanoseconds. + * @param endTime Optional end time in nanoseconds. + * @return Start and end byte offsets. + */ + std::pair byteRange(Timestamp startTime, + Timestamp endTime = MaxTime) const; + + /** + * @brief Returns a pointer to the IReadable data source backing this reader. + * Will return nullptr if the reader is not open. + */ + IReadable* dataSource(); + + /** + * @brief Returns the parsed Header record, if it has been encountered. + */ + const std::optional
& header() const; + /** + * @brief Returns the parsed Footer record, if it has been encountered. + */ + const std::optional