From 6c24579da002fb050231a9486d2b008cc0d7cada Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 29 May 2026 21:01:14 +0800 Subject: [PATCH 01/15] go2 dds research --- data/.lfs/go2dds_data1.tar.gz | 3 + dimos/mapping/research/go2/go2_cdr.py | 180 ++++++++++++++++ dimos/mapping/research/go2/mcap_to_rerun.py | 217 ++++++++++++++++++++ 3 files changed, 400 insertions(+) create mode 100644 data/.lfs/go2dds_data1.tar.gz create mode 100644 dimos/mapping/research/go2/go2_cdr.py create mode 100644 dimos/mapping/research/go2/mcap_to_rerun.py diff --git a/data/.lfs/go2dds_data1.tar.gz b/data/.lfs/go2dds_data1.tar.gz new file mode 100644 index 0000000000..318feb9dbb --- /dev/null +++ b/data/.lfs/go2dds_data1.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:59b27bc170b2c6f95c2b362196d5b1c5b9a03d730ac9026df9246c658c4e5779 +size 178850722 diff --git a/dimos/mapping/research/go2/go2_cdr.py b/dimos/mapping/research/go2/go2_cdr.py new file mode 100644 index 0000000000..8e8487ff78 --- /dev/null +++ b/dimos/mapping/research/go2/go2_cdr.py @@ -0,0 +1,180 @@ +# 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. + +"""Minimal little-endian CDR (XCDR1) decoders for the Go2 MCAP channels. + +Hand-rolled so the converter needs no ROS/IDL deps — just the field layouts we +recorded (see go2-station/reference/harvest/idl + 20260529-07/08). Alignment is +body-relative (the 4-byte encapsulation header is skipped first). +""" + +import struct + +import numpy as np + + +class Cur: + def __init__(self, b): + self.b = b + self.p = 4 # skip 4-byte CDR encapsulation header + + def _al(self, n): + m = (self.p - 4) % n + if m: + self.p += n - m + + def u8(self): + v = self.b[self.p] + self.p += 1 + return v + + def u16(self): + self._al(2) + v = struct.unpack_from("= 32 else np.empty(0, _PC_DT) + return {"stamp_ns": stamp, "width": w, "point_step": ps, "arr": arr} + + +def decode_imu(data): + c = Cur(data) + stamp = _stamp_ns(c) + c.s() + o = c.f64n(4) # orientation x,y,z,w + c.f64n(9) # orientation_covariance + av = c.f64n(3) # angular_velocity + c.f64n(9) + la = c.f64n(3) # linear_acceleration + return {"stamp_ns": stamp, "orientation": o, "ang_vel": av, "lin_acc": la} + + +def decode_odometry(data): + c = Cur(data) + stamp = _stamp_ns(c) + c.s() + c.s() # header.stamp, frame_id, child_frame_id + pos = c.f64n(3) # pose.pose.position + quat = c.f64n(4) # pose.pose.orientation x,y,z,w + return {"stamp_ns": stamp, "position": pos, "orientation": quat} + + +def decode_sportmode(data): + c = Cur(data) + stamp = _stamp_ns(c) # TimeSpec + c.u32() # error_code + c.f32n(4) + c.f32n(3) + c.f32n(3) + c.f32n(3) + c.u8() # imu_state + temperature + mode = c.u8() + c.f32() + c.u8() + c.f32() # mode, progress, gait_type, foot_raise_height + pos = c.f32n(3) + body_h = c.f32() + vel = c.f32n(3) + yaw = c.f32() + return { + "stamp_ns": stamp, + "mode": mode, + "position": pos, + "body_height": body_h, + "velocity": vel, + "yaw_speed": yaw, + } + + +def decode_compressed_image(data): + c = Cur(data) + stamp = _stamp_ns(c) + c.s() # header.stamp, frame_id + fmt = c.s() + n = c.u32() + blob = bytes(data[c.p : c.p + n]) + return {"stamp_ns": stamp, "format": fmt, "data": blob} diff --git a/dimos/mapping/research/go2/mcap_to_rerun.py b/dimos/mapping/research/go2/mcap_to_rerun.py new file mode 100644 index 0000000000..fc1bab72db --- /dev/null +++ b/dimos/mapping/research/go2/mcap_to_rerun.py @@ -0,0 +1,217 @@ +#!/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. + +"""Convert a go2-station MCAP recording into a Rerun .rrd. + +Usage: + uv run --with mcap --with rerun-sdk --with numpy --with pillow \ + python scripts/mcap_to_rerun.py [out.rrd] + +Logs, on the **onboard `publish_time`** timeline: + rt/utlidar/cloud → Points3D (xyz, intensity-shaded) under world/base/lidar + rt/utlidar/robot_odom → Transform3D world/base (places the cloud in world frame) + rt/utlidar/imu → accel/gyro scalars + rt/sportmodestate → mode / body_height / vel / yaw scalars + rt/frontvideo → camera image (JPEG, if the recording has it) + latency/ → (log_time - publish_time) ms, for channels with an onboard stamp +Static base_link→lidar extrinsic from 20260529-06. +""" + +import json +import math +import sys + +import go2_cdr as cdr +from mcap.reader import make_reader +import numpy as np +import rerun as rr + +# base_link → lidar extrinsic (20260529-06): [x,y,z, roll,pitch,yaw] +BASE_LIDAR_T = [0.28216, 0.0, -0.02467] +BASE_LIDAR_RPY = [0.0, 2.88, 0.0] + + +def quat_rpy(r, p, y): + cr, sr = math.cos(r / 2), math.sin(r / 2) + cp, sp = math.cos(p / 2), math.sin(p / 2) + cy, sy = math.cos(y / 2), math.sin(y / 2) + return [ + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy, + cr * cp * cy + sr * sp * sy, + ] + + +def scalar(v): + return rr.Scalars(v) if hasattr(rr, "Scalars") else rr.Scalar(v) + + +def enc_image(jpeg): + if hasattr(rr, "EncodedImage"): + return rr.EncodedImage(contents=jpeg, media_type="image/jpeg") + import io + + from PIL import Image + + return rr.Image(np.asarray(Image.open(io.BytesIO(jpeg)).convert("RGB"))) + + +def set_t(ns): + # Rerun's time API changed across versions: old set_time_nanos/_seconds, + # new set_time(timeline, timestamp=epoch_seconds). + if hasattr(rr, "set_time_nanos"): + rr.set_time_nanos("onboard", int(ns)) + elif hasattr(rr, "set_time"): + try: + rr.set_time("onboard", timestamp=ns / 1e9) + except TypeError: + rr.set_time("onboard", duration=ns / 1e9) + else: + rr.set_time_seconds("onboard", ns / 1e9) + + +def main(): + inp = sys.argv[1] + out = sys.argv[2] if len(sys.argv) > 2 else inp.rsplit(".", 1)[0] + ".rrd" + + rr.init("go2-station") + # Embedded blueprint: group multi-axis scalars into single plots (origin = + # the parent path → all child series in one view) + 3D + camera. Avoids the + # per-leaf auto-layout (the "only see accel x,y" / stale-view confusion). + try: + import rerun.blueprint as rrb + + bp = rrb.Blueprint( + rrb.Horizontal( + rrb.Spatial3DView(origin="/world", name="lidar + pose"), + rrb.Vertical( + rrb.Spatial2DView(origin="/camera", name="camera"), + rrb.Grid( + rrb.TimeSeriesView(origin="/imu/accel", name="accel"), + rrb.TimeSeriesView(origin="/imu/gyro", name="gyro"), + rrb.TimeSeriesView(origin="/state", name="state"), + rrb.TimeSeriesView(origin="/seen", name="telemetry"), + rrb.TimeSeriesView(origin="/cmd", name="cmd vel"), + rrb.TimeSeriesView(origin="/latency", name="latency (ms)"), + ), + row_shares=[1, 2], + ), + column_shares=[2, 1], + ), + collapse_panels=True, + ) + rr.save(out, default_blueprint=bp) + except Exception: + rr.save(out) # older rerun without blueprint API + + # static lidar mount under the (moving) base. + rr.log( + "world/base/lidar", + rr.Transform3D( + translation=BASE_LIDAR_T, rotation=rr.Quaternion(xyzw=quat_rpy(*BASE_LIDAR_RPY)) + ), + static=True, + ) + + counts = {} + with open(inp, "rb") as f: + for _sch, ch, msg in make_reader(f).iter_messages(): + t = ch.topic + counts[t] = counts.get(t, 0) + 1 + set_t(msg.publish_time) + # latency (receive - onboard), for channels carrying an onboard stamp + if msg.log_time != msg.publish_time and t in ( + "rt/utlidar/cloud", + "rt/utlidar/imu", + "rt/utlidar/robot_odom", + "rt/sportmodestate", + ): + short = t.rsplit("/", 1)[-1] + rr.log(f"latency/{short}", scalar((msg.log_time - msg.publish_time) / 1e6)) + try: + if t == "rt/utlidar/cloud": + a = cdr.decode_pointcloud2(msg.data)["arr"] + if len(a): + xyz = np.stack([a["x"], a["y"], a["z"]], axis=1).astype(np.float32) + good = np.isfinite(xyz).all(axis=1) + xyz = xyz[good] + inten = a["intensity"][good] + n = np.clip(inten / (inten.max() + 1e-6), 0, 1) if len(inten) else inten + col = ( + (np.stack([n, n, n], axis=1) * 255).astype(np.uint8) if len(n) else None + ) + rr.log("world/base/lidar/points", rr.Points3D(xyz, colors=col, radii=0.01)) + elif t == "rt/utlidar/robot_odom": + o = cdr.decode_odometry(msg.data) + rr.log( + "world/base", + rr.Transform3D( + translation=o["position"], rotation=rr.Quaternion(xyzw=o["orientation"]) + ), + ) + elif t == "rt/utlidar/imu": + m = cdr.decode_imu(msg.data) + for ax, v in zip("xyz", m["lin_acc"], strict=False): + rr.log(f"imu/accel/{ax}", scalar(v)) + for ax, v in zip("xyz", m["ang_vel"], strict=False): + rr.log(f"imu/gyro/{ax}", scalar(v)) + elif t == "rt/sportmodestate": + m = cdr.decode_sportmode(msg.data) + rr.log("state/mode", scalar(m["mode"])) + rr.log("state/body_height", scalar(m["body_height"])) + rr.log("state/yaw_speed", scalar(m["yaw_speed"])) + for ax, v in zip("xy", m["velocity"][:2], strict=False): + rr.log(f"state/vel/{ax}", scalar(v)) + elif t == "rt/frontvideo": + img = cdr.decode_compressed_image(msg.data) + rr.log("camera", enc_image(img["data"])) + elif t == "control_log": + # JSON operator-intent events (not CDR). + e = json.loads(msg.data) + et = e.get("type", "") + if et == "velocity_input": + rr.log("cmd/vel/lx", scalar(e.get("lx", 0.0))) + rr.log("cmd/vel/ly", scalar(e.get("ly", 0.0))) + rr.log("cmd/vel/az", scalar(e.get("az", 0.0))) + else: + extra = " ".join(f"{k}={v}" for k, v in e.items() if k != "type") + rr.log("control/events", rr.TextLog(f"{et} {extra}".strip())) + elif t == "telemetry": + # JSON telemetry snapshot (not CDR) — same object the browser sees. + m = json.loads(msg.data) + rr.log("seen/battery", scalar(m.get("battery", 0.0))) + rr.log("seen/current_a", scalar(m.get("current_a", 0.0))) + rr.log("seen/body_h", scalar(m.get("body_h", 0.0))) + rr.log("seen/yaw", scalar(m.get("yaw", 0.0))) + vel = m.get("vel", [0.0, 0.0]) or [0.0, 0.0] + rr.log("seen/vel/x", scalar(vel[0] if len(vel) > 0 else 0.0)) + rr.log("seen/vel/y", scalar(vel[1] if len(vel) > 1 else 0.0)) + rr.log("seen/lidar_hz", scalar(m.get("lidar_hz", 0.0))) + rr.log("seen/imu_hz", scalar(m.get("imu_hz", 0.0))) + rr.log("seen/mode", scalar(m.get("mode", 0))) + rr.log("seen/obstacle", scalar(1 if m.get("obstacle") else 0)) + rr.log("seen/lidar", scalar(1 if m.get("lidar") else 0)) + rr.log("seen/rage", scalar(1 if m.get("rage") else 0)) + except Exception as e: + # tolerate a bad message rather than aborting the whole file + pass + + print("channels:", {k: counts[k] for k in sorted(counts)}) + print("wrote", out) + + +if __name__ == "__main__": + main() From 0d6fc44bfe9c78a6e55501706830af6a6fef1f4c Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 29 May 2026 21:39:16 +0800 Subject: [PATCH 02/15] autoresaerch package for go2 dds --- data/.lfs/go2dds_data1.tar.gz | 4 +- dimos/mapping/research/go2/.gitignore | 36 + .../research/go2/autoresearch/flake.lock | 209 ++ .../research/go2/autoresearch/flake.nix | 41 + .../go2/autoresearch/lio-1/.gitignore | 36 + .../research/go2/autoresearch/lio-1/README.md | 80 + .../go2/autoresearch/lio-1/config/v2_imu.yaml | 52 + .../autoresearch/lio-1/human-debug/README.md | 11 + .../lio-1/human-debug/mcap_to_plnr1.py | 72 + .../autoresearch/lio-1/point_lio/.gitignore | 2 + .../lio-1/point_lio/CMakeLists.txt | 81 + .../go2/autoresearch/lio-1/point_lio/LICENSE | 339 +++ .../autoresearch/lio-1/point_lio/Log/.gitkeep | 0 .../point_lio/PCD/do_not_delete_this_file.txt | 0 .../lio-1/point_lio/config/v2_imu.yaml | 52 + .../include/FOV_Checker/FOV_Checker.cpp | 471 ++++ .../include/FOV_Checker/FOV_Checker.h | 32 + .../lio-1/point_lio/include/IKFoM/.gitignore | 32 + .../IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp | 390 ++++ .../IKFoM/IKFoM_toolkit/esekfom/util.hpp | 82 + .../IKFoM_toolkit/mtk/build_manifold.hpp | 248 ++ .../IKFoM_toolkit/mtk/src/SubManifold.hpp | 123 + .../IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp | 294 +++ .../IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp | 168 ++ .../IKFoM/IKFoM_toolkit/mtk/startIdx.hpp | 328 +++ .../IKFoM/IKFoM_toolkit/mtk/types/S2.hpp | 326 +++ .../IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp | 333 +++ .../IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp | 364 +++ .../IKFoM/IKFoM_toolkit/mtk/types/vect.hpp | 511 +++++ .../mtk/types/wrapped_cv_mat.hpp | 113 + .../lio-1/point_lio/include/IKFoM/LICENSE | 339 +++ .../lio-1/point_lio/include/IKFoM/README.md | 488 ++++ .../lio-1/point_lio/include/common_lib.h | 180 ++ .../include/eigen_conversions/eigen_msg.h | 2 + .../point_lio/include/geometry_msgs/Vector3.h | 2 + .../point_lio/include/ikd-Tree/README.md | 2 + .../point_lio/include/ikd-Tree/ikd_Tree.cpp | 1727 ++++++++++++++ .../point_lio/include/ikd-Tree/ikd_Tree.h | 344 +++ .../point_lio/include/nav_msgs/Odometry.h | 2 + .../lio-1/point_lio/include/nav_msgs/Path.h | 2 + .../lio-1/point_lio/include/noros/compat.hpp | 339 +++ .../include/pcl_conversions/pcl_conversions.h | 2 + .../lio-1/point_lio/include/ros/ros.h | 2 + .../lio-1/point_lio/include/sensor_msgs/Imu.h | 2 + .../include/sensor_msgs/PointCloud2.h | 2 + .../lio-1/point_lio/include/so3_math.h | 113 + .../include/tf/transform_broadcaster.h | 2 + .../include/tf/transform_datatypes.h | 2 + .../include/visualization_msgs/Marker.h | 2 + .../lio-1/point_lio/src/Estimator.cpp | 436 ++++ .../lio-1/point_lio/src/Estimator.h | 118 + .../lio-1/point_lio/src/IMU_Processing.hpp | 185 ++ .../lio-1/point_lio/src/laserMapping.cpp | 1602 +++++++++++++ .../lio-1/point_lio/src/parameters.cpp | 192 ++ .../lio-1/point_lio/src/parameters.h | 41 + .../lio-1/point_lio/src/preprocess.cpp | 856 +++++++ .../lio-1/point_lio/src/preprocess.h | 181 ++ .../go2/autoresearch/lio-1/prepare.py | 166 ++ .../go2/autoresearch/lio-1/program.md | 122 + .../research/go2/autoresearch/lio-1/setup.sh | 32 + .../research/go2/autoresearch/lio-1/train.py | 180 ++ .../go2/autoresearch/lio-1/traj_ds.tsv | 2003 +++++++++++++++++ .../go2/autoresearch/lio-1/visualize.py | 116 + pyproject.toml | 1 + 64 files changed, 14613 insertions(+), 2 deletions(-) create mode 100644 dimos/mapping/research/go2/.gitignore create mode 100644 dimos/mapping/research/go2/autoresearch/flake.lock create mode 100644 dimos/mapping/research/go2/autoresearch/flake.nix create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/.gitignore create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/README.md create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/config/v2_imu.yaml create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/human-debug/README.md create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/human-debug/mcap_to_plnr1.py create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/.gitignore create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/LICENSE create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/Log/.gitkeep create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/PCD/do_not_delete_this_file.txt create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/config/v2_imu.yaml create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.cpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/FOV_Checker/FOV_Checker.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/.gitignore create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/LICENSE create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/IKFoM/README.md create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/common_lib.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/eigen_conversions/eigen_msg.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/geometry_msgs/Vector3.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/README.md create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.cpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ikd-Tree/ikd_Tree.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Odometry.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Path.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/noros/compat.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/pcl_conversions/pcl_conversions.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ros/ros.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/Imu.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/PointCloud2.h create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/so3_math.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_broadcaster.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_datatypes.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/visualization_msgs/Marker.h create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.cpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.h create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/IMU_Processing.hpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/laserMapping.cpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.cpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.h create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.cpp create mode 100755 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/prepare.py create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/program.md create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/setup.sh create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/train.py create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/traj_ds.tsv create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/visualize.py diff --git a/data/.lfs/go2dds_data1.tar.gz b/data/.lfs/go2dds_data1.tar.gz index 318feb9dbb..f44d478ad5 100644 --- a/data/.lfs/go2dds_data1.tar.gz +++ b/data/.lfs/go2dds_data1.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:59b27bc170b2c6f95c2b362196d5b1c5b9a03d730ac9026df9246c658c4e5779 -size 178850722 +oid sha256:dfd237ce49cef1f96074ba02576b9a8094db6d6e0dc9ca24b63e5fdaa84acaf2 +size 224684671 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/flake.lock b/dimos/mapping/research/go2/autoresearch/flake.lock new file mode 100644 index 0000000000..9fdd3cc461 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/flake.lock @@ -0,0 +1,209 @@ +{ + "nodes": { + "diagon": { + "locked": { + "lastModified": 1763299369, + "narHash": "sha256-z/q22EqZfF79vZQh6K/yCmt8iqDvUSkIVTH+Omhv1VE=", + "owner": "petertrotman", + "repo": "nixpkgs", + "rev": "dff059e25eee7aa958c606aeb6b5879ae1c674f0", + "type": "github" + }, + "original": { + "owner": "petertrotman", + "ref": "Diagon", + "repo": "nixpkgs", + "type": "github" + } + }, + "dimos": { + "inputs": { + "diagon": "diagon", + "flake-utils": "flake-utils", + "lib": "lib", + "nixpkgs": "nixpkgs", + "xome": "xome" + }, + "locked": { + "lastModified": 1780059674, + "narHash": "sha256-hyjquuqyjOqU+W9n6qfnP9jMBtaKLSgK84RGyV9T6nQ=", + "ref": "refs/heads/feat/go2_dds_data", + "rev": "6c24579da002fb050231a9486d2b008cc0d7cada", + "shallow": true, + "type": "git", + "url": "file:../../../../.." + }, + "original": { + "rev": "6c24579da002fb050231a9486d2b008cc0d7cada", + "shallow": true, + "type": "git", + "url": "file:../../../../.." + } + }, + "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" + } + }, + "home-manager": { + "inputs": { + "nixpkgs": [ + "dimos", + "xome", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1753983724, + "narHash": "sha256-2vlAOJv4lBrE+P1uOGhZ1symyjXTRdn/mz0tZ6faQcg=", + "owner": "nix-community", + "repo": "home-manager", + "rev": "7035020a507ed616e2b20c61491ae3eaa8e5462c", + "type": "github" + }, + "original": { + "owner": "nix-community", + "repo": "home-manager", + "type": "github" + } + }, + "lib": { + "inputs": { + "flakeUtils": [ + "dimos", + "flake-utils" + ], + "libSource": "libSource" + }, + "locked": { + "lastModified": 1764022662, + "narHash": "sha256-vS3EeyELqCskh88JkUW/ce8A8b3m+iRPLPd4kDRTqPY=", + "owner": "jeff-hykin", + "repo": "quick-nix-toolkits", + "rev": "de1cc174579ecc7b655de5ba9618548d1b72306c", + "type": "github" + }, + "original": { + "owner": "jeff-hykin", + "repo": "quick-nix-toolkits", + "type": "github" + } + }, + "libSource": { + "locked": { + "lastModified": 1766884708, + "narHash": "sha256-x8nyRwtD0HMeYtX60xuIuZJbwwoI7/UKAdCiATnQNz0=", + "owner": "nix-community", + "repo": "nixpkgs.lib", + "rev": "15177f81ad356040b4460a676838154cbf7f6213", + "type": "github" + }, + "original": { + "owner": "nix-community", + "repo": "nixpkgs.lib", + "type": "github" + } + }, + "libSource_2": { + "locked": { + "lastModified": 1753579242, + "narHash": "sha256-zvaMGVn14/Zz8hnp4VWT9xVnhc8vuL3TStRqwk22biA=", + "owner": "divnix", + "repo": "nixpkgs.lib", + "rev": "0f36c44e01a6129be94e3ade315a5883f0228a6e", + "type": "github" + }, + "original": { + "owner": "divnix", + "repo": "nixpkgs.lib", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1778869304, + "narHash": "sha256-30sZNZoA1cqF5JNO9fVX+wgiQYjB7HJqqJ4ztCDeBZE=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "d233902339c02a9c334e7e593de68855ad26c4cb", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos": "dimos", + "flake-utils": [ + "dimos", + "flake-utils" + ], + "nixpkgs": [ + "dimos", + "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" + } + }, + "xome": { + "inputs": { + "flake-utils": [ + "dimos", + "flake-utils" + ], + "home-manager": "home-manager", + "libSource": "libSource_2", + "nixpkgs": [ + "dimos", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1765466883, + "narHash": "sha256-c4YxXoS6U9BFcxP4TWZirwycaxT2oFyPMeyVp5vrME8=", + "owner": "jeff-hykin", + "repo": "xome", + "rev": "1f3507c4985e05177bd1a5b57d2862e30bb5da9b", + "type": "github" + }, + "original": { + "owner": "jeff-hykin", + "repo": "xome", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/mapping/research/go2/autoresearch/flake.nix b/dimos/mapping/research/go2/autoresearch/flake.nix new file mode 100644 index 0000000000..0ea34edceb --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/flake.nix @@ -0,0 +1,41 @@ +{ + description = "LIO autoresearch: Point-LIO build deps (pcl, yaml-cpp, boost) layered onto the dimos dev shell"; + + inputs = { + # The root dimos flake, pinned to a committed rev. The pin is load-bearing: + # this repo's worktree is ~100 G and usually dirty, so a bare git+file would + # copy the whole thing into the store. Pinning a rev exports from git objects + # instead — LFS files stay as pointers, untracked files are ignored, ~16 M + # closure. Bump this rev only if the root flake's dev shell changes. + dimos.url = "git+file:../../../../..?rev=6c24579da002fb050231a9486d2b008cc0d7cada&shallow=1"; + nixpkgs.follows = "dimos/nixpkgs"; + flake-utils.follows = "dimos/flake-utils"; + }; + + outputs = { self, dimos, nixpkgs, flake-utils, ... }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = import nixpkgs { inherit system; }; + in { + # The full dimos dev shell + the three native deps Point-LIO needs that + # the root flake doesn't already carry (eigen and cmake come from root). + devShells.default = + dimos.devShells.${system}.default.overrideAttrs (old: { + buildInputs = (old.buildInputs or []) ++ [ + pkgs.pcl + pkgs.yaml-cpp + pkgs.boost + ]; + # 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. Libs resolve via + # CMAKE_PREFIX_PATH (pcl is in buildInputs). + shellHook = (old.shellHook or "") + '' + 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/.gitignore b/dimos/mapping/research/go2/autoresearch/lio-1/.gitignore new file mode 100644 index 0000000000..8bb5cb9a39 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/.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/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/README.md new file mode 100644 index 0000000000..768af846b2 --- /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): + +- **`prepare.py`** — fixed data paths, the Point-LIO substrate location, and the + evaluation (`evaluate`, the ground-truth metric). Not modified. +- **`train.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 train.py +``` + +If those work, you're ready for autonomous mode: point your agent at +`program.md`. + +## Project structure + +``` +prepare.py — data paths (via get_data), substrate location, evaluation (do not modify) +train.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 +`go2-185959.bin` (LIO input) + `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/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/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..b734038dde --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt @@ -0,0 +1,81 @@ +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.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) + +# 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}) + +add_executable(pointlio_mapping + src/laserMapping.cpp + include/ikd-Tree/ikd_Tree.cpp + src/parameters.cpp + src/preprocess.cpp + src/Estimator.cpp +) +target_link_libraries(pointlio_mapping + ${PCL_COMMON_LIB} ${PCL_IO_LIB} ${PCL_FILTERS_LIB} ${PCL_KDTREE_LIB} + Boost::filesystem + yaml-cpp +) +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/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/nav_msgs/Odometry.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Odometry.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Odometry.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Path.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Path.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/nav_msgs/Path.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/noros/compat.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/noros/compat.hpp new file mode 100644 index 0000000000..da08c3aae5 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/noros/compat.hpp @@ -0,0 +1,339 @@ +// noros/compat.hpp — minimal stand-ins for ros::, sensor_msgs::, +// nav_msgs::, geometry_msgs::, visualization_msgs::, std_msgs::, tf:: +// and pcl_conversions:: just enough surface that point_lio_unilidar's +// .cpp/.hpp compiles without ROS installed. +// +// Design notes: +// +// * Almost everything is a POD with public data members exactly named +// like the ROS message field (header.stamp, linear_acceleration.x, …). +// The point-LIO source pokes those fields directly. +// * ros::Publisher::publish(...) is a no-op. The point-LIO outputs we +// actually want (the registered cloud → PCD file, the per-frame odom +// → ASCII log) are written from the offline main loop, not via these +// publishers. +// * ros::Subscriber is a tag struct. Our offline main bypasses the +// subscriber→callback path entirely and pushes data into +// lidar_buffer/time_buffer/imu_deque directly. So nh.subscribe(...) +// returns an empty Subscriber and that's fine. +// * pcl::fromROSMsg / pcl::toROSMsg are stubs that do nothing — they +// only need to compile, since at runtime we never call them (the +// ingestion path that would invoke them, standard_pcl_cbk → +// Preprocess::unilidar_handler, is bypassed by the offline main). +// * ros::ok() flips false when our signal handler fires. +// * Headers under include/ros/ros.h, include/sensor_msgs/Imu.h, etc. +// each just #include this file so the existing source's #include +// lines work unchanged. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// In real ROS, transitively pulls +// these in. We don't, so make sure the upstream code sees PCL types +// through any of our shims. +#include +#include + +namespace ros { + +inline std::atomic& _ok_flag() { + static std::atomic v{true}; + return v; +} + +struct Time { + int32_t sec = 0; + uint32_t nsec = 0; + Time() = default; + explicit Time(double s) { fromSec_(s); } + static Time fromSec(double s) { Time t; t.fromSec_(s); return t; } + void fromSec_(double s) { + sec = (int32_t)std::floor(s); + nsec = (uint32_t)std::llround((s - sec) * 1e9); + if (nsec >= 1000000000u) { sec++; nsec -= 1000000000u; } + } + double toSec() const { return (double)sec + (double)nsec * 1e-9; } +}; + +struct Duration { + double sec = 0.0; + Duration() = default; + Duration(double s) : sec(s) {} + double toSec() const { return sec; } +}; + +struct Subscriber {}; +struct Publisher { + template + void publish(const M&) const {} +}; + +struct NodeHandle { + NodeHandle() = default; + explicit NodeHandle(const std::string&) {} + // param(name, var, default) — no-op; in the offline binary the + // values are pre-loaded via readParametersYaml() before the original + // readParameters(nh) wrapper is called, so nothing flows through nh. + template + bool param(const std::string&, T&, const T&) const { return false; } + template + bool param(const std::string&, T&, const T&) { return false; } + template + bool getParam(const std::string&, T&) const { return false; } + // subscribe / advertise return inert tags. The offline main bypasses + // the callback layer and pushes data directly into the global + // buffers. + template + Subscriber subscribe(const std::string&, uint32_t, F) const { return {}; } + template + Publisher advertise(const std::string&, uint32_t) const { return {}; } +}; + +template struct MessageEvent {}; + +inline void init(int&, char**, const std::string&) {} +inline bool ok() { return _ok_flag().load(); } +inline void shutdown() { _ok_flag().store(false); } +inline void spinOnce() {} + +struct Rate { + std::chrono::steady_clock::time_point next; + std::chrono::nanoseconds period; + explicit Rate(double hz) + : next(std::chrono::steady_clock::now()), + period(std::chrono::nanoseconds((long long)(1e9 / hz))) { + next += period; + } + void sleep() { + std::this_thread::sleep_until(next); + next += period; + } + void reset() { + next = std::chrono::steady_clock::now() + period; + } +}; + +namespace console { enum { Info, Warn, Error, Debug, Fatal }; } + +} // namespace ros + +// ROS_* macros → printf +#define ROS_INFO(fmt, ...) do { std::printf("[info] " fmt "\n", ##__VA_ARGS__); } while (0) +#define ROS_WARN(fmt, ...) do { std::printf("[warn] " fmt "\n", ##__VA_ARGS__); } while (0) +#define ROS_ERROR(fmt, ...) do { std::fprintf(stderr, "[error] " fmt "\n", ##__VA_ARGS__); } while (0) +#define ROS_FATAL(fmt, ...) do { std::fprintf(stderr, "[fatal] " fmt "\n", ##__VA_ARGS__); } while (0) +#define ROS_ASSERT(cond) do { if (!(cond)) { std::fprintf(stderr, "[assert] %s\n", #cond); std::abort(); } } while (0) +#define ROS_INFO_STREAM(x) do { std::cout << "[info] " << x << "\n"; } while (0) + + +namespace std_msgs { + +struct Header { + uint32_t seq = 0; + ros::Time stamp; + std::string frame_id; +}; + +} // namespace std_msgs + + +namespace geometry_msgs { + +struct Vector3 { double x = 0, y = 0, z = 0; }; +struct Point { double x = 0, y = 0, z = 0; }; +struct Quaternion { double x = 0, y = 0, z = 0, w = 1; }; + +struct Pose { + Point position; + Quaternion orientation; +}; + +struct PoseStamped { + std_msgs::Header header; + Pose pose; +}; + +struct Twist { + Vector3 linear; + Vector3 angular; +}; + +struct PoseWithCovariance { + Pose pose; + std::array covariance{}; +}; + +struct TwistWithCovariance { + Twist twist; + std::array covariance{}; +}; + +} // namespace geometry_msgs + + +namespace sensor_msgs { + +struct PointField { + static constexpr uint8_t INT8 = 1, UINT8 = 2, INT16 = 3, UINT16 = 4, + INT32 = 5, UINT32 = 6, FLOAT32 = 7, FLOAT64 = 8; + std::string name; + uint32_t offset = 0; + uint8_t datatype = 0; + uint32_t count = 1; +}; + +struct PointCloud2 { + std_msgs::Header header; + uint32_t height = 0; + uint32_t width = 0; + std::vector fields; + bool is_bigendian = false; + uint32_t point_step = 0; + uint32_t row_step = 0; + std::vector data; + bool is_dense = false; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; +}; + +struct Imu { + std_msgs::Header header; + geometry_msgs::Quaternion orientation; + std::array orientation_covariance{}; + geometry_msgs::Vector3 angular_velocity; + std::array angular_velocity_covariance{}; + geometry_msgs::Vector3 linear_acceleration; + std::array linear_acceleration_covariance{}; + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; +}; + +typedef Imu::ConstPtr ImuConstPtr; + +} // namespace sensor_msgs + + +namespace nav_msgs { + +struct Odometry { + std_msgs::Header header; + std::string child_frame_id; + geometry_msgs::PoseWithCovariance pose; + geometry_msgs::TwistWithCovariance twist; +}; + +struct Path { + std_msgs::Header header; + std::vector poses; +}; + +} // namespace nav_msgs + + +namespace visualization_msgs { + +struct Marker { + std_msgs::Header header; + std::string ns; + int32_t id = 0; + int32_t type = 0; + int32_t action = 0; + geometry_msgs::Pose pose; + geometry_msgs::Vector3 scale; + struct { float r = 0, g = 0, b = 0, a = 1; } color; + ros::Duration lifetime; + bool frame_locked = false; + std::vector points; + std::vector colors; + std::string text; + std::string mesh_resource; + bool mesh_use_embedded_materials = false; +}; + +} // namespace visualization_msgs + + +// Minimal tf:: shims. point-LIO uses these only to broadcast pose for +// rviz — we drop the broadcasts entirely (br.sendTransform is a no-op). +namespace tf { + +struct Vector3 { + double x_ = 0, y_ = 0, z_ = 0; + Vector3() = default; + Vector3(double x, double y, double z) : x_(x), y_(y), z_(z) {} + double x() const { return x_; } double y() const { return y_; } double z() const { return z_; } +}; + +struct Quaternion { + double x_ = 0, y_ = 0, z_ = 0, w_ = 1; + void setX(double v) { x_ = v; } void setY(double v) { y_ = v; } void setZ(double v) { z_ = v; } void setW(double v) { w_ = v; } + void setRPY(double r, double p, double y) { + double cr = std::cos(r * 0.5), sr = std::sin(r * 0.5); + double cp = std::cos(p * 0.5), sp = std::sin(p * 0.5); + double cy = std::cos(y * 0.5), sy = std::sin(y * 0.5); + w_ = cr*cp*cy + sr*sp*sy; + x_ = sr*cp*cy - cr*sp*sy; + y_ = cr*sp*cy + sr*cp*sy; + z_ = cr*cp*sy - sr*sp*cy; + } +}; + +struct Transform { + Vector3 origin_; + Quaternion rotation_; + void setOrigin(const Vector3& v) { origin_ = v; } + void setRotation(const Quaternion& q) { rotation_ = q; } +}; + +struct StampedTransform { + Transform tf_; + ros::Time stamp_; + std::string frame_id_, child_frame_id_; + StampedTransform() = default; + StampedTransform(const Transform& t, const ros::Time& s, + const std::string& f, const std::string& c) + : tf_(t), stamp_(s), frame_id_(f), child_frame_id_(c) {} +}; + +struct TransformBroadcaster { + void sendTransform(const StampedTransform&) const {} +}; + +inline geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double r, double p, double y) { + Quaternion q; q.setRPY(r, p, y); + geometry_msgs::Quaternion m; m.x = q.x_; m.y = q.y_; m.z = q.z_; m.w = q.w_; + return m; +} + +} // namespace tf + + +// pcl_conversions stubs. point-LIO only calls fromROSMsg in the lidar +// callback (which we bypass in the offline main) and toROSMsg in the +// publishers (which are no-ops). Both stubbed. +namespace pcl { + +template +inline void fromROSMsg(const sensor_msgs::PointCloud2&, PCT&) { + // unreachable at runtime when driving from the offline main; the + // caller (Preprocess::*_handler) is bypassed. +} + +template +inline void toROSMsg(const PCT&, sensor_msgs::PointCloud2&) { + // no-op; the corresponding publish() is also a no-op. +} + +} // namespace pcl diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/pcl_conversions/pcl_conversions.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/pcl_conversions/pcl_conversions.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/pcl_conversions/pcl_conversions.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ros/ros.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ros/ros.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/ros/ros.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/Imu.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/Imu.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/Imu.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/PointCloud2.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/PointCloud2.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/sensor_msgs/PointCloud2.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/so3_math.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/so3_math.h new file mode 100755 index 0000000000..7938a9473a --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/so3_math.h @@ -0,0 +1,113 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include + +// #include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) +{ + Eigen::Matrix skew_sym_mat; + skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; + return skew_sym_mat; +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else + { + return Eye3; + } +} + +/* Logrithm of a Rotation Matrix */ +template +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +template +Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) +{ + T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); + bool singular = sy < 1e-6; + T x, y, z; + if(!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +#endif diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_broadcaster.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_broadcaster.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_broadcaster.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_datatypes.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_datatypes.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/tf/transform_datatypes.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/visualization_msgs/Marker.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/visualization_msgs/Marker.h new file mode 100644 index 0000000000..e742a36413 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/visualization_msgs/Marker.h @@ -0,0 +1,2 @@ +#pragma once +#include "noros/compat.hpp" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.cpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.cpp new file mode 100755 index 0000000000..99467143fc --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.cpp @@ -0,0 +1,436 @@ +// #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> +#include "Estimator.h" + +PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); +std::vector time_seq; +PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); +std::vector pbody_list; +std::vector Nearest_Points; +KD_TREE ikdtree; +std::vector pointSearchSqDis(NUM_MATCH_POINTS); +bool point_selected_surf[100000] = {0}; +std::vector crossmat_list; +int effct_feat_num = 0; +int k; +int idx; +esekfom::esekf kf_input; +esekfom::esekf kf_output; +state_input state_in; +state_output state_out; +input_ikfom input_in; +V3D angvel_avr, acc_avr; + +V3D Lidar_T_wrt_IMU(Zero3d); +M3D Lidar_R_wrt_IMU(Eye3d); + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 S2; +typedef MTK::vect<1, double> vect1; +typedef MTK::vect<2, double> vect2; + +Eigen::Matrix process_noise_cov_input() +{ + Eigen::Matrix cov; + cov.setZero(); + cov.block<3, 3>(3, 3).diagonal() << gyr_cov_input, gyr_cov_input, gyr_cov_input; + cov.block<3, 3>(12, 12).diagonal() << acc_cov_input, acc_cov_input, acc_cov_input; + cov.block<3, 3>(15, 15).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov; + cov.block<3, 3>(18, 18).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov; + // MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + // MTK::setDiagonal(cov, &process_noise_input::ng, gyr_cov_input);// 0.03 + // MTK::setDiagonal(cov, &process_noise_input::na, acc_cov_input); // *dt 0.01 0.01 * dt * dt 0.05 + // MTK::setDiagonal(cov, &process_noise_input::nbg, b_gyr_cov); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + // MTK::setDiagonal(cov, &process_noise_input::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +Eigen::Matrix process_noise_cov_output() +{ + Eigen::Matrix cov; + cov.setZero(); + cov.block<3, 3>(12, 12).diagonal() << vel_cov, vel_cov, vel_cov; + cov.block<3, 3>(15, 15).diagonal() << gyr_cov_output, gyr_cov_output, gyr_cov_output; + cov.block<3, 3>(18, 18).diagonal() << acc_cov_output, acc_cov_output, acc_cov_output; + cov.block<3, 3>(24, 24).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov; + cov.block<3, 3>(27, 27).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov; + // MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + // MTK::setDiagonal(cov, &process_noise_output::vel, vel_cov);// 0.03 + // MTK::setDiagonal(cov, &process_noise_output::ng, gyr_cov_output); // *dt 0.01 0.01 * dt * dt 0.05 + // MTK::setDiagonal(cov, &process_noise_output::na, acc_cov_output); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + // MTK::setDiagonal(cov, &process_noise_output::nbg, b_gyr_cov); //0.001 0.05 0.0001/out 0.01 + // MTK::setDiagonal(cov, &process_noise_output::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +Eigen::Matrix get_f_input(state_input &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + vect3 a_inertial = s.rot.normalized() * (in.acc-s.ba); + for(int i = 0; i < 3; i++ ){ + res(i) = s.vel[i]; + res(i + 3) = omega[i]; + res(i + 12) = a_inertial[i] + s.gravity[i]; + } + return res; +} + +Eigen::Matrix get_f_output(state_output &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 a_inertial = s.rot.normalized() * s.acc; + for(int i = 0; i < 3; i++ ){ + res(i) = s.vel[i]; + res(i + 3) = s.omg[i]; + res(i + 12) = a_inertial[i] + s.gravity[i]; + } + return res; +} + +Eigen::Matrix df_dx_input(state_input &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + vect3 acc_; + in.acc.boxminus(acc_, s.ba); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix()*MTK::hat(acc_); + cov.template block<3, 3>(12, 18) = -s.rot.normalized().toRotationMatrix(); + // Eigen::Matrix vec = Eigen::Matrix::Zero(); + // Eigen::Matrix grav_matrix; + // s.S2_Mx(grav_matrix, vec, 21); + cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix; + cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); + return cov; +} + +// Eigen::Matrix df_dw_input(state_input &s, const input_ikfom &in) +// { +// Eigen::Matrix cov = Eigen::Matrix::Zero(); +// cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix(); +// cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); +// return cov; +// } + +Eigen::Matrix df_dx_output(state_output &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix()*MTK::hat(s.acc); + cov.template block<3, 3>(12, 18) = s.rot.normalized().toRotationMatrix(); + // Eigen::Matrix vec = Eigen::Matrix::Zero(); + // Eigen::Matrix grav_matrix; + // s.S2_Mx(grav_matrix, vec, 21); + cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix; + cov.template block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); + return cov; +} + +// Eigen::Matrix df_dw_output(state_output &s) +// { +// Eigen::Matrix cov = Eigen::Matrix::Zero(); +// cov.template block<3, 3>(12, 0) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(15, 3) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(18, 6) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(24, 9) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(27, 12) = Eigen::Matrix3d::Identity(); +// return cov; +// } + +vect3 SO3ToEuler(const SO3 &orient) +{ + Eigen::Matrix _ang; + Eigen::Vector4d q_data = orient.coeffs().transpose(); + //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; + double sqw = q_data[3]*q_data[3]; + double sqx = q_data[0]*q_data[0]; + double sqy = q_data[1]*q_data[1]; + double sqz = q_data[2]*q_data[2]; + double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor + double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; + + if (test > 0.49999*unit) { // singularity at north pole + + _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; + } + if (test < -0.49999*unit) { // singularity at south pole + _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; + } + + _ang << + std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), + std::asin (2*test/unit), + std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; +} + +void h_model_input(state_input &s, esekfom::dyn_share_modified &ekfom_data) +{ + bool match_in_map = false; + VF(4) pabcd; + pabcd.setZero(); + normvec->resize(time_seq[k]); + int effect_num_k = 0; + for (int j = 0; j < time_seq[k]; j++) + { + PointType &point_body_j = feats_down_body->points[idx+j+1]; + PointType &point_world_j = feats_down_world->points[idx+j+1]; + pointBodyToWorld(&point_body_j, &point_world_j); + V3D p_body = pbody_list[idx+j+1]; + V3D p_world; + p_world << point_world_j.x, point_world_j.y, point_world_j.z; + + { + auto &points_near = Nearest_Points[idx+j+1]; + + ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); //1.0); //, 3.0); // 2.236; + + if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) // 5) + { + point_selected_surf[idx+j+1] = false; + } + else + { + point_selected_surf[idx+j+1] = false; + if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid) + { + float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3); + + if (p_body.norm() > match_s * pd2 * pd2) + { + point_selected_surf[idx+j+1] = true; + normvec->points[j].x = pabcd(0); + normvec->points[j].y = pabcd(1); + normvec->points[j].z = pabcd(2); + normvec->points[j].intensity = pabcd(3); + effect_num_k ++; + } + } + } + } + } + if (effect_num_k == 0) + { + ekfom_data.valid = false; + return; + } + ekfom_data.M_Noise = laser_point_cov; + ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12); + ekfom_data.z.resize(effect_num_k); + int m = 0; + for (int j = 0; j < time_seq[k]; j++) + { + if(point_selected_surf[idx+j+1]) + { + V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z); + + if (extrinsic_est_en) + { + V3D p_body = pbody_list[idx+j+1]; + M3D p_crossmat, p_imu_crossmat; + p_crossmat << SKEW_SYM_MATRX(p_body); + V3D point_imu = s.offset_R_L_I.normalized() * p_body + s.offset_T_L_I; + p_imu_crossmat << SKEW_SYM_MATRX(point_imu); + V3D C(s.rot.conjugate().normalized() * norm_vec); + V3D A(p_imu_crossmat * C); + V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * C); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + } + else + { + M3D point_crossmat = crossmat_list[idx+j+1]; + V3D C(s.rot.conjugate().normalized() * norm_vec); + V3D A(point_crossmat * C); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } + ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity; + m++; + } + } + effct_feat_num += effect_num_k; +} + +void h_model_output(state_output &s, esekfom::dyn_share_modified &ekfom_data) +{ + bool match_in_map = false; + VF(4) pabcd; + pabcd.setZero(); + + normvec->resize(time_seq[k]); + int effect_num_k = 0; + for (int j = 0; j < time_seq[k]; j++) + { + PointType &point_body_j = feats_down_body->points[idx+j+1]; + PointType &point_world_j = feats_down_world->points[idx+j+1]; + pointBodyToWorld(&point_body_j, &point_world_j); + V3D p_body = pbody_list[idx+j+1]; + V3D p_world; + p_world << point_world_j.x, point_world_j.y, point_world_j.z; + { + auto &points_near = Nearest_Points[idx+j+1]; + + ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); + + if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) + { + point_selected_surf[idx+j+1] = false; + } + else + { + point_selected_surf[idx+j+1] = false; + if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid) + { + float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3); + + if (p_body.norm() > match_s * pd2 * pd2) + { + // point_selected_surf[i] = true; + point_selected_surf[idx+j+1] = true; + normvec->points[j].x = pabcd(0); + normvec->points[j].y = pabcd(1); + normvec->points[j].z = pabcd(2); + normvec->points[j].intensity = pabcd(3); + effect_num_k ++; + } + } + } + } + } + if (effect_num_k == 0) + { + ekfom_data.valid = false; + return; + } + ekfom_data.M_Noise = laser_point_cov; + ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12); + ekfom_data.z.resize(effect_num_k); + int m = 0; + for (int j = 0; j < time_seq[k]; j++) + { + if(point_selected_surf[idx+j+1]) + { + V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z); + + if (extrinsic_est_en) + { + V3D p_body = pbody_list[idx+j+1]; + M3D p_crossmat, p_imu_crossmat; + p_crossmat << SKEW_SYM_MATRX(p_body); + V3D point_imu = s.offset_R_L_I.normalized() * p_body + s.offset_T_L_I; + p_imu_crossmat << SKEW_SYM_MATRX(point_imu); + V3D C(s.rot.conjugate().normalized() * norm_vec); + V3D A(p_imu_crossmat * C); + V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * C); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + } + else + { + M3D point_crossmat = crossmat_list[idx+j+1]; + V3D C(s.rot.conjugate().normalized() * norm_vec); + V3D A(point_crossmat * C); + // V3D A(point_crossmat * state.rot_end.transpose() * norm_vec); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } + ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity; + m++; + } + } + effct_feat_num += effect_num_k; +} + +void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified &ekfom_data) +{ + std::memset(ekfom_data.satu_check, false, 6); + ekfom_data.z_IMU.block<3,1>(0, 0) = angvel_avr - s.omg - s.bg; + ekfom_data.z_IMU.block<3,1>(3, 0) = acc_avr * G_m_s2 / acc_norm - s.acc - s.ba; + ekfom_data.R_IMU << imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_acc_cov, imu_meas_acc_cov, imu_meas_acc_cov; + if(check_satu) + { + if(fabs(angvel_avr(0)) >= 0.99 * satu_gyro) + { + ekfom_data.satu_check[0] = true; + ekfom_data.z_IMU(0) = 0.0; + } + + if(fabs(angvel_avr(1)) >= 0.99 * satu_gyro) + { + ekfom_data.satu_check[1] = true; + ekfom_data.z_IMU(1) = 0.0; + } + + if(fabs(angvel_avr(2)) >= 0.99 * satu_gyro) + { + ekfom_data.satu_check[2] = true; + ekfom_data.z_IMU(2) = 0.0; + } + + if(fabs(acc_avr(0)) >= 0.99 * satu_acc) + { + ekfom_data.satu_check[3] = true; + ekfom_data.z_IMU(3) = 0.0; + } + + if(fabs(acc_avr(1)) >= 0.99 * satu_acc) + { + ekfom_data.satu_check[4] = true; + ekfom_data.z_IMU(4) = 0.0; + } + + if(fabs(acc_avr(2)) >= 0.99 * satu_acc) + { + ekfom_data.satu_check[5] = true; + ekfom_data.z_IMU(5) = 0.0; + } + } +} + + +void pointBodyToWorld(PointType const * const pi, PointType * const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + + V3D p_global; + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + p_global = kf_output.x_.rot.normalized() * (kf_output.x_.offset_R_L_I.normalized() * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos; + } + else + { + p_global = kf_input.x_.rot.normalized() * (kf_input.x_.offset_R_L_I.normalized() * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos; + } + } + else + { + if (!use_imu_as_input) + { + p_global = kf_output.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; + } + else + { + p_global = kf_input.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; + } + } + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.h new file mode 100755 index 0000000000..76432c0355 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/Estimator.h @@ -0,0 +1,118 @@ +#ifndef Estimator_H +#define Estimator_H + +#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> +#include "common_lib.h" +#include "parameters.h" +#include +#include +#include +#include +#include +#include + +extern PointCloudXYZI::Ptr normvec; //(new PointCloudXYZI(100000, 1)); +extern std::vector time_seq; +extern PointCloudXYZI::Ptr feats_down_body; +extern PointCloudXYZI::Ptr feats_down_world; +extern std::vector pbody_list; +extern std::vector Nearest_Points; +extern KD_TREE ikdtree; +extern std::vector pointSearchSqDis; +extern bool point_selected_surf[100000]; // = {0}; +extern std::vector crossmat_list; +extern int effct_feat_num; +extern int k; +extern int idx; +extern V3D angvel_avr, acc_avr; + +extern V3D Lidar_T_wrt_IMU; //(Zero3d); +extern M3D Lidar_R_wrt_IMU; //(Eye3d); + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 S2; +typedef MTK::vect<1, double> vect1; +typedef MTK::vect<2, double> vect2; + +MTK_BUILD_MANIFOLD(state_input, +((vect3, pos)) +((SO3, rot)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +((vect3, vel)) +((vect3, bg)) +((vect3, ba)) +((vect3, gravity)) +); + +MTK_BUILD_MANIFOLD(state_output, +((vect3, pos)) +((SO3, rot)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +((vect3, vel)) +((vect3, omg)) +((vect3, acc)) +((vect3, gravity)) +((vect3, bg)) +((vect3, ba)) +); + +MTK_BUILD_MANIFOLD(input_ikfom, +((vect3, acc)) +((vect3, gyro)) +); + +MTK_BUILD_MANIFOLD(process_noise_input, +((vect3, ng)) +((vect3, na)) +((vect3, nbg)) +((vect3, nba)) +); + +MTK_BUILD_MANIFOLD(process_noise_output, +((vect3, vel)) +((vect3, ng)) +((vect3, na)) +((vect3, nbg)) +((vect3, nba)) +); + +extern esekfom::esekf kf_input; +extern esekfom::esekf kf_output; +extern state_input state_in; +extern state_output state_out; +extern input_ikfom input_in; + +Eigen::Matrix process_noise_cov_input(); + +Eigen::Matrix process_noise_cov_output(); + +//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia +//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); +Eigen::Matrix get_f_input(state_input &s, const input_ikfom &in); + +Eigen::Matrix get_f_output(state_output &s, const input_ikfom &in); + +Eigen::Matrix df_dx_input(state_input &s, const input_ikfom &in); + +// Eigen::Matrix df_dw_input(state_input &s, const input_ikfom &in); + +Eigen::Matrix df_dx_output(state_output &s, const input_ikfom &in); + +// Eigen::Matrix df_dw_output(state_output &s); + +vect3 SO3ToEuler(const SO3 &orient); + +void h_model_input(state_input &s, esekfom::dyn_share_modified &ekfom_data); + +void h_model_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); + +void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); + +void pointBodyToWorld(PointType const * const pi, PointType * const po); + +const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);}; + +#endif diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/IMU_Processing.hpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/IMU_Processing.hpp new file mode 100755 index 0000000000..4469d61849 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/IMU_Processing.hpp @@ -0,0 +1,185 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define MAX_INI_COUNT (100) + +class ImuProcess +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + ~ImuProcess(); + + void Reset(); + void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_); + void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot); + + ofstream fout_imu; + + int lidar_type; + bool imu_en; + V3D mean_acc, gravity_; + bool imu_need_init_ = true; + bool b_first_frame_ = true; + bool gravity_align_ = false; + + private: + void IMU_init(const MeasureGroup &meas, int &N); + V3D mean_gyr; + int init_iter_num = 1; +}; + +ImuProcess::ImuProcess() + : b_first_frame_(true), imu_need_init_(true), gravity_align_(false) +{ + imu_en = true; + init_iter_num = 1; + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); +} + +ImuProcess::~ImuProcess() {} + +void ImuProcess::Reset() +{ + ROS_WARN("Reset ImuProcess"); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + imu_need_init_ = true; + init_iter_num = 1; +} + +void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + V3D cur_acc, cur_gyr; + + if (b_first_frame_) + { + Reset(); + N = 1; + b_first_frame_ = false; + const auto &imu_acc = meas.imu.front()->linear_acceleration; + const auto &gyr_acc = meas.imu.front()->angular_velocity; + mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; + mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + } + + for (const auto &imu : meas.imu) + { + const auto &imu_acc = imu->linear_acceleration; + const auto &gyr_acc = imu->angular_velocity; + cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; + cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + + mean_acc += (cur_acc - mean_acc) / N; + mean_gyr += (cur_gyr - mean_gyr) / N; + + N ++; + } +} + +void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_un_) +{ + if (imu_en) + { + + if(meas.imu.empty()) + return; + ROS_ASSERT(meas.lidar != nullptr); + + + if (imu_need_init_) + { + + IMU_init(meas, init_iter_num); + + imu_need_init_ = true; + + if (init_iter_num > MAX_INI_COUNT) + { + ROS_INFO("IMU Initializing: %.1f %%", 100.0); + imu_need_init_ = false; + *cur_pcl_un_ = *(meas.lidar); + } + return; + } + if (!gravity_align_) + gravity_align_ = true; + *cur_pcl_un_ = *(meas.lidar); + return; + } + else + { + + if (!b_first_frame_) { + if (!gravity_align_) + gravity_align_ = true; + } + else{ + b_first_frame_ = false; + return; + } + + + *cur_pcl_un_ = *(meas.lidar); + return; + } +} + +void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + + M3D hat_grav; + hat_grav << 0.0, gravity_(2), -gravity_(1), + -gravity_(2), 0.0, gravity_(0), + gravity_(1), -gravity_(0), 0.0; + double align_norm = (hat_grav * tmp_gravity).norm() / tmp_gravity.norm() / gravity_.norm(); + double align_cos = gravity_.transpose() * tmp_gravity; + align_cos = align_cos / gravity_.norm() / tmp_gravity.norm(); + if (align_norm < 1e-6) + { + if (align_cos > 1e-6) + { + rot = Eye3d; + } + else + { + rot = -Eye3d; + } + } + else + { + V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos); + rot = Exp(align_angle(0), align_angle(1), align_angle(2)); + } +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/laserMapping.cpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/laserMapping.cpp new file mode 100755 index 0000000000..6fdccd9aae --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/laserMapping.cpp @@ -0,0 +1,1602 @@ +#include +#include +#include +#include +#include +#include +#include +// (dropped — was only here for matplotlibcpp, unused) +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +// #include +#include +#include +#include + +#include "IMU_Processing.hpp" +#include "parameters.h" +#include "Estimator.h" + +#define MAXN (720000) +#define PUBFRAME_PERIOD (20) + +const float MOV_THRESHOLD = 1.5f; + +mutex mtx_buffer; +condition_variable sig_buffer; + +string root_dir = ROOT_DIR; + +int feats_down_size = 0; +int time_log_counter = 0; +int scan_count = 0; +int publish_count = 0; + +int frame_ct = 0; +double time_update_last = 0.0; +double time_current = 0.0; +double time_predict_last_const = 0.0; +double t_last = 0.0; + +shared_ptr p_imu(new ImuProcess()); +bool init_map = false; +bool flg_first_scan = true; +PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI()); + +double T1[MAXN]; +double s_plot[MAXN]; +double s_plot2[MAXN]; +double s_plot3[MAXN]; +double s_plot11[MAXN]; + +double match_time = 0; +double solve_time = 0; +double propag_time = 0; +double update_time = 0; + +bool lidar_pushed = false; +bool flg_reset = false; +bool flg_exit = false; + +vector cub_needrm; + +deque lidar_buffer; +deque time_buffer; +deque imu_deque; + +PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_body_space(new PointCloudXYZI()); +PointCloudXYZI::Ptr init_feats_world(new PointCloudXYZI()); + +pcl::VoxelGrid downSizeFilterSurf; +pcl::VoxelGrid downSizeFilterMap; + +V3D euler_cur; + +MeasureGroup Measures; + +sensor_msgs::Imu imu_last, imu_next; +sensor_msgs::Imu::ConstPtr imu_last_ptr; +nav_msgs::Path path; +nav_msgs::Odometry odomAftMapped; +geometry_msgs::PoseStamped msg_body_pose; + + +void SigHandle(int sig) +{ + flg_exit = true; + ROS_WARN("catch sig %d", sig); + sig_buffer.notify_all(); +} + + +inline void dump_lio_state_to_log(FILE *fp) +{ + V3D rot_ang; + if (!use_imu_as_input) + { + rot_ang = SO3ToEuler(kf_output.x_.rot); + } + else + { + rot_ang = SO3ToEuler(kf_input.x_.rot); + } + + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); + fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); + if (use_imu_as_input) + { + + fprintf(fp, "%lf %lf %lf ", kf_input.x_.pos(0), kf_input.x_.pos(1), kf_input.x_.pos(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.vel(0), kf_input.x_.vel(1), kf_input.x_.vel(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.bg(0), kf_input.x_.bg(1), kf_input.x_.bg(2)); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.ba(0), kf_input.x_.ba(1), kf_input.x_.ba(2)); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.gravity(0), kf_input.x_.gravity(1), kf_input.x_.gravity(2)); + } + else + { + fprintf(fp, "%lf %lf %lf ", kf_output.x_.pos(0), kf_output.x_.pos(1), kf_output.x_.pos(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.vel(0), kf_output.x_.vel(1), kf_output.x_.vel(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.bg(0), kf_output.x_.bg(1), kf_output.x_.bg(2)); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.ba(0), kf_output.x_.ba(1), kf_output.x_.ba(2)); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.gravity(0), kf_output.x_.gravity(1), kf_output.x_.gravity(2)); + } + fprintf(fp, "\r\n"); + fflush(fp); +} + + +void pointBodyLidarToIMU(PointType const *const pi, PointType *const po) +{ + + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu; + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + p_body_imu = kf_output.x_.offset_R_L_I.normalized() * p_body_lidar + kf_output.x_.offset_T_L_I; + } + else + { + p_body_imu = kf_input.x_.offset_R_L_I.normalized() * p_body_lidar + kf_input.x_.offset_T_L_I; + } + } + else + { + p_body_imu = Lidar_R_wrt_IMU * p_body_lidar + Lidar_T_wrt_IMU; + } + + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + + po->intensity = pi->intensity; +} + +int points_cache_size = 0; +void points_cache_collect() +{ + PointVector points_history; + ikdtree.acquire_removed_points(points_history); + points_cache_size = points_history.size(); +} + + +BoxPointType LocalMap_Points; +bool Localmap_Initialized = false; +void lasermap_fov_segment() +{ + cub_needrm.shrink_to_fit(); + + V3D pos_LiD; + if (use_imu_as_input) + { + pos_LiD = kf_input.x_.pos + kf_input.x_.rot.normalized() * Lidar_T_wrt_IMU; + } + else + { + pos_LiD = kf_output.x_.pos + kf_output.x_.rot.normalized() * Lidar_T_wrt_IMU; + } + + if (!Localmap_Initialized) + { + for (int i = 0; i < 3; i++) + { + LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; + LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; + } + Localmap_Initialized = true; + return; + } + + float dist_to_map_edge[3][2]; + bool need_move = false; + for (int i = 0; i < 3; i++) + { + dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) + need_move = true; + } + + if (!need_move) + return; + BoxPointType New_LocalMap_Points, tmp_boxpoints; + New_LocalMap_Points = LocalMap_Points; + float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1))); + for (int i = 0; i < 3; i++) + { + tmp_boxpoints = LocalMap_Points; + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) + { + New_LocalMap_Points.vertex_max[i] -= mov_dist; + New_LocalMap_Points.vertex_min[i] -= mov_dist; + tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; + cub_needrm.emplace_back(tmp_boxpoints); + } + else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) + { + New_LocalMap_Points.vertex_max[i] += mov_dist; + New_LocalMap_Points.vertex_min[i] += mov_dist; + tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; + cub_needrm.emplace_back(tmp_boxpoints); + } + } + LocalMap_Points = New_LocalMap_Points; + + points_cache_collect(); + if (cub_needrm.size() > 0) + int kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); +} + + +void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + // std::cout << "standard_pcl_cbk() run once!\n"; + + mtx_buffer.lock(); + + scan_count++; + + double preprocess_start_time = omp_get_wtime(); + + if (msg->header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + last_timestamp_lidar = msg->header.stamp.toSec(); + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + + double time_div = msg->header.stamp.toSec(); + + p_pre->process(msg, ptr); + + // std::cout << "ptr->points.size() = " << ptr->points.size() << std::endl; + + if (cut_frame) + { + + sort(ptr->points.begin(), ptr->points.end(), time_list); + + for (int i = 0; i < ptr->size(); i++) + { + + ptr_div->push_back(ptr->points[i]); + + if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) + { + if (ptr_div->size() < 1) + continue; + + PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + *ptr_div_i = *ptr_div; + + lidar_buffer.push_back(ptr_div_i); + + time_buffer.push_back(time_div); + time_div += ptr->points[i].curvature / double(1000); + ptr_div->clear(); + } + } + + if (!ptr_div->empty()) + { + lidar_buffer.push_back(ptr_div); + + time_buffer.push_back(time_div); + } + } + else if (con_frame) + { + + if (frame_ct == 0) + { + time_con = last_timestamp_lidar; + } + + if (frame_ct < con_frame_num) + { + for (int i = 0; i < ptr->size(); i++) + { + ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000; + ptr_con->push_back(ptr->points[i]); + } + frame_ct++; + } + + else + { + PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); + *ptr_con_i = *ptr_con; + lidar_buffer.push_back(ptr_con_i); + double time_con_i = time_con; + time_buffer.push_back(time_con_i); + ptr_con->clear(); + frame_ct = 0; + } + } + else + { + lidar_buffer.emplace_back(ptr); + time_buffer.emplace_back(msg->header.stamp.toSec()); + } + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + + +// void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) +// { + +// mtx_buffer.lock(); + +// double preprocess_start_time = omp_get_wtime(); + +// scan_count++; + +// if (msg->header.stamp.toSec() < last_timestamp_lidar) +// { +// ROS_ERROR("lidar loop back, clear buffer"); + +// mtx_buffer.unlock(); +// sig_buffer.notify_all(); +// return; +// } + +// last_timestamp_lidar = msg->header.stamp.toSec(); + +// PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); +// PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + +// p_pre->process(msg, ptr); +// double time_div = msg->header.stamp.toSec(); + +// if (cut_frame) +// { + +// sort(ptr->points.begin(), ptr->points.end(), time_list); + +// for (int i = 0; i < ptr->size(); i++) +// { + +// ptr_div->push_back(ptr->points[i]); +// if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) +// { +// if (ptr_div->size() < 1) +// continue; +// PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + +// *ptr_div_i = *ptr_div; + +// lidar_buffer.push_back(ptr_div_i); +// time_buffer.push_back(time_div); +// time_div += ptr->points[i].curvature / double(1000); +// ptr_div->clear(); +// } +// } +// if (!ptr_div->empty()) +// { +// lidar_buffer.push_back(ptr_div); + +// time_buffer.push_back(time_div); +// } +// } +// else if (con_frame) +// { +// if (frame_ct == 0) +// { + +// time_con = last_timestamp_lidar; +// } +// if (frame_ct < con_frame_num) +// { +// for (int i = 0; i < ptr->size(); i++) +// { +// ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000; +// ptr_con->push_back(ptr->points[i]); +// } +// frame_ct++; +// } +// else +// { +// PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); +// *ptr_con_i = *ptr_con; +// double time_con_i = time_con; +// lidar_buffer.push_back(ptr_con_i); +// time_buffer.push_back(time_con_i); +// ptr_con->clear(); +// frame_ct = 0; +// } +// } +// else +// { +// lidar_buffer.emplace_back(ptr); +// time_buffer.emplace_back(msg->header.stamp.toSec()); +// } +// s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; +// mtx_buffer.unlock(); +// sig_buffer.notify_all(); +// } + +void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) +{ + + publish_count++; + + sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); + + msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_lag_imu_to_lidar); + + double timestamp = msg->header.stamp.toSec(); + + mtx_buffer.lock(); + + if (timestamp < last_timestamp_imu) + { + ROS_ERROR("imu loop back, clear deque"); + + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + imu_deque.emplace_back(msg); + + last_timestamp_imu = timestamp; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + + +bool sync_packages(MeasureGroup &meas) +{ + + if (!imu_en) + { + if (!lidar_buffer.empty()) + { + + meas.lidar = lidar_buffer.front(); + meas.lidar_beg_time = time_buffer.front(); + time_buffer.pop_front(); + lidar_buffer.pop_front(); + + if (meas.lidar->points.size() < 1) + { + cout << "lose lidar" << std::endl; + return false; + } + + double end_time = meas.lidar->points.back().curvature; + for (auto pt : meas.lidar->points) + { + if (pt.curvature > end_time) + { + end_time = pt.curvature; + } + } + lidar_end_time = meas.lidar_beg_time + end_time / double(1000); + + meas.lidar_last_time = lidar_end_time; + + return true; + } + return false; + } + + if (lidar_buffer.empty() || imu_deque.empty()) + { + return false; + } + + /*** push a lidar scan ***/ + if (!lidar_pushed) + { + + meas.lidar = lidar_buffer.front(); + + if (meas.lidar->points.size() < 1) + { + cout << "lose lidar" << endl; + lidar_buffer.pop_front(); + time_buffer.pop_front(); + return false; + } + + meas.lidar_beg_time = time_buffer.front(); + + double end_time = meas.lidar->points.back().curvature; + for (auto pt : meas.lidar->points) + { + if (pt.curvature > end_time) + { + end_time = pt.curvature; + } + } + lidar_end_time = meas.lidar_beg_time + end_time / double(1000); + + meas.lidar_last_time = lidar_end_time; + lidar_pushed = true; + } + + if (last_timestamp_imu < lidar_end_time) + { + return false; + } + + /*** push imu data, and pop from imu buffer ***/ + if (p_imu->imu_need_init_) + { + double imu_time = imu_deque.front()->header.stamp.toSec(); + meas.imu.shrink_to_fit(); + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_deque.front()->header.stamp.toSec(); + if (imu_time > lidar_end_time) + break; + meas.imu.emplace_back(imu_deque.front()); + imu_last = imu_next; + imu_last_ptr = imu_deque.front(); + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + } + else if (!init_map) + { + double imu_time = imu_deque.front()->header.stamp.toSec(); + meas.imu.shrink_to_fit(); + meas.imu.emplace_back(imu_last_ptr); + + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_deque.front()->header.stamp.toSec(); + if (imu_time > lidar_end_time) + break; + meas.imu.emplace_back(imu_deque.front()); + imu_last = imu_next; + imu_last_ptr = imu_deque.front(); + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + } + + lidar_buffer.pop_front(); + time_buffer.pop_front(); + lidar_pushed = false; + return true; +} + + +int process_increments = 0; +void map_incremental() +{ + PointVector PointToAdd; + PointVector PointNoNeedDownsample; + PointToAdd.reserve(feats_down_size); + PointNoNeedDownsample.reserve(feats_down_size); + + for (int i = 0; i < feats_down_size; i++) + { + if (!Nearest_Points[i].empty()) + { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; + /* If the nearest points is definitely outside the downsample box */ + if (fabs(points_near[0].x - mid_point.x) > 1.732 * filter_size_map_min || fabs(points_near[0].y - mid_point.y) > 1.732 * filter_size_map_min || fabs(points_near[0].z - mid_point.z) > 1.732 * filter_size_map_min) + { + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + continue; + } + /* Check if there is a point already in the downsample box */ + float dist = calc_dist(feats_down_world->points[i], mid_point); + for (int readd_i = 0; readd_i < points_near.size(); readd_i++) + { + /* Those points which are outside the downsample box should not be considered. */ + if (fabs(points_near[readd_i].x - mid_point.x) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].y - mid_point.y) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].z - mid_point.z) < 0.5 * filter_size_map_min) + { + need_add = false; + break; + } + } + if (need_add) + PointToAdd.emplace_back(feats_down_world->points[i]); + } + else + { + + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + } + } + int add_point_size = ikdtree.Add_Points(PointToAdd, true); + ikdtree.Add_Points(PointNoNeedDownsample, false); +} + +void publish_init_kdtree(const ros::Publisher &pubLaserCloudFullRes) +{ + int size_init_ikdtree = ikdtree.size(); + PointCloudXYZI::Ptr laserCloudInit(new PointCloudXYZI(size_init_ikdtree, 1)); + + sensor_msgs::PointCloud2 laserCloudmsg; + PointVector().swap(ikdtree.PCL_Storage); + ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); + + laserCloudInit->points = ikdtree.PCL_Storage; + pcl::toROSMsg(*laserCloudInit, laserCloudmsg); + + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); +} + + +PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); +PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); +void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes) +{ + if (scan_pub_en) + { + PointCloudXYZI::Ptr laserCloudFullRes(feats_down_body); + int size = laserCloudFullRes->points.size(); + + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + + laserCloudWorld->points[i].x = feats_down_world->points[i].x; + laserCloudWorld->points[i].y = feats_down_world->points[i].y; + laserCloudWorld->points[i].z = feats_down_world->points[i].z; + laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; + } + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); + + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; + } + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + if (pcd_save_en) + { + int size = feats_down_world->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + laserCloudWorld->points[i].x = feats_down_world->points[i].x; + laserCloudWorld->points[i].y = feats_down_world->points[i].y; + laserCloudWorld->points[i].z = feats_down_world->points[i].z; + laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; + } + + *pcl_wait_save += *laserCloudWorld; + + static int scan_wait_num = 0; + scan_wait_num++; + if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + pcd_index++; + string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd")); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << all_points_dir << endl; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + pcl_wait_save->clear(); + scan_wait_num = 0; + } + } +} + + +void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body) +{ + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + pointBodyLidarToIMU(&feats_undistort->points[i], + &laserCloudIMUBody->points[i]); + } + + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "body"; + pubLaserCloudFull_body.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; +} + +template +void set_posestamp(T &out) +{ + if (!use_imu_as_input) + { + out.position.x = kf_output.x_.pos(0); + out.position.y = kf_output.x_.pos(1); + out.position.z = kf_output.x_.pos(2); + out.orientation.x = kf_output.x_.rot.coeffs()[0]; + out.orientation.y = kf_output.x_.rot.coeffs()[1]; + out.orientation.z = kf_output.x_.rot.coeffs()[2]; + out.orientation.w = kf_output.x_.rot.coeffs()[3]; + } + else + { + out.position.x = kf_input.x_.pos(0); + out.position.y = kf_input.x_.pos(1); + out.position.z = kf_input.x_.pos(2); + out.orientation.x = kf_input.x_.rot.coeffs()[0]; + out.orientation.y = kf_input.x_.rot.coeffs()[1]; + out.orientation.z = kf_input.x_.rot.coeffs()[2]; + out.orientation.w = kf_input.x_.rot.coeffs()[3]; + } +} + +void publish_odometry(const ros::Publisher &pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + if (publish_odometry_without_downsample) + { + odomAftMapped.header.stamp = ros::Time().fromSec(time_current); + } + else + { + odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); + } + set_posestamp(odomAftMapped.pose.pose); + + pubOdomAftMapped.publish(odomAftMapped); + + static tf::TransformBroadcaster br; + tf::Transform transform; + tf::Quaternion q; + transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, + odomAftMapped.pose.pose.position.y, + odomAftMapped.pose.pose.position.z)); + q.setW(odomAftMapped.pose.pose.orientation.w); + q.setX(odomAftMapped.pose.pose.orientation.x); + q.setY(odomAftMapped.pose.pose.orientation.y); + q.setZ(odomAftMapped.pose.pose.orientation.z); + transform.setRotation(q); + br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped")); +} + +void publish_path(const ros::Publisher pubPath) +{ + set_posestamp(msg_body_pose.pose); + + msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); + msg_body_pose.header.frame_id = "camera_init"; + static int jjj = 0; + jjj++; + + { + path.poses.emplace_back(msg_body_pose); + pubPath.publish(path); + } +} + +// ----------------- offline binary stream driver ----------------- +// +// Replaces ros::Subscriber/spin: reads the binary file produced by +// work-20260528/scripts/extract_to_bin.py and pushes lidar frames / +// imu samples into the same global buffers (lidar_buffer, +// time_buffer, imu_deque) that the original ROS callbacks would have +// populated. Bypasses Preprocess::unilidar_handler entirely — we +// have the (x,y,z,intensity,ring,time) tuples in the file already and +// can construct PointCloudXYZI directly, skipping the ROS PointCloud2 +// round-trip and the pcl::fromROSMsg dependency. +// +// See dim/work-20260528/scripts/extract_to_bin.py for the format spec. + +#pragma pack(push, 1) +struct BinPoint { + float x, y, z, _pad_xyz; + float intensity; + uint16_t ring; + uint16_t _pad_ring; + float time; + float _pad_time; +}; +#pragma pack(pop) +static_assert(sizeof(BinPoint) == 32, "BinPoint must be 32 bytes (L1 unilidar layout)"); + +static constexpr uint8_t BIN_TYPE_LIDAR = 0; +static constexpr uint8_t BIN_TYPE_IMU = 1; +static constexpr char BIN_MAGIC[16] = {'P','L','N','R','1',0,0,0,0,0,0,0,0,0,0,0}; + +// Reads one record from the binary stream. Returns: +// 1 = record consumed, buffers may have grown +// 0 = EOF reached cleanly +// -1 = read error +static int pump_one_record(std::FILE* fp) { + uint8_t type = 0; + uint8_t pad[7]; + double ts = 0.0; + if (std::fread(&type, 1, 1, fp) != 1) return 0; // EOF + if (std::fread(pad, 1, 7, fp) != 7) return -1; + if (std::fread(&ts, 8, 1, fp) != 1) return -1; + + if (type == BIN_TYPE_LIDAR) { + uint32_t n = 0, pad32 = 0; + if (std::fread(&n, 4, 1, fp) != 1) return -1; + if (std::fread(&pad32, 4, 1, fp) != 1) return -1; + std::vector pts(n); + if (n && std::fread(pts.data(), sizeof(BinPoint), n, fp) != n) return -1; + + PointCloudXYZI::Ptr cloud(new PointCloudXYZI()); + cloud->points.reserve(n); + const double blind2 = p_pre->blind * p_pre->blind; + const int filter = std::max(1, p_pre->point_filter_num); + // Curvature is in MILLISECONDS downstream (laserMapping.cpp does + // `curvature / 1000.0` to convert back to seconds, matching + // preprocess.cpp's invariant). The previous table here was 1000× + // too small: SEC→1.0 left curvature in seconds, then /1000.0 + // squashed a full 65 ms sweep into 65 μs and degenerated Point-LIO's + // per-point predict into a batch-at-frame-boundary update. The L1 + // recording uses timestamp_unit=0 (SEC) so this fix is what reaches + // the running config; the other rows are kept consistent with + // preprocess.cpp:51-68. + const double time_unit_scale = + (p_pre->time_unit == 0) ? 1e3 : // seconds → ms + (p_pre->time_unit == 1) ? 1.0 : // ms → ms + (p_pre->time_unit == 2) ? 1e-3 : // us → ms + (p_pre->time_unit == 3) ? 1e-6 : 1.0; // ns → ms + for (uint32_t i = 0; i < n; i++) { + if ((int)(i % filter) != 0) continue; + const BinPoint& q = pts[i]; + double r2 = (double)q.x * q.x + (double)q.y * q.y + (double)q.z * q.z; + if (r2 <= blind2) continue; + PointType p; + p.normal_x = p.normal_y = p.normal_z = 0; + p.x = q.x; p.y = q.y; p.z = q.z; + p.intensity = q.intensity; + p.curvature = q.time * time_unit_scale; // see preprocess.cpp::unilidar_handler + cloud->points.push_back(p); + } + if (!cloud->points.empty()) { + std::lock_guard lk(mtx_buffer); + if (ts < last_timestamp_lidar) { + ROS_ERROR("lidar loop back, clear buffer"); + lidar_buffer.clear(); + time_buffer.clear(); + } + lidar_buffer.emplace_back(cloud); + time_buffer.emplace_back(ts); + last_timestamp_lidar = ts; + } + return 1; + } else if (type == BIN_TYPE_IMU) { + double v[6]; + if (std::fread(v, 8, 6, fp) != 6) return -1; + sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu()); + msg->header.stamp = ros::Time::fromSec(ts - time_lag_imu_to_lidar); + msg->header.frame_id = "unilidar_imu"; + msg->angular_velocity.x = v[0]; + msg->angular_velocity.y = v[1]; + msg->angular_velocity.z = v[2]; + msg->linear_acceleration.x = v[3]; + msg->linear_acceleration.y = v[4]; + msg->linear_acceleration.z = v[5]; + double ts_aligned = msg->header.stamp.toSec(); + std::lock_guard lk(mtx_buffer); + if (ts_aligned < last_timestamp_imu) { + // Same behaviour as the original imu_cbk on time monotonicity violation. + return 1; + } + imu_deque.emplace_back(msg); + last_timestamp_imu = ts_aligned; + return 1; + } + ROS_ERROR("unknown record type %u in binary stream", (unsigned)type); + return -1; +} + +int main(int argc, char **argv) +{ + // --yaml --bin + std::string yaml_path; + std::string bin_path; + for (int i = 1; i < argc; i++) { + std::string a = argv[i]; + if (a == "--yaml" && i + 1 < argc) yaml_path = argv[++i]; + else if (a == "--bin" && i + 1 < argc) bin_path = argv[++i]; + } + if (bin_path.empty()) { + std::fprintf(stderr, + "usage: %s --bin [--yaml ]\n", + argv[0]); + return 2; + } + readParametersYaml(yaml_path); + cout << "lidar_type: " << lidar_type << endl; + + path.header.stamp = ros::Time().fromSec(lidar_end_time); + path.header.frame_id = "camera_init"; + + int frame_num = 0; + double aver_time_consu = 0, + aver_time_icp = 0, + aver_time_match = 0, + aver_time_incre = 0, + aver_time_solve = 0, + aver_time_propag = 0; + std::time_t startTime, endTime; + + double FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); + double HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); + + memset(point_selected_surf, true, sizeof(point_selected_surf)); + + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + + Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT); + Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR); + + if (extrinsic_est_en) + { + + if (!use_imu_as_input) + { + kf_output.x_.offset_R_L_I = Lidar_R_wrt_IMU; + kf_output.x_.offset_T_L_I = Lidar_T_wrt_IMU; + } + + else + { + kf_input.x_.offset_R_L_I = Lidar_R_wrt_IMU; + kf_input.x_.offset_T_L_I = Lidar_T_wrt_IMU; + } + } + + p_imu->lidar_type = p_pre->lidar_type = lidar_type; + p_imu->imu_en = imu_en; + + kf_input.init_dyn_share_modified(get_f_input, df_dx_input, h_model_input); + kf_output.init_dyn_share_modified_2h(get_f_output, df_dx_output, h_model_output, h_model_IMU_output); + + Eigen::Matrix P_init = MD(24, 24)::Identity() * 0.01; + P_init.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001; + P_init.block<6, 6>(15, 15) = MD(6, 6)::Identity() * 0.001; + P_init.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001; + kf_input.change_P(P_init); + + Eigen::Matrix P_init_output = MD(30, 30)::Identity() * 0.01; + P_init_output.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001; + P_init_output.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001; + P_init_output.block<6, 6>(24, 24) = MD(6, 6)::Identity() * 0.001; + kf_input.change_P(P_init); + kf_output.change_P(P_init_output); + + Eigen::Matrix Q_input = process_noise_cov_input(); + Eigen::Matrix Q_output = process_noise_cov_output(); + + /*** debug record ***/ + FILE *fp; + string pos_log_dir = root_dir + "/Log/pos_log.txt"; + fp = fopen(pos_log_dir.c_str(), "w"); + + ofstream fout_out, fout_imu_pbp; + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), ios::out); + fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"), ios::out); + if (fout_out && fout_imu_pbp) + cout << "~~~~" << ROOT_DIR << " file opened" << endl; + else + cout << "~~~~" << ROOT_DIR << " doesn't exist" << endl; + + // Subscriber / Publisher tags — no-ops in the offline build, but the + // publishers are passed by reference into helpers like + // publish_frame_world(pubLaserCloudFullRes) so we still need lvalues + // with the right type. Construct them as default-init tags. + ros::Subscriber sub_pcl, sub_imu; + ros::Publisher pubLaserCloudFullRes; + ros::Publisher pubLaserCloudFullRes_body; + ros::Publisher pubLaserCloudEffect; + ros::Publisher pubLaserCloudMap; + ros::Publisher pubOdomAftMapped; + ros::Publisher pubPath; + ros::Publisher plane_pub; + + signal(SIGINT, SigHandle); + + std::FILE* fp_bin = std::fopen(bin_path.c_str(), "rb"); + if (!fp_bin) { + std::fprintf(stderr, "could not open %s: %s\n", bin_path.c_str(), strerror(errno)); + return 1; + } + char magic[16]; + if (std::fread(magic, 1, 16, fp_bin) != 16 || std::memcmp(magic, BIN_MAGIC, 16) != 0) { + std::fprintf(stderr, "%s: bad magic, not a PLNR1 binary stream\n", bin_path.c_str()); + return 1; + } + cout << "reading binary stream from " << bin_path << endl; + bool eof_seen = false; + + while (ros::ok()) + { + + if (flg_exit) + break; + + // Replaces ros::spinOnce(): pump records until sync_packages has + // enough data or we hit EOF. + if (!eof_seen) { + int rc = pump_one_record(fp_bin); + if (rc == 0) { + eof_seen = true; + cout << "binary stream EOF" << endl; + } else if (rc < 0) { + std::fprintf(stderr, "binary stream read error\n"); + flg_exit = true; + break; + } + } + + if (sync_packages(Measures) == false) + { + // No rate limit in offline mode — let the pump run as fast + // as the algorithm can consume. + if (eof_seen && lidar_buffer.empty()) { + flg_exit = true; + } + continue; + } + + if (flg_first_scan) + { + first_lidar_time = Measures.lidar_beg_time; + flg_first_scan = false; + cout << "first lidar time" << first_lidar_time << endl; + } + + if (flg_reset) + { + ROS_WARN("reset when rosbag play back"); + p_imu->Reset(); + flg_reset = false; + continue; + } + + double t0, t1, t2, t3, t4, t5, match_start, solve_start; + match_time = 0; + solve_time = 0; + propag_time = 0; + update_time = 0; + t0 = omp_get_wtime(); + + p_imu->Process(Measures, feats_undistort); + + if (feats_undistort->empty() || feats_undistort == NULL) + { + continue; + } + + if (imu_en) + { + if (!p_imu->gravity_align_) + { + while (Measures.lidar_beg_time > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + if (non_station_start) + { + state_in.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.acc << VEC_FROM_ARRAY(gravity_init); + state_out.acc *= -1; + } + else + { + state_in.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.acc = p_imu->mean_acc * G_m_s2 / acc_norm; + } + if (gravity_align) + { + Eigen::Matrix3d rot_init; + p_imu->gravity_ << VEC_FROM_ARRAY(gravity); + p_imu->Set_init(state_in.gravity, rot_init); + state_in.gravity = state_out.gravity = p_imu->gravity_; + state_in.rot = state_out.rot = rot_init; + state_in.rot.normalize(); + state_out.rot.normalize(); + state_out.acc = -rot_init.transpose() * state_out.gravity; + } + kf_input.change_x(state_in); + kf_output.change_x(state_out); + } + } + else + { + if (!p_imu->gravity_align_) + { + state_in.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.acc << VEC_FROM_ARRAY(gravity_init); + state_out.acc *= -1; + } + } + + /*** Segment the map in lidar FOV ***/ + lasermap_fov_segment(); + + t1 = omp_get_wtime(); + if (space_down_sample) + { + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } + else + { + feats_down_body = Measures.lidar; + + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } + time_seq = time_compressing(feats_down_body); + feats_down_size = feats_down_body->points.size(); + + /*** initialize the map kdtree ***/ + if (!init_map) + { + if (ikdtree.Root_Node == nullptr) + + { + ikdtree.set_downsample_param(filter_size_map_min); + } + + feats_down_world->resize(feats_down_size); + for (int i = 0; i < feats_down_size; i++) + { + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + } + + for (size_t i = 0; i < feats_down_world->size(); i++) + { + init_feats_world->points.emplace_back(feats_down_world->points[i]); + } + + if (init_feats_world->size() < init_map_size) + continue; + + ikdtree.Build(init_feats_world->points); + init_map = true; + + publish_init_kdtree(pubLaserCloudMap); + continue; + } + + /*** ICP and Kalman filter update ***/ + + normvec->resize(feats_down_size); + feats_down_world->resize(feats_down_size); + + Nearest_Points.resize(feats_down_size); + + t2 = omp_get_wtime(); + + /*** iterated state estimation ***/ + + crossmat_list.reserve(feats_down_size); + pbody_list.reserve(feats_down_size); + + for (size_t i = 0; i < feats_down_body->size(); i++) + { + + V3D point_this(feats_down_body->points[i].x, + feats_down_body->points[i].y, + feats_down_body->points[i].z); + pbody_list[i] = point_this; + + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + + point_this = kf_output.x_.offset_R_L_I.normalized() * point_this + kf_output.x_.offset_T_L_I; + } + else + { + + point_this = kf_input.x_.offset_R_L_I.normalized() * point_this + kf_input.x_.offset_T_L_I; + } + } + else + { + point_this = Lidar_R_wrt_IMU * point_this + Lidar_T_wrt_IMU; + } + + + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + crossmat_list[i]=point_crossmat; + } + + if (!use_imu_as_input) + { + bool imu_upda_cov = false; + effct_feat_num = 0; + + /**** point by point update ****/ + + double pcl_beg_time = Measures.lidar_beg_time; + idx = -1; + for (k = 0; k < time_seq.size(); k++) + { + + PointType &point_body = feats_down_body->points[idx + time_seq[k]]; + + time_current = point_body.curvature / 1000.0 + pcl_beg_time; + + if (is_first_frame) + { + if (imu_en) + { + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + + angvel_avr << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z; + acc_avr << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z; + } + is_first_frame = false; + imu_upda_cov = true; + time_update_last = time_current; + time_predict_last_const = time_current; + } + + if (imu_en) + { + bool imu_comes = time_current > imu_next.header.stamp.toSec(); + while (imu_comes) + { + imu_upda_cov = true; + angvel_avr << imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z; + acc_avr << imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z; + + /*** covariance update ***/ + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + double dt = imu_last.header.stamp.toSec() - time_predict_last_const; + kf_output.predict(dt, Q_output, input_in, true, false); + time_predict_last_const = imu_last.header.stamp.toSec(); + imu_comes = time_current > imu_next.header.stamp.toSec(); + + { + double dt_cov = imu_last.header.stamp.toSec() - time_update_last; + + if (dt_cov > 0.0) + { + time_update_last = imu_last.header.stamp.toSec(); + double propag_imu_start = omp_get_wtime(); + + kf_output.predict(dt_cov, Q_output, input_in, false, true); + + propag_time += omp_get_wtime() - propag_imu_start; + double solve_imu_start = omp_get_wtime(); + kf_output.update_iterated_dyn_share_IMU(); + solve_time += omp_get_wtime() - solve_imu_start; + } + } + } + } + + double dt = time_current - time_predict_last_const; + double propag_state_start = omp_get_wtime(); + if (!prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (dt_cov > 0.0) + { + kf_output.predict(dt_cov, Q_output, input_in, false, true); + time_update_last = time_current; + } + } + kf_output.predict(dt, Q_output, input_in, true, false); + propag_time += omp_get_wtime() - propag_state_start; + time_predict_last_const = time_current; + + double t_update_start = omp_get_wtime(); + + if (feats_down_size < 1) + { + ROS_WARN("No point, skip this scan!\n"); + idx += time_seq[k]; + continue; + } + if (!kf_output.update_iterated_dyn_share_modified()) + { + idx = idx + time_seq[k]; + continue; + } + + if (prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (!imu_en && (dt_cov >= imu_time_inte)) + { + double propag_cov_start = omp_get_wtime(); + kf_output.predict(dt_cov, Q_output, input_in, false, true); + imu_upda_cov = false; + time_update_last = time_current; + propag_time += omp_get_wtime() - propag_cov_start; + } + } + + solve_start = omp_get_wtime(); + + if (publish_odometry_without_downsample) + { + /******* Publish odometry *******/ + + publish_odometry(pubOdomAftMapped); + if (runtime_pos_log) + { + state_out = kf_output.x_; + euler_cur = SO3ToEuler(state_out.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() + << " " << state_out.omg.transpose() << " " << state_out.acc.transpose() << " " << state_out.gravity.transpose() << " " << state_out.bg.transpose() << " " << state_out.ba.transpose() << " " << feats_undistort->points.size() << endl; + } + } + + for (int j = 0; j < time_seq[k]; j++) + { + PointType &point_body_j = feats_down_body->points[idx + j + 1]; + PointType &point_world_j = feats_down_world->points[idx + j + 1]; + pointBodyToWorld(&point_body_j, &point_world_j); + } + + solve_time += omp_get_wtime() - solve_start; + + update_time += omp_get_wtime() - t_update_start; + idx += time_seq[k]; + } + } + else + { + bool imu_prop_cov = false; + effct_feat_num = 0; + + double pcl_beg_time = Measures.lidar_beg_time; + idx = -1; + for (k = 0; k < time_seq.size(); k++) + { + PointType &point_body = feats_down_body->points[idx + time_seq[k]]; + time_current = point_body.curvature / 1000.0 + pcl_beg_time; + if (is_first_frame) + { + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + imu_prop_cov = true; + + is_first_frame = false; + t_last = time_current; + time_update_last = time_current; + + { + input_in.gyro << imu_last.angular_velocity.x, + imu_last.angular_velocity.y, + imu_last.angular_velocity.z; + + input_in.acc << imu_last.linear_acceleration.x, + imu_last.linear_acceleration.y, + imu_last.linear_acceleration.z; + + input_in.acc = input_in.acc * G_m_s2 / acc_norm; + } + } + + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + input_in.gyro << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z; + input_in.acc << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z; + + input_in.acc = input_in.acc * G_m_s2 / acc_norm; + double dt = imu_last.header.stamp.toSec() - t_last; + + double dt_cov = imu_last.header.stamp.toSec() - time_update_last; + if (dt_cov > 0.0) + { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = imu_last.header.stamp.toSec(); + } + kf_input.predict(dt, Q_input, input_in, true, false); + t_last = imu_last.header.stamp.toSec(); + imu_prop_cov = true; + } + + double dt = time_current - t_last; + t_last = time_current; + double propag_start = omp_get_wtime(); + + if (!prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (dt_cov > 0.0) + { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = time_current; + } + } + kf_input.predict(dt, Q_input, input_in, true, false); + + propag_time += omp_get_wtime() - propag_start; + + double t_update_start = omp_get_wtime(); + + if (feats_down_size < 1) + { + ROS_WARN("No point, skip this scan!\n"); + + idx += time_seq[k]; + continue; + } + if (!kf_input.update_iterated_dyn_share_modified()) + { + idx = idx + time_seq[k]; + continue; + } + + solve_start = omp_get_wtime(); + + if (publish_odometry_without_downsample) + { + /******* Publish odometry *******/ + + publish_odometry(pubOdomAftMapped); + if (runtime_pos_log) + { + state_in = kf_input.x_; + euler_cur = SO3ToEuler(state_in.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() + << " " << state_in.bg.transpose() << " " << state_in.ba.transpose() << " " << state_in.gravity.transpose() << " " << feats_undistort->points.size() << endl; + } + } + + for (int j = 0; j < time_seq[k]; j++) + { + PointType &point_body_j = feats_down_body->points[idx + j + 1]; + PointType &point_world_j = feats_down_world->points[idx + j + 1]; + pointBodyToWorld(&point_body_j, &point_world_j); + } + solve_time += omp_get_wtime() - solve_start; + + update_time += omp_get_wtime() - t_update_start; + idx = idx + time_seq[k]; + } + } + + /******* Publish odometry downsample *******/ + if (!publish_odometry_without_downsample) + { + publish_odometry(pubOdomAftMapped); + } + + /*** add the feature points to map kdtree ***/ + t3 = omp_get_wtime(); + + if (feats_down_size > 4) + { + map_incremental(); + } + + t5 = omp_get_wtime(); + + /******* Publish points *******/ + + if (path_en) + publish_path(pubPath); + + if (scan_pub_en || pcd_save_en) + publish_frame_world(pubLaserCloudFullRes); + + if (scan_pub_en && scan_body_pub_en) + publish_frame_body(pubLaserCloudFullRes_body); + + /*** Debug variables Logging ***/ + if (runtime_pos_log) + { + frame_num++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; + { + aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + update_time / frame_num; + } + aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num; + aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + solve_time / frame_num; + aver_time_propag = aver_time_propag * (frame_num - 1) / frame_num + propag_time / frame_num; + T1[time_log_counter] = Measures.lidar_beg_time; + s_plot[time_log_counter] = t5 - t0; + s_plot2[time_log_counter] = feats_undistort->points.size(); + s_plot3[time_log_counter] = aver_time_consu; + time_log_counter++; + printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f propogate: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_propag); + if (!publish_odometry_without_downsample) + { + if (!use_imu_as_input) + { + state_out = kf_output.x_; + euler_cur = SO3ToEuler(state_out.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() + << " " << state_out.omg.transpose() << " " << state_out.acc.transpose() << " " << state_out.gravity.transpose() << " " << state_out.bg.transpose() << " " << state_out.ba.transpose() << " " << feats_undistort->points.size() << endl; + } + else + { + state_in = kf_input.x_; + euler_cur = SO3ToEuler(state_in.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() + << " " << state_in.bg.transpose() << " " << state_in.ba.transpose() << " " << state_in.gravity.transpose() << " " << feats_undistort->points.size() << endl; + } + } + dump_lio_state_to_log(fp); + } + + // (offline build: no rate limit — run as fast as the algorithm + // can consume records from the binary stream) + } + + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + + if (pcl_wait_save->size() > 0 && pcd_save_en) + { + string file_name = string("scans.pcd"); + string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); + std::cout << "Saving map to file: " << all_points_dir << std::endl; + pcl::PCDWriter pcd_writer; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + } + fout_out.close(); + fout_imu_pbp.close(); + + return 0; +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.cpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.cpp new file mode 100755 index 0000000000..c4d8544d4b --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.cpp @@ -0,0 +1,192 @@ +#include "parameters.h" + +bool is_first_frame = true; + +double lidar_end_time = 0.0; +double first_lidar_time = 0.0; +double time_con = 0.0; + +double last_timestamp_lidar = -1.0; +double last_timestamp_imu = -1.0; + +int pcd_index = 0; + +std::string lid_topic; +std::string imu_topic; + +bool prop_at_freq_of_imu; +bool check_satu; +bool con_frame; +bool cut_frame; + +bool use_imu_as_input; +bool space_down_sample; +bool publish_odometry_without_downsample; + +int init_map_size; +int con_frame_num; + +double match_s; +double satu_acc; +double satu_gyro; +double cut_frame_time_interval; + +float plane_thr; + +double filter_size_surf_min; +double filter_size_map_min; +double fov_deg; + +double cube_len; +float DET_RANGE; + +bool imu_en; +bool gravity_align; +bool non_station_start; + +double imu_time_inte; + +double laser_point_cov; +double acc_norm; + +double vel_cov; +double acc_cov_input; +double gyr_cov_input; + +double gyr_cov_output; +double acc_cov_output; +double b_gyr_cov; +double b_acc_cov; + +double imu_meas_acc_cov; +double imu_meas_omg_cov; + +int lidar_type; +int pcd_save_interval; + +std::vector gravity_init; +std::vector gravity; + +std::vector extrinT; +std::vector extrinR; + +bool runtime_pos_log; +bool pcd_save_en; +bool path_en; +bool extrinsic_est_en = true; + +bool scan_pub_en; +bool scan_body_pub_en; + +shared_ptr p_pre; +double time_lag_imu_to_lidar = 0.0; + +// yaml-cpp drives readParameters in the no-ROS port. The launch file's +// values (which were never in the yaml) are baked in +// as our defaults here, matching launch/mapping_unilidar_l1.launch. +#include +#include + +namespace { + +template +T y_get(const YAML::Node& root, const std::string& dotted, const T& def) { + YAML::Node n = YAML::Clone(root); + size_t p = 0; + while (true) { + size_t q = dotted.find('/', p); + std::string key = dotted.substr(p, q - p); + if (!n.IsMap() || !n[key]) return def; + n = n[key]; + if (q == std::string::npos) break; + p = q + 1; + } + try { return n.as(); } catch (...) { return def; } +} + +} // namespace + +void readParametersYaml(const std::string& yaml_path) { + p_pre.reset(new Preprocess()); + + YAML::Node cfg; + if (!yaml_path.empty()) { + try { + cfg = YAML::LoadFile(yaml_path); + } catch (const std::exception& e) { + std::fprintf(stderr, "readParametersYaml: could not load %s: %s\n", + yaml_path.c_str(), e.what()); + } + } + + // Launch-file values (mapping_unilidar_l1.launch). These were never + // in the yaml; bake them in here as our defaults so the binary runs + // out-of-the-box on L1 data. + prop_at_freq_of_imu = true; + use_imu_as_input = false; + check_satu = true; + init_map_size = 10; + space_down_sample = true; + p_pre->point_filter_num = 1; + filter_size_surf_min = y_get(cfg, "mapping/filter_size_surf", 0.1); + filter_size_map_min = y_get(cfg, "mapping/filter_size_map", 0.1); + cube_len = 1000.0; + runtime_pos_log = true; // enable Log/mat_out.txt trajectory dump (offline analysis) + + // Yaml-driven values (config/unilidar_l1.yaml). + satu_acc = y_get(cfg, "mapping/satu_acc", 30.0); + satu_gyro = y_get(cfg, "mapping/satu_gyro", 35.0); + acc_norm = y_get(cfg, "mapping/acc_norm", 9.81); + plane_thr = y_get(cfg, "mapping/plane_thr", 0.1f); + lid_topic = y_get(cfg, "common/lid_topic", "/unilidar/cloud"); + imu_topic = y_get(cfg, "common/imu_topic", "/unilidar/imu"); + con_frame = y_get(cfg, "common/con_frame", false); + con_frame_num = y_get(cfg, "common/con_frame_num", 1); + cut_frame = y_get(cfg, "common/cut_frame", false); + cut_frame_time_interval = y_get(cfg, "common/cut_frame_time_interval", 0.1); + time_lag_imu_to_lidar = y_get(cfg, "common/time_lag_imu_to_lidar", 0.0); + DET_RANGE = y_get(cfg, "mapping/det_range", 100.0f); + fov_deg = y_get(cfg, "mapping/fov_degree", 180.0); + imu_en = y_get(cfg, "mapping/imu_en", true); + non_station_start = y_get(cfg, "mapping/start_in_aggressive_motion", false); + extrinsic_est_en = y_get(cfg, "mapping/extrinsic_est_en", false); + imu_time_inte = y_get(cfg, "mapping/imu_time_inte", 0.004); + laser_point_cov = y_get(cfg, "mapping/lidar_meas_cov", 0.01); + acc_cov_input = y_get(cfg, "mapping/acc_cov_input", 0.1); + vel_cov = y_get(cfg, "mapping/vel_cov", 20.0); + gyr_cov_input = y_get(cfg, "mapping/gyr_cov_input", 0.01); + gyr_cov_output = y_get(cfg, "mapping/gyr_cov_output", 1000.0); + acc_cov_output = y_get(cfg, "mapping/acc_cov_output", 500.0); + b_gyr_cov = y_get(cfg, "mapping/b_gyr_cov", 0.0001); + b_acc_cov = y_get(cfg, "mapping/b_acc_cov", 0.0001); + imu_meas_acc_cov = y_get(cfg, "mapping/imu_meas_acc_cov", 0.1); + imu_meas_omg_cov = y_get(cfg, "mapping/imu_meas_omg_cov", 0.1); + p_pre->blind = y_get(cfg, "preprocess/blind", 0.5); + lidar_type = y_get(cfg, "preprocess/lidar_type", 5); + p_pre->N_SCANS = y_get(cfg, "preprocess/scan_line", 18); + p_pre->SCAN_RATE = y_get(cfg, "preprocess/scan_rate", 10); + p_pre->time_unit = y_get(cfg, "preprocess/timestamp_unit", 0); + match_s = y_get(cfg, "mapping/match_s", 81.0); + gravity_align = y_get(cfg, "mapping/gravity_align", true); + gravity = y_get>(cfg, "mapping/gravity", {0.0, 0.0, -9.81}); + gravity_init = y_get>(cfg, "mapping/gravity_init", {0.0, 0.0, -9.81}); + extrinT = y_get>(cfg, "mapping/extrinsic_T", {0.007698, 0.014655, -0.00667}); + extrinR = y_get>(cfg, "mapping/extrinsic_R", {1.0,0.0,0.0, 0.0,1.0,0.0, 0.0,0.0,1.0}); + publish_odometry_without_downsample = + y_get(cfg, "odometry/publish_odometry_without_downsample", true); + path_en = y_get(cfg, "publish/path_en", true); + scan_pub_en = y_get(cfg, "publish/scan_publish_en", true); + scan_body_pub_en = y_get(cfg, "publish/scan_bodyframe_pub_en", false); + pcd_save_en = y_get(cfg, "pcd_save/pcd_save_en", true); + pcd_save_interval = y_get(cfg, "pcd_save/interval", -1); +} + +// Kept for source-compat with laserMapping.cpp's main(); the offline +// main() calls readParametersYaml(path) BEFORE this, so by the time +// readParameters runs, all globals are set and the NodeHandle ref is +// unused. +void readParameters(ros::NodeHandle& /*nh*/) { + if (p_pre == nullptr) { + readParametersYaml(std::string()); // last-resort defaults + } +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.h new file mode 100755 index 0000000000..bf6c281b6f --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/parameters.h @@ -0,0 +1,41 @@ +// #ifndef PARAM_H +// #define PARAM_H +#pragma once +#include +#include +#include +#include +#include "preprocess.h" + +extern bool is_first_frame; +extern double lidar_end_time, first_lidar_time, time_con; +extern double last_timestamp_lidar, last_timestamp_imu; +extern int pcd_index; + +extern std::string lid_topic, imu_topic; +extern bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame; +extern bool use_imu_as_input, space_down_sample; +extern bool extrinsic_est_en, publish_odometry_without_downsample; +extern int init_map_size, con_frame_num; +extern double match_s, satu_acc, satu_gyro, cut_frame_time_interval; +extern float plane_thr; +extern double filter_size_surf_min, filter_size_map_min, fov_deg; +extern double cube_len; +extern float DET_RANGE; +extern bool imu_en, gravity_align, non_station_start; +extern double imu_time_inte; +extern double laser_point_cov, acc_norm; +extern double acc_cov_input, gyr_cov_input, vel_cov; +extern double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov; +extern double imu_meas_acc_cov, imu_meas_omg_cov; +extern int lidar_type, pcd_save_interval; +extern std::vector gravity_init, gravity; +extern std::vector extrinT; +extern std::vector extrinR; +extern bool runtime_pos_log, pcd_save_en, path_en; +extern bool scan_pub_en, scan_body_pub_en; +extern shared_ptr p_pre; +extern double time_lag_imu_to_lidar; + +void readParameters(ros::NodeHandle &n); +void readParametersYaml(const std::string& yaml_path); diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.cpp b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.cpp new file mode 100755 index 0000000000..c2b0a36f7b --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.cpp @@ -0,0 +1,856 @@ +#include "preprocess.h" + +#define RETURN0 0x00 +#define RETURN0AND1 0x10 + +Preprocess::Preprocess() + :lidar_type(AVIA), blind(0.01), point_filter_num(1) +{ + inf_bound = 10; + N_SCANS = 6; + SCAN_RATE = 10; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid =6.25; + limit_midmin =6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit/180*M_PI); + jump_down_limit = cos(jump_down_limit/180*M_PI); + cos160 = cos(cos160/180*M_PI); + smallp_intersect = cos(smallp_intersect/180*M_PI); +} + +Preprocess::~Preprocess() {} + +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) +{ + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; +} + +// void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +// { +// avia_handler(msg); +// *pcl_out = pl_surf; +// } + +void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + switch (time_unit) + { + case SEC: + time_unit_scale = 1.e3f; + break; + case MS: + time_unit_scale = 1.f; + break; + case US: + time_unit_scale = 1.e-3f; + break; + case NS: + time_unit_scale = 1.e-6f; + break; + default: + time_unit_scale = 1.f; + break; + } + + switch (lidar_type) + { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case HESAIxt32: + hesai_handler(msg); + break; + + case UNILIDAR: + unilidar_handler(msg); + break; + + default: + printf("Error LiDAR Type"); + break; + } + + *pcl_out = pl_surf; +} + +// void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) +// { +// pl_surf.clear(); +// pl_corn.clear(); +// pl_full.clear(); +// double t1 = omp_get_wtime(); +// int plsize = msg->point_num; + +// pl_corn.reserve(plsize); +// pl_surf.reserve(plsize); +// pl_full.resize(plsize); + +// uint valid_num = 0; + +// for(uint i=1; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) +// { +// valid_num ++; +// if (valid_num % point_filter_num == 0) +// { +// pl_full[i].x = msg->points[i].x; +// pl_full[i].y = msg->points[i].y; +// pl_full[i].z = msg->points[i].z; +// pl_full[i].intensity = msg->points[i].reflectivity; +// pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms + +// if (i==0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; +// else pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i-1].curvature) < 1.0 ? pl_full[i].curvature : pl_full[i-1].curvature + 0.004166667f; + +// if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) +// || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) +// || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7) +// && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) +// { +// pl_surf.push_back(pl_full[i]); +// } +// } +// } +// } + +// } + +void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + + + double time_stamp = msg->header.stamp.toSec(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < (blind * blind)) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms + + pl_surf.points.push_back(added_pt); + } + + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].time > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + +} + + +void Preprocess::unilidar_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + + pl_surf.reserve(plsize); + + // std::cout << "plsize = " << plsize << ", given_offset_time = " << given_offset_time << std::endl; + int countElimnated = 0; + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + + added_pt.intensity = pl_orig.points[i].intensity; + + added_pt.curvature = pl_orig.points[i].time * time_unit_scale; + + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + else + { + countElimnated++; + } + } + + // std::cout << "pl_surf.size() = " << pl_surf.size() << ", countElimnated = " << countElimnated << std::endl; + +} + + +void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].timestamp > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + double time_head = pl_orig.points[0].timestamp; + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + +} + +void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) +{ + int plsize = pl.size(); + int plsize2; + if(plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while(types[head].range < blind) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for(uint i=head; i0.5) + if(last_state==1 && last_direct.norm()>0.1) + { + double mod = last_direct.transpose() * curr_direct; + if(mod>-0.707 && mod<0.707) + { + types[i].ftype = Edge_Plane; + } + else + { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for(uint i=head+3; i=Real_Plane) + { + continue; + } + + if(types[i-1].dista<1e-16 || types[i].dista<1e-16) + { + continue; + } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for(int j=0; j<2; j++) + { + int m = -1; + if(j == 1) + { + m = 1; + } + + if(types[i+m].range < blind) + { + if(types[i].range > inf_bound) + { + types[i].edj[j] = Nr_inf; + } + else + { + types[i].edj[j] = Nr_blind; + } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if(types[i].angle[j] < jump_up_limit) + { + types[i].edj[j] = Nr_180; + } + else if(types[i].angle[j] > jump_down_limit) + { + types[i].edj[j] = Nr_zero; + } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + + } + else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) + { + if(types[i].ftype == Nor) + { + types[i].ftype = Wire; + } + } + } + + plsize2 = plsize-1; + double ratio; + for(uint i=head+1; i types[i].dista) + { + ratio = types[i-1].dista / types[i].dista; + } + else + { + ratio = types[i].dista / types[i-1].dista; + } + + if(types[i].intersect &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA*types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for(i_nex=i_cur; i_nex= pl.size()) || (i_nex >= pl.size())) break; + + if(types[i_nex].range < blind) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx*vx + vy*vy + vz*vz; + if(two_dis >= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for(uint j=i_cur+1; j= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1]*vz - vy*v1[2]; + v2[1] = v1[2]*vx - v1[0]*vz; + v2[2] = v1[0]*vy - vx*v1[1]; + + double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]; + if(lw > leng_wid) + { + leng_wid = lw; + } + } + + + if((two_dis*two_dis/leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for(uint j=0; j=limit_maxmid || dismid_min>=limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize-2]; + if(dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) +{ + if(nor_dir == 0) + { + if(types[i-1].rangeedgea*d2 || (d1-d2)>edgeb) + { + return false; + } + + return true; +} diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.h b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.h new file mode 100755 index 0000000000..7c35a14ff0 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/preprocess.h @@ -0,0 +1,181 @@ +#pragma once + +#include +#include +#include +// #include + +using namespace std; + +#define IS_VALID(a) ((abs(a)>1e8) ? true : false) + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; + +enum LID_TYPE{ + AVIA = 1, + VELO16, + OUST64, + HESAIxt32, + UNILIDAR +}; //{1, 2, 3, 4} + + +enum TIME_UNIT{ + SEC = 0, + MS = 1, + US = 2, + NS = 3 +}; +enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; +enum Surround{Prev, Next}; +enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; + +struct orgtype +{ + double range; + double dista; + double angle[2]; + double intersect; + E_jump edj[2]; + Feature ftype; + orgtype() + { + range = 0; + edj[Prev] = Nr_nor; + edj[Next] = Nr_nor; + ftype = Nor; + intersect = 2; + } +}; + +namespace velodyne_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + float time; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace velodyne_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, time, time) + (std::uint16_t, ring, ring) +) + +/** + * @brief Unilidar Point Type + */ +namespace unilidar_ros { +struct Point +{ + PCL_ADD_POINT4D + PCL_ADD_INTENSITY + std::uint16_t ring; + float time; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; +} +POINT_CLOUD_REGISTER_POINT_STRUCT(unilidar_ros::Point, + (float, x, x)(float, y, y)(float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (float, time, time) +) + + +namespace hesai_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + double timestamp; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace velodyne_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (double, timestamp, timestamp) + (std::uint16_t, ring, ring) +) + +namespace ouster_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + uint32_t t; + uint16_t reflectivity; + uint8_t ring; + uint16_t ambient; + uint32_t range; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace ouster_ros + +// clang-format off +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + // use std::uint32_t to avoid conflicting with pcl::uint32_t + (std::uint32_t, t, t) + (std::uint16_t, reflectivity, reflectivity) + (std::uint8_t, ring, ring) + (std::uint16_t, ambient, ambient) + (std::uint32_t, range, range) +) + +class Preprocess +{ + public: +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Preprocess(); + ~Preprocess(); + + // void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void set(bool feat_en, int lid_type, double bld, int pfilt_num); + + // sensor_msgs::PointCloud2::ConstPtr pointcloud; + PointCloudXYZI pl_full, pl_corn, pl_surf; + PointCloudXYZI pl_buff[128]; //maximum 128 line lidar + vector typess[128]; //maximum 128 line lidar + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; + int time_unit; + double blind; + bool given_offset_time; + ros::Publisher pub_full, pub_surf, pub_corn; + + + private: + // void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); + void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void unilidar_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void give_feature(PointCloudXYZI &pl, vector &types); + void pub_func(PointCloudXYZI &pl, const ros::Time &ct); + int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); + bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); + + int group_size; + double disA, disB, inf_bound; + double limit_maxmid, limit_midmin, limit_maxmin; + double p2l_ratio; + double jump_up_limit, jump_down_limit; + double cos160; + double edgea, edgeb; + double smallp_intersect, smallp_ratio; + double vx, vy, vz; +}; diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/prepare.py b/dimos/mapping/research/go2/autoresearch/lio-1/prepare.py new file mode 100644 index 0000000000..3bc87890a4 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/prepare.py @@ -0,0 +1,166 @@ +# 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 +train.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. The bin + GT are pre-made; see human-debug/ for how. --- +BIN_PATH = str(get_data("go2dds_data1/go2-185959.bin")) # PLNR1 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") # train.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 bin, GT, and built binary all present.""" + ok = True + for label, p in [ + ("input bin", BIN_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__": + print("LIO autoresearch — data + harness check") + print(f" HERE: {HERE}") + ok = check_data() + if ok: + 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" + ) + print("Done! Ready to run train.py.") + else: + print("Setup incomplete — run ./setup.sh.") diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/program.md b/dimos/mapping/research/go2/autoresearch/lio-1/program.md new file mode 100644 index 0000000000..3ec1dd0ebe --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/program.md @@ -0,0 +1,122 @@ +# lio-autoresearch + +This is an experiment to have the LLM do its own research: autonomously tune a +LiDAR-inertial odometry (Point-LIO) pipeline so its trajectory best agrees with +the robot's onboard leg-inertial odometry (`robot_odom`), which is our rough +ground truth. + +## Setup + +To set up a new experiment, work with the user to: + +1. **Agree on a run tag**: propose a tag based on today's date (e.g. `mar5`). The branch `autoresearch/` must not already exist — this is a fresh run. +2. **Create the branch**: `git checkout -b autoresearch/` from current master. +3. **Read the in-scope files**: The repo is small. Read these files for full context: + - `README.md` — repository context. + - `prepare.py` — fixed data paths, the Point-LIO substrate location, and the evaluation (the `evaluate` ground-truth metric). Do not modify. + - `train.py` — the file you modify. The Point-LIO `CONFIG` and the run/eval driver. +4. **Verify the build + data exist**: Check that `point_lio/build/pointlio_mapping` exists and that `python prepare.py` reports the input bin and ground truth present. If not, tell the human to run `./setup.sh`. +5. **Initialize results.tsv**: Create `results.tsv` with just the header row. The baseline will be recorded after the first run. +6. **Confirm and go**: Confirm setup looks good. + +Once you get confirmation, kick off the experimentation. + +## Experimentation + +Each experiment runs the offline Point-LIO once on the recorded Go2 LiDAR+IMU +stream (a fixed input). It runs to completion on CPU — typically ~1-3 minutes. +You launch it simply as: `python train.py` (in the dimos venv / `nix develop`). + +**What you CAN do:** +- Modify `train.py` — this is the only file you edit. The `CONFIG` dict (which maps 1:1 to the Point-LIO yaml) is fair game: covariances, plane threshold, match scale, voxel filter sizes, IMU integration dt, extrinsics, blind range, FoV, etc. You may also adjust the Python pre/post-processing in this file. + +**What you CANNOT do:** +- Modify `prepare.py`. It is read-only. It contains the fixed data paths, the substrate location, and the evaluation harness. +- Modify the `point_lio/` C++ substrate. It is the fixed Point-LIO engine, built once by `setup.sh` (the analog of a fixed model framework). You tune its configuration via `CONFIG`, not its source. +- Install new packages or add dependencies. You can only use what's already in the dimos venv (numpy, matplotlib, and the dimos package). +- Modify the evaluation. The `evaluate` function in `prepare.py` is the ground truth metric. +- Use anything in `human-debug/`. The convert script (`mcap_to_plnr1.py`), the raw `.mcap` recording, and the `.rrd` rerun file there are for **human debugging only** — they are NOT part of the experiment. The input bin and ground truth are already prepared in `data/`; never regenerate them in the loop. + +**The goal is simple: get the lowest `val_ate_xy`** — the 2D (xy) absolute trajectory error of the rigid-aligned LIO trajectory vs the `robot_odom` ground truth, in meters. The metric is **2D only** for now (this recording is flat and single-story, so the robot's z is near-constant and uninformative); 3D criteria (measured height/range change at specific timesteps) will be added later once a stairs recording is captured. The only constraint is that the code runs without crashing and finishes within the time budget. + +**Simplicity criterion**: All else being equal, simpler is better. A small improvement that adds ugly complexity is not worth it. Conversely, removing something and getting equal or better results is a great outcome — that's a simplification win. When evaluating whether to keep a change, weigh the complexity cost against the improvement magnitude. + +**The first run**: Your very first run should always be to establish the baseline, so you will run the training script as is. + +## Output format + +Once the script finishes it prints a summary like this: + +``` +--- +val_ate_xy: 10.964000 +final_err_xy: 46.178 +loop_close_xy: 55.854 +gt_loop_xy: 0.465 +path_len: 182.41 +gt_path_len: 16.52 +num_poses: 576204 +overlap_s: 63.0 +run_seconds: 95.3 +``` + +You can extract the key metric from the log file: + +``` +grep "^val_ate_xy:" run.log +``` + +Each run also writes `viz.png` (top-down LIO-vs-gt + error-over-time) and +`traj_ds.tsv` (a small downsampled trajectory). On a **keep**, commit these two +alongside the experiment (e.g. `git add viz.png traj_ds.tsv` then amend or a +follow-up commit) so each kept experiment in the git history carries its +visualization. The full trajectory stays in `point_lio/Log/` (untracked). + +## Logging results + +When an experiment is done, log it to `results.tsv` (tab-separated, NOT comma-separated — commas break in descriptions). + +The TSV has a header row and 4 columns: + +``` +commit val_ate_xy status description +``` + +1. git commit hash (short, 7 chars) +2. val_ate_xy achieved (e.g. 10.964000) — use 0.000000 for crashes +3. status: `keep`, `discard`, or `crash` +4. short text description of what this experiment tried + +Example: + +``` +commit val_ate_xy status description +a1b2c3d 10.964000 keep baseline (v2_imu) +b2c3d4e 9.880000 keep loosen lidar_meas_cov to 0.05 +c3d4e5f 12.300000 discard match_s 9 (lidar-only style) +d4e5f6g 0.000000 crash extrinsic_R singular (typo) +``` + +## The experiment loop + +The experiment runs on a dedicated branch (e.g. `autoresearch/mar5`). + +LOOP FOREVER: + +1. Look at the git state: the current branch/commit we're on +2. Tune `train.py`'s `CONFIG` with an experimental idea by directly hacking the code. +3. git commit +4. Run the experiment: `python train.py > run.log 2>&1` (redirect everything — do NOT use tee or let output flood your context) +5. Read out the results: `grep "^val_ate_xy:\|^run_seconds:" run.log` +6. If the grep output is empty, the run crashed. Run `tail -n 50 run.log` to read the trace and attempt a fix. If you can't get things to work after more than a few attempts, give up. +7. Record the results in the tsv (NOTE: do not commit the results.tsv file, leave it untracked by git) +8. If val_ate_xy improved (lower), you "advance" the branch, keeping the git commit +9. If val_ate_xy is equal or worse, you git reset back to where you started + +The idea is that you are a completely autonomous researcher trying things out. If they work, keep. If they don't, discard. And you're advancing the branch so that you can iterate. If you feel like you're getting stuck in some way, you can rewind but you should probably do this very very sparingly (if ever). + +**Timeout**: Each experiment should take ~1-3 minutes. If a run exceeds 10 minutes it is killed automatically (treated as a failure — discard and revert). + +**Crashes**: If a run crashes (a bad config, a bug, etc.), use your judgment: If it's something dumb and easy to fix (e.g. a typo, a singular matrix), fix it and re-run. If the idea itself is fundamentally broken, just skip it, log "crash" as the status in the tsv, and move on. + +**NEVER STOP**: Once the experiment loop has begun (after the initial setup), do NOT pause to ask the human if you should continue. Do NOT ask "should I keep going?" or "is this a good stopping point?". The human might be asleep, or gone from a computer and expects you to continue working *indefinitely* until you are manually stopped. You are autonomous. If you run out of ideas, think harder — re-read the in-scope files for new angles, try combining previous near-misses, try more radical configuration changes. The loop runs until the human interrupts you, period. diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh b/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh new file mode 100644 index 0000000000..e8fd781c0e --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh @@ -0,0 +1,32 @@ +#!/usr/bin/env bash +# Environment prep for the LIO autoresearch experiment. Run once before the +# experiment loop. Builds the Point-LIO substrate. Python deps (numpy, +# matplotlib) and the dimos package (for get_data) come from the dimos venv — +# this package no longer carries its own uv env. Run inside `nix develop` (or +# with the dimos .venv active) so cmake/eigen/pcl/yaml-cpp/boost are present. +set -euo pipefail +cd "$(dirname "$0")" + +# --- build tools / native deps come from the nix dev shell --- +echo ">> checking build tools..." +command -v cmake >/dev/null || { echo "ERROR: cmake not found (run inside 'nix develop')"; exit 1; } + +# --- dimos venv: numpy/matplotlib + dimos.get_data all live here --- +echo ">> checking dimos venv" +if ! python -c "import dimos, numpy, matplotlib" 2>/dev/null; then + root=$(git rev-parse --show-toplevel) + # shellcheck disable=SC1091 + [ -f "$root/.venv/bin/activate" ] && . "$root/.venv/bin/activate" +fi +python -c "import dimos, numpy, matplotlib" || { + echo "ERROR: dimos venv not active. Activate the dimos .venv or run inside 'nix develop'."; exit 1; } + +# --- build the Point-LIO substrate (fixed; not edited by the agent) --- +echo ">> building point_lio" +cmake -S point_lio -B point_lio/build -DCMAKE_BUILD_TYPE=Release >/dev/null +cmake --build point_lio/build -j"$(nproc 2>/dev/null || echo 4)" + +# --- sanity check (pulls the data via get_data on first run) --- +echo ">> data + harness check" +python prepare.py +echo ">> setup done. Run a baseline with: python train.py > run.log 2>&1" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/train.py b/dimos/mapping/research/go2/autoresearch/lio-1/train.py new file mode 100644 index 0000000000..7ece433532 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/train.py @@ -0,0 +1,180 @@ +# 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 prepare.py, and prints a summary block. + +The Point-LIO C++ substrate (point_lio/) and the evaluation (prepare.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 prepare + +# ---------------------------------------------------------------------------- +# 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 train.py — do not hand-edit; edit CONFIG in train.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 main(): + if not os.path.exists(prepare.POINTLIO_BIN): + raise SystemExit("pointlio binary not built — run ./setup.sh first.") + write_yaml(CONFIG, prepare.ACTIVE_YAML) + if os.path.exists(prepare.TRAJ_PATH): + os.remove(prepare.TRAJ_PATH) + + t0 = time.time() + try: + subprocess.run( + [prepare.POINTLIO_BIN, "--yaml", prepare.ACTIVE_YAML, "--bin", prepare.BIN_PATH], + cwd=prepare.POINTLIO_DIR, + timeout=prepare.RUN_TIMEOUT, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + ) + except subprocess.TimeoutExpired: + raise SystemExit(f"run exceeded {prepare.RUN_TIMEOUT}s — treat as failure") + run_s = time.time() - t0 + + m = prepare.evaluate() # raises if trajectory missing/malformed (= crash) + + 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: {run_s:.1f}") + + # Render viz.png + downsampled traj_ds.tsv for this run (non-fatal: a plotting + # failure must never fail the experiment). Commit these on a "keep". + try: + import visualize + + visualize.render() + except Exception as e: + print(f"(visualize skipped: {e})") + + +if __name__ == "__main__": + main() diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/traj_ds.tsv b/dimos/mapping/research/go2/autoresearch/lio-1/traj_ds.tsv new file mode 100644 index 0000000000..9596a0078f --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/traj_ds.tsv @@ -0,0 +1,2003 @@ +# downsampled raw LIO trajectory +# t_rel x y z +0.32597 0.00095 -0.00080 -0.00334 +0.39621 -0.02611 0.04170 -0.01406 +0.39621 -0.01324 0.02273 -0.01292 +0.45568 0.01529 0.03350 -0.01014 +0.45568 0.03036 0.05168 -0.00274 +0.45568 0.02076 0.04503 -0.00453 +0.52149 0.00911 0.03317 -0.00364 +0.52149 0.00991 0.02698 -0.00189 +0.58683 0.00776 0.03662 0.00020 +0.58683 0.00690 0.05135 -0.00506 +0.65099 -0.01643 0.02819 -0.01138 +0.65099 0.00540 0.02647 -0.00708 +0.71649 0.01067 0.02714 -0.00404 +0.71649 -0.00543 0.02669 -0.00657 +0.78235 -0.01080 0.01159 -0.00548 +0.78235 -0.00606 0.01345 -0.00316 +0.84649 0.00034 0.01876 -0.00375 +0.84649 -0.00122 0.01973 -0.00988 +0.91574 0.01084 0.01367 -0.00691 +0.91574 0.01025 0.01122 -0.00584 +0.97682 0.01480 -0.00365 0.00041 +0.97682 0.01880 0.00388 0.00266 +0.97682 0.01875 -0.00622 0.00578 +1.04241 0.01112 -0.00219 0.00571 +1.04241 0.02159 0.00620 0.00580 +1.10744 0.02540 0.00278 0.00082 +1.10744 0.03168 0.00537 0.00012 +1.17198 0.02292 0.00861 -0.00011 +1.17198 0.01927 0.01409 -0.00095 +1.23750 0.02094 0.00976 0.00102 +1.23750 0.01907 0.00930 -0.00014 +1.30303 0.02011 0.01677 0.00112 +1.30303 0.01082 0.00471 -0.00228 +1.36750 0.01381 0.00273 -0.00180 +1.36750 0.01839 -0.00312 -0.00091 +1.43247 0.00959 -0.00094 0.00243 +1.43247 -0.00483 0.00612 -0.00445 +1.49849 0.00033 0.00862 -0.00128 +1.49849 -0.00478 0.00727 -0.00416 +1.49849 0.00199 0.01501 -0.00438 +1.56327 -0.00402 0.00434 -0.00785 +1.56327 0.00408 0.01107 -0.00636 +1.62800 0.00267 0.01520 -0.00463 +1.62800 -0.00138 0.02630 -0.00799 +1.69395 -0.00649 0.00806 -0.00293 +1.69395 -0.00772 0.00083 -0.00055 +1.75852 -0.00529 0.00661 0.00081 +1.75852 -0.01139 0.00665 -0.00569 +1.82316 -0.00656 0.01257 -0.00757 +1.82316 -0.00603 0.01289 -0.00603 +1.88908 -0.01044 0.01350 -0.00471 +1.88908 -0.02511 0.01890 -0.00811 +1.95399 -0.02402 0.01757 -0.00707 +1.95399 -0.02511 0.01515 -0.00692 +1.95399 -0.02160 0.02619 -0.00989 +2.01914 -0.04047 0.01284 -0.01517 +2.01914 -0.02381 0.01163 -0.01368 +2.08401 -0.02035 0.00486 -0.00868 +2.08401 -0.02624 0.00504 -0.01052 +2.14973 -0.03597 0.00738 -0.01164 +2.14973 -0.03380 0.00075 -0.00848 +2.21445 -0.02763 0.00446 -0.00538 +2.21445 -0.03034 -0.01223 -0.00818 +2.27949 -0.02213 -0.01214 -0.00736 +2.27949 -0.02265 -0.01258 -0.00436 +2.34507 -0.02819 -0.01289 -0.00517 +2.34507 -0.02515 -0.01268 -0.00277 +2.41390 -0.02410 -0.01783 0.00055 +2.41390 -0.03198 -0.01879 -0.00220 +2.47497 -0.02311 -0.01144 -0.00265 +2.47497 -0.01100 0.00433 -0.00597 +2.47497 -0.00070 -0.00041 -0.00320 +2.53989 -0.01014 0.00409 -0.00604 +2.53989 -0.01981 0.00695 -0.00883 +2.60514 -0.02326 0.00158 -0.00551 +2.60514 -0.02594 -0.00250 -0.00676 +2.67002 -0.02777 0.00428 -0.00639 +2.67002 -0.03423 -0.00187 -0.00925 +2.73584 -0.03718 -0.00621 -0.01057 +2.73584 -0.02999 -0.01305 -0.00915 +2.80050 -0.03146 -0.01108 -0.00838 +2.80050 -0.04659 -0.01229 -0.01027 +2.86558 -0.04865 -0.01344 -0.00845 +2.86558 -0.05233 -0.01622 -0.00934 +2.86558 -0.04882 -0.00614 -0.01012 +2.93095 -0.06307 -0.01955 -0.01635 +2.93095 -0.05422 -0.01636 -0.01379 +2.99552 -0.06055 -0.01678 -0.01093 +2.99552 -0.06045 -0.01243 -0.01306 +3.06047 -0.06983 -0.01473 -0.01202 +3.06047 -0.07261 -0.01548 -0.01116 +3.12602 -0.07590 -0.00656 -0.01181 +3.12602 -0.07654 -0.00675 -0.01512 +3.19106 -0.09460 -0.01397 -0.01790 +3.19106 -0.09183 -0.01130 -0.01724 +3.25597 -0.08989 -0.00458 -0.01795 +3.25597 -0.09835 -0.00498 -0.01728 +3.32152 -0.09909 -0.01513 -0.01729 +3.32152 -0.10202 -0.01846 -0.01590 +3.38654 -0.09770 -0.01079 -0.01759 +3.38654 -0.12406 -0.03074 -0.02291 +3.45154 -0.12446 -0.04014 -0.02258 +3.45154 -0.12873 -0.04015 -0.01983 +3.45154 -0.13702 -0.04190 -0.02096 +3.51697 -0.16706 -0.05012 -0.02440 +3.51697 -0.17135 -0.05524 -0.02203 +3.58152 -0.17193 -0.05584 -0.02294 +3.58152 -0.17783 -0.06718 -0.02463 +3.64695 -0.18885 -0.07233 -0.02802 +3.64695 -0.19025 -0.07871 -0.02330 +3.71251 -0.19689 -0.07919 -0.02647 +3.71251 -0.20692 -0.08048 -0.02844 +3.77699 -0.21442 -0.08733 -0.02515 +3.77699 -0.22237 -0.09303 -0.02281 +3.84208 -0.22330 -0.08122 -0.02569 +3.84208 -0.24482 -0.08798 -0.03591 +3.90779 -0.24676 -0.09294 -0.03116 +3.90779 -0.24585 -0.09820 -0.02590 +3.90779 -0.24787 -0.08901 -0.03222 +3.97246 -0.25604 -0.09526 -0.03142 +3.97246 -0.26551 -0.10819 -0.02816 +4.03752 -0.27461 -0.10940 -0.02787 +4.03752 -0.27340 -0.09817 -0.03367 +4.10347 -0.29029 -0.11214 -0.03758 +4.10347 -0.29120 -0.11614 -0.03346 +4.16847 -0.30007 -0.11532 -0.03528 +4.16847 -0.31582 -0.11846 -0.04133 +4.23299 -0.31912 -0.12199 -0.03940 +4.23299 -0.32958 -0.13068 -0.03691 +4.29877 -0.33130 -0.13027 -0.03804 +4.29877 -0.35873 -0.15248 -0.04315 +4.36302 -0.36313 -0.15505 -0.03813 +4.36302 -0.37593 -0.16275 -0.03774 +4.36302 -0.38593 -0.16290 -0.04455 +4.42846 -0.39486 -0.16997 -0.04305 +4.42846 -0.40742 -0.18322 -0.03632 +4.49396 -0.42258 -0.18502 -0.03903 +4.49396 -0.42772 -0.17961 -0.04633 +4.55854 -0.45459 -0.19241 -0.04972 +4.55854 -0.45924 -0.19417 -0.04296 +4.62397 -0.47035 -0.19851 -0.04703 +4.62397 -0.48119 -0.20066 -0.05212 +4.68854 -0.49400 -0.21069 -0.05152 +4.68854 -0.50171 -0.21722 -0.04726 +4.75447 -0.50894 -0.21650 -0.05107 +4.75447 -0.54391 -0.22495 -0.06060 +4.81898 -0.55080 -0.23005 -0.05875 +4.81898 -0.56656 -0.24655 -0.05544 +4.88410 -0.58622 -0.25618 -0.06078 +4.88410 -0.60560 -0.26408 -0.06223 +4.88410 -0.62111 -0.26818 -0.05724 +4.94995 -0.63533 -0.27057 -0.06449 +4.94995 -0.64847 -0.28010 -0.07523 +5.01573 -0.67622 -0.30525 -0.07903 +5.01573 -0.68288 -0.30600 -0.07284 +5.08818 -0.69717 -0.30537 -0.07768 +5.08818 -0.70007 -0.32150 -0.08501 +5.14548 -0.71048 -0.33986 -0.07803 +5.14548 -0.72747 -0.34742 -0.07210 +5.21004 -0.73677 -0.34636 -0.07935 +5.21004 -0.75802 -0.36554 -0.08853 +5.27518 -0.75882 -0.37540 -0.08174 +5.27518 -0.76886 -0.37765 -0.08061 +5.34047 -0.77274 -0.37841 -0.08807 +5.34047 -0.79457 -0.38874 -0.08720 +5.40499 -0.80971 -0.40089 -0.07928 +5.40499 -0.82372 -0.40322 -0.08317 +5.47050 -0.85474 -0.40219 -0.10329 +5.47050 -0.86885 -0.41235 -0.10103 +5.47050 -0.86844 -0.43077 -0.09294 +5.53905 -0.87831 -0.43550 -0.09833 +5.53905 -0.89713 -0.44376 -0.10317 +5.60047 -0.91681 -0.44736 -0.09663 +5.60047 -0.92818 -0.44784 -0.09652 +5.66550 -0.94304 -0.44372 -0.10720 +5.66550 -0.99649 -0.45361 -0.11381 +5.73239 -1.01475 -0.47023 -0.10867 +5.73239 -1.02373 -0.47443 -0.11053 +5.79619 -1.03708 -0.47328 -0.12095 +5.79619 -1.05404 -0.48181 -0.11577 +5.86142 -1.07032 -0.49491 -0.11075 +5.86142 -1.08979 -0.49568 -0.11762 +5.92650 -1.04690 -0.50470 -0.10795 +5.92650 -1.04726 -0.48227 -0.10942 +5.99198 -1.04664 -0.47875 -0.11012 +5.99198 -1.05275 -0.47258 -0.12053 +6.05608 -1.02781 -0.47115 -0.11648 +6.05608 -1.01583 -0.44709 -0.11357 +6.12197 -1.01293 -0.44150 -0.11666 +6.12197 -0.98541 -0.42143 -0.11665 +6.18655 -0.97587 -0.40826 -0.11519 +6.18655 -0.97503 -0.39118 -0.11413 +6.25176 -0.97383 -0.38296 -0.12165 +6.25176 -0.95745 -0.36885 -0.12851 +6.31748 -0.95442 -0.35498 -0.12673 +6.31748 -0.94566 -0.35367 -0.12258 +6.38244 -0.93514 -0.34445 -0.12448 +6.38244 -0.92371 -0.33575 -0.13234 +6.44747 -0.91391 -0.30902 -0.13183 +6.44747 -0.90768 -0.29655 -0.13400 +6.51296 -0.90048 -0.28875 -0.13149 +6.51296 -0.89766 -0.27334 -0.13652 +6.57747 -0.89193 -0.25236 -0.13392 +6.57747 -0.89238 -0.24380 -0.13570 +6.57747 -0.87738 -0.21683 -0.14303 +6.64317 -0.87784 -0.19804 -0.14344 +6.64317 -0.86733 -0.18037 -0.14034 +6.70915 -0.86836 -0.16387 -0.14122 +6.70915 -0.86631 -0.14626 -0.14946 +6.77298 -0.85012 -0.11882 -0.15399 +6.77298 -0.83638 -0.09642 -0.15759 +6.83849 -0.83676 -0.08865 -0.15648 +6.83849 -0.83149 -0.07322 -0.16695 +6.90396 -0.82352 -0.06028 -0.16848 +6.90396 -0.82011 -0.05518 -0.16664 +6.96851 -0.81326 -0.04768 -0.17203 +6.96851 -0.79848 -0.03790 -0.16809 +7.03397 -0.79144 -0.03687 -0.16331 +7.03397 -0.79075 -0.04645 -0.16466 +7.09919 -0.78119 -0.05035 -0.17263 +7.09919 -0.77696 -0.06008 -0.17357 +7.16460 -0.77414 -0.05509 -0.16980 +7.16460 -0.77395 -0.05977 -0.16899 +7.22946 -0.76933 -0.05347 -0.17051 +7.22946 -0.76172 -0.05553 -0.16479 +7.29498 -0.75638 -0.06773 -0.16676 +7.35998 -0.75091 -0.06905 -0.17525 +7.35998 -0.74503 -0.08378 -0.17113 +7.42531 -0.74103 -0.09320 -0.16743 +7.42531 -0.71870 -0.09915 -0.17016 +7.49048 -0.70861 -0.10908 -0.16574 +7.49048 -0.69052 -0.11569 -0.15969 +7.55545 -0.67442 -0.13498 -0.15884 +7.55545 -0.65606 -0.14391 -0.15545 +7.62047 -0.64258 -0.14290 -0.15649 +7.62047 -0.62535 -0.16195 -0.14780 +7.68607 -0.63776 -0.17651 -0.13170 +7.68607 -0.64162 -0.18398 -0.12333 +7.75098 -0.63636 -0.21357 -0.11198 +7.75098 -0.63409 -0.22455 -0.09488 +7.81598 -0.64880 -0.23207 -0.08384 +7.81598 -0.64682 -0.23603 -0.07496 +7.88146 -0.64863 -0.26147 -0.06333 +7.88146 -0.65751 -0.28443 -0.05215 +7.94744 -0.66938 -0.29596 -0.04959 +7.94744 -0.66443 -0.31305 -0.04684 +8.01211 -0.67435 -0.32919 -0.04413 +8.01211 -0.68557 -0.34284 -0.04626 +8.07774 -0.68925 -0.34716 -0.05361 +8.07774 -0.69183 -0.36257 -0.05119 +8.14208 -0.70732 -0.37614 -0.04581 +8.14208 -0.73063 -0.39071 -0.05401 +8.14208 -0.73458 -0.40558 -0.05496 +8.20809 -0.74283 -0.43538 -0.04673 +8.27247 -0.75355 -0.45065 -0.04952 +8.27247 -0.77173 -0.46716 -0.05683 +8.27247 -0.77736 -0.47865 -0.05981 +8.33816 -0.78872 -0.48466 -0.04648 +8.33816 -0.79867 -0.49995 -0.05567 +8.40350 -0.80505 -0.50436 -0.06124 +8.40350 -0.79775 -0.52319 -0.05427 +8.46798 -0.80343 -0.53126 -0.04600 +8.46798 -0.80767 -0.54197 -0.05226 +8.53349 -0.80381 -0.55027 -0.05147 +8.53349 -0.79471 -0.56440 -0.04103 +8.59907 -0.79584 -0.57152 -0.03096 +8.59907 -0.79728 -0.57623 -0.03448 +8.66396 -0.78887 -0.57745 -0.02829 +8.66396 -0.77431 -0.57380 -0.01851 +8.66396 -0.76313 -0.57341 -0.02696 +8.72946 -0.75009 -0.57158 -0.03353 +8.72946 -0.72307 -0.57138 -0.02409 +8.79447 -0.70783 -0.57775 -0.01369 +8.79447 -0.68948 -0.58072 -0.02120 +8.85949 -0.67113 -0.57363 -0.02006 +8.85949 -0.63899 -0.57204 0.00171 +8.92447 -0.61516 -0.56742 0.00855 +8.92447 -0.59853 -0.56559 -0.00161 +8.98996 -0.57259 -0.55096 0.00327 +8.98996 -0.54812 -0.52656 0.00695 +9.05572 -0.53020 -0.50692 0.00563 +9.05572 -0.51158 -0.48844 -0.01071 +9.12006 -0.47941 -0.46140 -0.01312 +9.12006 -0.45515 -0.43857 -0.00292 +9.18522 -0.43488 -0.41144 -0.00642 +9.18522 -0.40589 -0.38237 -0.01395 +9.25048 -0.38393 -0.34503 -0.01641 +9.25048 -0.37210 -0.32493 -0.00410 +9.31548 -0.36579 -0.30558 -0.00998 +9.31548 -0.33927 -0.26582 -0.01971 +9.38097 -0.30995 -0.22608 -0.01548 +9.38097 -0.30431 -0.20417 -0.01377 +9.44596 -0.27377 -0.16266 -0.02575 +9.44596 -0.24660 -0.12894 -0.03060 +9.44596 -0.22605 -0.10000 -0.02298 +9.51100 -0.21481 -0.06921 -0.02044 +9.51100 -0.19037 -0.02423 -0.02978 +9.57906 -0.16080 0.03028 -0.03311 +9.57906 -0.13306 0.07077 -0.03829 +9.64185 -0.12147 0.10049 -0.05134 +9.64185 -0.08678 0.12655 -0.06003 +9.70648 -0.05440 0.16341 -0.06739 +9.70648 -0.03198 0.19002 -0.07909 +9.77200 0.00553 0.23098 -0.08582 +9.77200 0.03807 0.27004 -0.09390 +9.83692 0.06479 0.30563 -0.10137 +9.83692 0.08355 0.32589 -0.11157 +9.90197 0.10929 0.36033 -0.11292 +9.90197 0.15264 0.39789 -0.11509 +9.96762 0.18540 0.41950 -0.11098 +9.96762 0.22287 0.44704 -0.11112 +10.03300 0.25742 0.48323 -0.11715 +10.03300 0.30074 0.52062 -0.11807 +10.09760 0.34347 0.57080 -0.12490 +10.09760 0.37526 0.61513 -0.12871 +10.16300 0.42015 0.66211 -0.12734 +10.16300 0.44323 0.69000 -0.12523 +10.22810 0.47113 0.73796 -0.11737 +10.22810 0.51023 0.77994 -0.10708 +10.22810 0.55050 0.83185 -0.10911 +10.29320 0.57634 0.85135 -0.11530 +10.29320 0.60423 0.87799 -0.12325 +10.35850 0.63534 0.91551 -0.12072 +10.35850 0.66655 0.95984 -0.12260 +10.42400 0.68771 0.98931 -0.12899 +10.42400 0.72108 1.03331 -0.11851 +10.48900 0.74805 1.07502 -0.11510 +10.48900 0.77192 1.11686 -0.12153 +10.55400 0.79759 1.15600 -0.12781 +10.55400 0.81487 1.19453 -0.13247 +10.55400 0.84039 1.24199 -0.13934 +10.61850 0.86056 1.29191 -0.14896 +10.61850 0.88358 1.33578 -0.15547 +10.68400 0.90418 1.37425 -0.15638 +10.68400 0.93013 1.42152 -0.16294 +10.74950 0.94063 1.47214 -0.16543 +10.74950 0.96192 1.51815 -0.17361 +10.81400 0.98175 1.56877 -0.17267 +10.81400 1.00534 1.62717 -0.18007 +10.87910 1.01023 1.68577 -0.19338 +10.87910 1.02605 1.72427 -0.20680 +10.94450 1.03753 1.77643 -0.21663 +10.94450 1.05198 1.82947 -0.22667 +10.94450 1.06739 1.87702 -0.23477 +11.00970 1.09805 1.92984 -0.23257 +11.00970 1.11267 1.98077 -0.23857 +11.07450 1.11746 2.02872 -0.24329 +11.07450 1.13670 2.09164 -0.24279 +11.14000 1.15101 2.14043 -0.25165 +11.14000 1.16158 2.19320 -0.26363 +11.20460 1.17767 2.25453 -0.27207 +11.20460 1.20083 2.31778 -0.27822 +11.26950 1.21514 2.36971 -0.29522 +11.26950 1.22354 2.43236 -0.30703 +11.33550 1.22759 2.48785 -0.30620 +11.33550 1.24580 2.52543 -0.31258 +11.33550 1.25417 2.57316 -0.32213 +11.40000 1.26731 2.63891 -0.32533 +11.40000 1.27905 2.68990 -0.33881 +11.46580 1.27399 2.73641 -0.36039 +11.46580 1.28049 2.78793 -0.38173 +11.53010 1.28308 2.84077 -0.39603 +11.53010 1.28044 2.87751 -0.41645 +11.59580 1.28951 2.92690 -0.42114 +11.59580 1.29265 2.98587 -0.42447 +11.66050 1.29147 3.04266 -0.43699 +11.66050 1.31230 3.13435 -0.44181 +11.72550 1.28666 3.18857 -0.45272 +11.72550 1.28886 3.24193 -0.45914 +11.79110 1.29248 3.29722 -0.46323 +11.79110 1.28512 3.34212 -0.47717 +11.79110 1.27495 3.38622 -0.48883 +11.85550 1.27839 3.44220 -0.50028 +11.85550 1.29246 3.49481 -0.50878 +11.92060 1.27243 3.53319 -0.52612 +11.92060 1.25640 3.59786 -0.53666 +11.98600 1.25985 3.66180 -0.54868 +11.98600 1.25089 3.71169 -0.56320 +12.05100 1.23764 3.76236 -0.58004 +12.05100 1.22429 3.81413 -0.59201 +12.11600 1.22701 3.87644 -0.60130 +12.11600 1.23392 3.93431 -0.61702 +12.18110 1.20628 3.98206 -0.62972 +12.18110 1.19554 4.03072 -0.64480 +12.24600 1.19784 4.07921 -0.64645 +12.24600 1.18306 4.11895 -0.65548 +12.24600 1.16835 4.15503 -0.66693 +12.31110 1.16046 4.19704 -0.67565 +12.31110 1.15520 4.24597 -0.68330 +12.37650 1.14351 4.26813 -0.69844 +12.37650 1.12088 4.29141 -0.71443 +12.44140 1.11816 4.32974 -0.73313 +12.44140 1.12435 4.36697 -0.73859 +12.50650 1.10901 4.40250 -0.74478 +12.50650 1.09561 4.43601 -0.74485 +12.50650 1.09039 4.48323 -0.74264 +12.57200 1.10214 4.52140 -0.74263 +12.57200 1.09080 4.55516 -0.75560 +12.63710 1.08335 4.57636 -0.76711 +12.63710 1.08695 4.60574 -0.77188 +12.70150 1.08791 4.63406 -0.78446 +12.70150 1.09329 4.66857 -0.79677 +12.76750 1.09061 4.69518 -0.81079 +12.76750 1.09111 4.72156 -0.81479 +12.83250 1.10066 4.73787 -0.81393 +12.83250 1.10451 4.74871 -0.81805 +12.83250 1.09402 4.76744 -0.82115 +12.89910 1.10226 4.79212 -0.81263 +12.89910 1.11934 4.80902 -0.81031 +12.96250 1.12189 4.84242 -0.80841 +12.96250 1.11555 4.86197 -0.81094 +13.02750 1.12262 4.88622 -0.81249 +13.02750 1.14400 4.89742 -0.81212 +13.09250 1.14190 4.90447 -0.82573 +13.09250 1.13779 4.92617 -0.83422 +13.09250 1.13693 4.95061 -0.83751 +13.15770 1.14311 4.96437 -0.83613 +13.15770 1.11752 4.98320 -0.84404 +13.22300 1.11231 5.00677 -0.84434 +13.22300 1.11000 5.02969 -0.84128 +13.28800 1.11201 5.04743 -0.84174 +13.28800 1.11847 5.08126 -0.83708 +13.28800 1.11005 5.10461 -0.84520 +13.35350 1.11263 5.12896 -0.84743 +13.35350 1.11992 5.14791 -0.85473 +13.41860 1.11507 5.17657 -0.86556 +13.41860 1.11078 5.19767 -0.87683 +13.48350 1.11569 5.21596 -0.88084 +13.48350 1.12382 5.22539 -0.88492 +13.54850 1.13637 5.23766 -0.89070 +13.54850 1.12963 5.25262 -0.89036 +13.54850 1.13247 5.27524 -0.88563 +13.61400 1.14370 5.28865 -0.88141 +13.61400 1.14759 5.30015 -0.88260 +13.67850 1.14230 5.31685 -0.88310 +13.67850 1.14412 5.33759 -0.88494 +13.74600 1.16208 5.34987 -0.88838 +13.74600 1.16537 5.35622 -0.89145 +13.74600 1.16133 5.37270 -0.89177 +13.80910 1.16708 5.39208 -0.88674 +13.80910 1.17936 5.40429 -0.87977 +13.87450 1.17268 5.41007 -0.88058 +13.87450 1.17150 5.42222 -0.88244 +13.93950 1.17717 5.43295 -0.88243 +13.93950 1.18777 5.45079 -0.88507 +14.00450 1.19778 5.46348 -0.89036 +14.00450 1.20045 5.47766 -0.90033 +14.00450 1.20778 5.49627 -0.90756 +14.06950 1.21140 5.51402 -0.91326 +14.06950 1.20334 5.52892 -0.91372 +14.13450 1.19273 5.54572 -0.92097 +14.13450 1.19251 5.56279 -0.91951 +14.20000 1.19164 5.58242 -0.92542 +14.20000 1.18906 5.60701 -0.93038 +14.20000 1.17843 5.62418 -0.93299 +14.26500 1.17820 5.64626 -0.93393 +14.26500 1.17996 5.66758 -0.93298 +14.33050 1.17359 5.68868 -0.93393 +14.33050 1.17366 5.70513 -0.94155 +14.39620 1.17786 5.72525 -0.94329 +14.39620 1.18006 5.74521 -0.94890 +14.46000 1.17345 5.75228 -0.95061 +14.46000 1.16783 5.76922 -0.95469 +14.46000 1.17081 5.78827 -0.95647 +14.52510 1.17340 5.81057 -0.95281 +14.52510 1.17375 5.81819 -0.95230 +14.59040 1.17436 5.83061 -0.95655 +14.59040 1.17510 5.85092 -0.95883 +14.65550 1.21752 5.86364 -0.95437 +14.65550 1.22933 5.87677 -0.96182 +14.72050 1.24825 5.89260 -0.96629 +14.72050 1.25656 5.90688 -0.96688 +14.78580 1.26607 5.92256 -0.96586 +14.78580 1.27023 5.93361 -0.97025 +14.78580 1.27090 5.94696 -0.96359 +14.85120 1.27653 5.96307 -0.95945 +14.85120 1.28598 5.98258 -0.96106 +14.91600 1.28149 5.99379 -0.97103 +14.91600 1.27978 6.00725 -0.97224 +14.98150 1.28280 6.02236 -0.96973 +14.98150 1.28667 6.04010 -0.97230 +15.04660 1.28404 6.05821 -0.98033 +15.04660 1.27454 6.07406 -0.98826 +15.04660 1.27389 6.09143 -0.99320 +15.11150 1.27147 6.10824 -0.99706 +15.11150 1.25048 6.12368 -1.00173 +15.17700 1.23346 6.13768 -0.99933 +15.17700 1.22957 6.15662 -0.99682 +15.24150 1.22524 6.17521 -0.99251 +15.24150 1.22438 6.19261 -0.99393 +15.30700 1.22108 6.20963 -0.99851 +15.30700 1.22863 6.22557 -1.00158 +15.37250 1.23627 6.24484 -1.00434 +15.37250 1.22987 6.25619 -1.01691 +15.37250 1.24636 6.27308 -1.01976 +15.43700 1.26078 6.30104 -1.02199 +15.43700 1.26918 6.30419 -1.02392 +15.50230 1.28392 6.31522 -1.02202 +15.50230 1.31010 6.32888 -1.00782 +15.56750 1.32686 6.34271 -1.00453 +15.56750 1.33524 6.34631 -1.01025 +15.63250 1.33486 6.34845 -1.01218 +15.63250 1.34390 6.35976 -1.00440 +15.69800 1.36275 6.37170 -1.00212 +15.69800 1.37121 6.37646 -1.00727 +15.69800 1.38055 6.38206 -1.01541 +15.76250 1.38628 6.39710 -1.01637 +15.76250 1.40078 6.41096 -1.01539 +15.82850 1.41277 6.41950 -1.00669 +15.82850 1.40842 6.43235 -0.99805 +15.89330 1.40741 6.44860 -0.99004 +15.89330 1.41260 6.45934 -0.98142 +15.95800 1.41172 6.47178 -0.98160 +15.95800 1.41455 6.48242 -0.98683 +16.02300 1.42397 6.49400 -0.99547 +16.02300 1.42795 6.52095 -1.00962 +16.02300 1.44375 6.53555 -1.01250 +16.08850 1.44551 6.54998 -1.02111 +16.15350 1.45320 6.57501 -1.01852 +16.15350 1.46352 6.59577 -1.01696 +16.15350 1.46516 6.61494 -1.01865 +16.21940 1.46479 6.63500 -1.01668 +16.21940 1.46894 6.65988 -1.01455 +16.28420 1.47697 6.67338 -1.02379 +16.28420 1.47726 6.69430 -1.02946 +16.34860 1.48204 6.72294 -1.03055 +16.34860 1.49959 6.74105 -1.02870 +16.41450 1.49962 6.75392 -1.03403 +16.41450 1.50688 6.76758 -1.03457 +16.47910 1.51056 6.78421 -1.03555 +16.47910 1.51775 6.80125 -1.04241 +16.54400 1.52945 6.81692 -1.04968 +16.54400 1.54134 6.83403 -1.05849 +16.60950 1.55604 6.85532 -1.05809 +16.60950 1.55929 6.87394 -1.06411 +16.60950 1.55721 6.88834 -1.06494 +16.67920 1.55520 6.90611 -1.06464 +16.73950 1.55647 6.92906 -1.06561 +16.73950 1.56137 6.94625 -1.07005 +16.73950 1.55514 6.96481 -1.07363 +16.80500 1.55443 6.98531 -1.06472 +16.80500 1.55022 7.00576 -1.06375 +16.87010 1.55270 7.01767 -1.06510 +16.87010 1.56077 7.03310 -1.06934 +16.93500 1.56936 7.05127 -1.06904 +16.93500 1.57515 7.06519 -1.07092 +17.00050 1.57103 7.09631 -1.05919 +17.00050 1.56580 7.11718 -1.04834 +17.06540 1.57524 7.13942 -1.04767 +17.06540 1.57426 7.15963 -1.05205 +17.13050 1.57810 7.18327 -1.04956 +17.13050 1.58809 7.20738 -1.05089 +17.19570 1.60333 7.22835 -1.05913 +17.19570 1.60269 7.24865 -1.07515 +17.26060 1.62597 7.27700 -1.07946 +17.26060 1.65187 7.30014 -1.08125 +17.32550 1.66422 7.31641 -1.08867 +17.32550 1.68144 7.33545 -1.09127 +17.39090 1.69903 7.36029 -1.08462 +17.39090 1.71569 7.37764 -1.08131 +17.45560 1.72581 7.38030 -1.09144 +17.45560 1.73973 7.38940 -1.08991 +17.45560 1.76202 7.40634 -1.08408 +17.52120 1.78151 7.41394 -1.08781 +17.52120 1.78948 7.42842 -1.09139 +17.58650 1.80180 7.44315 -1.08955 +17.58650 1.82169 7.45577 -1.09064 +17.65160 1.83356 7.45545 -1.09566 +17.65160 1.82059 7.45804 -1.10509 +17.71650 1.82426 7.47364 -1.10083 +17.71650 1.83104 7.48404 -1.09366 +17.78180 1.82563 7.48859 -1.09015 +17.78180 1.81869 7.49784 -1.08412 +17.84710 1.82201 7.50875 -1.07185 +17.84710 1.83039 7.51805 -1.06654 +17.91200 1.82727 7.52413 -1.06518 +17.91200 1.82444 7.53052 -1.06968 +17.97750 1.83105 7.53891 -1.07451 +17.97750 1.84790 7.54446 -1.07680 +17.97750 1.85148 7.54635 -1.08419 +18.04200 1.85792 7.55701 -1.08540 +18.10710 1.88308 7.56223 -1.07598 +18.10710 1.88495 7.56170 -1.08351 +18.10710 1.88460 7.55375 -1.09341 +18.17250 1.88906 7.55150 -1.08705 +18.17250 1.90270 7.54656 -1.08452 +18.23800 1.91419 7.55848 -1.07142 +18.23800 1.92278 7.56037 -1.06844 +18.30300 1.92975 7.55945 -1.07388 +18.30300 1.93883 7.56353 -1.07885 +18.36800 1.95822 7.57366 -1.07400 +18.36800 1.95768 7.58334 -1.07575 +18.43300 1.97312 7.58707 -1.07018 +18.43300 1.97957 7.59486 -1.06648 +18.43300 1.98918 7.60518 -1.06132 +18.49810 1.98821 7.61793 -1.06101 +18.56350 1.99506 7.62776 -1.06583 +18.56350 2.01518 7.64636 -1.06424 +18.56350 2.02658 7.66042 -1.06134 +18.62820 2.02577 7.67611 -1.06465 +18.62820 2.02481 7.69248 -1.06859 +18.69370 2.03956 7.71554 -1.06023 +18.69370 2.04569 7.73408 -1.06368 +18.75850 2.05616 7.75460 -1.06747 +18.75850 2.06476 7.77636 -1.06926 +18.82400 2.07844 7.79945 -1.06623 +18.82400 2.07595 7.82074 -1.06867 +18.88950 2.07932 7.83796 -1.08694 +18.88950 2.08495 7.86519 -1.09069 +18.95390 2.08537 7.88681 -1.09309 +18.95390 2.07989 7.91476 -1.09532 +19.01900 2.07850 7.93935 -1.10370 +19.01900 2.08918 7.95916 -1.10369 +19.08450 2.08158 7.98736 -1.10432 +19.08450 2.08539 8.00820 -1.11064 +19.14900 2.07954 8.03626 -1.11775 +19.14900 2.09102 8.05163 -1.11975 +19.21450 2.10061 8.07456 -1.12686 +19.21450 2.10724 8.09591 -1.13157 +19.27950 2.11791 8.11519 -1.12676 +19.27950 2.11423 8.14145 -1.13100 +19.27950 2.12089 8.16192 -1.12965 +19.34450 2.11840 8.18575 -1.13452 +19.34450 2.12883 8.20613 -1.13056 +19.41000 2.13302 8.22699 -1.13371 +19.41000 2.14221 8.24418 -1.13012 +19.47510 2.14459 8.26397 -1.13532 +19.47510 2.15643 8.27557 -1.13805 +19.54000 2.17202 8.28889 -1.14070 +19.54000 2.17589 8.30284 -1.14966 +19.60550 2.17683 8.31904 -1.15472 +19.60550 2.17390 8.33753 -1.15605 +19.67050 2.18048 8.35150 -1.14778 +19.67050 2.18756 8.36822 -1.14769 +19.73550 2.19414 8.38242 -1.14716 +19.73550 2.19921 8.39370 -1.14550 +19.73550 2.20838 8.40730 -1.14453 +19.80100 2.22332 8.42513 -1.14685 +19.80100 2.24388 8.44210 -1.14648 +19.86560 2.25972 8.45348 -1.14565 +19.86560 2.27134 8.46379 -1.14709 +19.93040 2.27725 8.48071 -1.14850 +19.93040 2.29544 8.49052 -1.14781 +19.99990 2.30769 8.50392 -1.14208 +19.99990 2.31861 8.51283 -1.14298 +20.06100 2.32597 8.52107 -1.14381 +20.06100 2.33867 8.52504 -1.14484 +20.12650 2.34638 8.53051 -1.13911 +20.12650 2.35655 8.53334 -1.13802 +20.19240 2.36831 8.54519 -1.13224 +20.19240 2.36870 8.54934 -1.13285 +20.25650 2.37406 8.55542 -1.14370 +20.25650 2.37174 8.56823 -1.15330 +20.25650 2.38038 8.57540 -1.15024 +20.32150 2.38369 8.58620 -1.14433 +20.32150 2.38538 8.59515 -1.14261 +20.38700 2.38787 8.60839 -1.13992 +20.38700 2.39936 8.61864 -1.13521 +20.45200 2.39739 8.62915 -1.13140 +20.45200 2.39623 8.64061 -1.13746 +20.51700 2.39595 8.66121 -1.13914 +20.51700 2.41436 8.67679 -1.13533 +20.51700 2.42716 8.69115 -1.13732 +20.58250 2.44103 8.70443 -1.14373 +20.58250 2.46125 8.72338 -1.14264 +20.64750 2.48223 8.72740 -1.14703 +20.64750 2.49403 8.73978 -1.14167 +20.71250 2.50917 8.74673 -1.14475 +20.71250 2.51606 8.75738 -1.14496 +20.77800 2.52356 8.76639 -1.14474 +20.77800 2.53180 8.77131 -1.13770 +20.84260 2.54194 8.78048 -1.13450 +20.84260 2.54867 8.78760 -1.12939 +20.90800 2.55679 8.79212 -1.13237 +20.90800 2.55688 8.79744 -1.13932 +20.97350 2.55603 8.80827 -1.14843 +20.97350 2.56517 8.81481 -1.14645 +20.97350 2.57122 8.82281 -1.14071 +21.03800 2.57704 8.83058 -1.13921 +21.03800 2.58820 8.84426 -1.13449 +21.10350 2.59748 8.84922 -1.13100 +21.10350 2.60317 8.85308 -1.13011 +21.16910 2.61585 8.86487 -1.13368 +21.16910 2.62620 8.87656 -1.13935 +21.23510 2.63551 8.88243 -1.14273 +21.23510 2.64353 8.88993 -1.14198 +21.29910 2.64931 8.90885 -1.14438 +21.29910 2.65916 8.92058 -1.13900 +21.36400 2.67564 8.92394 -1.13270 +21.36400 2.68516 8.93660 -1.13582 +21.42910 2.69215 8.95172 -1.13643 +21.42910 2.70838 8.95994 -1.13292 +21.49400 2.72025 8.96946 -1.13138 +21.49400 2.73140 8.99066 -1.13727 +21.55910 2.76129 9.00574 -1.13176 +21.55910 2.77229 9.01661 -1.13374 +21.62450 2.78180 9.03624 -1.13144 +21.62450 2.79284 9.05891 -1.13164 +21.68950 2.80212 9.07562 -1.12867 +21.68950 2.81365 9.09110 -1.12848 +21.75450 2.82333 9.11315 -1.12816 +21.75450 2.83968 9.12956 -1.12544 +21.82000 2.84798 9.14327 -1.12744 +21.82000 2.86462 9.16287 -1.13201 +21.88490 2.86834 9.17303 -1.13619 +21.88490 2.87555 9.18452 -1.13589 +21.95010 2.88171 9.19885 -1.13310 +21.95010 2.89157 9.21495 -1.13793 +22.01550 2.90917 9.23162 -1.13701 +22.01550 2.91830 9.24590 -1.14157 +22.08000 2.92756 9.25850 -1.14409 +22.08000 2.94400 9.26320 -1.14461 +22.14550 2.93881 9.27460 -1.14505 +22.21050 2.94554 9.28271 -1.14618 +22.21050 2.97533 9.29044 -1.14718 +22.27560 2.97620 9.29396 -1.15107 +22.27560 2.98019 9.30019 -1.14181 +22.34100 2.97646 9.30776 -1.14692 +22.34100 2.97721 9.31070 -1.13852 +22.40600 2.97374 9.31363 -1.12676 +22.40600 2.97862 9.31652 -1.12032 +22.47050 2.99222 9.32345 -1.12049 +22.47050 2.98476 9.32540 -1.12345 +22.53630 2.99169 9.32411 -1.12632 +22.53630 3.00066 9.33134 -1.13458 +22.60150 3.00835 9.33276 -1.13417 +22.60150 2.99789 9.33794 -1.13801 +22.66600 3.01027 9.33778 -1.13629 +22.66600 3.00340 9.34743 -1.13357 +22.73240 3.00011 9.34610 -1.12505 +22.79660 2.99780 9.34580 -1.13208 +22.79660 3.00142 9.34727 -1.13480 +22.86170 2.99436 9.34390 -1.13709 +22.86170 3.00151 9.34406 -1.13819 +22.92670 3.00892 9.34263 -1.14620 +22.92670 3.02502 9.34467 -1.13414 +22.99240 3.02358 9.34378 -1.13355 +22.99240 3.02241 9.34179 -1.13662 +23.05710 3.04182 9.34223 -1.12788 +23.05710 3.04377 9.33723 -1.11866 +23.12200 3.04543 9.33357 -1.12522 +23.12200 3.05426 9.34689 -1.13727 +23.18790 3.05393 9.34778 -1.12708 +23.18790 3.05115 9.34866 -1.13002 +23.25250 3.04506 9.35081 -1.13837 +23.25250 3.05287 9.35143 -1.12741 +23.31700 3.04775 9.34781 -1.11966 +23.31700 3.04019 9.34944 -1.12135 +23.38250 3.04861 9.35865 -1.11920 +23.38250 3.03938 9.35401 -1.11056 +23.44810 3.02989 9.35101 -1.12386 +23.51250 3.02577 9.35088 -1.13426 +23.51250 3.01914 9.34561 -1.13613 +23.57800 3.00065 9.35093 -1.13644 +23.57800 3.00434 9.35500 -1.13978 +23.64360 3.01157 9.35572 -1.13077 +23.64360 2.99802 9.35903 -1.12792 +23.70800 2.99861 9.35493 -1.13626 +23.70800 2.99379 9.35706 -1.12960 +23.77300 2.98559 9.35465 -1.12623 +23.83850 2.98484 9.34703 -1.14065 +23.83850 2.98278 9.33828 -1.13944 +23.90300 2.97258 9.33496 -1.13526 +23.90300 2.97338 9.32054 -1.14113 +23.97310 2.97588 9.30382 -1.13744 +23.97310 2.97760 9.28891 -1.12909 +24.03400 2.96927 9.27177 -1.12393 +24.03400 2.96481 9.25563 -1.11246 +24.09900 2.94974 9.25371 -1.10953 +24.09900 2.91821 9.24431 -1.09404 +24.16500 2.90934 9.23219 -1.10433 +24.16500 2.91070 9.22621 -1.09683 +24.22960 2.89969 9.21875 -1.09421 +24.22960 2.86441 9.22888 -1.09512 +24.29390 2.85170 9.21860 -1.10262 +24.29390 2.83516 9.21077 -1.10517 +24.35950 2.82381 9.20236 -1.11289 +24.35950 2.81958 9.18701 -1.13709 +24.42450 2.82017 9.17206 -1.13349 +24.42450 2.82227 9.15395 -1.14130 +24.48950 2.81853 9.14161 -1.13636 +24.48950 2.82049 9.13396 -1.14277 +24.55450 2.82572 9.12676 -1.15087 +24.55450 2.82661 9.12450 -1.15769 +24.62000 2.82416 9.11775 -1.15518 +24.62000 2.81448 9.11298 -1.16179 +24.68500 2.80161 9.11092 -1.16850 +24.68500 2.79951 9.10705 -1.16160 +24.75050 2.79630 9.10754 -1.16456 +24.75050 2.79139 9.10545 -1.16659 +24.81560 2.78149 9.10403 -1.16661 +24.81560 2.77375 9.11025 -1.16498 +24.88100 2.77309 9.12194 -1.16460 +24.88100 2.75230 9.13273 -1.16460 +24.94600 2.74506 9.13289 -1.16512 +24.94600 2.75202 9.14231 -1.15578 +25.01150 2.74379 9.14986 -1.15249 +25.01150 2.74888 9.15012 -1.14523 +25.07650 2.74388 9.14820 -1.14750 +25.07650 2.75365 9.15412 -1.14253 +25.14150 2.75720 9.15668 -1.13524 +25.14150 2.75459 9.15724 -1.13909 +25.20700 2.75981 9.16580 -1.14028 +25.20700 2.76750 9.16853 -1.14393 +25.27200 2.77479 9.17100 -1.13483 +25.27200 2.77353 9.16984 -1.14306 +25.33650 2.78540 9.17715 -1.14786 +25.33650 2.79239 9.17481 -1.14701 +25.40250 2.79397 9.17584 -1.14486 +25.40250 2.79119 9.18192 -1.14608 +25.46750 2.79957 9.18653 -1.14838 +25.46750 2.81326 9.17883 -1.14607 +25.53260 2.81776 9.17746 -1.14811 +25.53260 2.81996 9.17688 -1.14756 +25.53260 2.82023 9.17333 -1.14120 +25.59750 2.82607 9.16734 -1.12870 +25.59750 2.82165 9.16437 -1.13540 +25.66250 2.84687 9.17766 -1.11879 +25.66250 2.84753 9.17762 -1.10673 +25.72750 2.85335 9.17295 -1.10855 +25.72750 2.86832 9.17060 -1.10403 +25.79300 2.87437 9.16707 -1.09009 +25.79300 2.87828 9.16673 -1.08339 +25.85800 2.88370 9.16044 -1.08819 +25.85800 2.89168 9.15892 -1.08650 +25.92330 2.89790 9.14889 -1.07930 +25.92330 2.90452 9.13858 -1.08614 +25.98850 2.90497 9.12452 -1.08899 +25.98850 2.90027 9.11235 -1.08793 +26.05350 2.90057 9.09532 -1.08754 +26.05350 2.90284 9.08208 -1.09817 +26.11820 2.91737 9.06645 -1.09668 +26.11820 2.91812 9.04975 -1.10054 +26.18400 2.91797 9.02853 -1.10865 +26.18400 2.91967 9.00642 -1.11355 +26.24910 2.91622 8.98976 -1.11332 +26.24910 2.90968 8.96395 -1.11492 +26.32000 2.91027 8.93735 -1.12002 +26.32000 2.95319 8.92643 -1.10993 +26.37900 2.95663 8.90643 -1.10499 +26.37900 2.96260 8.88915 -1.10436 +26.44420 2.97427 8.87570 -1.09836 +26.44420 2.98759 8.85937 -1.08054 +26.50900 2.98937 8.83914 -1.07075 +26.50900 2.99388 8.82239 -1.07192 +26.57450 3.02975 8.80857 -1.05139 +26.57450 3.04089 8.78824 -1.05158 +26.63950 3.04798 8.77307 -1.05699 +26.63950 3.05726 8.75834 -1.05676 +26.70450 3.08901 8.74207 -1.03678 +26.70450 3.10092 8.72328 -1.03630 +26.77000 3.11018 8.70865 -1.03594 +26.77000 3.14494 8.69895 -1.02394 +26.84040 3.15803 8.68069 -1.01339 +26.84040 3.16314 8.66990 -1.01372 +26.89950 3.16585 8.65625 -1.02310 +26.89950 3.18786 8.64128 -1.01384 +26.96530 3.19505 8.62543 -1.00939 +26.96530 3.19969 8.61567 -1.00630 +27.03000 3.22501 8.60702 -0.99896 +27.03000 3.23973 8.59103 -0.99182 +27.09500 3.24865 8.58198 -0.98826 +27.09500 3.24835 8.57656 -0.98886 +27.16030 3.27967 8.56621 -0.97402 +27.16030 3.28960 8.56360 -0.97349 +27.22520 3.29481 8.57100 -0.98155 +27.22520 3.32417 8.58268 -0.97836 +27.29000 3.33042 8.59564 -0.97799 +27.29000 3.34100 8.60625 -0.97889 +27.35500 3.34138 8.61372 -0.98424 +27.35500 3.35485 8.61644 -0.97658 +27.42070 3.35450 8.62917 -0.97507 +27.42070 3.36144 8.63211 -0.97478 +27.48530 3.38062 8.63356 -0.96835 +27.48530 3.38445 8.63677 -0.96865 +27.55050 3.38842 8.63405 -0.96984 +27.55050 3.38553 8.63130 -0.97329 +27.61600 3.37911 8.63174 -0.97209 +27.68100 3.37018 8.63319 -0.97290 +27.68100 3.36438 8.63326 -0.97426 +27.68100 3.40378 8.63257 -0.95373 +27.74990 3.40278 8.63169 -0.95118 +27.74990 3.39907 8.63019 -0.95609 +27.81150 3.38898 8.63077 -0.96209 +27.81150 3.39434 8.61787 -0.95605 +27.87650 3.38858 8.61674 -0.95424 +27.87650 3.38242 8.61273 -0.95774 +27.94150 3.39215 8.61041 -0.95835 +27.94150 3.38078 8.60507 -0.95771 +28.00650 3.37184 8.59693 -0.96049 +28.00650 3.36694 8.58602 -0.96545 +28.07180 3.36933 8.56938 -0.95632 +28.07180 3.35586 8.56382 -0.95614 +28.13700 3.34355 8.56050 -0.95899 +28.13700 3.34695 8.55313 -0.95813 +28.20200 3.33422 8.54442 -0.95676 +28.20200 3.32265 8.53511 -0.95885 +28.26700 3.30932 8.52542 -0.96255 +28.26700 3.31192 8.51680 -0.95925 +28.33200 3.29548 8.50858 -0.95960 +28.33200 3.28217 8.50068 -0.95870 +28.39750 3.29394 8.49347 -0.95300 +28.39750 3.28426 8.48326 -0.95001 +28.46200 3.27464 8.47704 -0.95324 +28.46200 3.26021 8.47110 -0.96258 +28.52700 3.25997 8.46103 -0.96153 +28.52700 3.24384 8.45448 -0.95701 +28.59300 3.22998 8.45026 -0.96391 +28.59300 3.21963 8.44674 -0.96349 +28.65800 3.22501 8.44161 -0.95563 +28.65800 3.21384 8.43533 -0.96043 +28.72300 3.19790 8.42873 -0.96568 +28.72300 3.19450 8.41696 -0.96491 +28.78840 3.17471 8.41014 -0.96680 +28.78840 3.15653 8.40635 -0.97338 +28.85270 3.14150 8.40416 -0.97614 +28.85270 3.13302 8.39723 -0.97939 +28.91800 3.11560 8.38911 -0.98464 +28.91800 3.09742 8.38290 -0.99274 +28.98400 3.10159 8.36978 -0.98565 +28.98400 3.07761 8.36367 -0.98536 +29.04850 3.05829 8.36048 -0.98823 +29.04850 3.06209 8.34979 -0.98609 +29.11360 3.05219 8.33829 -0.98581 +29.11360 3.03508 8.33093 -0.98621 +29.17900 3.01906 8.32535 -0.99288 +29.17900 3.02104 8.31777 -0.99125 +29.24400 3.00038 8.30980 -0.98762 +29.24400 2.97855 8.30639 -0.99506 +29.30910 2.97474 8.29502 -0.99700 +29.30910 2.97410 8.28393 -0.99296 +29.37450 2.95669 8.27455 -0.99737 +29.37450 2.94002 8.26779 -1.00558 +29.43910 2.93171 8.25395 -1.00253 +29.43910 2.90720 8.24144 -0.99670 +29.50400 2.88559 8.23813 -1.00036 +29.50400 2.88176 8.22736 -1.00453 +29.56960 2.88865 8.21235 -1.00022 +29.56960 2.87610 8.20014 -0.99905 +29.63460 2.85804 8.19239 -1.01007 +29.63460 2.85192 8.17955 -1.00613 +29.69960 2.83495 8.17050 -1.00457 +29.69960 2.81352 8.16469 -1.00564 +29.76500 2.79644 8.16000 -1.01327 +29.76500 2.80758 8.15093 -1.00823 +29.82990 2.79052 8.13940 -1.01017 +29.82990 2.76842 8.13479 -1.01950 +29.89500 2.76382 8.12084 -1.01768 +29.89500 2.73867 8.10763 -1.01603 +29.96000 2.71517 8.10087 -1.02221 +29.96000 2.69662 8.08846 -1.03034 +30.02520 2.68874 8.07629 -1.02887 +30.02520 2.66170 8.06642 -1.03521 +30.09100 2.63620 8.05837 -1.04267 +30.09100 2.62633 8.04358 -1.04090 +30.15580 2.59315 8.03677 -1.03552 +30.15580 2.56083 8.02604 -1.03468 +30.22100 2.53330 8.01802 -1.04581 +30.22100 2.53774 8.00723 -1.04303 +30.28550 2.50392 8.00093 -1.04962 +30.28550 2.47276 7.99159 -1.05543 +30.35110 2.46138 7.97865 -1.05719 +30.35110 2.42507 7.97338 -1.05562 +30.41600 2.38626 7.96176 -1.06111 +30.41600 2.36736 7.95214 -1.07100 +30.48100 2.35536 7.94127 -1.07165 +30.48100 2.32118 7.93052 -1.07618 +30.54990 2.29318 7.91965 -1.08476 +30.54990 2.26265 7.90859 -1.08247 +30.61150 2.22795 7.89455 -1.08053 +30.61150 2.18364 7.88233 -1.08318 +30.67650 2.15002 7.87072 -1.10133 +30.67650 2.14436 7.85879 -1.09214 +30.74150 2.09155 7.84382 -1.10135 +30.74150 2.05314 7.83189 -1.11781 +30.80750 2.02952 7.81439 -1.11171 +30.80750 1.98410 7.80244 -1.10696 +30.87200 1.93511 7.78678 -1.11719 +30.87200 1.89395 7.77096 -1.13104 +30.93660 1.90386 7.75814 -1.11892 +30.93660 1.85261 7.74123 -1.12905 +31.00260 1.81353 7.71975 -1.14232 +31.00260 1.78874 7.69871 -1.13910 +31.06700 1.74426 7.68184 -1.13275 +31.06700 1.69545 7.66426 -1.14037 +31.13200 1.65733 7.64948 -1.15311 +31.13200 1.64476 7.62167 -1.14481 +31.19750 1.59985 7.60144 -1.14590 +31.19750 1.55134 7.58369 -1.15563 +31.26260 1.50068 7.55968 -1.16309 +31.26260 1.46690 7.52778 -1.16055 +31.32750 1.40866 7.50011 -1.16563 +31.32750 1.36308 7.47319 -1.18296 +31.39360 1.30157 7.44835 -1.18332 +31.39360 1.23588 7.41321 -1.18772 +31.45800 1.17997 7.38735 -1.19526 +31.45800 1.12511 7.35718 -1.19921 +31.52850 1.05275 7.32043 -1.20157 +31.52850 0.98530 7.28522 -1.20519 +31.59300 0.92824 7.25521 -1.21647 +31.59300 0.85593 7.22318 -1.22858 +31.65860 0.77915 7.17504 -1.23313 +31.65860 0.70799 7.13485 -1.23970 +31.72350 0.65964 7.09661 -1.24826 +31.72350 0.56953 7.04833 -1.24590 +31.78850 0.49110 7.00233 -1.24138 +31.78850 0.42937 6.95889 -1.24900 +31.85350 0.35853 6.91558 -1.25458 +31.85350 0.27193 6.86122 -1.25896 +31.91900 0.20244 6.81763 -1.25593 +31.91900 0.13154 6.77565 -1.26124 +31.98400 0.03134 6.71983 -1.27114 +31.98400 -0.06303 6.66159 -1.26634 +32.04950 -0.12273 6.62419 -1.26561 +32.04950 -0.22417 6.56045 -1.27624 +32.11450 -0.30736 6.50829 -1.27142 +32.11450 -0.38924 6.45281 -1.26336 +32.17950 -0.45663 6.41410 -1.26066 +32.17950 -0.55740 6.35067 -1.26890 +32.24500 -0.63195 6.29697 -1.26659 +32.24500 -0.70589 6.24003 -1.25807 +32.31050 -0.78158 6.18489 -1.25723 +32.31050 -0.87490 6.12383 -1.25494 +32.37550 -0.95749 6.06015 -1.25449 +32.37550 -1.03739 6.01097 -1.24213 +32.37550 -1.12911 5.95096 -1.23100 +32.44050 -1.22835 5.87546 -1.22789 +32.44050 -1.29879 5.82014 -1.23084 +32.50600 -1.37528 5.76627 -1.22225 +32.50600 -1.47100 5.69300 -1.21077 +32.57100 -1.56476 5.62478 -1.20250 +32.57100 -1.63505 5.58617 -1.19440 +32.63580 -1.71964 5.52352 -1.19031 +32.63580 -1.81388 5.44756 -1.17905 +32.70150 -1.87322 5.39636 -1.14050 +32.70150 -1.94377 5.33541 -1.12914 +32.76650 -2.03295 5.25919 -1.11200 +32.83210 -2.11122 5.19266 -1.09378 +32.83210 -2.18501 5.12850 -1.07636 +32.89700 -2.26011 5.04981 -1.06816 +32.89700 -2.33375 4.97930 -1.07056 +32.96200 -2.41581 4.91883 -1.07447 +32.96200 -2.49841 4.85139 -1.06354 +33.02750 -2.56170 4.79776 -1.05489 +33.02750 -2.63933 4.75200 -1.06776 +33.09240 -2.70284 4.70270 -1.06094 +33.09240 -2.79384 4.64245 -1.04327 +33.15760 -2.84003 4.61073 -1.03354 +33.15760 -2.90216 4.56831 -1.03399 +33.22310 -2.97287 4.51672 -1.00876 +33.22310 -3.03655 4.47473 -0.99967 +33.28800 -3.07821 4.44832 -0.99088 +33.28800 -3.13450 4.41258 -0.98040 +33.35350 -3.18358 4.38293 -0.95923 +33.35350 -3.23007 4.35938 -0.94195 +33.42250 -3.26921 4.33347 -0.92798 +33.42250 -3.31980 4.30080 -0.92234 +33.48350 -3.36342 4.26972 -0.90897 +33.48350 -3.37204 4.23258 -0.90297 +33.54910 -3.40586 4.20376 -0.89884 +33.54910 -3.44007 4.17280 -0.88574 +33.61460 -3.47125 4.14506 -0.87228 +33.61460 -3.51841 4.13534 -0.86896 +33.67910 -3.55214 4.11744 -0.86462 +33.67910 -3.59024 4.08628 -0.85587 +33.74400 -3.60113 4.05518 -0.85696 +33.74400 -3.63450 4.04029 -0.85466 +33.74400 -3.65043 4.02748 -0.85046 +33.80950 -3.67227 4.01011 -0.84922 +33.80950 -3.68976 3.99523 -0.84878 +33.87450 -3.70641 3.98732 -0.84658 +33.87450 -3.72509 3.97605 -0.84432 +33.94260 -3.74596 3.96449 -0.84073 +33.94260 -3.76656 3.95948 -0.83059 +34.00500 -3.78379 3.95418 -0.82919 +34.00500 -3.78324 3.95702 -0.83181 +34.06970 -3.80019 3.94756 -0.82856 +34.06970 -3.79887 3.95370 -0.82590 +34.13570 -3.80544 3.95344 -0.82588 +34.13570 -3.80922 3.94970 -0.82720 +34.20050 -3.81681 3.94302 -0.82382 +34.20050 -3.81346 3.94229 -0.82207 +34.26930 -3.81546 3.94001 -0.81834 +34.26930 -3.81926 3.94008 -0.82028 +34.26930 -3.81904 3.93734 -0.81940 +34.33060 -3.81645 3.94048 -0.81930 +34.33060 -3.81095 3.94150 -0.81976 +34.39580 -3.80757 3.94531 -0.82411 +34.39580 -3.80829 3.94863 -0.82198 +34.46100 -3.80225 3.94816 -0.82178 +34.46100 -3.79628 3.94643 -0.82272 +34.52580 -3.79606 3.94306 -0.82614 +34.52580 -3.78645 3.94578 -0.82328 +34.59120 -3.76372 3.94905 -0.82342 +34.59120 -3.75233 3.95301 -0.82549 +34.59120 -3.74803 3.94901 -0.83008 +34.65600 -3.73604 3.95113 -0.82925 +34.65600 -3.72171 3.95131 -0.82856 +34.72100 -3.70588 3.95468 -0.82818 +34.72100 -3.67123 3.96051 -0.83203 +34.78660 -3.64470 3.97117 -0.83382 +34.78660 -3.62048 3.97236 -0.83117 +34.85160 -3.60397 3.96548 -0.82931 +34.85160 -3.58840 3.97678 -0.83345 +34.85160 -3.56620 3.97623 -0.83715 +34.91650 -3.53746 3.97823 -0.84144 +34.91650 -3.51012 3.98555 -0.84012 +34.98210 -3.48228 3.99489 -0.83407 +34.98210 -3.44972 3.99354 -0.83529 +35.04730 -3.41625 3.99409 -0.83512 +35.04730 -3.39286 3.99126 -0.83627 +35.11200 -3.35458 3.99254 -0.84974 +35.11200 -3.31282 3.99347 -0.85024 +35.18440 -3.27872 3.99115 -0.84818 +35.18440 -3.25673 3.99004 -0.84443 +35.24250 -3.21756 3.99616 -0.83623 +35.24250 -3.17439 3.99585 -0.83149 +35.30700 -3.14124 3.98946 -0.83279 +35.30700 -3.12928 3.98292 -0.83349 +35.37250 -3.09933 3.98947 -0.83744 +35.37250 -3.06029 3.99323 -0.83973 +35.43800 -3.05742 3.99404 -0.84025 +35.43800 -3.02501 3.99823 -0.84032 +35.50260 -2.99218 4.00365 -0.84061 +35.50260 -2.95721 4.01125 -0.84089 +35.50260 -2.97546 4.01125 -0.85520 +35.56800 -2.95150 4.01572 -0.85910 +35.63350 -2.92605 4.02166 -0.86110 +35.63350 -2.91238 4.02450 -0.85347 +35.63350 -2.88884 4.03357 -0.84595 +35.69850 -2.86678 4.03916 -0.84945 +35.69850 -2.85164 4.03990 -0.84873 +35.76350 -2.85980 4.03962 -0.84880 +35.76350 -2.84317 4.04855 -0.85190 +35.82870 -2.83368 4.04697 -0.84933 +35.82870 -2.82179 4.05211 -0.85049 +35.89400 -2.81923 4.05230 -0.84817 +35.89400 -2.80446 4.05708 -0.84403 +35.95900 -2.79594 4.05482 -0.84130 +35.95900 -2.78685 4.06036 -0.84165 +35.95900 -2.79531 4.05659 -0.84225 +36.02460 -2.79023 4.05912 -0.84552 +36.02460 -2.78787 4.05996 -0.84985 +36.08900 -2.78708 4.06213 -0.84834 +36.08900 -2.78899 4.06075 -0.84540 +36.15400 -2.78648 4.06626 -0.84728 +36.15400 -2.78331 4.06361 -0.84718 +36.21950 -2.78807 4.06054 -0.84637 +36.21950 -2.81578 4.05215 -0.84895 +36.21950 -2.81980 4.04958 -0.85152 +36.28450 -2.82003 4.04952 -0.85125 +36.28450 -2.81276 4.04696 -0.84768 +36.34950 -2.81812 4.04464 -0.84774 +36.34950 -2.81786 4.04372 -0.85129 +36.41500 -2.81626 4.04146 -0.84865 +36.41500 -2.81932 4.04500 -0.84432 +36.47950 -2.81835 4.04132 -0.84696 +36.47950 -2.81626 4.03648 -0.84782 +36.54500 -2.81105 4.03993 -0.84171 +36.54500 -2.80137 4.03125 -0.83935 +36.61000 -2.79767 4.02537 -0.85010 +36.61000 -2.78536 4.03172 -0.85427 +36.67510 -2.77247 4.03399 -0.85622 +36.67510 -2.77020 4.04323 -0.86004 +36.67510 -2.76335 4.04421 -0.86250 +36.74050 -2.75102 4.04538 -0.86481 +36.74050 -2.74361 4.04830 -0.85186 +36.80520 -2.73739 4.04455 -0.85083 +36.80520 -2.73318 4.04105 -0.84928 +36.87060 -2.72194 4.04216 -0.85341 +36.87060 -2.72513 4.03802 -0.85607 +36.93550 -2.74425 4.02672 -0.86279 +36.93550 -2.73722 4.02347 -0.86643 +37.00290 -2.73353 4.02163 -0.85935 +37.00290 -2.74729 4.00868 -0.85673 +37.06570 -2.76913 4.00080 -0.85551 +37.06570 -2.77476 3.98459 -0.85255 +37.13100 -2.77144 3.98368 -0.84536 +37.13100 -2.75960 3.97885 -0.83883 +37.13100 -2.76778 3.96219 -0.84231 +37.19650 -2.76529 3.95513 -0.84751 +37.19650 -2.76595 3.94609 -0.84316 +37.26080 -2.79990 3.93601 -0.84286 +37.26080 -2.81263 3.92789 -0.84194 +37.32560 -2.80517 3.93938 -0.83807 +37.32560 -2.81676 3.93062 -0.83208 +37.39100 -2.81066 3.91141 -0.82459 +37.39100 -2.81618 3.90033 -0.82149 +37.45650 -2.81433 3.89218 -0.81165 +37.45650 -2.82563 3.88097 -0.80820 +37.52150 -2.82369 3.87170 -0.80918 +37.52150 -2.82874 3.86017 -0.80460 +37.58650 -2.83892 3.83997 -0.79364 +37.58650 -2.84073 3.83459 -0.78661 +37.58650 -2.84076 3.81372 -0.78706 +37.65150 -2.84459 3.79795 -0.78461 +37.65150 -2.84129 3.78970 -0.77667 +37.71700 -2.85352 3.76807 -0.76857 +37.71700 -2.85803 3.75599 -0.77029 +37.78120 -2.84581 3.75871 -0.77147 +37.78120 -2.84485 3.74462 -0.76193 +37.84700 -2.82064 3.73213 -0.76803 +37.84700 -2.82099 3.71435 -0.78316 +37.91200 -2.81441 3.70582 -0.78759 +37.91200 -2.80373 3.69072 -0.78141 +37.97700 -2.79420 3.68317 -0.77501 +37.97700 -2.79689 3.67381 -0.77498 +38.04280 -2.78904 3.66324 -0.76594 +38.04280 -2.75205 3.66027 -0.75752 +38.10700 -2.74024 3.64831 -0.75356 +38.10700 -2.72986 3.64176 -0.75625 +38.17210 -2.71237 3.62640 -0.75373 +38.17210 -2.67483 3.61980 -0.75059 +38.17210 -2.64836 3.61240 -0.74643 +38.23750 -2.62489 3.60696 -0.74374 +38.23750 -2.59764 3.59608 -0.73025 +38.30250 -2.57975 3.58548 -0.72498 +38.30250 -2.56777 3.57727 -0.72235 +38.36790 -2.54500 3.56944 -0.71694 +38.36790 -2.53893 3.55019 -0.70508 +38.43300 -2.52047 3.53542 -0.70622 +38.43300 -2.51075 3.52011 -0.71403 +38.49750 -2.49621 3.50721 -0.71815 +38.49750 -2.48041 3.50223 -0.72050 +38.56300 -2.48053 3.48690 -0.73435 +38.56300 -2.47579 3.48026 -0.73777 +38.62850 -2.46509 3.47398 -0.73285 +38.62850 -2.46980 3.46819 -0.72976 +38.69300 -2.46777 3.46235 -0.72395 +38.69300 -2.46131 3.45933 -0.72146 +38.75900 -2.42163 3.46181 -0.71261 +38.75900 -2.40884 3.46088 -0.71436 +38.82400 -2.38819 3.46390 -0.70691 +38.82400 -2.37460 3.46175 -0.70133 +38.88920 -2.37002 3.46517 -0.70305 +38.88920 -2.36556 3.46537 -0.70086 +38.95400 -2.35629 3.46894 -0.69126 +38.95400 -2.33373 3.48120 -0.68847 +39.01900 -2.32828 3.47113 -0.69244 +39.01900 -2.31496 3.47361 -0.69655 +39.08410 -2.28940 3.48737 -0.70466 +39.08410 -2.27555 3.49911 -0.71475 +39.14900 -2.25613 3.51361 -0.72385 +39.21400 -2.23583 3.53522 -0.73127 +39.21400 -2.23392 3.53900 -0.73769 +39.21400 -2.23080 3.55074 -0.74708 +39.27950 -2.22903 3.56393 -0.74845 +39.34450 -2.23832 3.57654 -0.74956 +39.34450 -2.22907 3.58031 -0.74836 +39.40950 -2.22931 3.59136 -0.74307 +39.40950 -2.23904 3.59702 -0.74102 +39.47500 -2.23564 3.59364 -0.73447 +39.47500 -2.24015 3.59566 -0.73214 +39.54010 -2.24483 3.60310 -0.72342 +39.54010 -2.25707 3.60091 -0.72314 +39.60980 -2.26293 3.60093 -0.72379 +39.60980 -2.27386 3.59968 -0.72100 +39.67050 -2.28373 3.60203 -0.72498 +39.67050 -2.28503 3.60007 -0.73030 +39.73560 -2.29157 3.59550 -0.73075 +39.73560 -2.30336 3.59663 -0.72777 +39.80050 -2.30695 3.59792 -0.72693 +39.80050 -2.31058 3.59595 -0.72597 +39.86600 -2.32195 3.59579 -0.72323 +39.86600 -2.33576 3.59246 -0.72764 +39.93100 -2.33834 3.58176 -0.72779 +39.93100 -2.34668 3.58059 -0.72442 +39.99600 -2.36107 3.58106 -0.72171 +39.99600 -2.36456 3.57663 -0.72275 +40.06110 -2.36926 3.57170 -0.72273 +40.06110 -2.38095 3.56778 -0.72038 +40.12660 -2.38856 3.56388 -0.72056 +40.12660 -2.38592 3.55230 -0.71998 +40.19100 -2.39282 3.54884 -0.71496 +40.19100 -2.40114 3.54320 -0.71153 +40.25700 -2.39278 3.53511 -0.71161 +40.25700 -2.39670 3.52586 -0.71143 +40.32150 -2.40421 3.51888 -0.70884 +40.32150 -2.39308 3.50948 -0.70491 +40.38700 -2.37323 3.49648 -0.69851 +40.38700 -2.36973 3.48699 -0.69390 +40.45200 -2.36195 3.47672 -0.69187 +40.45200 -2.34499 3.46693 -0.68902 +40.51700 -2.33425 3.46039 -0.68692 +40.51700 -2.32715 3.45048 -0.68543 +40.58200 -2.32161 3.43552 -0.68295 +40.58200 -2.30987 3.42966 -0.68223 +40.64750 -2.30734 3.42167 -0.68138 +40.64750 -2.30009 3.41207 -0.68119 +40.71250 -2.26072 3.41331 -0.66962 +40.71250 -2.24823 3.40458 -0.66899 +40.77800 -2.23977 3.39592 -0.66658 +40.77800 -2.22977 3.38458 -0.65946 +40.84300 -2.21661 3.37310 -0.65230 +40.84300 -2.19377 3.35742 -0.65635 +40.84300 -2.18457 3.35014 -0.65626 +40.90800 -2.17257 3.33756 -0.65090 +40.90800 -2.19578 3.33077 -0.64890 +40.97310 -2.18980 3.32308 -0.64731 +40.97310 -2.18564 3.31305 -0.64509 +41.03850 -2.20347 3.28294 -0.63276 +41.03850 -2.21135 3.26905 -0.63166 +41.10350 -2.21597 3.25293 -0.63536 +41.10350 -2.22152 3.23775 -0.63027 +41.16900 -2.21945 3.23010 -0.62434 +41.16900 -2.23433 3.21999 -0.62487 +41.23450 -2.23836 3.21257 -0.62043 +41.23450 -2.24675 3.19698 -0.61457 +41.29900 -2.24210 3.18830 -0.60765 +41.29900 -2.23709 3.17337 -0.60848 +41.29900 -2.23625 3.16103 -0.60424 +41.36450 -2.22423 3.15208 -0.59651 +41.36450 -2.23217 3.14022 -0.59352 +41.42950 -2.23431 3.12728 -0.59046 +41.42950 -2.23711 3.11009 -0.58348 +41.49450 -2.24038 3.09036 -0.57821 +41.49450 -2.25363 3.07724 -0.58241 +41.55980 -2.25785 3.06617 -0.57763 +41.55980 -2.26131 3.05068 -0.57430 +41.62500 -2.29309 3.04166 -0.58069 +41.62500 -2.30136 3.02937 -0.57837 +41.69630 -2.31599 3.02171 -0.57483 +41.69630 -2.33181 3.00637 -0.57520 +41.75500 -2.35110 3.01014 -0.57197 +41.75500 -2.36471 3.00210 -0.57121 +41.75500 -2.38063 2.99893 -0.56622 +41.82050 -2.40736 2.99334 -0.56671 +41.82050 -2.43658 2.99549 -0.57111 +41.88580 -2.46324 2.99365 -0.57063 +41.88580 -2.49482 2.99313 -0.57011 +41.95060 -2.51363 2.99974 -0.56919 +41.95060 -2.55614 2.99452 -0.57845 +42.01600 -2.58816 3.00061 -0.57271 +42.01600 -2.61942 2.99873 -0.57551 +42.08100 -2.65677 2.98474 -0.57708 +42.08100 -2.68520 2.98419 -0.57437 +42.14600 -2.71749 2.98709 -0.57164 +42.14600 -2.76278 2.98142 -0.57232 +42.21150 -2.77992 2.97920 -0.56835 +42.21150 -2.81387 2.97901 -0.56720 +42.27650 -2.84641 2.97812 -0.56986 +42.27650 -2.87591 2.97470 -0.56977 +42.34150 -2.90072 2.96904 -0.56805 +42.34150 -2.93233 2.96584 -0.57166 +42.40700 -2.96646 2.96197 -0.57342 +42.40700 -2.97573 2.95387 -0.56973 +42.40700 -2.99616 2.94912 -0.57139 +42.47260 -3.01756 2.94306 -0.57596 +42.47260 -3.05208 2.93201 -0.57277 +42.53700 -3.06890 2.93720 -0.57050 +42.53700 -3.09821 2.93543 -0.55731 +42.60200 -3.13058 2.92610 -0.55971 +42.60200 -3.15001 2.90818 -0.54326 +42.66700 -3.17568 2.89959 -0.53178 +42.66700 -3.21573 2.88857 -0.53957 +42.73250 -3.25255 2.87306 -0.56092 +42.73250 -3.27204 2.87159 -0.55797 +42.79750 -3.29208 2.86364 -0.56140 +42.79750 -3.31909 2.85496 -0.56459 +42.86200 -3.35420 2.84613 -0.56660 +42.86200 -3.39037 2.84075 -0.56798 +42.86200 -3.41946 2.83461 -0.56822 +42.92770 -3.45322 2.83140 -0.57128 +42.99260 -3.49069 2.82423 -0.57058 +42.99260 -3.50174 2.81442 -0.56019 +42.99260 -3.53282 2.81090 -0.55652 +43.05750 -3.56809 2.80896 -0.55011 +43.05750 -3.55730 2.81265 -0.54109 +43.12300 -3.57875 2.80077 -0.53469 +43.12300 -3.60684 2.79018 -0.53765 +43.18850 -3.62855 2.77418 -0.55005 +43.18850 -3.64539 2.77211 -0.54843 +43.25300 -3.65379 2.75537 -0.54414 +43.25300 -3.68137 2.74549 -0.53927 +43.31800 -3.70921 2.73139 -0.53835 +43.31800 -3.73985 2.70539 -0.52758 +43.38300 -3.76944 2.68577 -0.52111 +43.38300 -3.80075 2.67058 -0.51924 +43.44910 -3.83243 2.64318 -0.52043 +43.44910 -3.85974 2.62681 -0.51500 +43.44910 -3.89248 2.60558 -0.51164 +43.51350 -3.92037 2.58640 -0.50245 +43.51350 -3.96014 2.56608 -0.49843 +43.57850 -3.97631 2.55828 -0.49277 +43.57850 -4.00726 2.53424 -0.48427 +43.64400 -4.02438 2.51803 -0.47983 +43.64400 -4.05215 2.48911 -0.46889 +43.70910 -4.07084 2.47441 -0.46325 +43.70910 -4.08542 2.45453 -0.46273 +43.77400 -4.10373 2.43569 -0.45975 +43.77400 -4.09683 2.45130 -0.46879 +43.77400 -4.10322 2.44194 -0.46685 +43.83950 -4.10984 2.43264 -0.46531 +43.83950 -4.11472 2.41802 -0.47089 +43.90450 -4.13208 2.39557 -0.46913 +43.90450 -4.13270 2.38981 -0.46692 +43.96950 -4.13790 2.37208 -0.45849 +43.96950 -4.13815 2.35508 -0.46028 +44.03570 -4.14155 2.34972 -0.45531 +44.03570 -4.14178 2.34549 -0.44943 +44.10000 -4.14892 2.33212 -0.44182 +44.10000 -4.15207 2.31520 -0.44509 +44.10000 -4.15873 2.31513 -0.45181 +44.16550 -4.16153 2.30339 -0.44332 +44.16550 -4.16411 2.28940 -0.44240 +44.23050 -4.16381 2.27068 -0.44250 +44.23050 -4.17052 2.25411 -0.44094 +44.29550 -4.17331 2.23272 -0.43702 +44.29550 -4.17848 2.22066 -0.43819 +44.29550 -4.18834 2.20440 -0.43484 +44.36100 -4.19016 2.19981 -0.43238 +44.36100 -4.18781 2.19008 -0.42181 +44.42560 -4.19281 2.17543 -0.41767 +44.42560 -4.18991 2.16040 -0.41358 +44.49100 -4.16762 2.17005 -0.41457 +44.49100 -4.16602 2.16193 -0.41195 +44.55650 -4.16437 2.14963 -0.41125 +44.55650 -4.16208 2.14014 -0.40826 +44.55650 -4.15991 2.12935 -0.40412 +44.62150 -4.16034 2.11750 -0.39722 +44.62150 -4.16382 2.10439 -0.39753 +44.68650 -4.17596 2.08020 -0.39491 +44.68650 -4.17903 2.06392 -0.38686 +44.75570 -4.17902 2.04309 -0.38547 +44.75570 -4.18127 2.01694 -0.38852 +44.81720 -4.18995 1.98717 -0.38437 +44.81720 -4.18601 1.96678 -0.37559 +44.88200 -4.18707 1.94280 -0.37618 +44.88200 -4.18746 1.91717 -0.37967 +44.88200 -4.16029 1.92279 -0.37642 +44.94750 -4.15377 1.90928 -0.37618 +44.94750 -4.14484 1.89463 -0.38008 +45.01200 -4.12569 1.90422 -0.38301 +45.01200 -4.11987 1.90027 -0.37807 +45.07750 -4.11538 1.88863 -0.37273 +45.07750 -4.11228 1.87687 -0.37428 +45.14300 -4.08096 1.90959 -0.38153 +45.14300 -4.07419 1.90082 -0.37742 +45.20800 -4.05722 1.88821 -0.38344 +45.20800 -4.04238 1.89417 -0.37984 +45.20800 -4.03167 1.89058 -0.37285 +45.27300 -4.02173 1.88534 -0.37463 +45.27300 -4.00830 1.88388 -0.37240 +45.33850 -3.99820 1.88286 -0.37482 +45.33850 -3.98904 1.86210 -0.36577 +45.40320 -3.98206 1.85428 -0.36629 +45.40320 -3.97285 1.84859 -0.36775 +45.46800 -3.96028 1.83837 -0.37435 +45.46800 -3.94414 1.85075 -0.37630 +45.46800 -3.93293 1.85723 -0.37837 +45.53340 -3.92287 1.86476 -0.38433 +45.53340 -3.91546 1.87058 -0.39156 +45.60320 -3.90777 1.86429 -0.38711 +45.60320 -3.88845 1.86414 -0.38566 +45.66350 -3.87809 1.86273 -0.38714 +45.66350 -3.86550 1.86549 -0.38864 +45.72850 -3.85257 1.86495 -0.37750 +45.72850 -3.83875 1.87244 -0.37438 +45.72850 -3.82897 1.87427 -0.37465 +45.79400 -3.81843 1.87295 -0.37100 +45.79400 -3.80701 1.86999 -0.36479 +45.85850 -3.78824 1.87651 -0.36876 +45.85850 -3.77401 1.87609 -0.37213 +45.92410 -3.75853 1.87422 -0.36958 +45.92410 -3.75022 1.86600 -0.35979 +45.92410 -3.73477 1.86524 -0.35745 +45.98900 -3.72210 1.86002 -0.36155 +45.98900 -3.71027 1.85463 -0.36294 +46.05410 -3.71105 1.83870 -0.36272 +46.05410 -3.69800 1.83574 -0.36463 +46.11900 -3.68876 1.82520 -0.37205 +46.11900 -3.67979 1.82148 -0.37507 +46.18450 -3.67387 1.81302 -0.36916 +46.18450 -3.66746 1.81028 -0.36450 +46.24950 -3.65932 1.80487 -0.36507 +46.24950 -3.65154 1.79529 -0.36619 +46.24950 -3.65707 1.77941 -0.35608 +46.31450 -3.64330 1.77979 -0.35000 +46.31450 -3.63678 1.77680 -0.34715 +46.37950 -3.63290 1.77185 -0.34337 +46.37950 -3.63537 1.76206 -0.33912 +46.44500 -3.63588 1.75778 -0.33968 +46.44500 -3.63896 1.75338 -0.34463 +46.51000 -3.64144 1.75676 -0.34702 +46.51000 -3.64975 1.76149 -0.34524 +46.51000 -3.65566 1.76325 -0.34449 +46.57500 -3.66156 1.76194 -0.34761 +46.57500 -3.66964 1.75914 -0.34716 +46.64030 -3.67294 1.76503 -0.33802 +46.64030 -3.68067 1.76727 -0.33435 +46.70500 -3.68916 1.76544 -0.33693 +46.70500 -3.69757 1.76273 -0.34083 +46.77760 -3.71146 1.75812 -0.34022 +46.77760 -3.71554 1.75545 -0.34563 +46.83560 -3.71932 1.75159 -0.35253 +46.83560 -3.72407 1.74920 -0.35098 +46.83560 -3.72624 1.75307 -0.34197 +46.90060 -3.73544 1.74569 -0.34063 +46.90060 -3.74083 1.73656 -0.34096 +46.96570 -3.75086 1.72928 -0.33630 +46.96570 -3.77386 1.69626 -0.31925 +47.03150 -3.79815 1.67152 -0.31379 +47.03150 -3.81050 1.65683 -0.31403 +47.09610 -3.82348 1.64562 -0.31037 +47.09610 -3.83954 1.64239 -0.30550 +47.16110 -3.85977 1.62866 -0.31218 +47.16110 -3.87750 1.62677 -0.31544 +47.16110 -3.89266 1.62852 -0.30517 +47.22600 -3.91641 1.61275 -0.30054 +47.22600 -3.94375 1.60429 -0.30222 +47.29100 -3.96792 1.59712 -0.29724 +47.29100 -3.99431 1.59374 -0.28328 +47.35660 -4.02344 1.58744 -0.27630 +47.35660 -4.05815 1.58374 -0.28399 +47.42150 -4.08306 1.58388 -0.28586 +47.42150 -4.10784 1.58190 -0.28170 +47.42150 -4.13550 1.56864 -0.27990 +47.48670 -4.17046 1.55635 -0.29171 +47.48670 -4.19282 1.54910 -0.28138 +47.55250 -4.21716 1.54440 -0.27120 +47.55250 -4.24277 1.53756 -0.26753 +47.61700 -4.28141 1.53585 -0.27200 +47.61700 -4.31116 1.52750 -0.26899 +47.68230 -4.34430 1.51196 -0.26059 +47.68230 -4.37990 1.49209 -0.25502 +47.74700 -4.41962 1.48364 -0.26164 +47.74700 -4.44914 1.47978 -0.25173 +47.81250 -4.49498 1.47320 -0.25365 +47.81250 -4.54326 1.47256 -0.25362 +47.87700 -4.57804 1.46102 -0.25197 +47.87700 -4.61479 1.45674 -0.25525 +47.94250 -4.65402 1.45635 -0.24535 +47.94250 -4.71311 1.44963 -0.24530 +48.00800 -4.75931 1.44309 -0.25436 +48.00800 -4.80063 1.43896 -0.25332 +48.07250 -4.84479 1.43699 -0.25376 +48.07250 -4.89253 1.43333 -0.25774 +48.07250 -4.92944 1.43173 -0.26660 +48.13820 -4.96711 1.42185 -0.25784 +48.20300 -5.01988 1.41784 -0.25131 +48.20300 -5.06769 1.41654 -0.25508 +48.26800 -5.11808 1.41658 -0.25364 +48.26800 -5.15844 1.40797 -0.25364 +48.33300 -5.21457 1.40602 -0.25006 +48.33300 -5.25723 1.40652 -0.25077 +48.33300 -5.30457 1.39933 -0.24759 +48.39850 -5.35548 1.39516 -0.24447 +48.46360 -5.40765 1.39571 -0.24899 +48.46360 -5.46142 1.40404 -0.25132 +48.46360 -5.49122 1.40206 -0.25268 +48.52850 -5.54854 1.40091 -0.25102 +48.52850 -5.60177 1.39277 -0.26183 +48.59370 -5.64920 1.37788 -0.26396 +48.59370 -5.69681 1.38414 -0.26171 +48.65910 -5.75148 1.38388 -0.26565 +48.72410 -5.79722 1.38384 -0.26052 +48.72410 -5.83088 1.37225 -0.25255 +48.78910 -5.88775 1.36787 -0.24884 +48.78910 -5.93299 1.36684 -0.24457 +48.78910 -5.97852 1.37109 -0.24969 +48.85400 -6.01014 1.36908 -0.25051 +48.91900 -6.06904 1.37358 -0.26183 +48.91900 -6.11563 1.36510 -0.26137 +48.91900 -6.13241 1.36847 -0.25741 +48.98450 -6.17736 1.36777 -0.25940 +49.04960 -6.21192 1.37308 -0.25411 +49.04960 -6.25511 1.37266 -0.25071 +49.11500 -6.28234 1.33943 -0.24579 +49.11500 -6.32800 1.34222 -0.25032 +49.18000 -6.37212 1.33806 -0.25016 +49.18000 -6.40309 1.31489 -0.24158 +49.24500 -6.44097 1.31350 -0.24560 +49.24500 -6.47204 1.31044 -0.24970 +49.31000 -6.50773 1.30377 -0.24636 +49.31000 -6.54528 1.30478 -0.24149 +49.37560 -6.58230 1.30280 -0.23603 +49.37560 -6.61982 1.30491 -0.23701 +49.43990 -6.64514 1.28988 -0.22849 +49.43990 -6.68200 1.29358 -0.22385 +49.50950 -6.71866 1.29919 -0.22508 +49.50950 -6.75647 1.29857 -0.22940 +49.57100 -6.79382 1.29352 -0.22829 +49.57100 -6.83560 1.30072 -0.23720 +49.63550 -6.87706 1.30236 -0.24085 +49.63550 -6.91957 1.30419 -0.23430 +49.70050 -6.96151 1.32138 -0.23211 +49.70050 -6.99467 1.33908 -0.23570 +49.76650 -7.04305 1.35262 -0.23323 +49.76650 -7.10268 1.39389 -0.24342 +49.83120 -7.14620 1.42261 -0.24840 +49.83120 -7.19379 1.44847 -0.25923 +49.89600 -7.23059 1.45272 -0.26652 +49.89600 -7.27202 1.49310 -0.26799 +49.96100 -7.30739 1.51943 -0.27735 +49.96100 -7.35449 1.53503 -0.28102 +50.02650 -7.40817 1.56690 -0.27889 +50.02650 -7.46363 1.59565 -0.28730 +50.09150 -7.51911 1.62753 -0.30430 +50.09150 -7.54865 1.62217 -0.29733 +50.15640 -7.59520 1.65009 -0.29288 +50.15640 -7.64961 1.67626 -0.29838 +50.22160 -7.70316 1.71778 -0.30627 +50.22160 -7.75349 1.76179 -0.30783 +50.28650 -7.80880 1.79850 -0.31439 +50.28650 -7.86463 1.83392 -0.32911 +50.35170 -7.91020 1.85776 -0.33434 +50.35170 -7.97258 1.89066 -0.33633 +50.41700 -8.01636 1.92598 -0.34101 +50.41700 -8.08458 1.95572 -0.34963 +50.48200 -8.13088 1.98001 -0.35787 +50.48200 -8.20109 2.03388 -0.37346 +50.54700 -8.24971 2.05866 -0.37290 +50.54700 -8.28960 2.05845 -0.37660 +50.61220 -8.32813 2.06085 -0.37051 +50.61220 -8.37711 2.08550 -0.36915 +50.67760 -8.42241 2.11831 -0.37033 +50.67760 -8.47158 2.17144 -0.38508 +50.74250 -8.51604 2.24438 -0.39052 +50.74250 -8.56402 2.29423 -0.39966 +50.80750 -8.61998 2.31371 -0.41576 +50.80750 -8.66374 2.34978 -0.42436 +50.87250 -8.69720 2.39182 -0.42856 +50.87250 -8.74517 2.42627 -0.42167 +50.94240 -8.80792 2.46346 -0.42523 +50.94240 -8.84424 2.48369 -0.42919 +51.00300 -8.88864 2.53118 -0.44311 +51.00300 -8.94167 2.56506 -0.44141 +51.00300 -8.98272 2.55993 -0.44204 +51.06800 -9.02673 2.58022 -0.44643 +51.06800 -9.07664 2.60320 -0.45162 +51.13850 -9.13920 2.61078 -0.44771 +51.13850 -9.18697 2.60067 -0.44139 +51.19840 -9.23304 2.62305 -0.44073 +51.19840 -9.28484 2.63306 -0.44031 +51.26580 -9.32641 2.62666 -0.43595 +51.26580 -9.37402 2.63226 -0.43830 +51.32900 -9.40901 2.64036 -0.43441 +51.32900 -9.46347 2.64643 -0.43338 +51.39400 -9.49459 2.68086 -0.44297 +51.39400 -9.54855 2.66843 -0.44453 +51.45900 -9.59917 2.67697 -0.44257 +51.45900 -9.64636 2.68829 -0.43596 +51.52400 -9.68510 2.68638 -0.42960 +51.52400 -9.71456 2.69074 -0.42455 +51.52400 -9.75345 2.69489 -0.42117 +51.58950 -9.80097 2.71219 -0.41819 +51.58950 -9.84979 2.69302 -0.41503 +51.65400 -9.89829 2.69746 -0.41389 +51.65400 -9.94166 2.70900 -0.42179 +51.71900 -9.99454 2.71765 -0.42322 +51.71900 -10.02890 2.71368 -0.42911 +51.78580 -10.06640 2.71278 -0.42486 +51.78580 -10.10900 2.72326 -0.42351 +51.84950 -10.13940 2.72753 -0.42380 +51.84950 -10.17950 2.73557 -0.42856 +51.91570 -10.21240 2.73724 -0.42979 +51.91570 -10.25100 2.75269 -0.42431 +51.98200 -10.28200 2.74330 -0.42011 +51.98200 -10.31270 2.75059 -0.41519 +51.98200 -10.33830 2.75621 -0.41602 +52.04500 -10.38110 2.77210 -0.42257 +52.04500 -10.40850 2.77231 -0.42414 +52.11060 -10.43510 2.77197 -0.42145 +52.11060 -10.46440 2.77553 -0.41686 +52.17510 -10.49860 2.77570 -0.41167 +52.17510 -10.52320 2.77508 -0.40344 +52.24050 -10.54560 2.78197 -0.40476 +52.24050 -10.57110 2.78413 -0.39794 +52.30550 -10.60130 2.79150 -0.39463 +52.30550 -10.62640 2.78357 -0.39585 +52.37050 -10.65270 2.79041 -0.39845 +52.37050 -10.68380 2.80102 -0.39812 +52.43600 -10.70250 2.79726 -0.39963 +52.43600 -10.72300 2.78591 -0.40342 +52.43600 -10.74030 2.78516 -0.40694 +52.50100 -10.76360 2.79774 -0.40370 +52.50100 -10.78400 2.79550 -0.40232 +52.56600 -10.80410 2.78990 -0.39876 +52.56600 -10.82470 2.79982 -0.39731 +52.63150 -10.84860 2.81050 -0.39396 +52.63150 -10.88470 2.80436 -0.38811 +52.69600 -10.90780 2.80882 -0.38813 +52.69600 -10.92810 2.81220 -0.39032 +52.76160 -10.95790 2.83125 -0.39434 +52.76160 -10.98520 2.81999 -0.38736 +52.76160 -11.01120 2.82413 -0.38358 +52.82700 -11.03030 2.83004 -0.37918 +52.82700 -11.05690 2.84321 -0.37455 +52.89240 -11.07160 2.83961 -0.37447 +52.89240 -11.09270 2.84763 -0.37576 +52.95650 -11.10740 2.85366 -0.37594 +52.95650 -11.13940 2.87186 -0.38114 +53.02940 -11.16450 2.86727 -0.38407 +53.02940 -11.18410 2.86308 -0.38592 +53.08750 -11.19990 2.87415 -0.38676 +53.08750 -11.21850 2.88613 -0.38389 +53.08750 -11.24290 2.87180 -0.38006 +53.15200 -11.25990 2.87803 -0.38043 +53.15200 -11.27900 2.89182 -0.37472 +53.21750 -11.28990 2.90583 -0.37785 +53.21750 -11.31130 2.89730 -0.37663 +53.28250 -11.32950 2.90239 -0.38022 +53.28250 -11.34380 2.92187 -0.38301 +53.34750 -11.36690 2.92994 -0.38258 +53.34750 -11.38200 2.92674 -0.38091 +53.34750 -11.40300 2.94049 -0.37537 +53.41300 -11.41970 2.96980 -0.38487 +53.41300 -11.45000 2.99202 -0.38704 +53.47800 -11.45110 3.03850 -0.39660 +53.47800 -11.46280 3.06662 -0.40187 +53.54300 -11.48480 3.10944 -0.40249 +53.54300 -11.48610 3.14923 -0.40155 +53.60850 -11.49580 3.18052 -0.38986 +53.60850 -11.50540 3.22231 -0.39730 +53.67350 -11.52100 3.27903 -0.40335 +53.67350 -11.53030 3.31630 -0.41199 +53.73830 -11.54850 3.35611 -0.42873 +53.73830 -11.56380 3.41391 -0.44762 +53.80350 -11.58800 3.43996 -0.45118 +53.80350 -11.59950 3.47652 -0.45587 +53.86850 -11.61390 3.51871 -0.45745 +53.86850 -11.63830 3.57869 -0.46689 +53.93360 -11.66920 3.62774 -0.46386 +53.93360 -11.69170 3.67011 -0.46160 +53.99950 -11.72440 3.71249 -0.47388 +53.99950 -11.75860 3.77417 -0.49045 +54.06410 -11.79550 3.79364 -0.48953 +54.06410 -11.82470 3.81825 -0.49300 +54.06410 -11.86330 3.85866 -0.50277 +54.13500 -11.89000 3.91988 -0.51312 +54.13500 -11.92780 3.95595 -0.51598 +54.19400 -11.96850 3.99509 -0.52479 +54.19400 -12.00470 4.05254 -0.53910 +54.25950 -12.05030 4.08722 -0.53687 +54.25950 -12.08590 4.11397 -0.53709 +54.32450 -12.12570 4.15308 -0.55177 +54.32450 -12.17620 4.20919 -0.56074 +54.38910 -12.21310 4.23098 -0.55937 +54.38910 -12.24870 4.27637 -0.56482 +54.45500 -12.28180 4.30920 -0.57090 +54.45500 -12.33780 4.36976 -0.57257 +54.51950 -12.39040 4.40302 -0.56898 +54.51950 -12.43530 4.45915 -0.57802 +54.58500 -12.49490 4.52416 -0.59272 +54.58500 -12.54790 4.59054 -0.60894 +54.65050 -12.58500 4.62215 -0.60542 +54.65050 -12.63110 4.67430 -0.61427 +54.71500 -12.68240 4.73433 -0.62018 +54.71500 -12.73030 4.77898 -0.61742 +54.78050 -12.78090 4.82537 -0.61609 +54.78050 -12.84590 4.89767 -0.62541 +54.84910 -12.90760 4.96930 -0.63236 +54.84910 -12.95660 4.99250 -0.63325 +54.91050 -13.00670 5.04566 -0.64224 +54.91050 -13.07470 5.11110 -0.64996 +54.97570 -13.12300 5.16032 -0.65287 +54.97570 -13.16880 5.21313 -0.66041 +55.04160 -13.22970 5.28818 -0.66873 +55.04160 -13.28230 5.35958 -0.67492 +55.10600 -13.34180 5.41233 -0.67456 +55.10600 -13.38510 5.46893 -0.67735 +55.17100 -13.44110 5.55838 -0.68437 +55.17100 -13.49240 5.61622 -0.69062 +55.23650 -13.54760 5.67869 -0.69201 +55.23650 -13.61140 5.77077 -0.71186 +55.30170 -13.66350 5.87617 -0.72606 +55.30170 -13.71050 5.93791 -0.72587 +55.36650 -13.75960 6.01326 -0.73092 +55.36650 -13.81170 6.09929 -0.73808 +55.43200 -13.86890 6.15500 -0.74123 +55.43200 -13.92920 6.22936 -0.74651 +55.49690 -13.99690 6.33149 -0.75625 +55.49690 -14.05210 6.43649 -0.77306 +55.56220 -14.11510 6.49998 -0.77746 +55.56220 -14.17000 6.58840 -0.79380 +55.62700 -14.23090 6.69409 -0.80525 +55.62700 -14.28850 6.79153 -0.81556 +55.69240 -14.34650 6.86479 -0.81768 +55.69240 -14.41470 6.97285 -0.82612 +55.75750 -14.47860 7.06865 -0.83466 +55.75750 -14.54580 7.15862 -0.84091 +55.82250 -14.61380 7.24368 -0.84542 +55.82250 -14.68050 7.35698 -0.85588 +55.88800 -14.74510 7.47362 -0.86904 +55.88800 -14.81310 7.56838 -0.88792 +55.95300 -14.89090 7.71897 -0.90892 +56.01800 -14.96620 7.83678 -0.91921 +56.01800 -15.03430 7.94065 -0.91602 +56.08300 -15.11070 8.06301 -0.92398 +56.08300 -15.18750 8.18605 -0.92797 +56.14800 -15.26800 8.29780 -0.93185 +56.14800 -15.36460 8.42343 -0.95950 +56.21350 -15.44640 8.54923 -0.97493 +56.21350 -15.50720 8.69163 -0.98973 +56.27850 -15.56560 8.80254 -0.98848 +56.27850 -15.65200 8.95067 -1.00645 +56.34350 -15.70900 9.08454 -1.02145 +56.34350 -15.77500 9.19541 -1.02231 +56.40900 -15.86280 9.35282 -1.02931 +56.47400 -15.90670 9.50795 -1.04874 +56.47400 -15.95270 9.64655 -1.05573 +56.53850 -16.01540 9.77928 -1.06457 +56.53850 -16.10310 9.89446 -1.06644 +56.60450 -16.15380 10.01710 -1.08069 +56.60450 -16.21310 10.13280 -1.09173 +56.66950 -16.28930 10.26040 -1.09704 +56.66950 -16.34150 10.41830 -1.11203 +56.73400 -16.38990 10.54590 -1.12417 +56.73400 -16.45130 10.71530 -1.13762 +56.80000 -16.52930 10.80000 -1.13444 +56.80000 -16.57830 10.93170 -1.14899 +56.86450 -16.63280 11.04190 -1.16322 +56.86450 -16.72360 11.18270 -1.15185 +56.92950 -16.76280 11.31830 -1.16378 +56.92950 -16.81890 11.43750 -1.17310 +56.99590 -16.87700 11.57480 -1.17840 +56.99590 -16.92530 11.70170 -1.18851 +57.05990 -16.96510 11.80220 -1.19945 +57.05990 -17.01240 11.96250 -1.21113 +57.12570 -17.09230 12.06800 -1.20701 +57.12570 -17.15030 12.18380 -1.21675 +57.19000 -17.19520 12.30070 -1.21787 +57.19000 -17.27710 12.45670 -1.21733 +57.25560 -17.34590 12.55860 -1.22414 +57.25560 -17.40690 12.67870 -1.22977 +57.32050 -17.48250 12.83280 -1.23792 +57.38600 -17.56860 12.96270 -1.23849 +57.38600 -17.60880 13.07840 -1.24336 +57.38600 -17.66870 13.21470 -1.24750 +57.45100 -17.75750 13.36640 -1.24620 +57.51600 -17.82820 13.47370 -1.24837 +57.51600 -17.88490 13.59650 -1.25221 +57.58170 -17.97060 13.77350 -1.25329 +57.58170 -18.05660 13.89720 -1.25711 +57.64690 -18.10440 14.03750 -1.25817 +57.64690 -18.16930 14.18500 -1.26683 +57.71150 -18.23290 14.32980 -1.26950 +57.71150 -18.31010 14.46270 -1.27245 +57.71150 -18.35600 14.59290 -1.28112 +57.77700 -18.42060 14.79530 -1.28738 +57.84180 -18.51640 14.94820 -1.28704 +57.84180 -18.57630 15.10540 -1.28184 +57.90910 -18.64920 15.27240 -1.28324 +57.90910 -18.72630 15.44120 -1.28366 +57.97190 -18.79340 15.58170 -1.28774 +57.97190 -18.85940 15.75040 -1.29048 +58.03760 -18.95190 15.96570 -1.29547 +58.03760 -19.04460 16.11490 -1.29367 +58.10250 -19.12020 16.27920 -1.29725 +58.10250 -19.20540 16.49490 -1.29931 +58.16800 -19.30500 16.66860 -1.29703 +58.16800 -19.38180 16.85010 -1.29452 +58.23300 -19.48210 17.09120 -1.29685 +58.29800 -19.58030 17.30650 -1.29847 +58.29800 -19.66750 17.48090 -1.29358 +58.36310 -19.77730 17.71270 -1.29774 +58.36310 -19.89560 17.92730 -1.28553 +58.42850 -19.99360 18.12160 -1.28588 +58.42850 -20.10530 18.37430 -1.28286 +58.49350 -20.20060 18.61550 -1.28060 +58.49350 -20.29190 18.80760 -1.28043 +58.55850 -20.40660 19.03440 -1.28962 +58.62430 -20.52600 19.29270 -1.28457 +58.62430 -20.61980 19.50880 -1.27486 +58.68900 -20.74200 19.76220 -1.26412 +58.68900 -20.83610 20.00460 -1.26843 +58.75400 -20.92230 20.22850 -1.26679 +58.81950 -21.02180 20.46020 -1.25901 +58.81950 -21.09910 20.69090 -1.26269 +58.88450 -21.16830 20.90250 -1.26727 +58.88450 -21.25460 21.11740 -1.26158 +58.95240 -21.31680 21.38210 -1.26156 +58.95240 -21.37550 21.60080 -1.26275 +59.01500 -21.43050 21.83740 -1.26458 +59.01500 -21.47750 22.04390 -1.26426 +59.08280 -21.51400 22.26660 -1.26564 +59.14510 -21.57300 22.47900 -1.26996 +59.14510 -21.65760 22.74040 -1.25770 +59.21050 -21.68030 22.96420 -1.25758 +59.21050 -21.72200 23.19410 -1.24988 +59.28850 -21.77640 23.40770 -1.24275 +59.28850 -21.81980 23.63920 -1.24354 +59.34050 -21.88160 23.84430 -1.23787 +59.40550 -21.93640 24.10830 -1.23334 +59.40550 -22.00310 24.31660 -1.22894 +59.47080 -22.04000 24.50330 -1.22434 +59.47080 -22.09460 24.74260 -1.21715 +59.53550 -22.16100 24.95890 -1.21193 +59.53550 -22.19970 25.16050 -1.21070 +59.60110 -22.25340 25.40020 -1.20433 +59.60110 -22.33830 25.65950 -1.19564 +59.66600 -22.39270 25.89100 -1.18992 +59.73100 -22.44890 26.12660 -1.18193 +59.73100 -22.50810 26.38020 -1.17628 +59.79650 -22.53940 26.58990 -1.18036 +59.79650 -22.58850 26.83930 -1.17618 +59.86150 -22.64080 27.08900 -1.16391 +59.92690 -22.66090 27.30020 -1.15126 +59.92690 -22.69460 27.51500 -1.14026 +59.99160 -22.70850 27.76390 -1.13982 +59.99160 -22.73530 27.96550 -1.13559 +60.05650 -22.75260 28.21010 -1.12613 +60.05650 -22.77090 28.47290 -1.11563 +60.12160 -22.78780 28.68120 -1.11460 +60.12160 -22.81170 28.91220 -1.11172 +60.18940 -22.83740 29.11870 -1.10351 +60.25200 -22.84560 29.34680 -1.09696 +60.25200 -22.88320 29.54390 -1.08892 +60.31700 -22.91420 29.81890 -1.07451 +60.31700 -22.92370 30.02510 -1.06999 +60.38300 -22.93330 30.23510 -1.06091 +60.38300 -22.95150 30.44960 -1.05503 +60.44750 -23.00280 30.63660 -1.04107 +60.44750 -23.00560 30.84060 -1.04023 +60.51580 -23.01630 31.06470 -1.02951 +60.57910 -23.04450 31.28740 -1.01328 +60.57910 -23.05140 31.45890 -1.01228 +60.64300 -23.05400 31.63750 -1.00243 +60.64300 -23.08780 31.85300 -0.99014 +60.70800 -23.14370 32.05320 -0.97527 +60.70800 -23.14910 32.25150 -0.96796 +60.77350 -23.18150 32.51780 -0.95134 +60.77350 -23.22690 32.73160 -0.93629 +60.83860 -23.24190 32.92190 -0.92993 +60.83860 -23.25460 33.16130 -0.91927 +60.90390 -23.30380 33.37240 -0.90234 +60.96950 -23.31830 33.58440 -0.89046 +60.96950 -23.34390 33.83620 -0.87970 +61.03400 -23.37190 34.06030 -0.87051 +61.03400 -23.38840 34.28350 -0.85654 +61.09900 -23.40990 34.49950 -0.84584 +61.09900 -23.44090 34.72130 -0.83142 +61.16400 -23.45200 34.91540 -0.82020 +61.22950 -23.47830 35.14950 -0.80648 +61.22950 -23.51320 35.36880 -0.79011 +61.29410 -23.51810 35.58810 -0.77857 +61.29410 -23.53810 35.81030 -0.76783 +61.35970 -23.54840 36.04220 -0.75682 +61.35970 -23.56490 36.26610 -0.74240 +61.42500 -23.58480 36.48960 -0.73066 +61.42500 -23.58590 36.74590 -0.71825 +61.48960 -23.59090 36.94490 -0.71044 +61.55500 -23.59950 37.17340 -0.69819 +61.55500 -23.58910 37.42230 -0.69211 +61.62050 -23.60670 37.64190 -0.67688 +61.62050 -23.60510 37.87310 -0.66746 +61.68500 -23.61540 38.12010 -0.65290 +61.68500 -23.60250 38.32190 -0.64590 +61.75050 -23.59310 38.56240 -0.63599 +61.75050 -23.58900 38.79680 -0.62718 +61.81600 -23.60590 39.00610 -0.61278 +61.81600 -23.59280 39.23140 -0.60791 +61.88050 -23.58520 39.51840 -0.59482 +61.94610 -23.58720 39.75100 -0.58014 +61.94610 -23.57970 39.96570 -0.56654 +62.01100 -23.57320 40.25290 -0.55537 +62.01100 -23.56430 40.48420 -0.54907 +62.07610 -23.54970 40.70400 -0.53866 +62.14360 -23.53340 41.00240 -0.52950 +62.14360 -23.51760 41.23160 -0.51778 +62.20650 -23.50200 41.45910 -0.50877 +62.20650 -23.48900 41.71570 -0.50070 +62.27630 -23.48190 41.95240 -0.48924 +62.27630 -23.45320 42.16560 -0.48520 +62.33650 -23.42200 42.42250 -0.48124 +62.40200 -23.39440 42.67690 -0.46802 +62.40200 -23.36120 42.90460 -0.46283 +62.46700 -23.34400 43.14290 -0.45683 +62.46700 -23.31170 43.40300 -0.45044 +62.53200 -23.27420 43.60980 -0.44884 +62.53200 -23.23880 43.85270 -0.44059 +62.59750 -23.17950 44.13380 -0.43436 +62.59750 -23.12520 44.38020 -0.43152 +62.66250 -23.07970 44.61400 -0.42599 +62.72760 -23.01200 44.87570 -0.42978 +62.72760 -22.97740 45.10770 -0.42022 +62.79300 -22.92600 45.33710 -0.41692 +62.79300 -22.85290 45.62120 -0.41346 +62.85750 -22.81820 45.86300 -0.40293 +62.85750 -22.76840 46.09730 -0.40042 +62.92300 -22.71000 46.35690 -0.39805 +62.92300 -22.68610 46.57690 -0.38542 +62.98850 -22.62660 46.82690 -0.38478 +63.05300 -22.56280 47.13070 -0.37638 +63.05300 -22.54250 47.37050 -0.36036 +63.11800 -22.46890 47.61780 -0.35834 +63.11800 -22.41310 47.89650 -0.35092 +63.18350 -22.38180 48.14050 -0.34153 +63.18350 -22.33190 48.37880 -0.33587 +63.24850 -22.27750 48.62160 -0.32491 +63.31350 -22.18880 48.93790 -0.32187 +63.31350 -22.14860 49.17470 -0.31189 +63.37910 -22.09120 49.41700 -0.30829 +63.37910 -22.04300 49.65530 -0.29698 +63.44400 -21.98170 49.89480 -0.29191 +63.44400 -21.91910 50.13460 -0.28578 +63.50900 -21.85580 50.39540 -0.27667 +63.50900 -21.78480 50.63980 -0.27452 +63.57450 -21.70250 50.88770 -0.26876 +63.63910 -21.63240 51.11180 -0.26773 +63.63910 -21.58020 51.37650 -0.25621 diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py b/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py new file mode 100644 index 0000000000..b28dbda628 --- /dev/null +++ b/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py @@ -0,0 +1,116 @@ +# 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. + +"""Render a trajectory visualization for the current run and save a small, +commit-friendly downsampled trajectory. Called automatically at the end of +train.py (non-fatal), or run standalone: `uv run visualize.py`. + +Outputs (overwritten each run; commit them on a "keep" so git history holds the +per-experiment version): + - viz.png — top-down xy (LIO rigid-aligned to GT) + position error vs time + - traj_ds.tsv — downsampled raw LIO trajectory (t x y z), ~2k poses, ~60 KB + +PNG (not JPEG): the plot is thin-line art + text, which PNG stores smaller and +without the ringing artifacts JPEG adds to lines. +""" + +import matplotlib +import numpy as np + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import prepare + +DS_POSES = 2000 # downsample target for the saved trajectory + plot +FIT_WINDOW = 30.0 # seconds; the second top-down panel aligns only on the early +# window (where LIO still tracks) to show where it peels off +VIZ_PNG = "viz.png" +TRAJ_DS = "traj_ds.tsv" + + +def render(traj_path=prepare.TRAJ_PATH, out_png=VIZ_PNG, out_traj=TRAJ_DS): + gt_t, gt_p = prepare.load_gt() + t, P = prepare.load_traj(traj_path) + + # --- save downsampled raw trajectory (commit-friendly) --- + step = max(1, len(t) // DS_POSES) + ds = np.column_stack([t[::step], P[::step]]) + np.savetxt( + out_traj, + ds, + fmt="%.5f", + delimiter="\t", + header="downsampled raw LIO trajectory\nt_rel\tx\ty\tz", + ) + + # --- 2D rigid-align LIO onto GT: full-run (the metric) and first-FIT_WINDOW s --- + gt_xy, xy = gt_p[:, :2], P[:, :2] + lo, hi = max(t[0], gt_t[0]), min(t[-1], gt_t[-1]) + ov = (t >= lo) & (t <= hi) + tv, xyv = t[ov], xy[ov] + gti = np.column_stack([np.interp(tv, gt_t, gt_xy[:, k]) for k in range(2)]) + + def align(fit_mask): + R, tr = prepare._umeyama_2d(xyv[fit_mask], gti[fit_mask]) + al = (R @ xyv.T).T + tr + return al, np.linalg.norm(al - gti, axis=1) + + early = tv <= (tv[0] + FIT_WINDOW) + al_early, err_early = align(early) + al_full, err_full = align(np.ones(len(tv), bool)) + ate = float(np.sqrt(np.mean(err_full**2))) # the metric: standard full-run ATE + + def topdown(a, al, title): + a.plot(gt_xy[:, 0], gt_xy[:, 1], "k-", lw=2, label="robot_odom (gt)", zorder=5) + a.plot(al[:, 0], al[:, 1], "tab:blue", lw=1, alpha=0.85, label="LIO (aligned)") + a.plot(*gti[0], "go", ms=9, zorder=6) + a.plot(*gti[-1], "kx", ms=11, mew=3, zorder=6) + cx, cy = gt_xy[:, 0].mean(), gt_xy[:, 1].mean() + r = ( + max(gt_xy[:, 0].max() - gt_xy[:, 0].min(), gt_xy[:, 1].max() - gt_xy[:, 1].min()) * 0.75 + + 1.5 + ) + a.set_xlim(cx - r, cx + r) + a.set_ylim(cy - r, cy + r) + a.set_aspect("equal") + a.grid(alpha=0.3) + a.legend(fontsize=8) + a.set_title(title) + a.set_xlabel("x (m)") + a.set_ylabel("y (m)") + + fig, ax = plt.subplots(1, 3, figsize=(19, 5.5)) + topdown( + ax[0], + al_early, + f"Top-down xy — aligned on first {FIT_WINDOW:.0f}s\n(early tracking, then where it peels off)", + ) + topdown(ax[1], al_full, "Top-down xy — full-run aligned (ATE metric)\ngreen=start kx=end") + ax[2].plot(tv, err_early, "tab:green", lw=1.2, label=f"first-{FIT_WINDOW:.0f}s align") + ax[2].plot(tv, err_full, "tab:blue", lw=1.2, label="full-run align") + ax[2].axvline(FIT_WINDOW, color="gray", ls=":", lw=1) + ax[2].set_title(f"position error vs gt (val_ate_xy = {ate:.3f} m)") + ax[2].set_xlabel("t (s)") + ax[2].set_ylabel("|p - gt| (m)") + ax[2].grid(alpha=0.3) + ax[2].legend(fontsize=8) + fig.tight_layout() + fig.savefig(out_png, dpi=100) + plt.close(fig) + return ate + + +if __name__ == "__main__": + ate = render() + print(f"wrote {VIZ_PNG} + {TRAJ_DS} (val_ate_xy={ate:.3f})") diff --git a/pyproject.toml b/pyproject.toml index 5cd748fbfa..9245b21ad3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -620,4 +620,5 @@ ignore = [ "dimos/manipulation/manipulation_module.py", "dimos/navigation/nav_stack/modules/*/main.cpp", "dimos/navigation/nav_stack/common/*.hpp", + "dimos/mapping/research/go2/autoresearch/*/*" ] From 0de69bf2e94dd49d38c9dc2d8ef30311e7feecd1 Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 29 May 2026 22:01:12 +0800 Subject: [PATCH 03/15] autoresearch fix --- .../research/go2/autoresearch/flake.nix | 19 +++++++--- .../research/go2/autoresearch/lio-1/README.md | 10 +++--- .../autoresearch/lio-1/{train.py => algo.py} | 26 +++++++------- .../lio-1/{prepare.py => evaluate.py} | 35 +++++++++++-------- .../go2/autoresearch/lio-1/program.md | 18 +++++----- .../research/go2/autoresearch/lio-1/setup.sh | 4 +-- .../go2/autoresearch/lio-1/visualize.py | 12 +++---- 7 files changed, 70 insertions(+), 54 deletions(-) rename dimos/mapping/research/go2/autoresearch/lio-1/{train.py => algo.py} (88%) rename dimos/mapping/research/go2/autoresearch/lio-1/{prepare.py => evaluate.py} (88%) diff --git a/dimos/mapping/research/go2/autoresearch/flake.nix b/dimos/mapping/research/go2/autoresearch/flake.nix index 0ea34edceb..1eb4e6a19b 100644 --- a/dimos/mapping/research/go2/autoresearch/flake.nix +++ b/dimos/mapping/research/go2/autoresearch/flake.nix @@ -2,11 +2,20 @@ description = "LIO autoresearch: Point-LIO build deps (pcl, yaml-cpp, boost) layered onto the dimos dev shell"; inputs = { - # The root dimos flake, pinned to a committed rev. The pin is load-bearing: - # this repo's worktree is ~100 G and usually dirty, so a bare git+file would - # copy the whole thing into the store. Pinning a rev exports from git objects - # instead — LFS files stay as pointers, untracked files are ignored, ~16 M - # closure. Bump this rev only if the root flake's dev shell changes. + # The root dimos flake, layered under this one to inherit the full dev shell. + # + # GOTCHA: the rev pin below is load-bearing. To evaluate a flake input, Nix + # copies that flake's source into the store, and the dimos worktree is ~100 G + # (≈20 G tracked with smudged LFS, plus untracked junk) and usually dirty: + # - `path:../../../../..` -> copies the whole 100 G tree. No. + # - bare `git+file:` (dirty tree) -> copies the smudged ~20 G working tree. No. + # - `git+file:...?rev=` -> exports from git OBJECTS at that commit: + # LFS files are ~130 B pointers, untracked + # files don't exist. ~16 M closure, locks + # in <1 s. <- this. + # Bump only when the root flake's dev shell changes (it rarely does). + # Also note: this flake.nix must be `git add`ed — Nix won't see an untracked + # flake file that lives inside a git repo. dimos.url = "git+file:../../../../..?rev=6c24579da002fb050231a9486d2b008cc0d7cada&shallow=1"; nixpkgs.follows = "dimos/nixpkgs"; flake-utils.follows = "dimos/flake-utils"; diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/README.md index 768af846b2..e555a28ea8 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/README.md +++ b/dimos/mapping/research/go2/autoresearch/lio-1/README.md @@ -11,9 +11,9 @@ odometry, keep or discard, repeat. Three files matter (same idiom as the original autoresearch): -- **`prepare.py`** — fixed data paths, the Point-LIO substrate location, and the +- **`evaluate.py`** — fixed data paths, the Point-LIO substrate location, and the evaluation (`evaluate`, the ground-truth metric). Not modified. -- **`train.py`** — the single file the agent edits. Holds the Point-LIO `CONFIG` +- **`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.** @@ -52,7 +52,7 @@ cd lio-1 ./setup.sh # 3. Run a single baseline experiment (~1-3 min) -python train.py +python algo.py ``` If those work, you're ready for autonomous mode: point your agent at @@ -61,8 +61,8 @@ If those work, you're ready for autonomous mode: point your agent at ## Project structure ``` -prepare.py — data paths (via get_data), substrate location, evaluation (do not modify) -train.py — Point-LIO CONFIG + run/eval driver (agent modifies this) +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 diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/train.py b/dimos/mapping/research/go2/autoresearch/lio-1/algo.py similarity index 88% rename from dimos/mapping/research/go2/autoresearch/lio-1/train.py rename to dimos/mapping/research/go2/autoresearch/lio-1/algo.py index 7ece433532..cafa20e6d4 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/train.py +++ b/dimos/mapping/research/go2/autoresearch/lio-1/algo.py @@ -15,9 +15,9 @@ """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 prepare.py, and prints a summary block. +harness in evaluate.py, and prints a summary block. -The Point-LIO C++ substrate (point_lio/) and the evaluation (prepare.py) are +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. @@ -32,7 +32,7 @@ import subprocess import time -import prepare +import evaluate # ---------------------------------------------------------------------------- # CONFIG — the knobs you tune. These map 1:1 to the Point-LIO yaml. @@ -77,7 +77,7 @@ def write_yaml(cfg, path): R = cfg["extrinsic_R"] T = cfg["extrinsic_T"] - txt = f"""# auto-generated by train.py — do not hand-edit; edit CONFIG in train.py + 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" @@ -134,26 +134,26 @@ def write_yaml(cfg, path): def main(): - if not os.path.exists(prepare.POINTLIO_BIN): + if not os.path.exists(evaluate.POINTLIO_BIN): raise SystemExit("pointlio binary not built — run ./setup.sh first.") - write_yaml(CONFIG, prepare.ACTIVE_YAML) - if os.path.exists(prepare.TRAJ_PATH): - os.remove(prepare.TRAJ_PATH) + write_yaml(CONFIG, evaluate.ACTIVE_YAML) + if os.path.exists(evaluate.TRAJ_PATH): + os.remove(evaluate.TRAJ_PATH) t0 = time.time() try: subprocess.run( - [prepare.POINTLIO_BIN, "--yaml", prepare.ACTIVE_YAML, "--bin", prepare.BIN_PATH], - cwd=prepare.POINTLIO_DIR, - timeout=prepare.RUN_TIMEOUT, + [evaluate.POINTLIO_BIN, "--yaml", evaluate.ACTIVE_YAML, "--bin", evaluate.BIN_PATH], + cwd=evaluate.POINTLIO_DIR, + timeout=evaluate.RUN_TIMEOUT, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, ) except subprocess.TimeoutExpired: - raise SystemExit(f"run exceeded {prepare.RUN_TIMEOUT}s — treat as failure") + raise SystemExit(f"run exceeded {evaluate.RUN_TIMEOUT}s — treat as failure") run_s = time.time() - t0 - m = prepare.evaluate() # raises if trajectory missing/malformed (= crash) + m = evaluate.evaluate() # raises if trajectory missing/malformed (= crash) print("---") print(f"val_ate_xy: {m['val_ate_xy']:.6f}") diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/prepare.py b/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py similarity index 88% rename from dimos/mapping/research/go2/autoresearch/lio-1/prepare.py rename to dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py index 3bc87890a4..8ef84a20cb 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/prepare.py +++ b/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py @@ -17,7 +17,7 @@ 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 -train.py. +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 @@ -50,7 +50,7 @@ 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") # train.py writes this +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 @@ -151,16 +151,23 @@ def evaluate_3d(traj_path=TRAJ_PATH): if __name__ == "__main__": - print("LIO autoresearch — data + harness check") + # 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}") - ok = check_data() - if ok: - 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" - ) - print("Done! Ready to run train.py.") - else: - print("Setup incomplete — run ./setup.sh.") + 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/program.md b/dimos/mapping/research/go2/autoresearch/lio-1/program.md index 3ec1dd0ebe..dda1de36a8 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/program.md +++ b/dimos/mapping/research/go2/autoresearch/lio-1/program.md @@ -13,9 +13,9 @@ To set up a new experiment, work with the user to: 2. **Create the branch**: `git checkout -b autoresearch/` from current master. 3. **Read the in-scope files**: The repo is small. Read these files for full context: - `README.md` — repository context. - - `prepare.py` — fixed data paths, the Point-LIO substrate location, and the evaluation (the `evaluate` ground-truth metric). Do not modify. - - `train.py` — the file you modify. The Point-LIO `CONFIG` and the run/eval driver. -4. **Verify the build + data exist**: Check that `point_lio/build/pointlio_mapping` exists and that `python prepare.py` reports the input bin and ground truth present. If not, tell the human to run `./setup.sh`. + - `evaluate.py` — fixed data paths, the Point-LIO substrate location, and the evaluation (the `evaluate` ground-truth metric). Do not modify. + - `algo.py` — the file you modify. The Point-LIO `CONFIG` and the run/eval driver. +4. **Verify the build + data exist**: Check that `point_lio/build/pointlio_mapping` exists and that `python evaluate.py` reports the input bin and ground truth present. If not, tell the human to run `./setup.sh`. 5. **Initialize results.tsv**: Create `results.tsv` with just the header row. The baseline will be recorded after the first run. 6. **Confirm and go**: Confirm setup looks good. @@ -25,16 +25,16 @@ Once you get confirmation, kick off the experimentation. Each experiment runs the offline Point-LIO once on the recorded Go2 LiDAR+IMU stream (a fixed input). It runs to completion on CPU — typically ~1-3 minutes. -You launch it simply as: `python train.py` (in the dimos venv / `nix develop`). +You launch it simply as: `python algo.py` (in the dimos venv / `nix develop`). **What you CAN do:** -- Modify `train.py` — this is the only file you edit. The `CONFIG` dict (which maps 1:1 to the Point-LIO yaml) is fair game: covariances, plane threshold, match scale, voxel filter sizes, IMU integration dt, extrinsics, blind range, FoV, etc. You may also adjust the Python pre/post-processing in this file. +- Modify `algo.py` — this is the only file you edit. The `CONFIG` dict (which maps 1:1 to the Point-LIO yaml) is fair game: covariances, plane threshold, match scale, voxel filter sizes, IMU integration dt, extrinsics, blind range, FoV, etc. You may also adjust the Python pre/post-processing in this file. **What you CANNOT do:** -- Modify `prepare.py`. It is read-only. It contains the fixed data paths, the substrate location, and the evaluation harness. +- Modify `evaluate.py`. It is read-only. It contains the fixed data paths, the substrate location, and the evaluation harness. - Modify the `point_lio/` C++ substrate. It is the fixed Point-LIO engine, built once by `setup.sh` (the analog of a fixed model framework). You tune its configuration via `CONFIG`, not its source. - Install new packages or add dependencies. You can only use what's already in the dimos venv (numpy, matplotlib, and the dimos package). -- Modify the evaluation. The `evaluate` function in `prepare.py` is the ground truth metric. +- Modify the evaluation. The `evaluate` function in `evaluate.py` is the ground truth metric. - Use anything in `human-debug/`. The convert script (`mcap_to_plnr1.py`), the raw `.mcap` recording, and the `.rrd` rerun file there are for **human debugging only** — they are NOT part of the experiment. The input bin and ground truth are already prepared in `data/`; never regenerate them in the loop. **The goal is simple: get the lowest `val_ate_xy`** — the 2D (xy) absolute trajectory error of the rigid-aligned LIO trajectory vs the `robot_odom` ground truth, in meters. The metric is **2D only** for now (this recording is flat and single-story, so the robot's z is near-constant and uninformative); 3D criteria (measured height/range change at specific timesteps) will be added later once a stairs recording is captured. The only constraint is that the code runs without crashing and finishes within the time budget. @@ -104,9 +104,9 @@ The experiment runs on a dedicated branch (e.g. `autoresearch/mar5`). LOOP FOREVER: 1. Look at the git state: the current branch/commit we're on -2. Tune `train.py`'s `CONFIG` with an experimental idea by directly hacking the code. +2. Tune `algo.py`'s `CONFIG` with an experimental idea by directly hacking the code. 3. git commit -4. Run the experiment: `python train.py > run.log 2>&1` (redirect everything — do NOT use tee or let output flood your context) +4. Run the experiment: `python algo.py > run.log 2>&1` (redirect everything — do NOT use tee or let output flood your context) 5. Read out the results: `grep "^val_ate_xy:\|^run_seconds:" run.log` 6. If the grep output is empty, the run crashed. Run `tail -n 50 run.log` to read the trace and attempt a fix. If you can't get things to work after more than a few attempts, give up. 7. Record the results in the tsv (NOTE: do not commit the results.tsv file, leave it untracked by git) diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh b/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh index e8fd781c0e..d784e9c67c 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh +++ b/dimos/mapping/research/go2/autoresearch/lio-1/setup.sh @@ -28,5 +28,5 @@ cmake --build point_lio/build -j"$(nproc 2>/dev/null || echo 4)" # --- sanity check (pulls the data via get_data on first run) --- echo ">> data + harness check" -python prepare.py -echo ">> setup done. Run a baseline with: python train.py > run.log 2>&1" +python evaluate.py +echo ">> setup done. Run a baseline with: python algo.py > run.log 2>&1" diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py b/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py index b28dbda628..b82a8c6c9e 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py +++ b/dimos/mapping/research/go2/autoresearch/lio-1/visualize.py @@ -14,7 +14,7 @@ """Render a trajectory visualization for the current run and save a small, commit-friendly downsampled trajectory. Called automatically at the end of -train.py (non-fatal), or run standalone: `uv run visualize.py`. +algo.py (non-fatal), or run standalone: `uv run visualize.py`. Outputs (overwritten each run; commit them on a "keep" so git history holds the per-experiment version): @@ -29,8 +29,8 @@ import numpy as np matplotlib.use("Agg") +import evaluate import matplotlib.pyplot as plt -import prepare DS_POSES = 2000 # downsample target for the saved trajectory + plot FIT_WINDOW = 30.0 # seconds; the second top-down panel aligns only on the early @@ -39,9 +39,9 @@ TRAJ_DS = "traj_ds.tsv" -def render(traj_path=prepare.TRAJ_PATH, out_png=VIZ_PNG, out_traj=TRAJ_DS): - gt_t, gt_p = prepare.load_gt() - t, P = prepare.load_traj(traj_path) +def render(traj_path=evaluate.TRAJ_PATH, out_png=VIZ_PNG, out_traj=TRAJ_DS): + gt_t, gt_p = evaluate.load_gt() + t, P = evaluate.load_traj(traj_path) # --- save downsampled raw trajectory (commit-friendly) --- step = max(1, len(t) // DS_POSES) @@ -62,7 +62,7 @@ def render(traj_path=prepare.TRAJ_PATH, out_png=VIZ_PNG, out_traj=TRAJ_DS): gti = np.column_stack([np.interp(tv, gt_t, gt_xy[:, k]) for k in range(2)]) def align(fit_mask): - R, tr = prepare._umeyama_2d(xyv[fit_mask], gti[fit_mask]) + R, tr = evaluate._umeyama_2d(xyv[fit_mask], gti[fit_mask]) al = (R @ xyv.T).T + tr return al, np.linalg.norm(al - gti, axis=1) From 04c7de680c22c387d73e4e6fb5c743f4e91d9a79 Mon Sep 17 00:00:00 2001 From: arkluc Date: Fri, 29 May 2026 22:20:04 +0800 Subject: [PATCH 04/15] go2dds_data2 ankle killer --- data/.lfs/go2dds_data2.tar.gz | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 data/.lfs/go2dds_data2.tar.gz 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 From ce70f9d14285ded1b2d348bec008366bb8849d2f Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Fri, 29 May 2026 22:23:50 +0800 Subject: [PATCH 05/15] lio-autoresearch: Point-LIO reads the MCAP directly (drop the PLNR1 .bin) Point-LIO now consumes the Go2 .mcap (rt/utlidar/cloud + rt/utlidar/imu) directly via a vendored Foxglove MCAP C++ reader (point_lio/include/mcap/, zstd chunks), removing the separate PLNR1 .bin and its converter from the loop. - mcap_source.{h,cpp}: read_mcap_as_plnr1() re-emits the mcap as the in-memory PLNR1 byte stream (CDR-decode IMU, lidar blob verbatim, ts = publish_time), fed to the unchanged record loop via fmemopen. C++ port of mcap_to_plnr1.py. - laserMapping.cpp: --bin removed; --mcap only. - CMakeLists/flake: vendor mcap headers, link zstd, MCAP_COMPRESSION_NO_LZ4. - evaluate.py/algo.py: input is MCAP_PATH; algo runs the binary with --mcap. Validated: the .bin is byte-identical to the mcap-derived stream (same sha256), and the --mcap run yields a trajectory bit-identical to the old --bin run (val_ate_xy 10.973953733898254, max abs diff 0.0 over 576204 poses). --- .../research/go2/autoresearch/flake.nix | 1 + .../research/go2/autoresearch/lio-1/README.md | 4 +- .../research/go2/autoresearch/lio-1/algo.py | 2 +- .../go2/autoresearch/lio-1/evaluate.py | 8 +- .../lio-1/point_lio/CMakeLists.txt | 10 +- .../lio-1/point_lio/include/mcap/crc32.hpp | 109 + .../lio-1/point_lio/include/mcap/errors.hpp | 120 + .../lio-1/point_lio/include/mcap/internal.hpp | 193 ++ .../point_lio/include/mcap/intervaltree.hpp | 303 +++ .../lio-1/point_lio/include/mcap/mcap.hpp | 4 + .../point_lio/include/mcap/read_job_queue.hpp | 147 ++ .../lio-1/point_lio/include/mcap/reader.hpp | 743 ++++++ .../lio-1/point_lio/include/mcap/reader.inl | 2017 +++++++++++++++++ .../lio-1/point_lio/include/mcap/types.hpp | 407 ++++ .../lio-1/point_lio/include/mcap/types.inl | 86 + .../point_lio/include/mcap/visibility.hpp | 28 + .../lio-1/point_lio/include/mcap/writer.hpp | 514 +++++ .../lio-1/point_lio/include/mcap/writer.inl | 1168 ++++++++++ .../lio-1/point_lio/include/mcap_source.h | 26 + .../lio-1/point_lio/src/laserMapping.cpp | 29 +- .../lio-1/point_lio/src/mcap_source.cpp | 169 ++ 21 files changed, 6071 insertions(+), 17 deletions(-) create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/crc32.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/errors.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/internal.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/intervaltree.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/mcap.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/read_job_queue.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/reader.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/reader.inl create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/types.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/types.inl create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/visibility.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/writer.hpp create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap/writer.inl create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/include/mcap_source.h create mode 100644 dimos/mapping/research/go2/autoresearch/lio-1/point_lio/src/mcap_source.cpp diff --git a/dimos/mapping/research/go2/autoresearch/flake.nix b/dimos/mapping/research/go2/autoresearch/flake.nix index 1eb4e6a19b..10579af5a5 100644 --- a/dimos/mapping/research/go2/autoresearch/flake.nix +++ b/dimos/mapping/research/go2/autoresearch/flake.nix @@ -34,6 +34,7 @@ pkgs.pcl pkgs.yaml-cpp pkgs.boost + pkgs.zstd # MCAP zstd chunk decompression (vendored mcap C++ reader) ]; # nixpkgs lays PCL headers under include/pcl-/, which # point_lio's hand-rolled find_path (HINTS only /usr/include/pcl*) diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/README.md b/dimos/mapping/research/go2/autoresearch/lio-1/README.md index e555a28ea8..bf7d0914c0 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/README.md +++ b/dimos/mapping/research/go2/autoresearch/lio-1/README.md @@ -71,8 +71,8 @@ human-debug/ — convert script, raw .mcap, .rrd rerun — HUMAN USE ONLY, ``` Data lives in the dimos LFS store, not here: `data/go2dds_data1/` holds -`go2-185959.bin` (LIO input) + `gt_robot_odom.tsv` (ground truth), fetched via -`get_data("go2dds_data1/...")`. +`20260529-185959.mcap` (LIO input — Point-LIO reads it directly) + +`gt_robot_odom.tsv` (ground truth), fetched via `get_data("go2dds_data1/...")`. ## License diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/algo.py b/dimos/mapping/research/go2/autoresearch/lio-1/algo.py index cafa20e6d4..ac01ae3bae 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/algo.py +++ b/dimos/mapping/research/go2/autoresearch/lio-1/algo.py @@ -143,7 +143,7 @@ def main(): t0 = time.time() try: subprocess.run( - [evaluate.POINTLIO_BIN, "--yaml", evaluate.ACTIVE_YAML, "--bin", evaluate.BIN_PATH], + [evaluate.POINTLIO_BIN, "--yaml", evaluate.ACTIVE_YAML, "--mcap", evaluate.MCAP_PATH], cwd=evaluate.POINTLIO_DIR, timeout=evaluate.RUN_TIMEOUT, stdout=subprocess.DEVNULL, diff --git a/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py b/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py index 8ef84a20cb..844e0e8691 100644 --- a/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py +++ b/dimos/mapping/research/go2/autoresearch/lio-1/evaluate.py @@ -42,8 +42,8 @@ # --- fixed data inputs, pulled from the dimos LFS data store. get_data resolves # /data/go2dds_data1/, downloading + decompressing the LFS archive on first -# use. The bin + GT are pre-made; see human-debug/ for how. --- -BIN_PATH = str(get_data("go2dds_data1/go2-185959.bin")) # PLNR1 lidar+imu input +# 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) --- @@ -60,10 +60,10 @@ def check_data(): - """Setup-step sanity check: input bin, GT, and built binary all present.""" + """Setup-step sanity check: input mcap, GT, and built binary all present.""" ok = True for label, p in [ - ("input bin", BIN_PATH), + ("input mcap", MCAP_PATH), ("ground truth", GT_PATH), ("pointlio binary", POINTLIO_BIN), ]: 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 index b734038dde..515a3f8f52 100755 --- a/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt +++ b/dimos/mapping/research/go2/autoresearch/lio-1/point_lio/CMakeLists.txt @@ -58,23 +58,31 @@ 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}) +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) 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