diff --git a/README.md b/README.md index 2bd2149..30b1468 100644 --- a/README.md +++ b/README.md @@ -71,3 +71,4 @@ To merge your changes/added files into the official match-ROS repository, we nee | [250408](student_code/250408_Dynamic_Model_for_mobile_robots/README.md) | Entwicklung eines dynamischen Modells zur flexiblen Implementierung auf mobilen Robotersystemen | Dynamische Modellierung von mobilen Robotern, wobei das Modell flexibel auf verschiedenste Roboter übertragen werden kann | | [250428](student_code/250428_Scan_to_map_localization_Mid360_Simulation/README.md) | Development and implementation of a concept for localization using 3D-LiDAR | Algorithm for scan-to-map based 3D real-time localization | | [250429](student_code/250429_real_time_stabilization/README.md) | Real-Time Compensation of Ground Irregularities for Mobile Robot in Construction Additive Manufacturing | Algorithm to stabilize the nozzle for additive manufacturing purposes using IMU-based inclination data during movement on uneven terrain | +| [251127](student_code/251127_print_texture_localization/README.md) | Concept development for robot localization based on an object to be manufactured | Concept for texture-based localization approach for mobile manipulator based on the initial layers of the printed object, using image descriptors and ICP refinement| diff --git a/student_code/251127_print_texture_localization/README.md b/student_code/251127_print_texture_localization/README.md new file mode 100644 index 0000000..effd079 --- /dev/null +++ b/student_code/251127_print_texture_localization/README.md @@ -0,0 +1,51 @@ +# Wall Localization Demo + +## Overview +This folder contains the code developed for wall-based scanner localization using geometric and appearance-based matching. The approach localizes a vertical profiler or scanner relative to a known wall geometry by combining 2D image-based matching (LBP or SIFT) with 3D ICP refinement. + +**Author:** Linh Thach Dang +**E-Mail:** linh.thach.dang@stud.uni-hannover.de + + +## Installation +Copy this folder into the `src` directory of a catkin workspace and build it using `catkin_tools`. + +```bash +cd ~/catkin_ws/src +cp -r wall_localization_demo . +cd ~/catkin_ws +catkin build +source devel/setup.bash +``` + +## Package +### wall_localization_demo + +This package contains all nodes, scripts, and launch files required for: +- offline wall map generation, +- live localization, +- evaluation against ground truth. + + +## Scripts + +All core scripts are located in `src/wall_localization_demo/`. + +### main.py +Runs the live localization pipeline. +Subscribes to a LaserScan topic, transforms scan points into the wall frame, builds a local patch image, performs 2D localization (SIFT or LBP), refines the pose using ICP, and publishes the estimated scanner pose as the TF frame `scanner_icp_refined`. + +### localization_core.py +Implements the core localization algorithms used by `main.py`, including patch generation, descriptor extraction, database matching, ICP refinement, and pose estimation. + +### compare_poses.py +Logs ground truth and estimated poses during localization. +Samples TF and writes two CSV files: +- `groundtruth_poses.csv` (wall → vertical_profiler_link) +- `estimated_poses.csv` (wall → scanner_icp_refined) + +### move_along_wall.py +Moves the robot along the wall at a fixed offset to generate continuous scan data during experiments. + +### pcl2texture.py +Utility script to convert point cloud data into image or texture representations of the wall. \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/.gitignore b/student_code/251127_print_texture_localization/wall_localization_demo/.gitignore new file mode 100644 index 0000000..eb58c55 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/.gitignore @@ -0,0 +1,12 @@ +rosbag +.vscode +.git + +*.csv +*.png +*.jpg +*.obj +*.dae +*.npy +*.npz +*.pkl \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/CMakeLists.txt b/student_code/251127_print_texture_localization/wall_localization_demo/CMakeLists.txt new file mode 100644 index 0000000..33045cd --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wall_localization_demo) + +## Python3 required +set(CMAKE_CXX_STANDARD 14) +set(PYTHON_EXECUTABLE /usr/bin/python3) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + tf2_ros + tf_conversions + pcl_ros + pcl_msgs + gazebo_ros +) + +## OpenCV (used inside localization_2d.py and main.py) +find_package(OpenCV REQUIRED) + +catkin_python_setup() + +## Open3D is a Python dependency, not C++. +## sklearn/skimage/numpy also Python dependencies. + +catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + tf2_ros + tf_conversions + pcl_ros + pcl_msgs + gazebo_ros +) + +############# +## INSTALL ## +############# + +## Install Python scripts so rosrun/roslaunch can find them +catkin_install_python( + PROGRAMS + src/wall_localization_demo/main.py + src/wall_localization_demo/move_along_wall.py + src/wall_localization_demo/compare_poses.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Install launch files +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + +## Install config files +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) + +## Install models (for Gazebo) +install(DIRECTORY models/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models +) + +## Install worlds +install(DIRECTORY worlds/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/worlds +) + +## Install URDF files +install(DIRECTORY urdf/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf +) + +## Install any data (optional) +install(DIRECTORY data/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data +) diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/README.md b/student_code/251127_print_texture_localization/wall_localization_demo/README.md new file mode 100644 index 0000000..32d1661 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/README.md @@ -0,0 +1,118 @@ +# wall_localization_demo + +## Overview +This package implements a wall-based localization approach for a vertical profiler using LaserScan data in ROS Noetic. +The scanner pose is estimated relative to a known wall geometry by combining 2D image-based feature matching (LBP or SIFT) with 3D ICP refinement. +The package supports offline wall map generation, live localization in Gazebo simulation, and quantitative evaluation against ground truth data. + +**Author:** Linh Thach Dang +**E-Mail:** linh.thach.dang@stud.uni-hannover.de + +## Usage +To run the live localization experiment in Gazebo, launch: +```bash +roslaunch wall_localization_demo run_localization.launch +``` +This starts the simulation, initializes the localization pipeline, publishes required static transforms, and records ground truth and estimated poses. + + +## Config files + +- ```offline_map.yaml``` +Contains file paths to the offline wall map, point cloud, and descriptor databases used during localization. + +- ```estimator_params.yaml``` +Defines parameters for the live localization pipeline, including frame definitions, filtering limits, and ICP settings. + +- ```localization_params.yaml``` +Contains parameters for the 2D feature matching stage, such as descriptor configuration and matching thresholds. + + +## Launch files + +- ```run_localization.launch``` +Runs the complete live localization pipeline in simulation. + +Argument list: +method (type: string, default: sift): Feature matching method used for localization (sift or lbp). +pointcloud_topic (type: string, default: /scan_cloud): LaserScan input topic. + +- ```build_offline_map.launch``` +Builds the offline wall map and descriptor databases from recorded scan data. + +- ```run_mapping_recording.launch``` +Records wall scan data while the robot moves along the wall. + +- ```spawn_world_and_robot.launch``` +Spawns the Gazebo world, wall model, and robot equipped with the vertical profiler sensor. + + +## Nodes + +```scanner_pose_estimator``` + +This node performs the live wall-based localization. It processes incoming LaserScan data, builds local wall patches, performs 2D image-based matching, refines the pose using ICP, and publishes the estimated scanner pose as a TF transform. + + +## Subscribed Topics + +```scan_cloud``` (sensor_msgs/LaserScan) +Laser scan data acquired by the vertical profiler sensor. + +```odom``` (nav_msgs/Odometry) +Odometry data of the mobile robot. + +## Parameters +```wall_frame``` (type: string, default: wall): Reference frame attached to the wall geometry. + +```scanner_frame``` (type: string, default: vertical_profiler_link): Frame of the vertical profiler sensor. + +```base_frame``` (type: string, default: base_link): Robot base frame used for odometry and TF lookups. + +```odom_topic``` (type: string, default: /odom): Odometry topic used for motion tracking. + +```s_min_live``` (type: float, default: -0.6): Minimum along-wall coordinate for live scan filtering. + +```s_max_live``` (type: float, default: 0.6): Maximum along-wall coordinate for live scan filtering. + +```z_min_live``` (type: float, default: 0.0): Minimum height for live scan filtering. + +```z_max_live``` (type: float, default: 2.0): Maximum height for live scan filtering. + +```live_img_w``` (type: int, default: 6): Width of the live patch image. + +```live_img_h``` (type: int, default: 11): Height of the live patch image. + +```icp_max_iter``` (type: int, default: 50): Maximum number of ICP iterations. + +```icp_max_dist``` (type: float, default: 0.0035): Maximum correspondence distance used during ICP refinement. + +```method``` (type: string, default: sift): Feature matching method used for 2D localization. + +```stride``` (type: int, default: 16): Step size used when sliding the query window over the offline wall map during 2D localization. + +```max_query_kps``` (type: int, default: 60): Maximum number of keypoints extracted from the live patch and used for feature matching. + +```patch_h``` (type: int, default: 100): Height of the image patch used for 2D feature extraction and matching. + +```patch_w``` (type: int, default: 100): Width of the image patch used for 2D feature extraction and matching. + +```lbp_radius``` (type: int, default: 2): Radius of the circular neighborhood used for LBP feature computation. + +```lbp_points``` (type: int, default: 16): Number of sampling points on the LBP circular neighborhood. + +```lbphf_keep``` (type: int, default: 24): Number of most significant LBP-HF histogram bins retained for matching. + +```coarse_topk``` (type: int, default: 10): Number of top candidate matches retained during coarse 2D localization. + +```sift_ratio``` (type: float, default: 0.7): Lowe’s ratio threshold used to filter ambiguous SIFT feature matches. + +```sift_pca_dim``` (type: int, default: 4): Dimensionality of SIFT descriptors after PCA compression. + +```scale_bins``` (type: int, default: 3): Number of scale bins used for multi-scale SIFT feature extraction. + +```flann_trees``` (type: int, default: 4): Number of KD-trees used by the FLANN-based SIFT matcher. + +```flann_checks``` (type: int, default: 60): Number of search checks performed by the FLANN matcher during feature matching. + + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/config/estimator_params.yaml b/student_code/251127_print_texture_localization/wall_localization_demo/config/estimator_params.yaml new file mode 100644 index 0000000..0965c20 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/config/estimator_params.yaml @@ -0,0 +1,18 @@ +wall_frame: "wall" +scanner_frame: "vertical_profiler_link" +base_frame: "base_link" + +s_min_live: -1.0 +s_max_live: 1.0 +z_min_live: 0.0 +z_max_live: 5.0 + +live_patch_s_length: 0.20 + +method: "lbp" + +icp_max_dist: 0.05 +icp_max_iter: 50 + +live_img_h: 11 # matches PATCH_H +live_img_w: 6 # matches PATCH_W diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/config/localization_params.yaml b/student_code/251127_print_texture_localization/wall_localization_demo/config/localization_params.yaml new file mode 100644 index 0000000..e77f1b7 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/config/localization_params.yaml @@ -0,0 +1,27 @@ +# ============================================================ +# LOCALIZATION_2D PARAMETERS (OVERWRITES DEFAULT CONSTANTS) +# ============================================================ + +loc2d: + + # -------- General -------- + stride: 16 + max_query_kps: 60 + patch_h: 100 + patch_w: 100 + + # -------- LBP-HF -------- + lbp_radius: 2 + lbp_points: 16 + lbphf_keep: 24 + coarse_topk: 10 + + # -------- SIFT -------- + sift_ratio: 0.7 + sift_pca_dim: 4 + scale_bins: 3 + + # -------- FLANN (SIFT matcher) -------- + flann_trees: 4 + flann_checks: 60 + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/config/offline_map.yaml b/student_code/251127_print_texture_localization/wall_localization_demo/config/offline_map.yaml new file mode 100644 index 0000000..9936a3c --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/config/offline_map.yaml @@ -0,0 +1,5 @@ +cloud: "$(find wall_localization_demo)/data/demo_points.csv" +out_map: "offline_map.npy" +out_points: "offline_points.npy" +delta_s: 0.005 +delta_z: 0.005 diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/launch/build_offline_map.launch b/student_code/251127_print_texture_localization/wall_localization_demo/launch/build_offline_map.launch new file mode 100644 index 0000000..4c3488b --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/launch/build_offline_map.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/launch/run_localization.launch b/student_code/251127_print_texture_localization/wall_localization_demo/launch/run_localization.launch new file mode 100644 index 0000000..8558529 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/launch/run_localization.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/launch/run_mapping_recording.launch b/student_code/251127_print_texture_localization/wall_localization_demo/launch/run_mapping_recording.launch new file mode 100644 index 0000000..df2dafa --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/launch/run_mapping_recording.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/launch/spawn_world_and_robot.launch b/student_code/251127_print_texture_localization/wall_localization_demo/launch/spawn_world_and_robot.launch new file mode 100644 index 0000000..d02fc06 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/launch/spawn_world_and_robot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/models/printed_wall/model.config b/student_code/251127_print_texture_localization/wall_localization_demo/models/printed_wall/model.config new file mode 100644 index 0000000..9250a08 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/models/printed_wall/model.config @@ -0,0 +1,11 @@ + + + printed_wall + 1.0 + + model.sdf + + + A printed concrete wall generated from demo_image.py using a custom OBJ mesh. + + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/models/printed_wall/model.sdf b/student_code/251127_print_texture_localization/wall_localization_demo/models/printed_wall/model.sdf new file mode 100644 index 0000000..e5bbc9b --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/models/printed_wall/model.sdf @@ -0,0 +1,54 @@ + + + + + + true + + + + + + + + model://printed_wall/wall_mesh.obj + 10 10 10 + + + + + + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + + + + + + + + + model://printed_wall/wall_mesh.obj + 10 10 10 + + + + + + + + 1.0 + 1.0 + + + + + + + + + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/package.xml b/student_code/251127_print_texture_localization/wall_localization_demo/package.xml new file mode 100644 index 0000000..a223eea --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/package.xml @@ -0,0 +1,48 @@ + + + wall_localization_demo + 0.0.1 + Gazebo + LiDAR wall localization demo using SIFT, LBP-HF, and ICP + + Your Name + BSD + + + catkin + + + rospy + geometry_msgs + sensor_msgs + nav_msgs + std_msgs + + tf2_ros + tf2_py + tf_conversions + + pcl_ros + pcl_msgs + + gazebo_ros + + cv_bridge + image_transport + + + + python3-opencv + + python3-numpy + python3-scipy + python3-skimage + python3-sklearn + + + python3-open3d + + + + + + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/setup.py b/student_code/251127_print_texture_localization/wall_localization_demo/setup.py new file mode 100644 index 0000000..17b4c80 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/setup.py @@ -0,0 +1,13 @@ +# Source - https://stackoverflow.com/a +# Posted by J.V. +# Retrieved 2025-11-27, License - CC BY-SA 4.0 + +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup +# fetch values from package.xml +setup_args = generate_distutils_setup( +packages=['wall_localization_demo'], +package_dir={'': 'src'}, +) +setup(**setup_args) diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/csv2mesh.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/csv2mesh.py new file mode 100644 index 0000000..4c2e832 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/csv2mesh.py @@ -0,0 +1,97 @@ +import numpy as np +import open3d as o3d + +# ================================ +# 1. Load CSV as point cloud +# ================================ +csv_path = "../data/demo_points.csv" + +print("Loading CSV...") +pts = np.loadtxt(csv_path, delimiter=",") + +# Remove NaN or infinite rows +pts = pts[~np.isnan(pts).any(axis=1)] +pts = pts[np.isfinite(pts).all(axis=1)] + +pcd = o3d.geometry.PointCloud() +pcd.points = o3d.utility.Vector3dVector(pts) + +print("Loaded", len(pts), "points") + + +# ================================ +# 2. Estimate normals +# ================================ +print("Estimating normals...") + +pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=0.010, # Tune: depends on point spacing + max_nn=50 + ) +) + +# Make normals consistent (important for Poisson) +pcd.orient_normals_consistent_tangent_plane(100) +# Alternative: +# pcd.orient_normals_towards_camera_location(camera_location=np.array([0,0,0])) + +print("Normals estimated.") + + +# ================================ +# 3. Poisson Reconstruction +# ================================ +print("Running Poisson reconstruction...") + +mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson( + pcd, + depth=10, # main Poisson resolution parameter + width=0, + scale=1.1, # enlarge Poisson bbox slightly + linear_fit=False +) + +print("Poisson reconstruction complete.") +print("Mesh has", len(mesh.vertices), "vertices and", len(mesh.triangles), "triangles") + + +# ================================ +# 4. Remove floating artifacts +# ================================ +print("Removing low-density artifacts...") + +densities = np.asarray(densities) +density_threshold = np.quantile(densities, 0.02) # Remove bottom 2% + +mask = densities < density_threshold +mesh.remove_vertices_by_mask(mask) + +print("Cleaned mesh now has", len(mesh.vertices), "vertices.") + + +# ================================ +# 5. Final cleaning +# ================================ +mesh.remove_degenerate_triangles() +mesh.remove_duplicated_triangles() +mesh.remove_duplicated_vertices() +mesh.remove_non_manifold_edges() + +mesh.compute_vertex_normals() + + +# ================================ +# 6. Save as OBJ +# ================================ +o3d.io.write_triangle_mesh("wall_poisson.obj", mesh) +print("Saved wall_poisson.obj") + +# If you want DAE, convert with meshlabserver or trimesh +# Example: +# meshlabserver -i wall_poisson.obj -o wall_poisson.dae + +# ================================ +# 7. Visualize +# ================================ +o3d.visualization.draw_geometries([mesh]) diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/demo_image.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/demo_image.py new file mode 100644 index 0000000..b9bfcce --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/demo_image.py @@ -0,0 +1,463 @@ +import numpy as np +import matplotlib.pyplot as plt +import argparse +from pathlib import Path +import imageio + +# ============================================================ +# 0. CLI for output paths +# ============================================================ +parser = argparse.ArgumentParser( + description="Generate synthetic wall point cloud + mid-path, with sine wall in z and random blobs." +) +parser.add_argument("--out_img", type=str, default="", help="Optional output grayscale image path (png/jpg).") +parser.add_argument("--out_npy", type=str, default="", help="Optional output .npy mapping (not used here).") +parser.add_argument("--out_cloud_csv", type=str, default="../data/demo_points.csv", + help="Output CSV for 3D point cloud (x,y,z).") +parser.add_argument("--out_midpath_csv", type=str, default="../data/demo_midpath.csv", + help="Output CSV for mid-path waypoints (x,y,z).") +parser.add_argument("--out_obj", type=str, default="", + help="Output OBJ mesh for the wall surface.") +parser.add_argument("--out_dae", type=str, default="", + help="Optional output DAE mesh for the wall surface.") +args = parser.parse_args() + +waypoint = 1000 # number of mid-path waypoints +wall_length = 5.0 # meters (length in x direction) +wall_height = 2.0 # meters (height in z direction) +num_points = 500000 # number of 3D points to simulate +num_blobs = 2000 # number of random blobs on wall surface +# Define bin sizes for s and z +Delta_s = 0.005 +Delta_z = 0.005 + +# ============================================================ +# 1. Straight mid-path on the xy-plane (z = 0) +# ============================================================ + +# Parameter along the nominal path (not yet arc-length) +t = np.linspace(0.0, wall_length, waypoint) + +# Mid-path shape on the ground plane (straight line in x, y=0) +mid_x = t +mid_y = np.zeros_like(t) +mid_z = np.zeros_like(t) + +# Assemble mid-path waypoints q_k = (x_k, y_k, z_k) +Q = np.vstack((mid_x, mid_y, mid_z)).T # shape (M, 3) + +# Compute cumulative arc-length s_k according to eq. (2) (straight line => ~t, but keep general) +segment_vecs = Q[1:, :2] - Q[:-1, :2] # only xy for length +segment_lengths = np.linalg.norm(segment_vecs, axis=1) +s_k = np.zeros(Q.shape[0]) +s_k[1:] = np.cumsum(segment_lengths) +L = s_k[-1] # total length + +# ============================================================ +# 2. Generate a 3D wall surface around the mid-path +# - straight mid-path in x +# - y has a sine modulation as a function of z + blobs +# ============================================================ + +# Sample random indices of segments and local parameters on segments +seg_indices = np.random.randint(0, Q.shape[0] - 1, size=num_points) +lambda_samples = np.random.rand(num_points) + +# Corresponding mid-path points for each sample +q_start = Q[seg_indices] +q_end = Q[seg_indices + 1] +mid_points = q_start + lambda_samples[:, None] * (q_end - q_start) + +mx = mid_points[:, 0] +my = mid_points[:, 1] +mz = mid_points[:, 2] # this should be zero + +# Tangent vectors of the chosen segments in xy-plane +seg_vec_xy = (q_end - q_start)[:, :2] +seg_len = np.linalg.norm(seg_vec_xy, axis=1) +t_hat_xy = seg_vec_xy / seg_len[:, None] # unit tangent in xy + +# Outward normal in xy-plane (rotate tangent by 90 degrees) +n_hat_xy = np.empty_like(t_hat_xy) +n_hat_xy[:, 0] = -t_hat_xy[:, 1] +n_hat_xy[:, 1] = t_hat_xy[:, 0] + +# Radial offset in normal direction (wall thickness + small variation) +radial_offset = 0.2 + 0.02 * np.random.randn(num_points) + +# ------------------------------------------------------------ +# Base wall y profile: sine in y as function of "z" +# Waves travel along the height of the wall. +# ------------------------------------------------------------ +# Sample wall height uniformly from 0 to wall_height (z direction) +z_nominal = np.random.uniform(0.0, wall_height, num_points) + +# Use a sine wave along z to modulate y (wall surface features) +# Sine varies with z (vertical direction), affecting y (depth/normal direction) +y_sine = 0.07 * np.sin(2.0 * np.pi * 2.0 * z_nominal / wall_height) # 4 cycles over wall height + +# ------------------------------------------------------------ +# Superimpose random ± blobs on the wall surface in y +# Blobs are positioned in (x,z) and affect y (surface normal direction) +# ------------------------------------------------------------ +blob_centers_x = np.random.uniform(0.0, wall_length, size=num_blobs) +blob_centers_z = np.random.uniform(0.0, wall_height, size=num_blobs) + +blob_amplitudes = np.random.uniform(-0.02, 0.02, size=num_blobs) # 1cm +blob_sigma_x = np.random.uniform(0.02, 0.07, size=num_blobs) # spatial extent in x +blob_sigma_z = np.random.uniform(0.02, 0.07, size=num_blobs) # extent along z + +blob_delta_y = np.zeros(num_points, dtype=np.float32) +for cx, cz, A, sx, sz in zip( + blob_centers_x, blob_centers_z, + blob_amplitudes, blob_sigma_x, blob_sigma_z +): + dx = mx - cx + dz = z_nominal - cz + r2 = (dx*dx) / (sx * sx) + (dz*dz) / (sz * sz) + blob_delta_y += A * np.exp(-0.5 * r2) + +# Final wall-normal profile in y (sine along z + blobs) +y_wall_center = y_sine + blob_delta_y + +# Vertical structure (small noise around nominal height) +z_offset_noise = 0 +z_wall = z_nominal + z_offset_noise + +# Construct the 3D point cloud p_i = mid-point + radial (in xy) + wall y-shape +x_pts = mx + radial_offset * n_hat_xy[:, 0] +# instead of mid-path y + radial only, add wall center in y +y_pts = my + radial_offset * n_hat_xy[:, 1] + y_wall_center +z_pts = z_wall + +points = np.vstack((x_pts, y_pts, z_pts)).T # shape (N, 3) + +# ============================================================ +# 3. Depth d_i: projection onto mid-path normal (same as before) +# ============================================================ + +# For each point, recompute projection onto its associated segment + +# Tangent in xy (already t_hat_xy), build 3D tangent +t_hat = np.hstack((t_hat_xy, np.zeros((num_points, 1)))) + +# Normal in xy extended with zero z-component +n_hat = np.hstack((n_hat_xy, np.zeros((num_points, 1)))) + +# Projector point on mid-path gamma(s_i*) +gamma_si = mid_points # already on mid-path for given seg and lambda + +# Vector r_i = p_i - gamma(s_i*) +r_vec = points - gamma_si + +# Depth d_i = r_i^T n_i (horizontal offset from mid-path) +d_i = np.einsum('ij,ij->i', r_vec, n_hat) # dot product row-wise + +# ============================================================ +# 4. Compute arc-length s_i* and prepare binning in (s, z) +# ============================================================ + +# Arc-length of start of each used segment +s_start = s_k[seg_indices] # s_k from cumulative lengths + +# Arc-length position of each sampled mid-point +s_i_star = s_start + lambda_samples * seg_len # eq. (7) + +# Height coordinate z_i +z_i = z_pts + +# Domain for s and z +s_min, s_max = 0.0, L +z_min, z_max = np.min(z_i), np.max(z_i) + +Ns = int((s_max - s_min) / Delta_s) +Nz = int((z_max - z_min) / Delta_z) + +# ============================================================ +# 5. Aggregate depths into pixel intensities I(u,v) +# using the median of D_{u,v} (eq. (20)) +# ============================================================ + +I = np.zeros((Nz, Ns)) # pixel intensities (physical depth units) +count = np.zeros((Nz, Ns), dtype=int) + +# Compute bin indices for each point according to eq. (13) +u_idx = ((s_i_star - s_min) / Delta_s).astype(int) +v_idx = ((z_i - z_min) / Delta_z).astype(int) + +# Keep only indices inside valid range +valid = (u_idx >= 0) & (u_idx < Ns) & (v_idx >= 0) & (v_idx < Nz) + +# original point indices that are kept after filtering (needed for mapping pixels -> input points) +orig_indices = np.nonzero(valid)[0] + +u_idx = u_idx[valid] +v_idx = v_idx[valid] +d_valid = d_i[valid] + +# For median aggregation, collect depths per bin +# (simple approach using a dictionary for clarity) +from collections import defaultdict +bin_depths = defaultdict(list) +bin_points = defaultdict(list) # store original point indices per bin + +for u, v, d, oi in zip(u_idx, v_idx, d_valid, orig_indices): + bin_depths[(u, v)].append(d) + bin_points[(u, v)].append(int(oi)) + +for (u, v), d_list in bin_depths.items(): + I[Nz - 1 - v, u] = np.median(d_list) # flip v for display (z upwards) + count[Nz - 1 - v, u] = len(d_list) + +# ============================================================ +# 6. Normalize intensities to grayscale [0, 255] (eq. (24)) +# ============================================================ + +# Consider only non-empty pixels for min/max +non_empty_mask = count > 0 +I_non_empty = I[non_empty_mask] + +I_min = I_non_empty.min() +I_max = I_non_empty.max() + +I_gray = np.zeros_like(I) + +if I_max > I_min: + I_gray[non_empty_mask] = 255.0 * (I[non_empty_mask] - I_min) / (I_max - I_min) + +# ============================================================ +# 7. Figure 1: 3D mid-path and 3D point cloud +# ============================================================ + +fig1 = plt.figure() +ax1 = fig1.add_subplot(111, projection='3d') +ax1.scatter(points[:, 0], points[:, 1], points[:, 2], s=1, alpha=0.5) +ax1.plot(mid_x, mid_y, mid_z, linewidth=2, color='red') +ax1.set_xlabel("x") +ax1.set_ylabel("y") +ax1.set_zlabel("z") +ax1.set_title("Straight mid-path and simulated 3D wall surface") +plt.tight_layout() + +# ============================================================ +# 8. Figure 2: Local projection and depth geometry in xy-plane +# ============================================================ + +# Select a small subset of points for visualization +num_demo = 50 +demo_indices = np.random.choice(np.where(valid)[0], size=num_demo, replace=False) + +fig2, ax2 = plt.subplots() +ax2.plot(mid_x, mid_y) +ax2.set_aspect('equal', 'box') + +for idx in demo_indices: + px, py = points[idx, 0], points[idx, 1] + gx, gy = gamma_si[idx, 0], gamma_si[idx, 1] + nx, ny = n_hat[idx, 0], n_hat[idx, 1] + + # Plot point and projection + ax2.scatter(px, py, s=10) + ax2.scatter(gx, gy, s=10) + + # Line from projection to point (depth direction) + ax2.plot([gx, px], [gy, py]) + + # Small arrow for normal direction at the projection point + scale = 0.05 + ax2.arrow(gx, gy, scale * nx, scale * ny, head_width=0.02, length_includes_head=True) + +ax2.set_xlabel("x") +ax2.set_ylabel("y") +ax2.set_title("Local projection of surface points onto straight mid-path and depth definition") +plt.tight_layout() + +# ============================================================ +# 9. Figure 3: 2D grayscale texture map I_gray(s,z) +# ============================================================ + +fig3 = plt.figure() +extent = [s_min, s_max, z_min, z_max] +plt.imshow(I_gray, cmap='gray', aspect='auto', extent=extent, origin='lower') +plt.xlabel("s (arc-length along mid-path)") +plt.ylabel("z (height)") +plt.title("Grayscale texture map derived from 3D point cloud (I_gray)") +plt.colorbar(label="Intensity (0–255)") +plt.tight_layout() + +# ============================================================ +# 10. Optional outputs +# ============================================================ + +if args.out_img: + out_path = Path(args.out_img) + ext = out_path.suffix.lower() + if ext not in (".png", ".jpg", ".jpeg"): + out_path = out_path.with_suffix(".png") + try: + I_uint8 = np.clip(I_gray, 0, 255).astype(np.uint8) + imageio.imwrite(str(out_path), I_uint8) + print(f"Saved final texture image (raw) to: {out_path}") + except Exception as e: + print(f"Failed to save final image to {out_path}: {e}") + +# mapping .npy is not requested here, but keep stub if needed later +if args.out_npy: + out_npy = Path(args.out_npy) + try: + mapping = {} + for (u, v), pts_idx in bin_points.items(): + row = Nz - 1 - v + col = u + gray_val = int(np.clip(I_gray[row, col], 0, 255)) + mapping[(int(row), int(col))] = (int(gray_val), np.array(pts_idx, dtype=int)) + np.save(str(out_npy), mapping, allow_pickle=True) + print(f"Saved pixel->points mapping to: {out_npy}") + except Exception as e: + print(f"Failed to save mapping .npy to {out_npy}: {e}") + +# save point cloud and mid-path as separate CSV +try: + np.savetxt(args.out_cloud_csv, points, delimiter=",") + print(f"Saved point cloud to {args.out_cloud_csv}") +except Exception as e: + print(f"Failed to save point cloud CSV: {e}") + +try: + np.savetxt(args.out_midpath_csv, Q, delimiter=",") + print(f"Saved mid-path waypoints to {args.out_midpath_csv}") +except Exception as e: + print(f"Failed to save mid-path CSV: {e}") +# ============================================================ +# 11. Mesh export (OBJ + optional DAE) +# ============================================================ + +def export_wall_mesh(points, bin_points, Nz, Ns, out_obj_path=None, out_dae_path=None): + """ + Build a structured wall mesh from binned points and export to OBJ/DAE. + - vertices: average of points in each non-empty bin + - faces: two triangles per quad in the (s,z) grid + """ + # vertex grid, NaN for empty + verts_grid = np.full((Nz, Ns, 3), np.nan, dtype=np.float32) + + for (u, v), idx_list in bin_points.items(): + idx_arr = np.array(idx_list, dtype=int) + pts_bin = points[idx_arr] + mean_pt = pts_bin.mean(axis=0) + row = Nz - 1 - v # consistent with I_gray row indexing + col = u + if 0 <= row < Nz and 0 <= col < Ns: + verts_grid[row, col, :] = mean_pt + + vertices = [] + index_grid = -np.ones((Nz, Ns), dtype=int) + + # Assign vertex indices + for r in range(Nz): + for c in range(Ns): + if not np.isnan(verts_grid[r, c, 0]): + index_grid[r, c] = len(vertices) + 1 # OBJ is 1-based + vertices.append(verts_grid[r, c, :]) + + faces = [] + for r in range(Nz - 1): + for c in range(Ns - 1): + v00 = index_grid[r, c] + v10 = index_grid[r, c + 1] + v01 = index_grid[r + 1, c] + v11 = index_grid[r + 1, c + 1] + if v00 > 0 and v10 > 0 and v01 > 0 and v11 > 0: + # two triangles + faces.append((v00, v10, v11)) + faces.append((v00, v11, v01)) + + if not vertices or not faces: + print("Mesh export: no valid vertices/faces, skipping.") + return + + # Write OBJ + if out_obj_path: + try: + with open(out_obj_path, "w") as f: + f.write("# Wall mesh generated by demo_image.py\n") + for v in vertices: + f.write(f"v {v[0]} {v[1]} {v[2]}\n") + for (i1, i2, i3) in faces: + f.write(f"f {i1} {i2} {i3}\n") + print(f"Saved wall mesh OBJ to {out_obj_path}") + except Exception as e: + print(f"Failed to save OBJ to {out_obj_path}: {e}") + + # Write DAE (COLLADA) + if out_dae_path: + try: + # DAE uses 0-based indices + tri_indices = [] + for (i1, i2, i3) in faces: + tri_indices.extend([i1 - 1, i2 - 1, i3 - 1]) + + dae = [] + dae.append('') + dae.append('') + dae.append(' ') + dae.append(' demo_image.py') + dae.append(' ') + dae.append(' Z_UP') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + # positions + dae.append(' ') + dae.append(f' ') + dae.append(' ' + ' '.join(f"{v[0]} {v[1]} {v[2]}" for v in vertices)) + dae.append(' ') + dae.append(' ') + dae.append(f' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(f' ') + dae.append(' ') + dae.append('

