LocalizationNode by ar961na · Pull Request #1 · ar961na/LocNode

ar961na · 2026-05-19T08:53:43Z

No description provided.

rongirl · 2026-05-20T11:15:19Z

+RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F42ED6FBAB17C654
+
+# Install livox_ros_driver2 for Livox LiDAR support
+RUN apt-get update && apt-get install -y git build-essential \


Добавь после установки зависимостей rm -rf /var/lib/apt/lists/*

rongirl · 2026-05-20T11:16:13Z

+
+numpy==1.23.5
+scipy==1.9.3
+scikit-learn>=0.23.0


Стоит указать точную версию

rongirl · 2026-05-20T11:28:34Z

+        self.ransac_max_iterations = 1_000_000  # Reduced for 30% speedup
+        self.ransac_confidence = 200  # Reduced for early stopping


Немного запутывает название self.ransac_confidence, confidence это больше про вероятность. Тут судя по документации более уместно max_validation. Также лучше вынести эти параметры в конструктор класса.

rongirl · 2026-05-20T11:29:32Z

+        """Return current RANSAC parameters."""
+        return {
+            "max_iterations": self.ransac_max_iterations,
+            "confidence": self.ransac_confidence,


Поменяй слово confidence здесь

rongirl · 2026-05-20T11:43:22Z

+    Returns:
+        Transformed trajectory with same format.
+    """
+    from scipy.spatial.transform import Rotation


Лучше располагать все импорты в начале файла согласно PEP 8

rongirl · 2026-05-20T11:44:13Z

@@ -0,0 +1 @@
+pyyaml


Можно указать конкретную версию пакета

vnmsklnk · 2026-05-20T12:12:43Z

+RUN apt-get update && apt-get install -y git build-essential \
+    && mkdir -p /home/mars_ugv/livox_ws/src \
+    && cd /home/mars_ugv/livox_ws/src \
+    && git clone https://github.com/Livox-SDK/livox_ros_driver2.git \


Здесь лучше зафиксировать конкретный коммит

vnmsklnk · 2026-05-20T12:14:09Z

+
+# Copy application code
+COPY nodes/ /opt/fastlio_localization/nodes/
+#COPY docker/pipeline_all_in_docker.py /opt/fastlio_localization/


Забыла убрать

vnmsklnk · 2026-05-20T12:19:51Z

+COPY config/ /opt/fastlio_localization/config/
+COPY scripts/downsample_reference.py /opt/fastlio_localization/scripts/downsample_reference.py
+
+RUN chmod +x /opt/fastlio_localization/scripts/downsample_reference.py


Можно сделать любые файлы исполняемыми у себя локально и закоммитить. Тогда после git clone репозитория файлы сразу будут исполняемыми и вот так делать не придется

vnmsklnk · 2026-05-20T12:24:47Z

+        if ext == ".pcd":
+            return PointCloudProcessor.load_pcd(filepath)
+        elif ext == ".obj":
+            mesh = o3d.io.read_triangle_mesh(filepath)
+            pcd = mesh.sample_points_uniformly(number_of_points=int(1e6))
+            if len(pcd.points) == 0:
+                raise ValueError(f"Empty mesh/point cloud: {filepath}")
+            return pcd
+        elif ext == ".ply":
+            pcd = o3d.io.read_point_cloud(filepath)
+            if len(pcd.points) == 0:
+                raise ValueError(f"Empty point cloud: {filepath}")
+            return pcd


Ну как будто pcd и ply по итогу одинаково загружаются

vnmsklnk · 2026-05-20T12:29:59Z

+        point = np.array([x, y, z])
+        transformed_point = transform_point(point, T)
+
+        quat = np.array([qx, qy, qz, qw])
+        R_odom = Rotation.from_quat(quat).as_matrix()
+        R_ref = T[:3, :3] @ R_odom
+        R_rot = Rotation.from_matrix(R_ref)
+        quat_transformed = R_rot.as_quat()


Можно просто сразу сделать матрицу 4х4 и умножить её на T, тогда за одно умножение получится то же самое

vnmsklnk · 2026-05-20T12:33:29Z

+        self.trajectory_buffer.append([stamp, x, y, z, qx, qy, qz, qw])
+
+        # Limit buffer size
+        if len(self.trajectory_buffer) > 10000:


Лучше вынести константу куда-нибудь в инициализацию класса

vnmsklnk · 2026-05-20T12:34:29Z

+        # Transform position
+        pos_h = np.append(pos, 1.0)
+        pos_ref = (T @ pos_h)[:3]
+
+        # Transform orientation
+        R_odom = Rotation.from_quat(quat).as_matrix()
+        R_ref = T[:3, :3] @ R_odom
+        quat_ref = Rotation.from_matrix(R_ref).as_quat()


Здесь тот же самый момент, можно обойтись одним умножением

vnmsklnk · 2026-05-20T12:35:24Z

+from PIL import Image
+
+
+def load_point_cloud(filepath: str) -> o3d.geometry.PointCloud:


Кажется уже где-то это было

Да, точно такая же функция используется в nodes/common.py PointCloudProcessor.

В унифицированном скрипте scripts/visualize.py я оставила копию функции загрузки облака, чтобы его можно было использовать отдельно от узла локализации.

vnmsklnk · 2026-05-20T12:37:36Z

+    return geometries
+
+
+def _save_visualization_as_gif(


Очень похоже на то, что происходит в create_gif.py, может можно унифицировать?

объединила функционал create_gif.py и visualize_trajectory.py в один файл visualize.py

- Add PointCloudProcessor.load_downsampled() for memoized static clouds; - Update Global and LocalAlign to use pre-processed clouds and cached targets; - Implement LocalAlign._select_seed() for warm-starting with candidate seeds (previous + fresh global); - Add buffer_lock for thread-safe concurrent access; - Switch to deque with automatic pruning.

- Update README.md, config/pipeline_config.yaml and .gitignore; - Simplify docker/docker_build_and_run.sh print_header function; - Simplify scripts/visualize.py logic.

ar961na

Спасибо, исправила комментарии

ar961na · 2026-06-08T18:19:58Z

+RUN apt-get update && apt-get install -y git build-essential \
+    && mkdir -p /home/mars_ugv/livox_ws/src \
+    && cd /home/mars_ugv/livox_ws/src \
+    && git clone https://github.com/Livox-SDK/livox_ros_driver2.git \


ar961na · 2026-06-08T18:20:14Z

+RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F42ED6FBAB17C654
+
+# Install livox_ros_driver2 for Livox LiDAR support
+RUN apt-get update && apt-get install -y git build-essential \


ar961na · 2026-06-08T18:27:11Z

+
+# Copy application code
+COPY nodes/ /opt/fastlio_localization/nodes/
+#COPY docker/pipeline_all_in_docker.py /opt/fastlio_localization/


ar961na · 2026-06-08T18:27:25Z

+COPY config/ /opt/fastlio_localization/config/
+COPY scripts/downsample_reference.py /opt/fastlio_localization/scripts/downsample_reference.py
+
+RUN chmod +x /opt/fastlio_localization/scripts/downsample_reference.py


ar961na · 2026-06-08T18:27:31Z

+
+numpy==1.23.5
+scipy==1.9.3
+scikit-learn>=0.23.0


ar961na · 2026-06-08T18:33:16Z

+    return geometries
+
+
+def _save_visualization_as_gif(


объединила функционал create_gif.py и visualize_trajectory.py в один файл visualize.py

ar961na · 2026-06-08T18:47:02Z

+from PIL import Image
+
+
+def load_point_cloud(filepath: str) -> o3d.geometry.PointCloud:


Да, точно такая же функция используется в nodes/common.py PointCloudProcessor.

В унифицированном скрипте scripts/visualize.py я оставила копию функции загрузки облака, чтобы его можно было использовать отдельно от узла локализации.

ar961na · 2026-06-08T19:25:06Z

+        point = np.array([x, y, z])
+        transformed_point = transform_point(point, T)
+
+        quat = np.array([qx, qy, qz, qw])
+        R_odom = Rotation.from_quat(quat).as_matrix()
+        R_ref = T[:3, :3] @ R_odom
+        R_rot = Rotation.from_matrix(R_ref)
+        quat_transformed = R_rot.as_quat()


ar961na · 2026-06-08T19:27:34Z

+        # Transform position
+        pos_h = np.append(pos, 1.0)
+        pos_ref = (T @ pos_h)[:3]
+
+        # Transform orientation
+        R_odom = Rotation.from_quat(quat).as_matrix()
+        R_ref = T[:3, :3] @ R_odom
+        quat_ref = Rotation.from_matrix(R_ref).as_quat()


ar961na · 2026-06-08T19:28:18Z

+        self.trajectory_buffer.append([stamp, x, y, z, qx, qy, qz, qw])
+
+        # Limit buffer size
+        if len(self.trajectory_buffer) > 10000:


- Add optional initial_pose parameter to config and pipeline for complex cases; - Add rebase rotation/translation thresholds for detecting basin switches;- Save both trajectory_reference.txt (rebased) and trajectory_reference_raw.txt (published) odometries; - Add config and it's parameters (e.g. duration, align_interval, map_accumulation_time) v alidation in docker build script; - Fix issue with leftover tail being not localized.

Initial commit with files

5a7f444

rongirl reviewed May 20, 2026

View reviewed changes

vnmsklnk requested changes May 20, 2026

View reviewed changes

ar961na added 4 commits June 8, 2026 13:51

Fix comments

2a02989

Update files

2d18a71

- Update README.md, config/pipeline_config.yaml and .gitignore; - Simplify docker/docker_build_and_run.sh print_header function; - Simplify scripts/visualize.py logic.

Refactor trajectory transformation in localization node

bdeeab3

ar961na commented Jun 8, 2026

View reviewed changes

ar961na requested review from rongirl and vnmsklnk June 9, 2026 10:27

ar961na added 5 commits June 10, 2026 17:44

Add metrics (ATE/RPE) computation

ad288b2

Update metrics trajectory load

79ba6fa

Fix bugs

7de4b88

Simplify docker build

41015f1

		self.ransac_max_iterations = 1_000_000 # Reduced for 30% speedup
		self.ransac_confidence = 200 # Reduced for early stopping

		from PIL import Image


		def load_point_cloud(filepath: str) -> o3d.geometry.PointCloud:

		@@ -0,0 +1 @@
		pyyaml No newline at end of file

Conversation

ar961na commented May 19, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ar961na left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!