') + dae.append(' ' + ' '.join(str(i) for i in tri_indices)) + dae.append('

') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append('') + + with open(out_dae_path, "w") as f: + f.write("\n".join(dae)) + + print(f"Saved wall mesh DAE to {out_dae_path}") + except Exception as e: + print(f"Failed to save DAE to {out_dae_path}: {e}") + + +# Call mesh export if requested +out_obj_path = args.out_obj if args.out_obj else None +out_dae_path = args.out_dae if args.out_dae else None + +if out_obj_path or out_dae_path: + export_wall_mesh(points, bin_points, Nz, Ns, out_obj_path, out_dae_path) + +plt.show() \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/generate_mesh.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/generate_mesh.py new file mode 100644 index 0000000..1ecab4c --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/generate_mesh.py @@ -0,0 +1,355 @@ +import numpy as np +import matplotlib.pyplot as plt +import argparse +from pathlib import Path +import imageio + +# ============================================================ +# 0. CLI for output paths +# ============================================================ +parser = argparse.ArgumentParser( + description="Generate synthetic wall point cloud + mid-path, with sine wall in z and random blobs." +) +parser.add_argument("--out_img", type=str, default="", help="Optional output grayscale image path (png/jpg).") +parser.add_argument("--out_cloud_csv", type=str, default="../data/demo_points.csv", + help="Output CSV for 3D point cloud (x,y,z).") +parser.add_argument("--out_midpath_csv", type=str, default="../data/demo_midpath.csv", + help="Output CSV for mid-path waypoints (x,y,z).") +parser.add_argument("--out_obj", type=str, default="wall_mesh.obj", + help="Output OBJ mesh for the wall surface.") +parser.add_argument("--out_dae", type=str, default="", + help="Optional output DAE mesh for the wall surface.") +args = parser.parse_args() + +waypoint = 1000 # number of mid-path waypoints +wall_length = 10.0 # meters (length in x direction) +wall_height = 4.0 # meters (height in z direction) +num_points = 500000 # number of 3D points to simulate +num_blobs = 400 # number of random blobs on wall surface +# Define bin sizes for s and z +Delta_s = 0.005 +Delta_z = 0.005 + +# ============================================================ +# 1. Straight mid-path on the xy-plane (z = 0) +# ============================================================ + +t = np.linspace(0.0, wall_length, waypoint) + +mid_x = t +mid_y = np.zeros_like(t) +mid_z = np.zeros_like(t) + +Q = np.vstack((mid_x, mid_y, mid_z)).T # shape (M, 3) + +segment_vecs = Q[1:, :2] - Q[:-1, :2] +segment_lengths = np.linalg.norm(segment_vecs, axis=1) +s_k = np.zeros(Q.shape[0]) +s_k[1:] = np.cumsum(segment_lengths) +L = s_k[-1] + +# ============================================================ +# 2. Generate 3D wall surface around the mid-path +# ============================================================ + +seg_indices = np.random.randint(0, Q.shape[0] - 1, size=num_points) +lambda_samples = np.random.rand(num_points) + +q_start = Q[seg_indices] +q_end = Q[seg_indices + 1] +mid_points = q_start + lambda_samples[:, None] * (q_end - q_start) + +mx = mid_points[:, 0] +my = mid_points[:, 1] +mz = mid_points[:, 2] + +seg_vec_xy = (q_end - q_start)[:, :2] +seg_len = np.linalg.norm(seg_vec_xy, axis=1) +t_hat_xy = seg_vec_xy / seg_len[:, None] + +n_hat_xy = np.empty_like(t_hat_xy) +n_hat_xy[:, 0] = -t_hat_xy[:, 1] +n_hat_xy[:, 1] = t_hat_xy[:, 0] + +radial_offset = 0.1 + 0.02 * np.random.randn(num_points) + +z_nominal = np.random.uniform(0.0, wall_height, num_points) + +y_sine = 0.02 * np.sin(2.0 * np.pi * 2.0 * z_nominal / wall_height) + +blob_centers_x = np.random.uniform(0.0, wall_length, size=num_blobs) +blob_centers_z = np.random.uniform(0.0, wall_height, size=num_blobs) + +blob_amplitudes = np.random.uniform(-0.02, 0.02, size=num_blobs) +blob_sigma_x = np.random.uniform(0.03, 0.1, size=num_blobs) +blob_sigma_z = np.random.uniform(0.03, 0.1, size=num_blobs) + +blob_delta_y = np.zeros(num_points, dtype=np.float32) +for cx, cz, A, sx, sz in zip( + blob_centers_x, blob_centers_z, + blob_amplitudes, blob_sigma_x, blob_sigma_z +): + dx = mx - cx + dz = z_nominal - cz + r2 = (dx*dx) / (sx * sx) + (dz*dz) / (sz * sz) + blob_delta_y += A * np.exp(-0.5 * r2) + +y_wall_center = y_sine + blob_delta_y + +z_offset_noise = 0.01 * np.random.randn(num_points) +z_wall = z_nominal + z_offset_noise + +x_pts = mx + radial_offset * n_hat_xy[:, 0] +y_pts = my + radial_offset * n_hat_xy[:, 1] + y_wall_center +z_pts = z_wall + +points = np.vstack((x_pts, y_pts, z_pts)).T # (N, 3) + +# ============================================================ +# 3. Depth d_i and (s,z) binning +# ============================================================ + +t_hat = np.hstack((t_hat_xy, np.zeros((num_points, 1)))) +n_hat = np.hstack((n_hat_xy, np.zeros((num_points, 1)))) +gamma_si = mid_points + +r_vec = points - gamma_si +d_i = np.einsum('ij,ij->i', r_vec, n_hat) + +s_start = s_k[seg_indices] +s_i_star = s_start + lambda_samples * seg_len + +z_i = z_pts + +s_min, s_max = 0.0, L +z_min, z_max = np.min(z_i), np.max(z_i) + +Ns = int((s_max - s_min) / Delta_s) +Nz = int((z_max - z_min) / Delta_z) + +# ============================================================ +# 5. Aggregate depths into pixel intensities I(u,v) +# ============================================================ + +I = np.zeros((Nz, Ns)) +count = np.zeros((Nz, Ns), dtype=int) + +u_idx = ((s_i_star - s_min) / Delta_s).astype(int) +v_idx = ((z_i - z_min) / Delta_z).astype(int) + +valid = (u_idx >= 0) & (u_idx < Ns) & (v_idx >= 0) & (v_idx < Nz) +orig_indices = np.nonzero(valid)[0] + +u_idx = u_idx[valid] +v_idx = v_idx[valid] +d_valid = d_i[valid] + +from collections import defaultdict +bin_depths = defaultdict(list) +bin_points = defaultdict(list) # store original point indices per bin + +for u, v, d, oi in zip(u_idx, v_idx, d_valid, orig_indices): + bin_depths[(u, v)].append(d) + bin_points[(u, v)].append(int(oi)) + +for (u, v), d_list in bin_depths.items(): + I[Nz - 1 - v, u] = np.median(d_list) + count[Nz - 1 - v, u] = len(d_list) + +# ============================================================ +# 6. Normalize intensities to grayscale [0, 255] +# ============================================================ + +non_empty_mask = count > 0 +I_non_empty = I[non_empty_mask] + +I_min = I_non_empty.min() +I_max = I_non_empty.max() + +I_gray = np.zeros_like(I) + +if I_max > I_min: + I_gray[non_empty_mask] = 255.0 * (I[non_empty_mask] - I_min) / (I_max - I_min) + +# ============================================================ +# 7–9. Plots (unchanged; omitted for brevity in this snippet) +# (You can keep your existing plotting code if you want) +# ============================================================ + +# ============================================================ +# 10. Optional outputs (existing) +# ============================================================ + +if args.out_img: + out_path = Path(args.out_img) + ext = out_path.suffix.lower() + if ext not in (".png", ".jpg", ".jpeg"): + out_path = out_path.with_suffix(".png") + try: + I_uint8 = np.clip(I_gray, 0, 255).astype(np.uint8) + imageio.imwrite(str(out_path), I_uint8) + print(f"Saved final texture image (raw) to: {out_path}") + except Exception as e: + print(f"Failed to save final image to {out_path}: {e}") + +if args.out_npy: + out_npy = Path(args.out_npy) + try: + mapping = {} + for (u, v), pts_idx in bin_points.items(): + row = Nz - 1 - v + col = u + gray_val = int(np.clip(I_gray[row, col], 0, 255)) + mapping[(int(row), int(col))] = (int(gray_val), np.array(pts_idx, dtype=int)) + np.save(str(out_npy), mapping, allow_pickle=True) + print(f"Saved pixel->points mapping to: {out_npy}") + except Exception as e: + print(f"Failed to save mapping .npy to {out_npy}: {e}") + +try: + np.savetxt(args.out_cloud_csv, points, delimiter=",") + print(f"Saved point cloud to {args.out_cloud_csv}") +except Exception as e: + print(f"Failed to save point cloud CSV: {e}") + +try: + np.savetxt(args.out_midpath_csv, Q, delimiter=",") + print(f"Saved mid-path CSV to {args.out_midpath_csv}") +except Exception as e: + print(f"Failed to save mid-path CSV: {e}") + +# ============================================================ +# 11. Mesh export (OBJ + optional DAE) +# ============================================================ + +def export_wall_mesh(points, bin_points, Nz, Ns, out_obj_path=None, out_dae_path=None): + """ + Build a structured wall mesh from binned points and export to OBJ/DAE. + - vertices: average of points in each non-empty bin + - faces: two triangles per quad in the (s,z) grid + """ + # vertex grid, NaN for empty + verts_grid = np.full((Nz, Ns, 3), np.nan, dtype=np.float32) + + for (u, v), idx_list in bin_points.items(): + idx_arr = np.array(idx_list, dtype=int) + pts_bin = points[idx_arr] + mean_pt = pts_bin.mean(axis=0) + row = Nz - 1 - v # consistent with I_gray row indexing + col = u + if 0 <= row < Nz and 0 <= col < Ns: + verts_grid[row, col, :] = mean_pt + + vertices = [] + index_grid = -np.ones((Nz, Ns), dtype=int) + + # Assign vertex indices + for r in range(Nz): + for c in range(Ns): + if not np.isnan(verts_grid[r, c, 0]): + index_grid[r, c] = len(vertices) + 1 # OBJ is 1-based + vertices.append(verts_grid[r, c, :]) + + faces = [] + for r in range(Nz - 1): + for c in range(Ns - 1): + v00 = index_grid[r, c] + v10 = index_grid[r, c + 1] + v01 = index_grid[r + 1, c] + v11 = index_grid[r + 1, c + 1] + if v00 > 0 and v10 > 0 and v01 > 0 and v11 > 0: + # two triangles + faces.append((v00, v10, v11)) + faces.append((v00, v11, v01)) + + if not vertices or not faces: + print("Mesh export: no valid vertices/faces, skipping.") + return + + # Write OBJ + if out_obj_path: + try: + with open(out_obj_path, "w") as f: + f.write("# Wall mesh generated by demo_image.py\n") + for v in vertices: + f.write(f"v {v[0]} {v[1]} {v[2]}\n") + for (i1, i2, i3) in faces: + f.write(f"f {i1} {i2} {i3}\n") + print(f"Saved wall mesh OBJ to {out_obj_path}") + except Exception as e: + print(f"Failed to save OBJ to {out_obj_path}: {e}") + + # Write DAE (COLLADA) + if out_dae_path: + try: + # DAE uses 0-based indices + tri_indices = [] + for (i1, i2, i3) in faces: + tri_indices.extend([i1 - 1, i2 - 1, i3 - 1]) + + dae = [] + dae.append('') + dae.append('') + dae.append(' ') + dae.append(' demo_image.py') + dae.append(' ') + dae.append(' Z_UP') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + # positions + dae.append(' ') + dae.append(f' ') + dae.append(' ' + ' '.join(f"{v[0]} {v[1]} {v[2]}" for v in vertices)) + dae.append(' ') + dae.append(' ') + dae.append(f' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(f' ') + dae.append(' ') + dae.append('

') + dae.append(' ' + ' '.join(str(i) for i in tri_indices)) + dae.append('

') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append(' ') + dae.append('') + + with open(out_dae_path, "w") as f: + f.write("\n".join(dae)) + + print(f"Saved wall mesh DAE to {out_dae_path}") + except Exception as e: + print(f"Failed to save DAE to {out_dae_path}: {e}") + + +# Call mesh export if requested +out_obj_path = args.out_obj if args.out_obj else None +out_dae_path = args.out_dae if args.out_dae else None + +if out_obj_path or out_dae_path: + export_wall_mesh(points, bin_points, Nz, Ns, out_obj_path, out_dae_path) +# ============================================================ +# 12. Show plots if you kept them above +# ============================================================ +plt.show() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/record_wall_scan.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/record_wall_scan.py new file mode 100644 index 0000000..47fa3c5 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/record_wall_scan.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan, PointCloud2 +from laser_geometry import LaserProjection +import sensor_msgs.point_cloud2 as pc2 +import tf2_ros +import tf_conversions + + +def pc2_to_numpy(msg): + pts = [] + for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): + pts.append([p[0], p[1], p[2]]) + return np.asarray(pts, dtype=np.float32) if pts else np.zeros((0,3)) + + +class WallScanRecorder: + def __init__(self): + self.world_frame = rospy.get_param("~world_frame", "world") + self.base_frame = rospy.get_param("~base_frame", "base_link") + + self.out_cloud = rospy.get_param("~out_cloud_csv", "wall_points_gazebo.csv") + self.out_midpath = rospy.get_param("~out_midpath_csv", "wall_midpath_gazebo.csv") + + # TF + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + # Output buffers + self.points_world = [] + self.midpath_world = [] + + # LaserScan → PointCloud2 converter + self.lp = LaserProjection() + + # Subscribe to LaserScan + topic = rospy.get_param("~scan_topic", "/scan_cloud") + self.sub = rospy.Subscriber(topic, LaserScan, self.laser_cb, queue_size=1) + rospy.loginfo("Listening to LaserScan: %s", topic) + + rospy.on_shutdown(self.on_shutdown) + + def transform_points(self, transform, pts): + if pts.size == 0: + return pts + t = transform.transform.translation + q = transform.transform.rotation + + T = tf_conversions.transformations.quaternion_matrix([q.x, q.y, q.z, q.w]) + T[:3, 3] = [t.x, t.y, t.z] + + pts_h = np.hstack([pts, np.ones((pts.shape[0], 1))]) + pts_tf = (T @ pts_h.T).T + return pts_tf[:, :3] + + # ------------------- + # LaserScan callback + # ------------------- + def laser_cb(self, scan_msg): + rospy.loginfo_once("Laser messages are arriving.") + + # Detect LaserScan frame automatically + laser_frame = scan_msg.header.frame_id + if not laser_frame: + rospy.logwarn_once("LaserScan has NO frame_id!") + return + + # Convert LaserScan → PointCloud2 + try: + cloud_msg = self.lp.projectLaser(scan_msg) + except Exception as e: + rospy.logwarn("Laser→PointCloud2 conversion failed: %s", e) + return + + # Convert msg → numpy + pts_scanner = pc2_to_numpy(cloud_msg) + if pts_scanner.size == 0: + rospy.logwarn_throttle(2, "LaserScan produced 0 points.") + return + + # Get transforms: + # 1. world ← laser_frame + # 2. world ← base_link (for mid-path) + try: + tf_w_s = self.tf_buffer.lookup_transform( + self.world_frame, + laser_frame, # <-- THIS FIXES YOUR SCRIPT + scan_msg.header.stamp, + rospy.Duration(0.2), + ) + + tf_w_b = self.tf_buffer.lookup_transform( + self.world_frame, + self.base_frame, + scan_msg.header.stamp, + rospy.Duration(0.2) + ) + except Exception as e: + rospy.logwarn_throttle(1.0, f"TF lookup failed: {e}") + return + + # Transform points → world + pts_world = self.transform_points(tf_w_s, pts_scanner) + self.points_world.append(pts_world) + + # Record robot mid-path → world + t = tf_w_b.transform.translation + self.midpath_world.append([t.x, t.y, t.z]) + + rospy.loginfo_throttle(2.0, + f"Stored frame {len(self.points_world)}, points={pts_world.shape[0]}") + + # ------------------- + # Save CSV on shutdown + # ------------------- + def on_shutdown(self): + rospy.logwarn("Saving CSV files...") + + # Save point cloud + if len(self.points_world) > 0: + pts = np.vstack(self.points_world) + np.savetxt(self.out_cloud, pts, delimiter=",") + rospy.logwarn(f"Saved global pointcloud → {self.out_cloud} " + f"({pts.shape[0]} points)") + else: + rospy.logerr("NO pointcloud data received.") + + # Save mid-path + if len(self.midpath_world) > 0: + mp = np.asarray(self.midpath_world) + np.savetxt(self.out_midpath, mp, delimiter=",") + rospy.logwarn(f"Saved robot path → {self.out_midpath} " + f"({mp.shape[0]} samples)") + else: + rospy.logerr("NO robot path data recorded.") + + +def main(): + rospy.init_node("wall_scan_recorder") + WallScanRecorder() + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/rosbag2csv.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/rosbag2csv.py new file mode 100644 index 0000000..d056dc0 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/rosbag2csv.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +import rosbag +import argparse +import numpy as np +from sensor_msgs.msg import PointCloud2 +import sensor_msgs.point_cloud2 as pc2 + + +def extract_xyz(msg): + """ + Convert PointCloud2 to Nx3 numpy array without ros_numpy. + """ + points = [] + + for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): + points.append([p[0], p[1], p[2]]) + + return np.array(points, dtype=float) + + +def main(): + parser = argparse.ArgumentParser(description="Export PointCloud2 from rosbag to CSV (no ros_numpy)") + parser.add_argument("bag", help="Path to rosbag file (.bag)") + parser.add_argument("--topic", default=None, help="PointCloud2 topic to extract") + parser.add_argument("--out", default="cloud.csv", help="Output CSV file name") + args = parser.parse_args() + + print(f"Opening bag: {args.bag}") + bag = rosbag.Bag(args.bag) + + # detect PointCloud2 topics + info = bag.get_type_and_topic_info()[1] + available = [t for t, v in info.items() if v.msg_type == "sensor_msgs/PointCloud2"] + + if len(available) == 0: + print("No PointCloud2 topics found in this bag.") + return + + if args.topic is None: + print("Available PointCloud2 topics:") + for t in available: + print(" ", t) + args.topic = available[0] + print(f"Using default topic: {args.topic}") + else: + if args.topic not in available: + print(f"Topic {args.topic} is not a PointCloud2 topic.") + return + + all_points = [] + + print(f"Extracting topic: {args.topic}") + for _, msg, _ in bag.read_messages(topics=[args.topic]): + pts = extract_xyz(msg) + if pts.size > 0: + all_points.append(pts) + + bag.close() + + if len(all_points) == 0: + print("No points extracted.") + return + + cloud = np.vstack(all_points) + print(f"Extracted {cloud.shape[0]} points.") + + np.savetxt(args.out, cloud, delimiter=",", header="x,y,z", comments="") + print(f"Saved CSV to: {args.out}") + + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/view_image_npy.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/view_image_npy.py new file mode 100644 index 0000000..fc974f4 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/view_image_npy.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +""" +view_image_npy.py — visualize the offline map produced by pcl2texture.py + +Usage: + ./view_image_npy.py --map ../data/mapping_run/offline_map.npy +""" + +import argparse +import numpy as np +import matplotlib.pyplot as plt + + +def reconstruct_image_from_mapping(mapping): + """ + mapping: dict[(row, col)] = (gray_value, indices_array) + Returns: img (H,W) uint8 + """ + if not mapping: + raise ValueError("Mapping dict is empty") + + rows = [rc[0] for rc in mapping.keys()] + cols = [rc[1] for rc in mapping.keys()] + H = max(rows) + 1 + W = max(cols) + 1 + + img = np.zeros((H, W), dtype=np.uint8) + for (r, c), (gray, _) in mapping.items(): + img[r, c] = np.uint8(gray) + + return img + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument( + "--map", + default="../data/mapping_run/offline_map.npy", + help="Path to offline_map.npy produced by pcl2texture.py/build.py", + ) + args = parser.parse_args() + + print("[LOAD] mapping:", args.map) + mapping = np.load(args.map, allow_pickle=True).item() + + img = reconstruct_image_from_mapping(mapping) + + H, W = img.shape + print(f"[INFO] image shape: {H} x {W} (HxW)") + + plt.figure(figsize=(8, 4)) + plt.imshow(img, cmap="gray", origin="lower") + plt.title(f"Offline map ({H} x {W})") + plt.colorbar(label="Gray value") + plt.xlabel("u (along s)") + plt.ylabel("v (along z)") + plt.tight_layout() + plt.show() + + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/view_pcl.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/view_pcl.py new file mode 100644 index 0000000..06395d9 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/view_pcl.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +""" +Simple viewer: load point cloud from CSV and visualize with Open3D. + +CSV formats supported: +- x,y,z +- If header exists, columns named x,y,z will be used. +""" +import argparse +import sys +import os + +import numpy as np +try: + import pandas as pd + _HAS_PANDAS = True +except Exception: + _HAS_PANDAS = False + +import open3d as o3d + +def load_csv(path, delim): + if _HAS_PANDAS: + try: + df = pd.read_csv(path, sep=delim, engine="python") + # look for columns by name + cols = [c.lower() for c in df.columns] + if set(['x','y','z']).issubset(cols): + xcol = df.columns[cols.index('x')] + ycol = df.columns[cols.index('y')] + zcol = df.columns[cols.index('z')] + pts = df[[xcol,ycol,zcol]].to_numpy(dtype=float) + return _filter_finite(pts) + else: + arr = df.to_numpy(dtype=float) + except Exception: + arr = np.genfromtxt(path, delimiter=delim) + else: + arr = np.genfromtxt(path, delimiter=delim) + arr = np.asarray(arr) + if arr.ndim == 1: + arr = arr.reshape(1, -1) + if arr.shape[1] < 3: + raise ValueError("CSV must contain at least 3 columns for x,y,z") + pts = arr[:,0:3].astype(float) + return _filter_finite(pts) + +def _filter_finite(pts): + # Remove rows that contain NaN or inf in any coordinate + mask = np.isfinite(pts).all(axis=1) + if not np.all(mask): + removed = np.count_nonzero(~mask) + # minimal informative warning to stderr + print(f"Warning: removed {removed} rows containing NaN/inf", file=sys.stderr) + pts = pts[mask] + if pts.size == 0: + raise ValueError("No valid points found after removing NaN/inf rows") + return pts + +def build_pcd(pts): + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(pts) + return pcd + +def visualize(pcd, point_size=1.0, window_name="PointCloud"): + vis = o3d.visualization.Visualizer() + vis.create_window(window_name=window_name) + vis.add_geometry(pcd) + opt = vis.get_render_option() + opt.point_size = float(point_size) + opt.background_color = np.asarray([0,0,0]) + vis.run() + vis.destroy_window() + +def main(): + parser = argparse.ArgumentParser(description="Visualize point cloud from CSV using Open3D") + parser.add_argument("csv", help="Path to CSV file") + parser.add_argument("--delimiter", "-d", default=",", help="CSV delimiter (default ',')") + parser.add_argument("--downsample", "-v", type=float, default=0.0, + help="Voxel size for voxel_down_sample (0 = no downsample)") + parser.add_argument("--point-size", type=float, default=2.0, help="Point size for rendering") + parser.add_argument("--save", "-s", help="Optional: save visualized point cloud to PLY") + args = parser.parse_args() + + if not os.path.isfile(args.csv): + print("File not found:", args.csv, file=sys.stderr) + sys.exit(1) + + try: + pts = load_csv(args.csv, args.delimiter) + except Exception as e: + print("Failed to load CSV:", e, file=sys.stderr) + sys.exit(1) + + pcd = build_pcd(pts) + if args.downsample and args.downsample > 0.0: + pcd = pcd.voxel_down_sample(voxel_size=args.downsample) + + if args.save: + try: + o3d.io.write_point_cloud(args.save, pcd) + print("Saved:", args.save) + except Exception as e: + print("Failed to save:", e, file=sys.stderr) + + visualize(pcd, point_size=args.point_size, window_name=os.path.basename(args.csv)) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/__init__.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/compare_poses.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/compare_poses.py new file mode 100644 index 0000000..5258137 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/compare_poses.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +import rospy +import tf2_ros +import tf_conversions +import csv +from geometry_msgs.msg import TransformStamped + +def mat_from_tf(tf_msg): + t = tf_msg.transform.translation + r = tf_msg.transform.rotation + M = tf_conversions.transformations.quaternion_matrix([r.x, r.y, r.z, r.w]) + M[0,3], M[1,3], M[2,3] = t.x, t.y, t.z + return M + +def main(): + rospy.init_node("compare_poses") + + wall_frame = "wall" + scanner_gt_frame = "vertical_profiler_link" + scanner_est_frame = "scanner_icp_refined" + + out_gt = rospy.get_param("~gt_csv", "groundtruth_poses.csv") + out_est = rospy.get_param("~est_csv", "estimated_poses.csv") + + tf_buffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(tf_buffer) + + rate = rospy.Rate(20) + + gt_file = open(out_gt, "w", newline="") + est_file = open(out_est, "w", newline="") + gt_writer = csv.writer(gt_file) + est_writer = csv.writer(est_file) + + gt_writer.writerow(["t", "x", "y", "z", "qx", "qy", "qz", "qw"]) + est_writer.writerow(["t", "x", "y", "z", "qx", "qy", "qz", "qw"]) + + rospy.loginfo("Recording GT and Estimated Scanner Poses...") + + while not rospy.is_shutdown(): + stamp = rospy.Time.now() + + try: + gt_tf = tf_buffer.lookup_transform(wall_frame, scanner_gt_frame, stamp, rospy.Duration(0.1)) + gt = gt_tf.transform + gt_writer.writerow([stamp.to_sec(), gt.translation.x, gt.translation.y, gt.translation.z, + gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w]) + except: + pass + + try: + est_tf = tf_buffer.lookup_transform(wall_frame, scanner_est_frame, stamp, rospy.Duration(0.1)) + est = est_tf.transform + est_writer.writerow([stamp.to_sec(), est.translation.x, est.translation.y, est.translation.z, + est.rotation.x, est.rotation.y, est.rotation.z, est.rotation.w]) + except: + pass + + rate.sleep() + + gt_file.close() + est_file.close() + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/localization_core.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/localization_core.py new file mode 100644 index 0000000..b22a022 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/localization_core.py @@ -0,0 +1,1131 @@ +#!/usr/bin/env python3 +""" +Two independent localization modes: + + --method sift : Use Zhang et al. style SIFT localization + --method lbp : Use LBP-HF + NCC localization + +You can pass either: + --image : a grayscale image file to use as the offline map + --pixel_map : a .npy file saved by pcl2texture containing a mapping + dict[(row,col)] = (grayscale, np.ndarray(point_indices)) + When provided, the script reconstructs the grayscale image + from the mapping and uses it as the offline map. + +Example: + python localization_2d.py --method lbp --pixel_map map_pixels.npy + +If neither is provided, the script will attempt to load OFFLINE_MAP_PATH; it must be +either a raw numpy image array or the mapping dict created by pcl2texture. Synthetic +map generation is disabled. +""" + +import numpy as np +import cv2 +import argparse +import os +import time +import pickle +from pathlib import Path +import matplotlib.pyplot as plt +from skimage.feature import local_binary_pattern +from scipy.fft import fft +from scipy.signal import correlate2d +from sklearn.neighbors import NearestNeighbors +from sklearn.decomposition import PCA +import csv +import yaml # <--- add for YAML loading + +### ====================================================== +### PARAMETERS (default placeholders, overwritten by YAML) +### ====================================================== +PATCH_H, PATCH_W = 200, 160 +STRIDE = 16 +MAX_QUERY_KPS = 60 +LBP_RADIUS = 2 +LBP_POINTS = 8 * LBP_RADIUS +LBPHF_KEEP = 24 +COARSE_TOPK = 8 +SIFT_RATIO = 0.7 +SIFT_PCA_DIM = 32 +SCALE_BINS = 3 +FLANN_TREES = 5 +FLANN_CHECKS = 50 + +# ---------------------------------------------------------------------- +# Global parameter synchronization (from YAML only) +# ---------------------------------------------------------------------- + +LOC2D_CONFIG = {} + +def set_loc2d_config(params: dict): + """ + Inject YAML-loaded parameters into this module. + This safely overwrites the global constants used by all functions. + """ + global LOC2D_CONFIG, PATCH_H, PATCH_W + LOC2D_CONFIG = params.copy() if params else {} + + # Optionally patch size from YAML (if present) + if "patch_h" in LOC2D_CONFIG: + PATCH_H = int(LOC2D_CONFIG["patch_h"]) + if "patch_w" in LOC2D_CONFIG: + PATCH_W = int(LOC2D_CONFIG["patch_w"]) + + # Overwrite generic params + globals()['STRIDE'] = LOC2D_CONFIG.get('stride', STRIDE) + globals()['MAX_QUERY_KPS'] = LOC2D_CONFIG.get('max_query_kps', MAX_QUERY_KPS) + + # Overwrite LBP parameters + globals()['LBP_RADIUS'] = LOC2D_CONFIG.get('lbp_radius', LBP_RADIUS) + globals()['LBP_POINTS'] = LOC2D_CONFIG.get('lbp_points', 8 * globals()['LBP_RADIUS']) + globals()['LBPHF_KEEP'] = LOC2D_CONFIG.get('lbphf_keep', LBPHF_KEEP) + globals()['COARSE_TOPK'] = LOC2D_CONFIG.get('coarse_topk', COARSE_TOPK) + + # Overwrite SIFT parameters + globals()['SIFT_RATIO'] = LOC2D_CONFIG.get('sift_ratio', SIFT_RATIO) + globals()['SIFT_PCA_DIM'] = LOC2D_CONFIG.get('sift_pca_dim', SIFT_PCA_DIM) + globals()['SCALE_BINS'] = LOC2D_CONFIG.get('scale_bins', SCALE_BINS) + globals()['FLANN_TREES'] = LOC2D_CONFIG.get('flann_trees', FLANN_TREES) + globals()['FLANN_CHECKS'] = LOC2D_CONFIG.get('flann_checks', FLANN_CHECKS) + +# Default offline paths (used only when no image is provided) +OFFLINE_MAP_PATH = Path("/home/thach/project_ws/src/wall_localization_demo/data/mapping_run/offline_map.npy") +OFFLINE_LBP_DB_PATH = Path("/home/thach/project_ws/src/wall_localization_demo/data/mapping_run/offline_lbp_db.pkl") +OFFLINE_SIFT_DB_PATH = Path("/home/thach/project_ws/src/wall_localization_demo/data/mapping_run/offline_sift_db.pkl") +PARAM_YAML_PATH = Path("/home/thach/project_ws/src/wall_localization_demo/config/localization_params.yaml") + +np.random.seed(1) + +### ====================================================== +### LBP-HF +### ====================================================== +def lbphf_descriptor(patch): + # local_binary_pattern expects integer images; convert from float [0,1] to uint8 + patch_u8 = (np.clip(patch, 0.0, 1.0) * 255.0).astype(np.uint8) + lbp = local_binary_pattern(patch_u8, LBP_POINTS, LBP_RADIUS, method="uniform") + n_bins = int(lbp.max() + 1) + hist, _ = np.histogram(lbp.ravel(), bins=n_bins, range=(0, n_bins), density=True) + hist_fft = fft(hist) + mag = np.abs(hist_fft) + mag_real = mag.real.astype(np.float32) + # Keep the magnitudes of low-frequency components only to reduce descriptor size + if mag_real.size >= LBPHF_KEEP: + return mag_real[:LBPHF_KEEP].astype(np.float32) + else: + out = np.zeros((LBPHF_KEEP,), dtype=np.float32) + out[:mag_real.size] = mag_real + return out + + +def compute_map_lbphf_descriptors(img): + H, W = img.shape + descs = [] + poses = [] + for y in range(0, H - PATCH_H + 1, STRIDE): + for x in range(0, W - PATCH_W + 1, STRIDE): + patch = img[y:y+PATCH_H, x:x+PATCH_W] + descs.append(lbphf_descriptor(patch)) + poses.append((y, x)) + return np.vstack(descs), poses + + +### ====================================================== +### SIFT +### ====================================================== +def sift_extract(image): + img8 = (255 * image).astype(np.uint8) + sift = cv2.SIFT_create() + kps, desc = sift.detectAndCompute(img8, None) + if desc is None: + return [], None + return kps, desc + + +def build_sift_database(img, params=None): + """ + Build SIFT database on the given offline image. + + Uses global SIFT_PCA_DIM, SCALE_BINS, FLANN_TREES, FLANN_CHECKS that were + already set by set_loc2d_config() from localization_params.yaml. + Optional 'params' is ignored (kept only for backward compatibility). + """ + # Extract SIFT features from the full offline map + kps, desc = sift_extract(img) + if desc is None or len(desc) == 0: + raise RuntimeError("No SIFT features found.") + + positions = np.array([kp.pt for kp in kps], dtype=np.float32) + scales = np.array([kp.size for kp in kps], dtype=np.float32) + + # PCA compression + pca = PCA(n_components=SIFT_PCA_DIM, random_state=0) + desc_pca = pca.fit_transform(desc.astype(np.float32)) + + # Scale bins + smin, smax = scales.min(), scales.max() + bin_edges = np.linspace(smin, smax + 1e-6, SCALE_BINS + 1) + bin_idx = np.digitize(scales, bin_edges) - 1 + bin_idx = np.clip(bin_idx, 0, SCALE_BINS - 1) + + nn_per_bin = {} + index_params = dict(algorithm=1, trees=FLANN_TREES) # KDTree + search_params = dict(checks=FLANN_CHECKS) + + for b in range(SCALE_BINS): + mask = bin_idx == b + if not np.any(mask): + continue + + flann = cv2.FlannBasedMatcher(index_params, search_params) + desc_bin = desc_pca[mask].astype(np.float32) + flann.add([desc_bin]) + flann.train() + + nn_per_bin[b] = { + "flann": flann, + "desc_pca": desc_bin, + "positions": positions[mask], + "scales": scales[mask], + } + + return { + "pca": pca, + "bin_edges": bin_edges, + "nn_per_bin": nn_per_bin, + "positions_all": positions, + "scales_all": scales, + } + + +# ----------------------- New helpers for SIFT DB serialization ----------------------- +def make_savable_sift_db(sift_db): + """ + Return a copy of sift_db suitable for pickle: remove the OpenCV FlannBasedMatcher objects. + Keeps PCA, bin_edges and per-bin arrays (desc_pca, positions, scales). + """ + sav = { + "pca": sift_db["pca"], + "bin_edges": sift_db["bin_edges"], + "nn_per_bin": {}, + "positions_all": sift_db.get("positions_all", None), + "scales_all": sift_db.get("scales_all", None), + } + for b, info in sift_db["nn_per_bin"].items(): + sav["nn_per_bin"][b] = { + # ensure arrays are numpy arrays (float32) and pickle-friendly + "desc_pca": np.array(info["desc_pca"], dtype=np.float32), + "positions": np.array(info["positions"], dtype=np.float32), + "scales": np.array(info["scales"], dtype=np.float32), + } + return sav + + +def reconstruct_sift_db(saved_db): + """ + Reconstruct full sift_db from a saved (pickleable) representation by rebuilding Flann matchers. + Uses current FLANN_TREES / FLANN_CHECKS globals. + """ + if saved_db is None: + return None + sift_db = { + "pca": saved_db.get("pca", None), + "bin_edges": saved_db.get("bin_edges", None), + "nn_per_bin": {}, + "positions_all": saved_db.get("positions_all", None), + "scales_all": saved_db.get("scales_all", None), + } + index_params = dict(algorithm=1, trees=FLANN_TREES) + search_params = dict(checks=FLANN_CHECKS) + for b, info in saved_db.get("nn_per_bin", {}).items(): + desc_bin = info.get("desc_pca", None) + positions = info.get("positions", None) + scales = info.get("scales", None) + nninfo = { + "desc_pca": np.array(desc_bin, dtype=np.float32) if desc_bin is not None else np.empty((0,)), + "positions": np.array(positions, dtype=np.float32) if positions is not None else np.empty((0,2)), + "scales": np.array(scales, dtype=np.float32) if scales is not None else np.empty((0,)), + } + # build FLANN matcher if descriptors exist + if nninfo["desc_pca"].size > 0: + try: + flann = cv2.FlannBasedMatcher(index_params, search_params) + flann.add([nninfo["desc_pca"].astype(np.float32)]) + flann.train() + nninfo["flann"] = flann + except Exception: + # In rare cases OpenCV flann may not be available/configured; leave flann out + nninfo["flann"] = None + else: + nninfo["flann"] = None + sift_db["nn_per_bin"][b] = nninfo + return sift_db +# ----------------------- End new helpers ----------------------------------- + + +### ====================================================== +### NCC +### ====================================================== +def ncc_score(A, B): + A = A - A.mean() + B = B - B.mean() + if np.linalg.norm(A) < 1e-9 or np.linalg.norm(B) < 1e-9: + return -1.0 + corr = correlate2d(A, B, mode="valid") + return float(corr.max() / (np.linalg.norm(A) * np.linalg.norm(B))) + + +### ====================================================== +### Localization Modes +### ====================================================== +def lbp_localize(img, lbp_db, params=None): + """ + Offline evaluation version of LBP-HF localization. + + Picks a random PATCH_H x PATCH_W query from img, then runs: + - LBP descriptor + - coarse KNN search in descriptor DB + - NCC refinement around candidates + - RANSAC over refined votes + + Returns: + est: (row, col) estimated top-left in img (or None on failure) + gt: (row, col) ground-truth top-left used for query + query: the query patch + dist: localization error in pixels (or None if failed) + """ + descs, poses, nn = lbp_db + H, W = img.shape + + # choose random ground-truth patch + y0 = np.random.randint(0, H - PATCH_H + 1) + x0 = np.random.randint(0, W - PATCH_W + 1) + query = img[y0:y0+PATCH_H, x0:x0+PATCH_W] + + # coarse KNN in descriptor space + q_desc = lbphf_descriptor(query).reshape(1, -1) + _, idxs = nn.kneighbors(q_desc, n_neighbors=COARSE_TOPK) + candidates = [poses[i] for i in idxs[0]] + + if 'DEBUG' in globals() and DEBUG: + print("[DEBUG LBP] q_desc.shape=", q_desc.shape) + print("[DEBUG LBP] neighbor idxs=", idxs) + print("[DEBUG LBP] candidate poses=", candidates) + + scored = [] + for (py, px) in candidates: + if py + PATCH_H > H or px + PATCH_W > W: + continue + patch = img[py:py+PATCH_H, px:px+PATCH_W] + score = ncc_score(patch, query) + if score <= 0: + continue + scored.append((py, px, score)) + + if len(scored) == 0: + print(f"[LBP] no valid candidates for GT=({y0},{x0})") + return None, (y0, x0), query, None + + # sort by NCC score, keep best few + scored.sort(key=lambda x: x[2], reverse=True) + top_n = min(6, len(scored)) + SR = STRIDE + STEP = max(1, STRIDE // 2) + + votes = [] + for i in range(top_n): + py0, px0, sc0 = scored[i] + best_local_score = sc0 + best_local_pos = (py0, px0) + + for dy in range(-SR, SR + 1, STEP): + for dx in range(-SR, SR + 1, STEP): + ny = py0 + dy + nx = px0 + dx + if ny < 0 or nx < 0 or ny + PATCH_H > H or nx + PATCH_W > W: + continue + patch = img[ny:ny+PATCH_H, nx:nx+PATCH_W] + s = ncc_score(patch, query) + if s > best_local_score: + best_local_score = s + best_local_pos = (ny, nx) + + if 'DEBUG' in globals() and DEBUG: + print(f"[DEBUG LBP] refined candidate {i}: start=({py0},{px0}) -> best={best_local_pos} score={best_local_score}") + + votes.append((best_local_pos[0], best_local_pos[1], best_local_score)) + + votes_arr = np.array([[v[0], v[1]] for v in votes], dtype=np.float32) + n_votes = votes_arr.shape[0] + if n_votes == 0: + print(f"[LBP] no refined votes for GT=({y0},{x0})") + return None, (y0, x0), query, None + + if n_votes == 1: + est_y, est_x = votes_arr[0] + else: + best_inliers = None + best_count = 0 + RANSAC_ITERS = 100 + RANSAC_THRESH = 8.0 + for _ in range(RANSAC_ITERS): + idx = np.random.randint(0, n_votes) + hyp = votes_arr[idx] + dists = np.linalg.norm(votes_arr - hyp, axis=1) + mask = dists <= RANSAC_THRESH + cnt = int(mask.sum()) + if cnt > best_count: + best_count = cnt + best_inliers = votes_arr[mask] + + if best_inliers is None or best_count < 2: + # fallback: best single refined vote + py, px, _ = max(votes, key=lambda v: v[2]) + est_y, est_x = py, px + else: + mean_in = best_inliers.mean(axis=0) + est_y = float(mean_in[0]) + est_x = float(mean_in[1]) + + est_y = int(np.clip(int(round(est_y)), 0, H - PATCH_H)) + est_x = int(np.clip(int(round(est_x)), 0, W - PATCH_W)) + dist = float(np.sqrt((est_y - y0) ** 2 + (est_x - x0) ** 2)) + print(f"[LBP] GT=({y0},{x0}), EST=({est_y},{est_x}), Error={dist:.2f}px") + + return (est_y, est_x), (y0, x0), query, dist +def sift_localize(img, sift_db): + H, W = img.shape + y0 = np.random.randint(0, H - PATCH_H + 1) + x0 = np.random.randint(0, W - PATCH_W + 1) + query = img[y0:y0+PATCH_H, x0:x0+PATCH_W] + + kps_q, desc_q = sift_extract(query) + if desc_q is None: + print("[SIFT] No keypoints in query.") + return None, (y0, x0), query, None + + # Limit number of query keypoints to reduce per-trial cost + if len(kps_q) > MAX_QUERY_KPS: + # pick top keypoints by response + responses = np.array([kp.response for kp in kps_q], dtype=np.float32) + top_idx = np.argsort(responses)[-MAX_QUERY_KPS:] + top_idx = np.sort(top_idx) + kps_q = [kps_q[i] for i in top_idx] + desc_q = desc_q[top_idx] + + desc_q_pca = sift_db["pca"].transform(desc_q.astype(np.float32)) + votes = [] + + # Diagnostic counters + cnt_total_kp = len(kps_q) + cnt_no_bin = 0 + cnt_no_matches = 0 + cnt_ratio_reject = 0 + cnt_scale_reject = 0 + cnt_oob = 0 + cnt_accepted = 0 + kp_reasons = [] # store small per-keypoint reason samples + + for i, kp in enumerate(kps_q): + s = kp.size + b = np.digitize([s], sift_db["bin_edges"]) - 1 + b = int(np.clip(b[0], 0, SCALE_BINS - 1)) + if b not in sift_db["nn_per_bin"]: + cnt_no_bin += 1 + if len(kp_reasons) < 8: + kp_reasons.append(("no_bin", i, float(s))) + continue + + nninfo = sift_db["nn_per_bin"][b] + + # Number of descriptors stored in this bin + n_fit = nninfo["desc_pca"].shape[0] + if n_fit == 0: + cnt_no_matches += 1 + if len(kp_reasons) < 8: + kp_reasons.append(("empty_bin", i)) + continue + + # k neighbors to request + k = min(3, n_fit) + qvec = desc_q_pca[i:i+1].astype(np.float32) + k = min(max(2, k), nninfo["desc_pca"].shape[0]) + matches = nninfo["flann"].knnMatch(qvec, k=k) + + x_q, y_q = kp.pt + + if len(matches) == 0 or len(matches[0]) == 0: + cnt_no_matches += 1 + if len(kp_reasons) < 8: + kp_reasons.append(("no_matches", i)) + continue + + best_matches = matches[0] + if len(best_matches) >= 2: + m0 = best_matches[0] + m1 = best_matches[1] + if m0.distance > SIFT_RATIO * m1.distance: + cnt_ratio_reject += 1 + if len(kp_reasons) < 8: + kp_reasons.append(("ratio_reject", i, float(m0.distance), float(m1.distance))) + continue + chosen = m0 + else: + chosen = best_matches[0] + + neigh_dist = float(chosen.distance) + idx = int(chosen.trainIdx) + x_db, y_db = nninfo["positions"][idx] + s_db = float(nninfo["scales"][idx]) if "scales" in nninfo else 0.0 + + # scale similarity check (avoid matching wildly different scales) + scale_tol = 1.8 + if kp.size > 0 and s_db > 0: + ratio = s_db / kp.size + if ratio < 1.0/scale_tol or ratio > scale_tol: + cnt_scale_reject += 1 + if len(kp_reasons) < 8: + kp_reasons.append(("scale_reject", i, float(ratio))) + continue + + # Predicted top-left of the query patch in the map is db_kp - query_kp + est_y_raw = float(y_db - y_q) + est_x_raw = float(x_db - x_q) + + # Discard votes outside the valid top-left range + if est_y_raw < 0 or est_x_raw < 0 or est_y_raw > (H - PATCH_H) or est_x_raw > (W - PATCH_W): + cnt_oob += 1 + if len(kp_reasons) < 8: + kp_reasons.append(("oob", i, est_y_raw, est_x_raw)) + continue + + est_y = float(est_y_raw) + est_x = float(est_x_raw) + + w_dist = 1.0 / (neigh_dist + 1e-6) + w_scale = 1.0 - abs(1.0 - (s_db / (kp.size + 1e-9))) + weight = max(0.0, w_dist * w_scale) + + votes.append((est_y, est_x, weight, x_db, y_db, x_q, y_q, neigh_dist)) + cnt_accepted += 1 + + # If no votes, try a global fallback search (concatenate all bin descriptors) with looser checks + if not votes: + print("[SIFT] No SIFT votes (all OOB or no matches). Diagnostics:") + print(f" total_kps={cnt_total_kp}, accepted={cnt_accepted}, no_bin={cnt_no_bin}, no_matches={cnt_no_matches}, ratio_reject={cnt_ratio_reject}, scale_reject={cnt_scale_reject}, oob={cnt_oob}") + if kp_reasons: + print(" sample reasons:", kp_reasons) + # Optionally dump a small debug file for offline inspection + if 'DUMP_DIR' in globals() and DUMP_DIR: + try: + os.makedirs(DUMP_DIR, exist_ok=True) + fname = os.path.join(DUMP_DIR, f"sift_no_votes_gt_{y0}_{x0}.txt") + with open(fname, "w") as f: + f.write(f"GT=({y0},{x0})\n") + f.write(f"total_kps={cnt_total_kp}\n") + f.write(f"accepted={cnt_accepted}\n") + f.write(f"no_bin={cnt_no_bin}\n") + f.write(f"no_matches={cnt_no_matches}\n") + f.write(f"ratio_reject={cnt_ratio_reject}\n") + f.write(f"scale_reject={cnt_scale_reject}\n") + f.write(f"oob={cnt_oob}\n") + f.write("sample_reasons:\n") + for r in kp_reasons: + f.write(str(r) + "\n") + except Exception: + pass + return None, (y0, x0), query, None + + votes_arr = np.array([[v[0], v[1]] for v in votes], dtype=np.float32) + weights = np.array([v[2] for v in votes], dtype=np.float32) + n_votes = votes_arr.shape[0] + + if n_votes == 1: + est = (int(round(votes_arr[0,0])), int(round(votes_arr[0,1]))) + else: + best_inliers = None + best_weight_sum = 0.0 + RANSAC_ITERS = 200 + RANSAC_THRESH = 12.0 # pixels (looser to account for discretization) + + for _ in range(RANSAC_ITERS): + idx = np.random.randint(0, n_votes) + hypothesis = votes_arr[idx] + dists_v = np.linalg.norm(votes_arr - hypothesis, axis=1) + inliers_mask = dists_v <= RANSAC_THRESH + weight_sum = float(weights[inliers_mask].sum()) + if weight_sum > best_weight_sum: + best_weight_sum = weight_sum + best_inliers = (votes_arr[inliers_mask], weights[inliers_mask]) + + if best_inliers is None or best_weight_sum <= 0.0: + # fallback to weighted mode: pick vote with max weight + best_idx = int(np.argmax(weights)) + est = (int(round(votes_arr[best_idx,0])), int(round(votes_arr[best_idx,1]))) + else: + in_coords, in_w = best_inliers + # weighted average of inlier coordinates + w_sum = float(in_w.sum()) + 1e-9 + mean_y = float((in_coords[:,0] * in_w).sum() / w_sum) + mean_x = float((in_coords[:,1] * in_w).sum() / w_sum) + est_y = int(np.clip(int(round(mean_y)), 0, H - PATCH_H)) + est_x = int(np.clip(int(round(mean_x)), 0, W - PATCH_W)) + est = (est_y, est_x) + + est_y, est_x = est + dist = np.sqrt((est_y - y0)**2 + (est_x - x0)**2) + print(f"[SIFT] GT=({y0},{x0}), EST=({est_y},{est_x}), Error={dist:.2f}px") + + # Optionally dump debug visualization for large errors + if 'DUMP_DIR' in globals() and DUMP_DIR: + try: + # save up to a few failing examples (based on error magnitude) + dump_threshold = max(PATCH_H, PATCH_W) * 1.0 + if dist >= dump_threshold: + os.makedirs(DUMP_DIR, exist_ok=True) + # construct debug figure + import matplotlib.pyplot as _plt + fig = _plt.figure(figsize=(10, 6)) + ax = fig.add_subplot(1, 1, 1) + ax.imshow(img, cmap='gray') + ax.axis('off') + # true and estimated rectangles + ax.add_patch(_plt.Rectangle((x0, y0), PATCH_W, PATCH_H, edgecolor='lime', facecolor='none', linewidth=2)) + ax.add_patch(_plt.Rectangle((est_x, est_y), PATCH_W, PATCH_H, edgecolor='red', facecolor='none', linewidth=2)) + + # plot a subset of vote connections + # sort votes by weight and take top 20 + votes_sorted = sorted(votes, key=lambda v: v[2], reverse=True) + for v in votes_sorted[:20]: + _, _, w, x_db, y_db, x_q, y_q, nd = v + # draw a line from db kp to the predicted top-left (db_kp - query_kp) + ax.plot([x_db - 0, x_db - x_q], [y_db - 0, y_db - y_q], color='yellow', alpha=min(1.0, 0.2 + 0.8*(w/ (w+1e-6)))) + + fname = os.path.join(DUMP_DIR, f"sift_debug_gt_{y0}_{x0}_est_{est_y}_{est_x}_err_{int(dist)}.png") + _plt.tight_layout() + _plt.savefig(fname, dpi=150) + _plt.close(fig) + except Exception as _e: + print("Failed to write SIFT debug dump:", _e) + return est, (y0, x0), query, dist + + +def sift_localize_query(query_patch, offline_img, sift_db): + """ + Localize a given query patch (PATCH_H x PATCH_W) inside offline_img using the + precomputed SIFT database (sift_db). + + query_patch : 2D float32 array in [0,1] or uint8 (live patch) + offline_img : 2D float32 array in [0,1] (offline map) + sift_db : database built by build_sift_database(offline_img) + + Returns: + (row, col) of top-left corner in offline_img, or None on failure. + """ + H, W = offline_img.shape + + # 1) Compute SIFT on query patch + if query_patch.dtype != np.float32: + qp = query_patch.astype(np.float32) / (255.0 if query_patch.max() > 1.5 else 1.0) + else: + qp = query_patch + kps_q, desc_q = sift_extract(qp) + if desc_q is None or len(kps_q) == 0: + return None + + # 2) Limit query keypoints by MAX_QUERY_KPS (as in sift_localize) + if len(kps_q) > MAX_QUERY_KPS: + responses = np.array([kp.response for kp in kps_q], dtype=np.float32) + top_idx = np.argsort(responses)[-MAX_QUERY_KPS:] + top_idx = np.sort(top_idx) + kps_q = [kps_q[i] for i in top_idx] + desc_q = desc_q[top_idx] + + # 3) Project query descriptors via DB PCA + desc_q_pca = sift_db["pca"].transform(desc_q.astype(np.float32)) + votes = [] + + cnt_total_kp = len(kps_q) + cnt_no_bin = cnt_no_matches = cnt_ratio_reject = 0 + cnt_scale_reject = cnt_oob = cnt_accepted = 0 + + for i, kp in enumerate(kps_q): + s = kp.size + b = np.digitize([s], sift_db["bin_edges"]) - 1 + b = int(np.clip(b[0], 0, SCALE_BINS - 1)) + if b not in sift_db["nn_per_bin"]: + cnt_no_bin += 1 + continue + + nninfo = sift_db["nn_per_bin"][b] + n_fit = nninfo["desc_pca"].shape[0] + if n_fit == 0 or nninfo.get("flann", None) is None: + cnt_no_matches += 1 + continue + + # k nearest neighbors + k = min(max(2, 3), n_fit) + qvec = desc_q_pca[i:i+1].astype(np.float32) + matches = nninfo["flann"].knnMatch(qvec, k=k) + + x_q, y_q = kp.pt + + if len(matches) == 0 or len(matches[0]) == 0: + cnt_no_matches += 1 + continue + + best_matches = matches[0] + if len(best_matches) >= 2: + m0, m1 = best_matches[0], best_matches[1] + if m0.distance > SIFT_RATIO * m1.distance: + cnt_ratio_reject += 1 + continue + chosen = m0 + else: + chosen = best_matches[0] + + neigh_dist = float(chosen.distance) + idx = int(chosen.trainIdx) + x_db, y_db = nninfo["positions"][idx] + s_db = float(nninfo["scales"][idx]) if "scales" in nninfo else 0.0 + + # scale check + scale_tol = 1.8 + if kp.size > 0 and s_db > 0: + ratio = s_db / kp.size + if ratio < 1.0/scale_tol or ratio > scale_tol: + cnt_scale_reject += 1 + continue + + # estimate top-left of patch in map: db_kp - query_kp + est_y_raw = float(y_db - y_q) + est_x_raw = float(x_db - x_q) + if est_y_raw < 0 or est_x_raw < 0 or est_y_raw > (H - PATCH_H) or est_x_raw > (W - PATCH_W): + cnt_oob += 1 + continue + + w_dist = 1.0 / (neigh_dist + 1e-6) + w_scale = 1.0 - abs(1.0 - (s_db / (kp.size + 1e-9))) + weight = max(0.0, w_dist * w_scale) + votes.append((est_y_raw, est_x_raw, weight)) + cnt_accepted += 1 + + if not votes: + return None + + votes_arr = np.array([[v[0], v[1]] for v in votes], dtype=np.float32) + weights = np.array([v[2] for v in votes], dtype=np.float32) + n_votes = votes_arr.shape[0] + + # 4) Robust aggregation (RANSAC on vote positions) + if n_votes == 1: + est_y, est_x = votes_arr[0] + else: + best_inliers = None + best_weight_sum = 0.0 + RANSAC_ITERS = 200 + RANSAC_THRESH = 12.0 + + for _ in range(RANSAC_ITERS): + idx = np.random.randint(0, n_votes) + hyp = votes_arr[idx] + dists_v = np.linalg.norm(votes_arr - hyp, axis=1) + inliers_mask = dists_v <= RANSAC_THRESH + weight_sum = float(weights[inliers_mask].sum()) + if weight_sum > best_weight_sum: + best_weight_sum = weight_sum + best_inliers = (votes_arr[inliers_mask], weights[inliers_mask]) + + if best_inliers is None or best_weight_sum <= 0.0: + best_idx = int(np.argmax(weights)) + est_y, est_x = votes_arr[best_idx] + else: + in_coords, in_w = best_inliers + w_sum = float(in_w.sum()) + 1e-9 + est_y = float((in_coords[:, 0] * in_w).sum() / w_sum) + est_x = float((in_coords[:, 1] * in_w).sum() / w_sum) + + # clamp to valid map region + est_y = int(np.clip(int(round(est_y)), 0, H - PATCH_H)) + est_x = int(np.clip(int(round(est_x)), 0, W - PATCH_W)) + return (est_y, est_x) + + +def lbp_localize_query(query_patch, offline_img, lbp_db): + """ + Localize a given query patch (PATCH_H x PATCH_W) inside offline_img using LBP DB. + Returns (est_row, est_col) or None. + """ + descs, poses, nn = lbp_db + H, W = offline_img.shape + + # Descriptor of query patch + q_desc = lbphf_descriptor(query_patch.astype(np.float32)).reshape(1, -1) + _, idxs = nn.kneighbors(q_desc, n_neighbors=COARSE_TOPK) + candidates = [poses[i] for i in idxs[0]] + + best_score = -1.0 + best_pos = None + for (py, px) in candidates: + if py + PATCH_H > H or px + PATCH_W > W: + continue + patch = offline_img[py:py+PATCH_H, px:px+PATCH_W] + score = ncc_score(patch, query_patch) + if score > best_score: + best_score = score + best_pos = (py, px) + + return best_pos + + +### ====================================================== +### Visualization +### ====================================================== +def visualize_result(img, est, gt, query): + y0, x0 = gt + true_patch = img[y0:y0+PATCH_H, x0:x0+PATCH_W] + + if est is not None: + ey, ex = est + est_patch = img[ey:ey+PATCH_H, ex:ex+PATCH_W] + else: + est_patch = np.zeros_like(true_patch) + + fig, axs = plt.subplots(1, 3, figsize=(18, 4)) + + # --------------------------------------------------- + # 0: full texture map with rectangles + # --------------------------------------------------- + axs[0].set_title("Offline Map (Green=True, Red=Estimated)") + axs[0].imshow(img, cmap='gray') + axs[0].axis('off') + + # true + axs[0].add_patch( + plt.Rectangle((x0, y0), PATCH_W, PATCH_H, + edgecolor='lime', facecolor='none', linewidth=2) + ) + + # estimated + if est is not None: + axs[0].add_patch( + plt.Rectangle((ex, ey), PATCH_W, PATCH_H, + edgecolor='red', facecolor='none', linewidth=2) + ) + + # --------------------------------------------------- + # 1: query patch + # --------------------------------------------------- + axs[1].set_title("Query Patch (online)") + axs[1].imshow(query, cmap='gray') + axs[1].axis('off') + + # --------------------------------------------------- + # 2: true location patch + # --------------------------------------------------- + axs[2].set_title("True Patch (ground truth)") + axs[2].imshow(true_patch, cmap='gray') + axs[2].axis('off') + + # --------------------------------------------------- + # 3: estimated patch + # --------------------------------------------------- + #axs[3].set_title("Matched Patch (estimated)") + #axs[3].imshow(est_patch, cmap='gray') + #axs[3].axis('off') + + plt.tight_layout() + plt.show() + + +# ----------------------- New: helper for mapping reconstruction ----------------------- +def reconstruct_image_from_mapping(mapping): + """ + mapping: dict with keys (row,col) -> (gray_value, point_indices_array) + returns: float32 image in [0,1] + """ + if not mapping: + raise ValueError("Empty mapping provided.") + # ensure keys are tuples of ints + keys = list(mapping.keys()) + rows = [int(k[0]) for k in keys] + cols = [int(k[1]) for k in keys] + H = max(rows) + 1 + W = max(cols) + 1 + img8 = np.zeros((H, W), dtype=np.uint8) + for (r, c), val in mapping.items(): + # val may be (gray, arr) or scalar + gray = val[0] if (isinstance(val, (tuple, list)) or (hasattr(val, '__len__') and not np.isscalar(val))) else val + img8[int(r), int(c)] = np.clip(int(gray), 0, 255) + return img8.astype(np.float32) / 255.0 +# ----------------------- End helper ----------------------------------- + +### ====================================================== +### Parameter Sweep +### ====================================================== +def run_param_sweep(method, img, args, out_csv=None): + """ + Parameter sweep is disabled: parameters now come only from YAML. + """ + raise RuntimeError("run_param_sweep is disabled. All parameters must be set in localization_params.yaml.") + +### ====================================================== +### MAIN +### ====================================================== +def main(): + global PATCH_H, PATCH_W + parser = argparse.ArgumentParser() + parser.add_argument("--method", choices=["lbp", "sift"], required=True) + parser.add_argument("--new", action="store_true") + parser.add_argument("--pixel_map", type=str, default="", help="Path to .npy pixel->points mapping (reconstructs grayscale image)") + parser.add_argument("--image", type=str, default="") + parser.add_argument("--trials", type=int, default=1, help="Number of random trials to run") + parser.add_argument("--nodisplay", action="store_true", help="Disable plotting/interactive display") + parser.add_argument("--debug", action="store_true", help="Print debug info for localization") + parser.add_argument("--dump_dir", type=str, default="", help="Directory to write SIFT debug dumps for failing trials") + parser.add_argument("--sweep", action="store_true", help="Run parameter sweep (DISABLED)") + parser.add_argument("--sweep_out", type=str, default="", help="Output CSV path for sweep (unused)") + args = parser.parse_args() + + # ------------------ load YAML params & apply ------------------ + if not PARAM_YAML_PATH.exists(): + raise RuntimeError(f"Parameter YAML not found at {PARAM_YAML_PATH}") + with open(PARAM_YAML_PATH, "r") as f: + cfg = yaml.safe_load(f) or {} + loc2d_cfg = cfg.get("loc2d", {}) + set_loc2d_config(loc2d_cfg) + print(f"[CONFIG] Loaded localization parameters from {PARAM_YAML_PATH}") + print(f"[CONFIG] {loc2d_cfg}") + print(f"[CONFIG] Using PATCH_H={PATCH_H}, PATCH_W={PATCH_W}, STRIDE={STRIDE}") + + global DEBUG, DUMP_DIR + DEBUG = args.debug + DUMP_DIR = args.dump_dir + + # ----------------------------------------------------- + # Load external image or pixel_map if provided + # ----------------------------------------------------- + if args.pixel_map: + pm_path = Path(args.pixel_map) + if not pm_path.exists(): + raise RuntimeError(f"Failed to find pixel_map: {pm_path}") + data = np.load(str(pm_path), allow_pickle=True).item() + keys = list(data.keys()) + if len(keys) == 0: + raise RuntimeError(f"pixel_map {pm_path} contains no entries") + rows = [int(k[0]) for k in keys] + cols = [int(k[1]) for k in keys] + H_img = max(rows) + 1 + W_img = max(cols) + 1 + I_uint8 = np.zeros((H_img, W_img), dtype=np.uint8) + for (r, c), val in data.items(): + gray_val = int(val[0]) if isinstance(val, (tuple, list)) else int(val) + I_uint8[int(r), int(c)] = np.clip(gray_val, 0, 255) + img = I_uint8.astype(np.float32) / 255.0 + print(f"Loaded pixel_map {pm_path} -> reconstructed image shape {img.shape}") + # NOTE: PATCH_H/PATCH_W are NOT changed here; use YAML only. + + elif args.image: + img = cv2.imread(args.image, cv2.IMREAD_GRAYSCALE) + if img is None: + raise RuntimeError(f"Failed to load image: {args.image}") + img = img.astype(np.float32) / 255.0 + print(f"Loaded input image: {args.image}") + # NOTE: PATCH_H/PATCH_W remain as set by YAML/defaults. + + else: + # Attempt to load OFFLINE_MAP_PATH; it can be either a raw image array or a mapping dict + if OFFLINE_MAP_PATH.exists() and not args.new: + try: + loaded = np.load(str(OFFLINE_MAP_PATH), allow_pickle=True) + if isinstance(loaded, np.ndarray) and loaded.dtype != np.object_: + img = loaded.astype(np.float32) + if img.max() > 1.5: + img = img / 255.0 + print(f"Loaded offline raw image from {OFFLINE_MAP_PATH} shape={img.shape}") + else: + mapping = loaded.item() if hasattr(loaded, 'item') else dict(loaded) + img = reconstruct_image_from_mapping(mapping) + print(f"Loaded OFFLINE_MAP_PATH mapping and reconstructed image shape={img.shape}") + except Exception as e: + raise RuntimeError(f"Failed to load offline map from {OFFLINE_MAP_PATH}: {e}") + else: + raise RuntimeError( + "No offline map found. Provide --image or --pixel_map, or create offline_map.npy beforehand." + ) + + if args.sweep: + raise RuntimeError("Parameter sweep is disabled. Tune parameters in localization_params.yaml instead.") + + # ----------------------------------------------------- + # LBP-HF MODE + # ----------------------------------------------------- + if args.method == "lbp": + print(f"Using LBP: PATCH_H={PATCH_H}, PATCH_W={PATCH_W}, STRIDE={STRIDE}") + # try loading DB + if not args.image and OFFLINE_LBP_DB_PATH.exists() and not args.new: + try: + with open(OFFLINE_LBP_DB_PATH, "rb") as f: + lbp_db = pickle.load(f) + # quick compatibility check + descs, poses, nn = lbp_db + H, W = img.shape + compatible = True + if descs.ndim != 2 or descs.shape[1] != LBPHF_KEEP: + compatible = False + if compatible: + for (py, px) in poses: + if py + PATCH_H > H or px + PATCH_W > W: + compatible = False + break + if not compatible: + print("Offline LBP DB incompatible with current image/patch size — rebuilding.") + lbp_db = None + else: + print("Loaded LBP DB.") + except Exception: + print("Failed to load offline LBP DB; will rebuild from current image.") + lbp_db = None + else: + lbp_db = None + + if lbp_db is None: + descs, poses = compute_map_lbphf_descriptors(img) + nn = NearestNeighbors(n_neighbors=COARSE_TOPK, metric="cosine") + nn.fit(descs) + lbp_db = (descs, poses, nn) + if not args.image: + with open(OFFLINE_LBP_DB_PATH, "wb") as f: + pickle.dump(lbp_db, f) + print("Built LBP DB.") + + per_trial_errors = [] + failures = 0 + runtimes = [] + for t in range(args.trials): + start_t = time.time() + est, gt, query, dist = lbp_localize(img, lbp_db) + elapsed = time.time() - start_t + runtimes.append(elapsed) + if dist is None: + failures += 1 + per_trial_errors.append(np.nan) + else: + per_trial_errors.append(dist) + if args.trials == 1 and not args.nodisplay: + visualize_result(img, est, gt, query) + + if args.trials > 1: + errors = np.array(per_trial_errors, dtype=float) + successes = int(np.isfinite(errors).sum()) + mean_err = float(np.nanmean(errors)) if successes > 0 else float("nan") + std_err = float(np.nanstd(errors)) if successes > 0 else float("nan") + print(f"LBP Trials={args.trials}, Failures={failures}, Successful={successes}") + if successes > 0: + print(f"LBP Mean Error={mean_err:.2f}px, Std={std_err:.2f}px") + # Combined plot: per-trial error (left y) and runtime (right y) + try: + fig, ax1 = plt.subplots(figsize=(10, 4)) + trials_idx = np.arange(1, len(errors) + 1) + ax1.plot(trials_idx, errors, marker='o', color='C0', label='Error (px)') + ax1.set_xlabel('Trial') + ax1.set_ylabel('Error (px)', color='C0') + ax1.tick_params(axis='y', labelcolor='C0') + ax2 = ax1.twinx() + ax2.plot(trials_idx, runtimes, marker='s', linestyle='--', color='C1', label='Runtime (s)') + ax2.set_ylabel('Runtime (s)', color='C1') + ax2.tick_params(axis='y', labelcolor='C1') + mean_rt = float(np.mean(runtimes)) if runtimes else float("nan") + ax1.set_title(f"LBP per-trial Error & Runtime (mean_err={mean_err if not np.isnan(mean_err) else 'nan'} px, mean_rt={mean_rt:.3f}s)") + # combined legend + lines_1, labels_1 = ax1.get_legend_handles_labels() + lines_2, labels_2 = ax2.get_legend_handles_labels() + ax1.legend(lines_1 + lines_2, labels_1 + labels_2, loc='best') + outname_comb = "results_lbp_combined.png" + plt.tight_layout() + plt.savefig(outname_comb) + plt.close(fig) + print(f"Saved combined LBP plot: {outname_comb}") + except Exception as e: + print("Failed to save combined LBP plot:", e) + + # ----------------------------------------------------- + # SIFT MODE + # ----------------------------------------------------- + if args.method == "sift": + print(f"Using SIFT: PATCH_H={PATCH_H}, PATCH_W={PATCH_W}, STRIDE={STRIDE}") + if not args.image and OFFLINE_SIFT_DB_PATH.exists() and not args.new: + try: + with open(OFFLINE_SIFT_DB_PATH, "rb") as f: + loaded = pickle.load(f) + sift_db = reconstruct_sift_db(loaded) + print("Loaded SIFT DB.") + except Exception: + print("Failed to load offline SIFT DB; will rebuild from current image.") + sift_db = None + else: + sift_db = None + + if sift_db is None: + sift_db = build_sift_database(img) + if not args.image: + try: + with open(OFFLINE_SIFT_DB_PATH, "wb") as f: + pickle.dump(make_savable_sift_db(sift_db), f) + except Exception as _e: + print("Warning: failed to save SIFT DB to disk:", _e) + print("Built SIFT DB.") + + per_trial_errors = [] + failures = 0 + runtimes = [] + for t in range(args.trials): + start_t = time.time() + est, gt, query, dist = sift_localize(img, sift_db) + elapsed = time.time() - start_t + runtimes.append(elapsed) + if dist is None: + failures += 1 + per_trial_errors.append(np.nan) + else: + per_trial_errors.append(dist) + if args.trials == 1 and not args.nodisplay: + visualize_result(img, est, gt, query) + + if args.trials > 1: + errors = np.array(per_trial_errors, dtype=float) + successes = int(np.isfinite(errors).sum()) + mean_err = float(np.nanmean(errors)) if successes > 0 else float("nan") + std_err = float(np.nanstd(errors)) if successes > 0 else float("nan") + print(f"SIFT Trials={args.trials}, Failures={failures}, Successful={successes}") + if successes > 0: + print(f"SIFT Mean Error={mean_err:.2f}px, Std={std_err:.2f}px") + # Combined plot: per-trial error (left y) and runtime (right y) + try: + fig, ax1 = plt.subplots(figsize=(10, 4)) + trials_idx = np.arange(1, len(errors) + 1) + ax1.plot(trials_idx, errors, marker='o', color='C0', label='Error (px)') + ax1.set_xlabel('Trial') + ax1.set_ylabel('Error (px)', color='C0') + ax1.tick_params(axis='y', labelcolor='C0') + ax2 = ax1.twinx() + ax2.plot(trials_idx, runtimes, marker='s', linestyle='--', color='C1', label='Runtime (s)') + ax2.set_ylabel('Runtime (s)', color='C1') + ax2.tick_params(axis='y', labelcolor='C1') + mean_rt = float(np.mean(runtimes)) if runtimes else float("nan") + ax1.set_title(f"SIFT per-trial Error & Runtime (mean_err={mean_err if not np.isnan(mean_err) else 'nan'} px, mean_rt={mean_rt:.3f}s)") + lines_1, labels_1 = ax1.get_legend_handles_labels() + lines_2, labels_2 = ax2.get_legend_handles_labels() + ax1.legend(lines_1 + lines_2, labels_1 + labels_2, loc='best') + outname_comb = "results_sift_combined.png" + plt.tight_layout() + plt.savefig(outname_comb) + plt.close(fig) + print(f"Saved combined SIFT plot: {outname_comb}") + except Exception as e: + print("Failed to save combined SIFT plot:", e) + + +if __name__ == "__main__": + main() + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/main.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/main.py new file mode 100644 index 0000000..6ec55ad --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/main.py @@ -0,0 +1,526 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import os +import pickle +import numpy as np +import rospy + +from pathlib import Path + +from sensor_msgs.msg import PointCloud2, LaserScan +from nav_msgs.msg import Odometry +import sensor_msgs.point_cloud2 as pc2 + +import tf2_ros +import tf_conversions +from geometry_msgs.msg import TransformStamped + +import open3d as o3d + +from wall_localization_demo.localization_core import ( + lbp_localize_query, + sift_localize_query, +) + +def reconstruct_image_from_mapping(mapping): + """ + mapping: dict with keys (row,col) -> (gray_value, point_indices_array) + returns: float32 image in [0,1] and the mapping itself + """ + if not mapping: + raise ValueError("Empty mapping provided.") + keys = list(mapping.keys()) + rows = [int(k[0]) for k in keys] + cols = [int(k[1]) for k in keys] + H = max(rows) + 1 + W = max(cols) + 1 + img8 = np.zeros((H, W), dtype=np.uint8) + for (r, c), val in mapping.items(): + gray = val[0] if isinstance(val, (tuple, list)) else val + img8[int(r), int(c)] = np.clip(int(gray), 0, 255) + return img8.astype(np.float32) / 255.0, mapping + + +def pc2_to_numpy(msg, fields=("x", "y", "z")): + """Convert PointCloud2 to (N,3) numpy array.""" + pts = [] + for p in pc2.read_points(msg, field_names=fields, skip_nans=True): + pts.append([p[0], p[1], p[2]]) + if not pts: + return np.empty((0, 3), dtype=np.float32) + return np.asarray(pts, dtype=np.float32) + + +def transform_points(transform, pts): + """Apply geometry_msgs/TransformStamped to Nx3 points.""" + if pts.size == 0: + return pts + + t = transform.transform.translation + q = transform.transform.rotation + T = tf_conversions.transformations.quaternion_matrix([q.x, q.y, q.z, q.w]) + T[0, 3] = t.x + T[1, 3] = t.y + T[2, 3] = t.z + + pts_h = np.hstack([pts, np.ones((pts.shape[0], 1))]) + pts_tf = (T @ pts_h.T).T + return pts_tf[:, :3] + + +class ScannerPoseEstimator(object): + def __init__(self): + # localization params are now only via ROS params / YAML; no set_loc2d_config + self.loc2d_params = rospy.get_param("~loc2d", {}) + + # ---------- parameters ---------- + # Offline data + self.offline_map_npy = rospy.get_param("~offline_map_npy", "offline_map.npy") + self.offline_points_npy = rospy.get_param( + "~offline_points_npy", "offline_points.npy" + ) + self.offline_lbp_db_pkl = rospy.get_param( + "~offline_lbp_db_pkl", "offline_lbp_db.pkl" + ) + self.offline_sift_db_pkl = rospy.get_param( + "~offline_sift_db_pkl", "offline_sift_db.pkl" + ) + + # Frames + self.wall_frame = rospy.get_param("~wall_frame", "wall") + self.scanner_frame = rospy.get_param("~scanner_frame", "vertical_profiler_link") + self.base_frame = rospy.get_param("~base_frame", "base_link") + self.icp_child_frame = rospy.get_param( + "~icp_child_frame", "scanner_icp_refined" + ) + + # Live patch extents (in wall frame) + self.s_min_live = float(rospy.get_param("~s_min_live", -1.0)) + self.s_max_live = float(rospy.get_param("~s_max_live", 1.0)) + self.z_min_live = float(rospy.get_param("~z_min_live", 0.0)) + self.z_max_live = float(rospy.get_param("~z_max_live", 0.6)) + self.live_patch_s_length = float( + rospy.get_param("~live_patch_s_length", 0.20) + ) # m + + # Method selection: "lbp", "sift" or "both" + self.method = rospy.get_param("~method", "lbp") + + # ICP params + self.icp_max_dist = float(rospy.get_param("~icp_max_dist", 0.05)) # m + self.icp_max_iter = int(rospy.get_param("~icp_max_iter", 50)) + + # Live image resolution (patch size for online localization) + self.live_img_h = int(rospy.get_param("~live_img_h", 11)) # default matches YAML + self.live_img_w = int(rospy.get_param("~live_img_w", 6)) + + # ---------- TF ---------- + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + + # ---------- load offline map ---------- + rospy.loginfo("Loading offline map from %s", self.offline_map_npy) + mapping = np.load(self.offline_map_npy, allow_pickle=True).item() + self.offline_img, self.pixel_to_indices = reconstruct_image_from_mapping( + mapping + ) + self.offline_img = self.offline_img.astype(np.float32) + + # Try to load offline 3D points (same indexing as mapping values) + if os.path.exists(self.offline_points_npy): + rospy.loginfo("Loading offline points from %s", self.offline_points_npy) + self.offline_points = np.load(self.offline_points_npy) + else: + rospy.logwarn( + "Offline points file %s not found. ICP will use approximate pixel grid.", + self.offline_points_npy, + ) + self.offline_points = None + + # ---------- build / load LBP database ---------- + if os.path.exists(self.offline_lbp_db_pkl): + rospy.loginfo("Loading LBP-HF DB from %s", self.offline_lbp_db_pkl) + with open(self.offline_lbp_db_pkl, "rb") as f: + self.lbp_db = pickle.load(f) + else: + rospy.logwarn("LBP DB file %s not found; online node expects it to exist.", self.offline_lbp_db_pkl) + self.lbp_db = None + + # ---------- build / load SIFT database ---------- + if os.path.exists(self.offline_sift_db_pkl): + rospy.loginfo("Loading SIFT DB from %s", self.offline_sift_db_pkl) + with open(self.offline_sift_db_pkl, "rb") as f: + self.sift_db = pickle.load(f) + else: + rospy.logwarn("SIFT DB file %s not found; SIFT mode will be disabled.", self.offline_sift_db_pkl) + self.sift_db = None + + # ---------- live data buffers ---------- + self.acc_pts_wall = [] # list of (N,3) in wall frame + + # ---------- subscribers ---------- + pc_topic = rospy.get_param("~pointcloud_topic", "/scan_cloud") + odom_topic = rospy.get_param("~odom_topic", "/odom") + + self.scan_sub = rospy.Subscriber(pc_topic, LaserScan, self.scan_callback) + self.odom_sub = rospy.Subscriber( + odom_topic, Odometry, self.odom_cb, queue_size=5 + ) + + self.last_odom = None + + rospy.loginfo("ScannerPoseEstimator initialized.") + + # ------------------------------------------------------------------ + # Helper: LaserScan -> Nx3 points (scanner frame) + # ------------------------------------------------------------------ + def laserscan_to_points(self, scan_msg): + """Convert LaserScan to Nx3 array in the scanner frame.""" + # angles and ranges + angles = scan_msg.angle_min + np.arange(len(scan_msg.ranges)) * scan_msg.angle_increment + ranges = np.array(scan_msg.ranges, dtype=np.float64) + + # filter valid ranges + mask = np.isfinite(ranges) & (ranges >= scan_msg.range_min) & (ranges <= scan_msg.range_max) + if not np.any(mask): + return np.empty((0, 3), dtype=np.float32) + + ranges = ranges[mask] + angles = angles[mask] + + xs = ranges * np.cos(angles) + ys = ranges * np.sin(angles) + zs = np.zeros_like(xs) + + pts = np.stack([xs, ys, zs], axis=1).astype(np.float32) # (N,3) + return pts + + # ------------------------------------------------------------------ + # Odometry callback (stored mainly for debugging / future use) + # ------------------------------------------------------------------ + def odom_cb(self, msg): + self.last_odom = msg + + # ------------------------------------------------------------------ + # Scan callback: accumulate patch, localize, run ICP + # ------------------------------------------------------------------ + def scan_callback(self, msg): + # msg is now a LaserScan + pts = self.laserscan_to_points(msg) + if pts.shape[0] == 0: + return + + # Replace any previous PointCloud2->array conversion with pts directly. + cloud_xyz = pts + + # 1) Convert to numpy in scanner frame + pts_scanner = cloud_xyz + if pts_scanner.size == 0: + return + + # 2) Transform to wall frame + try: + tf_s_w = self.tf_buffer.lookup_transform( + self.wall_frame, + self.scanner_frame, + msg.header.stamp, + rospy.Duration(0.2), + ) + except Exception as e: + rospy.logwarn("TF lookup failed: %s", str(e)) + return + + pts_wall = transform_points(tf_s_w, pts_scanner) + + # 3) Filter by height and lateral range in wall frame. + # Assumption: wall x-axis = along layer, y-axis = normal, z-axis = up. + s_vals = pts_wall[:, 0] # "along-wall" coordinate + z_vals = pts_wall[:, 2] # height + + mask = ( + (s_vals >= self.s_min_live) + & (s_vals <= self.s_max_live) + & (z_vals >= self.z_min_live) + & (z_vals <= self.z_max_live) + ) + + pts_wall = pts_wall[mask] + if pts_wall.size == 0: + return + + self.acc_pts_wall.append(pts_wall) + + # 4) Check accumulated length along s + all_pts = np.vstack(self.acc_pts_wall) + s_all = all_pts[:, 0] + + s_range = s_all.max() - s_all.min() + if s_range < self.live_patch_s_length: + # not enough coverage yet + return + + # Define patch window [s0, s1] + s0 = s_all.min() + s1 = s0 + self.live_patch_s_length + win_mask = (s_all >= s0) & (s_all <= s1) + live_pts_wall = all_pts[win_mask] + + # Once a patch is built, clear accumulator for the next patch + self.acc_pts_wall = [] + + # 5) Convert live points to (s,z,d) for image construction + s_live = live_pts_wall[:, 0] + z_live = live_pts_wall[:, 2] + d_live = live_pts_wall[:, 1] # wall-normal offset + + # Discretization + Delta_s = self.live_patch_s_length / float(self.live_img_w) + Delta_z = (self.z_max_live - self.z_min_live) / float(self.live_img_h) + + # Assign to pixels + u = np.floor((s_live - s0) / Delta_s).astype(int) + v = np.floor((z_live - self.z_min_live) / Delta_z).astype(int) + + # Clamp to image bounds + u = np.clip(u, 0, self.live_img_w - 1) + v = np.clip(v, 0, self.live_img_h - 1) + + # Aggregate depths per pixel + depth_bins = {} + for ui, vi, di in zip(u, v, d_live): + key = (vi, ui) # row, col + if key not in depth_bins: + depth_bins[key] = [] + depth_bins[key].append(di) + + I_raw = np.zeros((self.live_img_h, self.live_img_w), dtype=np.float32) + I_raw[:] = 0.0 + + for (vi, ui), vals in depth_bins.items(): + I_raw[vi, ui] = np.median(vals) + + # Normalize depths to [0,255] + nonzero = I_raw != 0 + if np.any(nonzero): + vals = I_raw[nonzero] + I_min = np.percentile(vals, 5) + I_max = np.percentile(vals, 95) + if I_max > I_min: + I_norm = (I_raw - I_min) / (I_max - I_min) + else: + I_norm = np.zeros_like(I_raw) + else: + I_norm = np.zeros_like(I_raw) + + patch_img = (255.0 * np.clip(I_norm, 0.0, 1.0)).astype(np.uint8) + + # ------------------------------------------------------------------ + # NEW: Cropping logic to enforce patch size consistency + # ------------------------------------------------------------------ + H_live, W_live = patch_img.shape + + # use configured patch size + patch_h = self.live_img_h + patch_w = self.live_img_w + + if H_live < patch_h or W_live < patch_w: + rospy.logwarn( + "Live patch (%d × %d) is smaller than required (%d × %d). Skipping patch.", + H_live, W_live, patch_h, patch_w, + ) + return + + # Center-crop to match offline patch size + h0 = (H_live - patch_h) // 2 + w0 = (W_live - patch_w) // 2 + patch_img = patch_img[h0:h0 + patch_h, w0:w0 + patch_w] + + # Optional: log cropping for debugging + rospy.loginfo( + "Cropped live patch from (%d × %d) to (%d × %d)", + H_live, W_live, patch_h, patch_w + ) + + rospy.loginfo("Built live patch image, starting 2D localization...") + + match_pos = None + + if self.method in ("lbp", "both") and self.lbp_db is not None: + try: + est_lbp = lbp_localize_query( + patch_img.astype(np.float32) / 255.0, + self.offline_img, + self.lbp_db, + ) + if est_lbp is not None: + match_pos = est_lbp + rospy.loginfo("LBP-HF match at (row=%d, col=%d)", est_lbp[0], est_lbp[1]) + except Exception as e: + rospy.logwarn("LBP localization failed: %s", str(e)) + + if self.method in ("sift", "both") and self.sift_db is not None: + try: + est_sift = sift_localize_query( + patch_img.astype(np.float32) / 255.0, + self.offline_img, + self.sift_db, + ) + if est_sift is not None: + match_pos = est_sift + rospy.loginfo("SIFT match at (row=%d, col=%d)", est_sift[0], est_sift[1]) + except Exception as e: + rospy.logwarn("SIFT localization failed: %s", str(e)) + + if match_pos is None: + rospy.logwarn("No successful 2D localization for this patch.") + return + + self.run_icp_and_report_pose(live_pts_wall, match_pos, msg.header.stamp) + + # ------------------------------------------------------------------ + def collect_offline_patch_points(self, match_pos): + """ + Collect 3D points from offline map corresponding to the matched patch. + match_pos: (row, col) of top-left corner of patch in offline_img. + """ + row0, col0 = match_pos + patch_h = self.live_img_h + patch_w = self.live_img_w + rows = range(row0, row0 + patch_h) + cols = range(col0, col0 + patch_w) + + idxs = [] + for r in rows: + for c in cols: + key = (int(r), int(c)) + if key in self.pixel_to_indices: + _, point_indices = self.pixel_to_indices[key] + idxs.extend(point_indices.tolist()) + + if not idxs: + rospy.logwarn("No offline points found for matched patch; using grid proxy.") + return None + + idxs = np.unique(np.asarray(idxs, dtype=int)) + + if self.offline_points is None: + return None + + pts = self.offline_points[idxs] + return pts + + # ------------------------------------------------------------------ + def run_icp_and_report_pose(self, live_pts_wall, match_pos, stamp): + """ + Align live patch (in wall frame) with offline patch using ICP and + publish refined scanner pose relative to the wall. + """ + # Collect offline points + offline_pts = self.collect_offline_patch_points(match_pos) + + if offline_pts is None or offline_pts.shape[0] < 20: + # Fallback: approximate patch points from pixel grid + r0, c0 = match_pos + H, W = self.offline_img.shape + patch_h = self.live_img_h + patch_w = self.live_img_w + r1 = min(r0 + patch_h, H) + c1 = min(c0 + patch_w, W) + ys, xs = np.mgrid[r0:r1, c0:c1] + offline_pts = np.stack( + [xs.ravel().astype(np.float32), np.zeros(xs.size), ys.ravel().astype(np.float32)], + axis=1, + ) + + if live_pts_wall.shape[0] < 20: + rospy.logwarn("Too few live points for ICP.") + return + + # Build Open3D point clouds in wall frame + src = o3d.geometry.PointCloud() + dst = o3d.geometry.PointCloud() + src.points = o3d.utility.Vector3dVector(live_pts_wall.astype(np.float64)) + dst.points = o3d.utility.Vector3dVector(offline_pts.astype(np.float64)) + + threshold = self.icp_max_dist + trans_init = np.eye(4) + + reg = o3d.pipelines.registration.registration_icp( + src, + dst, + threshold, + trans_init, + o3d.pipelines.registration.TransformationEstimationPointToPoint(), + o3d.pipelines.registration.ICPConvergenceCriteria( + max_iteration=self.icp_max_iter + ), + ) + + T_icp = reg.transformation # wall->wall correction + + # Current TF-based scanner pose in wall frame + try: + tf_w_s = self.tf_buffer.lookup_transform( + self.wall_frame, + self.scanner_frame, + stamp, + rospy.Duration(0.2), + ) + except Exception as e: + rospy.logwarn( + "TF lookup for scanner pose during ICP reporting failed: %s", str(e) + ) + return + + # Convert TF to matrix + t = tf_w_s.transform.translation + q = tf_w_s.transform.rotation + T_ws = tf_conversions.transformations.quaternion_matrix( + [q.x, q.y, q.z, q.w] + ) + T_ws[0, 3] = t.x + T_ws[1, 3] = t.y + T_ws[2, 3] = t.z + + # Refined scanner pose: apply ICP correction + T_ws_refined = T_icp @ T_ws + + # Publish as TF + T = T_ws_refined + trans = TransformStamped() + trans.header.stamp = stamp + trans.header.frame_id = self.wall_frame + trans.child_frame_id = self.icp_child_frame + trans.transform.translation.x = T[0, 3] + trans.transform.translation.y = T[1, 3] + trans.transform.translation.z = T[2, 3] + + q_ref = tf_conversions.transformations.quaternion_from_matrix(T) + trans.transform.rotation.x = q_ref[0] + trans.transform.rotation.y = q_ref[1] + trans.transform.rotation.z = q_ref[2] + trans.transform.rotation.w = q_ref[3] + + self.tf_broadcaster.sendTransform(trans) + + rospy.loginfo( + "ICP refined scanner pose: trans = (%.3f, %.3f, %.3f)", + T[0, 3], + T[1, 3], + T[2, 3], + ) + + # ------------------------------------------------------------------ + + +def main(): + rospy.init_node("scanner_pose_estimator") + node = ScannerPoseEstimator() + rospy.loginfo("ScannerPoseEstimator node running.") + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/move_along_wall.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/move_along_wall.py new file mode 100644 index 0000000..8b80cb2 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/move_along_wall.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import rospy +from geometry_msgs.msg import Twist + +def main(): + rospy.init_node("move_along_wall") + pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) + rate = rospy.Rate(20) + + speed = rospy.get_param("~speed", 0.05) # m/s + duration = rospy.get_param("~duration", 60.0) # s + + twist = Twist() + twist.linear.x = speed + twist.angular.z = 0.0 + + t0 = rospy.Time.now().to_sec() + rospy.loginfo("Starting forward motion for %.1f s", duration) + + while not rospy.is_shutdown(): + t = rospy.Time.now().to_sec() - t0 + if t > duration: + break + pub.publish(twist) + rate.sleep() + + # Stop + twist.linear.x = 0.0 + pub.publish(twist) + rospy.loginfo("Stopping robot.") + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/pcl2texture.py b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/pcl2texture.py new file mode 100644 index 0000000..7933627 --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/src/wall_localization_demo/pcl2texture.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python3 +""" +pcl2texture.py — Build the offline texture map and databases +-------------------------------------------------------------- +This script implements exactly the mathematical procedure described +in Section 4.2 “Building the Offline Database”. + +Inputs: + --cloud 3D points (x,y,z) + --midpath mid-path waypoints (x,y,z) + --delta_s horizontal bin size + --delta_z vertical bin size + --out_map + --out_points + +Outputs: + offline_map.npy — dict[(row,col)] = (gray_value, np.ndarray(point_indices)) + offline_points.npy — 3D points array (N,3) + +This output format is fully compatible with: + - localization_2d.py (SIFT / LBP matching) + - main.py (online localization + ICP) +""" + +import argparse +import numpy as np +from scipy.spatial import cKDTree +import pickle +import sys + +def reconstruct_image_from_mapping(mapping): + # mapping[(row, col)] = (gray, idx_array) + rows = [r for (r, c) in mapping.keys()] + cols = [c for (r, c) in mapping.keys()] + H = max(rows) + 1 + W = max(cols) + 1 + img = np.zeros((H, W), dtype=np.uint8) + for (r, c), (gray, _) in mapping.items(): + img[r, c] = np.uint8(gray) + return img + +# -------------------------------------------------------------------------- +# Utility: project point to polyline (mid-path) +# -------------------------------------------------------------------------- + +def proj_point_to_polyline(px, py, Q): + """ + Project point (px,py) onto the polyline Q (Nx2 array). + Returns: + s_star : arc-length coordinate + idx : index of segment used + """ + min_dist = 1e12 + best_s = 0.0 + best_idx = 0 + + # Precompute segment lengths and cumulative arc-lengths + seg = Q[1:] - Q[:-1] # (#seg, 2) + seg_len = np.linalg.norm(seg, axis=1) + cum = np.concatenate(([0], np.cumsum(seg_len))) + + for i in range(len(seg)): + A = Q[i] + B = Q[i+1] + AB = B - A + AB2 = AB.dot(AB) + if AB2 < 1e-12: + continue + + t = ((px - A[0]) * AB[0] + (py - A[1]) * AB[1]) / AB2 + t_clamped = max(0.0, min(1.0, t)) + proj = A + t_clamped * AB + dist = (proj[0] - px)**2 + (proj[1] - py)**2 + + if dist < min_dist: + min_dist = dist + best_idx = i + best_s = cum[i] + t_clamped * seg_len[i] + + return best_s, best_idx + + +# -------------------------------------------------------------------------- +# Compute tangent and normal along mid-path +# -------------------------------------------------------------------------- +def compute_tangent_normals(Q): + seg = Q[1:] - Q[:-1] + T = seg / (np.linalg.norm(seg, axis=1, keepdims=True) + 1e-12) + # replicate last tangent for convenience + T = np.vstack([T, T[-1]]) + # Horizontal normal (rotate +90°) + N = np.zeros_like(T) + N[:,0] = -T[:,1] + N[:,1] = T[:,0] + return T, N + + +# -------------------------------------------------------------------------- +# MAIN BUILD FUNCTION +# -------------------------------------------------------------------------- +def build_offline_database(cloud, Q3d, delta_s, delta_z): + """ + cloud: (N,3) array + Q3d: (M,3) array mid-path waypoints + Output: (map_uint8, mapping_dict) + """ + # Mid-path projection only uses XY + Q = Q3d[:, :2] + + # Precompute arc-lengths + seg = Q[1:] - Q[:-1] + seg_len = np.linalg.norm(seg, axis=1) + cum_s = np.concatenate(([0], np.cumsum(seg_len))) + L = cum_s[-1] + + # Precompute tangents + normals + T, N = compute_tangent_normals(Q) + + N_pts = cloud.shape[0] + s_star = np.zeros(N_pts) + z_vals = cloud[:,2] + depth = np.zeros(N_pts) + + # Project each point to mid-path (with progress) + print("[BUILD] projecting points to mid-path...") + last_pct = -1 + for i, (x,y,z) in enumerate(cloud): + s_i, idx = proj_point_to_polyline(x, y, Q) + s_star[i] = s_i + + # vector from mid-path point to p + if idx >= len(Q3d): + idx = len(Q3d)-1 + mp = Q3d[idx] + r = cloud[i] - mp + + # depth = r · n_hat + depth[i] = r[0]*N[idx,0] + r[1]*N[idx,1] + + # progress every 1% + pct = int((i + 1) * 100 / N_pts) + if pct != last_pct and pct % 1 == 0: + sys.stdout.write(f"\r {pct:3d}%") + sys.stdout.flush() + last_pct = pct + sys.stdout.write("\n") + + # Determine bin extents + s_min = 0.0 + s_max = L + z_min = np.percentile(z_vals, 5) + z_max = np.percentile(z_vals, 95) + + Ns = int(np.floor((s_max - s_min) / delta_s)) + Nz = int(np.floor((z_max - z_min) / delta_z)) + + # Create mapping structure + mapping = {} # (row,col): (gray, indices_array) + + # First pass: assign points to bins + bins = {} + for i in range(N_pts): + si = s_star[i] + zi = z_vals[i] + ui = int((si - s_min) / delta_s) + vi = int((zi - z_min) / delta_z) + + if ui < 0 or ui >= Ns or vi < 0 or vi >= Nz: + continue + + if (ui,vi) not in bins: + bins[(ui,vi)] = [] + bins[(ui,vi)].append(i) + + # Compute raw depths, median per pixel + raw = np.full((Nz, Ns), np.nan, dtype=float) + + for (ui,vi), indices in bins.items(): + dvals = depth[indices] + raw[vi, ui] = np.median(dvals) + + # Percentile normalization (clip extremes) + valid = raw[~np.isnan(raw)] + pmin = np.percentile(valid, 5) + pmax = np.percentile(valid, 95) + + img = np.zeros_like(raw) + img[:] = 0 # initialize + + for (ui,vi), indices in bins.items(): + d = raw[vi,ui] + d_norm = (d - pmin) / (pmax - pmin + 1e-12) + gray = int(np.clip(255 * d_norm, 0, 255)) + img[vi,ui] = gray + + # NOTE: use (row, col) = (vi, ui), and store as (gray, indices_array) + mapping[(vi,ui)] = (gray, np.array(indices, dtype=int)) + + return img.astype(np.uint8), mapping + + +# -------------------------------------------------------------------------- +# ENTRY POINT +# -------------------------------------------------------------------------- +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--cloud", default="../data/mapping_run/wall_points_bag.csv", required=True) + parser.add_argument("--midpath", default="../data/mapping_run/wall_midpath_bag.csv", required=True) + parser.add_argument("--delta_s", type=float, default=0.005, required=True) + parser.add_argument("--delta_z", type=float, default=0.005, required=True) + parser.add_argument("--out_map", default="../data/mapping_run/offline_map.npy") + parser.add_argument("--out_points", default="../data/mapping_run/offline_points.npy") + args = parser.parse_args() + + print("[LOAD] cloud:", args.cloud) + cloud = np.loadtxt(args.cloud, delimiter=",") + + print("[LOAD] midpath:", args.midpath) + mid = np.loadtxt(args.midpath, delimiter=",") + + print("[BUILD] constructing offline database …") + map_img, mapping = build_offline_database( + cloud, mid, + delta_s=args.delta_s, + delta_z=args.delta_z + ) + + # Save mapping dict as offline_map.npy + print("[SAVE] map (mapping dict) →", args.out_map) + np.save(args.out_map, mapping) + + # Save full 3D cloud as offline_points.npy + print("[SAVE] points (3D cloud) →", args.out_points) + np.save(args.out_points, cloud) + + print("Done.") + + +if __name__ == "__main__": + main() diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/urdf/robot_with_scanner.urdf.xacro b/student_code/251127_print_texture_localization/wall_localization_demo/urdf/robot_with_scanner.urdf.xacro new file mode 100644 index 0000000..05bb18e --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/urdf/robot_with_scanner.urdf.xacro @@ -0,0 +1,158 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + rear_left_joint + rear_right_joint + ${2*half_width} + ${2*wheel_radius} + base_link + cmd_vel + odom + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 30 + true + + + + + + 300 + 1 + -0.25 + 0.25 + + + + + 0.1 + 6.0 + 0.001 + + + + + /scan_cloud + vertical_profiler_link + + + + + + + diff --git a/student_code/251127_print_texture_localization/wall_localization_demo/worlds/textured_wall.world b/student_code/251127_print_texture_localization/wall_localization_demo/worlds/textured_wall.world new file mode 100644 index 0000000..dc6ac8c --- /dev/null +++ b/student_code/251127_print_texture_localization/wall_localization_demo/worlds/textured_wall.world @@ -0,0 +1,22 @@ + + + + + + + model://ground_plane + + + + model://sun + + + + model://printed_wall + printed_wall + 5 1.5 0 0 0 3.14159 + + + + +