yoi-hibino · March 16, 2025 02:37
diff --git a/lidar_locator.py b/lidar_locator.py
 import numpy as np
 import open3d as o3d
 import matplotlib.pyplot as plt
 import transforms3d as t3d
 import trimesh
 import scipy.spatial

 class SensorLocalizer:
    def __init__(self, cad_model_path, scale_factor=1.0):
        """
        Initialize the localizer with a 3D CAD model
        
        Args:
            cad_model_path: Path to the 3D CAD model file (OBJ, STL, etc.)
            scale_factor: Scale factor to convert CAD model units to real-world meters
                         (e.g., if CAD is in mm and real world is in m, use 0.001)
        """
        self.cad_model = trimesh.load(cad_model_path)
        self.scale_factor = scale_factor
        
        # Apply scale factor to the model if needed
        if scale_factor != 1.0:
            self.cad_model.apply_scale(scale_factor)
            
        # Generate arbitrary slices of the model
        self.reference_slices = self._extract_arbitrary_slices()
        
    def _extract_arbitrary_slices(self, num_slices=20):
        """
        Extract multiple 2D slices from the 3D CAD model at different orientations
        
        Args:
            num_slices: Number of different orientations to try
            
        Returns:
            Dictionary mapping orientation parameters to 2D point arrays
        """
        reference_slices = {}
        
        # Fixed height for central slicing
        center_z = self.cad_model.bounds.mean(axis=0)[2]
        
        # Generate a range of roll and pitch angles to try
        # We'll use a Fibonacci sphere to distribute points evenly on a hemisphere
        angles = []
        
        # Simple approach: grid of roll/pitch angles
        roll_angles = np.linspace(-np.pi/6, np.pi/6, int(np.sqrt(num_slices)))  # ±30 degrees
        pitch_angles = np.linspace(-np.pi/6, np.pi/6, int(np.sqrt(num_slices)))  # ±30 degrees
        
        for roll in roll_angles:
            for pitch in pitch_angles:
                angles.append((roll, pitch))
        
        # Try each orientation
        for roll, pitch in angles:
            # Create rotation matrix for this orientation
            R = t3d.euler.euler2mat(roll, pitch, 0, 'sxyz')
            
            # Normal vector for the slicing plane (rotated z-axis)
            normal = R @ np.array([0, 0, 1])
            
            # Use the center of the model as the plane origin
            origin = self.cad_model.bounds.mean(axis=0)
            
            try:
                # Create a cross-section with this orientation
                slice_path = self.cad_model.section(
                    plane_origin=origin,
                    plane_normal=normal
                )
                
                if slice_path is not None:
                    # Convert the slice to points
                    points = []
                    for entity in slice_path.entities:
                        for idx in entity.points:
                            points.append(slice_path.vertices[idx])
                    
                    if points:
                        # Store orientation parameters with the slice
                        orientation = (roll, pitch, origin)
                        reference_slices[orientation] = np.array(points)
            except Exception as e:
                print(f"Failed to extract slice at roll={np.degrees(roll):.1f}°, pitch={np.degrees(pitch):.1f}°: {e}")
        
        if not reference_slices:
            raise ValueError("No valid slices found in the CAD model")
            
        return reference_slices
    
    def process_lidar_scan(self, ranges, angles, imu_roll, imu_pitch, imu_yaw):
        """
        Process a 2D LiDAR scan and correct it using IMU data
        
        Args:
            ranges: Array of range measurements from LiDAR
            angles: Array of corresponding angles for each range
            imu_roll: Roll angle from IMU in radians
            imu_pitch: Pitch angle from IMU in radians
            imu_yaw: Yaw angle from IMU in radians
            
        Returns:
            3D point cloud after IMU correction
        """
        # Convert polar coordinates to 3D Cartesian in LiDAR frame
        # Initially, points are in x-y plane (z=0) from LiDAR's perspective
        x = ranges * np.cos(angles)
        y = ranges * np.sin(angles)
        z = np.zeros_like(x)
        
        # Create 3D point cloud
        points = np.column_stack((x, y, z))
        
        # Create rotation matrix from IMU data
        R = t3d.euler.euler2mat(imu_roll, imu_pitch, imu_yaw, 'sxyz')
        
        # Apply rotation to correct for sensor attitude
        # Now points are in the world frame with proper orientation
        corrected_points = np.dot(points, R.T)
        
        # Return full 3D points to handle non-horizontal LiDAR
        return corrected_points
    
    def estimate_position(self, scan_points_3d, initial_guess=None, max_iterations=50):
        """
        Estimate sensor position by matching raw LiDAR scan to arbitrarily sliced CAD model
        
        Args:
            scan_points_3d: 3D points from processed LiDAR scan
            initial_guess: Initial [x, y, z, roll, pitch, yaw] guess, if available
            max_iterations: Maximum ICP iterations
            
        Returns:
            Estimated [x, y, z, roll, pitch, yaw] position of the sensor in the CAD model
            (position values will be in the same units as your LiDAR data)
        """
        if initial_guess is None:
            # Start with a guess at the center of the map with no rotation
            initial_guess = [0, 0, 1.0, 0, 0, 0]
        
        best_error = float('inf')
        best_position = initial_guess
        best_orientation = None
        
        # Convert scan points to a more efficient representation for matching
        scan_pcd = o3d.geometry.PointCloud()
        scan_pcd.points = o3d.utility.Vector3dVector(scan_points_3d)
        
        # Try matching with each reference slice at different orientations
        for orientation, slice_points in self.reference_slices.items():
            roll, pitch, origin = orientation
            
            # Convert to Open3D format for efficient matching
            slice_pcd = o3d.geometry.PointCloud()
            slice_pcd.points = o3d.utility.Vector3dVector(slice_points)
            
            # Create initial pose guess incorporating this orientation
            ori_initial_guess = initial_guess.copy()
            ori_initial_guess[3:5] = [roll, pitch]  # Use this slice's orientation
            
            # Match at this orientation
            position, error = self._match_with_slice(scan_pcd, slice_pcd, ori_initial_guess, orientation, max_iterations)
            
            if error < best_error:
                best_error = error
                best_position = position
                best_orientation = orientation
        
        if best_orientation is None:
            raise ValueError("Could not find a good match with any orientation")
            
        best_roll, best_pitch, _ = best_orientation
        print(f"Best match found at orientation: roll={np.degrees(best_roll):.2f}°, pitch={np.degrees(best_pitch):.2f}°")
        return best_position, best_error
    
    def _match_with_slice(self, scan_pcd, slice_pcd, initial_guess, orientation, max_iterations):
        """
        Match scan points with a specific oriented slice
        
        Args:
            scan_pcd: Open3D PointCloud of the scan
            slice_pcd: Open3D PointCloud of the slice
            initial_guess: Initial pose guess [x, y, z, roll, pitch, yaw]
            orientation: (roll, pitch, origin) of the slice
            max_iterations: Maximum ICP iterations
            
        Returns:
            Estimated [x, y, z, roll, pitch, yaw] and error
        """
        # Extract orientation parameters
        slice_roll, slice_pitch, slice_origin = orientation
        
        # Create initial transformation from initial guess
        x, y, z, roll, pitch, yaw = initial_guess
        
        # Combine the slice orientation with the yaw component of the initial guess
        # This creates a transformation that aligns the scan with the slicing plane
        R_slice = t3d.euler.euler2mat(slice_roll, slice_pitch, yaw, 'sxyz')
        T_slice = np.eye(4)
        T_slice[:3, :3] = R_slice
        T_slice[:3, 3] = [x, y, z]
        
        # Convert numpy transformation to Open3D format
        init_transform = o3d.geometry.TransformationMatrix(T_slice)
        
        # Run point-to-point ICP
        result = o3d.pipelines.registration.registration_icp(
            scan_pcd, slice_pcd, 0.2,  # max_correspondence_distance
            init_transform.get_matrix(),
            o3d.pipelines.registration.TransformationEstimationPointToPoint(),
            o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=max_iterations)
        )
        
        # Extract the transformation matrix
        transformation = result.transformation
        
        # Convert back to our 6-DOF representation [x, y, z, roll, pitch, yaw]
        translation = transformation[:3, 3]
        rotation_matrix = transformation[:3, :3]
        
        # Extract Euler angles (might need refinement for gimbal lock cases)
        angles = t3d.euler.mat2euler(rotation_matrix, 'sxyz')
        
        # Final pose estimation
        estimated_pose = [translation[0], translation[1], translation[2], angles[0], angles[1], angles[2]]
        
        return estimated_pose, result.inlier_rmse
    
    def visualize_result(self, scan_points_3d, estimated_position, best_orientation=None):
        """
        Visualize the localization result
        
        Args:
            scan_points_3d: Original 3D scan points
            estimated_position: Estimated [x, y, z, roll, pitch, yaw] position
            best_orientation: The orientation parameters of the best matching slice
        """
        x, y, z, roll, pitch, yaw = estimated_position
        
        # Create full 3D transformation matrix from estimated position
        R = t3d.euler.euler2mat(roll, pitch, yaw, 'sxyz')
        transformation_3d = np.eye(4)
        transformation_3d[:3, :3] = R
        transformation_3d[:3, 3] = [x, y, z]
        
        # Apply transformation to 3D scan points
        scan_points_hom = np.column_stack((scan_points_3d, np.ones(len(scan_points_3d))))
        transformed_points_3d = np.dot(scan_points_hom, transformation_3d.T)[:, :3]
        
        # Get the best matching slice for visualization
        if best_orientation is None:
            # Find the closest orientation in our stored slices
            orient_diffs = []
            for orientation in self.reference_slices.keys():
                o_roll, o_pitch, _ = orientation
                diff = np.sqrt((o_roll - roll)**2 + (o_pitch - pitch)**2)
                orient_diffs.append((diff, orientation))
                
            _, best_orientation = min(orient_diffs)
            
        best_slice = self.reference_slices[best_orientation]
        best_roll, best_pitch, slice_origin = best_orientation
        
        # Create a 3D visualization
        # For better visualization, we can use Open3D's visualization tools
        scan_pcd = o3d.geometry.PointCloud()
        scan_pcd.points = o3d.utility.Vector3dVector(transformed_points_3d)
        scan_pcd.paint_uniform_color([1, 0, 0])  # Red for scan
        
        slice_pcd = o3d.geometry.PointCloud()
        slice_pcd.points = o3d.utility.Vector3dVector(best_slice)
        slice_pcd.paint_uniform_color([0, 0, 1])  # Blue for CAD slice
        
        # Create a coordinate frame to show the sensor position
        coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
        coordinate_frame.transform(transformation_3d)
        
        # Plot using matplotlib for simpler integration
        plt.figure(figsize=(12, 10))
        
        # Create a 3D subplot
        ax = plt.subplot(111, projection='3d')
        
        # Plot the scan points and slice
        ax.scatter(transformed_points_3d[:, 0], transformed_points_3d[:, 1], transformed_points_3d[:, 2], 
                 c='red', s=1, label='Aligned LiDAR Scan')
        ax.scatter(best_slice[:, 0], best_slice[:, 1], best_slice[:, 2], 
                 c='blue', s=1, alpha=0.5, label='Best CAD Model Slice')
        ax.scatter([x], [y], [z], c='green', s=100, marker='*', label='Estimated Sensor Position')
        
        # Draw sensor orientation (principal axes)
        axis_length = 0.5
        colors = ['g', 'b', 'r']  # x=green, y=blue, z=red
        
        for i, color in enumerate(colors):
            axis = np.zeros(3)
            axis[i] = axis_length
            rotated_axis = np.dot(R, axis)
            ax.quiver(x, y, z, rotated_axis[0], rotated_axis[1], rotated_axis[2],
                    color=color, linewidth=2)
                    
        # Add a plane representing the slice orientation
        # Create a grid of points on the best matching plane
        plane_size = 2.0
        xx, yy = np.meshgrid(np.linspace(-plane_size, plane_size, 5), 
                             np.linspace(-plane_size, plane_size, 5))
        zz = np.zeros_like(xx)
        plane_points = np.stack([xx.flatten(), yy.flatten(), zz.flatten()]).T
        
        # Rotate and position the plane
        R_plane = t3d.euler.euler2mat(best_roll, best_pitch, 0, 'sxyz')
        plane_points = np.dot(plane_points, R_plane.T) + slice_origin
        
        # Plot the plane wireframe
        ax.plot_wireframe(plane_points.reshape(5, 5, 3)[:,:,0], 
                         plane_points.reshape(5, 5, 3)[:,:,1], 
                         plane_points.reshape(5, 5, 3)[:,:,2], 
                         color='cyan', alpha=0.3)
        
        ax.set_xlabel('X')
        ax.set_ylabel('Y')
        ax.set_zlabel('Z')
        ax.set_title(f'LiDAR Localization using Arbitrary Slice Matching\n'
                    f'Roll: {np.degrees(roll):.1f}°, Pitch: {np.degrees(pitch):.1f}°, Yaw: {np.degrees(yaw):.1f}°')
        ax.legend()
        
        plt.tight_layout()
        plt.show()
        
        # Return Open3D visualization objects so they can be viewed interactively if desired
        return [scan_pcd, slice_pcd, coordinate_frame]


 # Example usage
 if __name__ == "__main__":
    # Example data (would come from your sensors in a real application)
    
    # Simulate a LiDAR scan (ranges and angles)
    angles = np.linspace(0, 2*np.pi, 360, endpoint=False)
    ranges = np.full_like(angles, 5.0)  # 5 meter range in all directions
    
    # Add some "walls" to the simulated data
    ranges[0:60] = 3.0  # Wall at 3m from 0 to 60 degrees
    ranges[180:240] = 2.0  # Wall at 2m from 180 to 240 degrees
    
    # Add noise to make it more realistic
    ranges += np.random.normal(0, 0.05, ranges.shape)
    
    # Simulated IMU data (in radians)
    imu_roll = 0.05  # Small roll
    imu_pitch = 0.02  # Small pitch
    imu_yaw = np.pi/6  # 30 degrees yaw
    
    # Path to your CAD model (replace with your actual model)
    cad_model_path = "room_model.stl"
    
    # Set the scale factor based on your CAD model units
    # Examples:
    # - If CAD is in millimeters and real world is in meters: scale_factor = 0.001
    # - If CAD is in inches and real world is in meters: scale_factor = 0.0254
    # - If CAD is in feet and real world is in meters: scale_factor = 0.3048
    scale_factor = 1.0  # Adjust this to your CAD model's scale
    
    # For demonstration purposes, if you don't have a CAD model, 
    # uncomment this code to create a simple model with walls and features:
    """
    import trimesh
    # Create a more complex room with walls at different heights and features
    room = trimesh.Trimesh()
    
    # Outer walls (full height)
    walls = []
    
    # Floor
    floor = trimesh.creation.box(extents=[10, 8, 0.1], transform=trimesh.transformations.translation_matrix([0, 0, -0.05]))
    
    # Ceiling
    ceiling = trimesh.creation.box(extents=[10, 8, 0.1], transform=trimesh.transformations.translation_matrix([0, 0, 2.95]))
    
    # Front wall
    front_wall = trimesh.creation.box(extents=[10, 0.2, 3], transform=trimesh.transformations.translation_matrix([0, -4, 1.5]))
    
    # Back wall
    back_wall = trimesh.creation.box(extents=[10, 0.2, 3], transform=trimesh.transformations.translation_matrix([0, 4, 1.5]))
    
    # Left wall
    left_wall = trimesh.creation.box(extents=[0.2, 8, 3], transform=trimesh.transformations.translation_matrix([-5, 0, 1.5]))
    
    # Right wall
    right_wall = trimesh.creation.box(extents=[0.2, 8, 3], transform=trimesh.transformations.translation_matrix([5, 0, 1.5]))
    
    # Add a partial height divider in the middle
    divider = trimesh.creation.box(extents=[5, 0.2, 1.5], transform=trimesh.transformations.translation_matrix([0, 1, 0.75]))
    
    # Add a column for feature recognition
    column = trimesh.creation.cylinder(radius=0.3, height=3, transform=trimesh.transformations.translation_matrix([3, -2, 1.5]))
    
    # Add furniture-like objects
    table = trimesh.creation.box(extents=[1.5, 0.8, 0.75], transform=trimesh.transformations.translation_matrix([-2, -2, 0.375]))
    
    # Combine all elements
    room = trimesh.util.concatenate([floor, ceiling, front_wall, back_wall, left_wall, right_wall, divider, column, table])
    
    # Export the model
    room.export('room_model.stl')
    cad_model_path = 'room_model.stl'
    """
    
    try:
        # Initialize the localizer with appropriate scale factor
        localizer = SensorLocalizer(cad_model_path, scale_factor=scale_factor)
        
        # Process the LiDAR scan with IMU correction
        scan_points_3d = localizer.process_lidar_scan(ranges, angles, imu_roll, imu_pitch, imu_yaw)
        
        # Estimate position with 6DOF
        initial_guess = [0.5, 0.5, 1.0, imu_roll, imu_pitch, imu_yaw]  # Starting guess: x, y, z, roll, pitch, yaw
        estimated_position, error = localizer.estimate_position(scan_points_3d, initial_guess)
        
        # Extract position components for readability
        x, y, z, roll, pitch, yaw = estimated_position
        
        print(f"Estimated sensor position: x={x:.2f}m, y={y:.2f}m, z={z:.2f}m")
        print(f"Estimated sensor orientation: roll={np.degrees(roll):.2f}°, pitch={np.degrees(pitch):.2f}°, yaw={np.degrees(yaw):.2f}°")
        print(f"Average error: {error:.3f}m")
        
        # Visualize the result
        localizer.visualize_result(scan_points_3d, estimated_position, best_height=z)
        
    except Exception as e:
        print(f"Error: {e}")
        print("If you don't have a CAD model available, use the code in the comments to create a simple one.")
	import numpy as np
	import open3d as o3d
	import matplotlib.pyplot as plt
	import transforms3d as t3d
	import trimesh
	import scipy.spatial

	class SensorLocalizer:
	def __init__(self, cad_model_path, scale_factor=1.0):
	"""
	Initialize the localizer with a 3D CAD model

	Args:
	cad_model_path: Path to the 3D CAD model file (OBJ, STL, etc.)
	scale_factor: Scale factor to convert CAD model units to real-world meters
	(e.g., if CAD is in mm and real world is in m, use 0.001)
	"""
	self.cad_model = trimesh.load(cad_model_path)
	self.scale_factor = scale_factor

	# Apply scale factor to the model if needed
	if scale_factor != 1.0:
	self.cad_model.apply_scale(scale_factor)

	# Generate arbitrary slices of the model
	self.reference_slices = self._extract_arbitrary_slices()

	def _extract_arbitrary_slices(self, num_slices=20):
	"""
	Extract multiple 2D slices from the 3D CAD model at different orientations

	Args:
	num_slices: Number of different orientations to try

	Returns:
	Dictionary mapping orientation parameters to 2D point arrays
	"""
	reference_slices = {}

	# Fixed height for central slicing
	center_z = self.cad_model.bounds.mean(axis=0)[2]

	# Generate a range of roll and pitch angles to try
	# We'll use a Fibonacci sphere to distribute points evenly on a hemisphere
	angles = []

	# Simple approach: grid of roll/pitch angles
	roll_angles = np.linspace(-np.pi/6, np.pi/6, int(np.sqrt(num_slices))) # ±30 degrees
	pitch_angles = np.linspace(-np.pi/6, np.pi/6, int(np.sqrt(num_slices))) # ±30 degrees

	for roll in roll_angles:
	for pitch in pitch_angles:
	angles.append((roll, pitch))

	# Try each orientation
	for roll, pitch in angles:
	# Create rotation matrix for this orientation
	R = t3d.euler.euler2mat(roll, pitch, 0, 'sxyz')

	# Normal vector for the slicing plane (rotated z-axis)
	normal = R @ np.array([0, 0, 1])

	# Use the center of the model as the plane origin
	origin = self.cad_model.bounds.mean(axis=0)

	try:
	# Create a cross-section with this orientation
	slice_path = self.cad_model.section(
	plane_origin=origin,
	plane_normal=normal
	)

	if slice_path is not None:
	# Convert the slice to points
	points = []
	for entity in slice_path.entities:
	for idx in entity.points:
	points.append(slice_path.vertices[idx])

	if points:
	# Store orientation parameters with the slice
	orientation = (roll, pitch, origin)
	reference_slices[orientation] = np.array(points)
	except Exception as e:
	print(f"Failed to extract slice at roll={np.degrees(roll):.1f}°, pitch={np.degrees(pitch):.1f}°: {e}")

	if not reference_slices:
	raise ValueError("No valid slices found in the CAD model")

	return reference_slices

	def process_lidar_scan(self, ranges, angles, imu_roll, imu_pitch, imu_yaw):
	"""
	Process a 2D LiDAR scan and correct it using IMU data

	Args:
	ranges: Array of range measurements from LiDAR
	angles: Array of corresponding angles for each range
	imu_roll: Roll angle from IMU in radians
	imu_pitch: Pitch angle from IMU in radians
	imu_yaw: Yaw angle from IMU in radians

	Returns:
	3D point cloud after IMU correction
	"""
	# Convert polar coordinates to 3D Cartesian in LiDAR frame
	# Initially, points are in x-y plane (z=0) from LiDAR's perspective
	x = ranges * np.cos(angles)
	y = ranges * np.sin(angles)
	z = np.zeros_like(x)

	# Create 3D point cloud
	points = np.column_stack((x, y, z))

	# Create rotation matrix from IMU data
	R = t3d.euler.euler2mat(imu_roll, imu_pitch, imu_yaw, 'sxyz')

	# Apply rotation to correct for sensor attitude
	# Now points are in the world frame with proper orientation
	corrected_points = np.dot(points, R.T)

	# Return full 3D points to handle non-horizontal LiDAR
	return corrected_points

	def estimate_position(self, scan_points_3d, initial_guess=None, max_iterations=50):
	"""
	Estimate sensor position by matching raw LiDAR scan to arbitrarily sliced CAD model

	Args:
	scan_points_3d: 3D points from processed LiDAR scan
	initial_guess: Initial [x, y, z, roll, pitch, yaw] guess, if available
	max_iterations: Maximum ICP iterations

	Returns:
	Estimated [x, y, z, roll, pitch, yaw] position of the sensor in the CAD model
	(position values will be in the same units as your LiDAR data)
	"""
	if initial_guess is None:
	# Start with a guess at the center of the map with no rotation
	initial_guess = [0, 0, 1.0, 0, 0, 0]

	best_error = float('inf')
	best_position = initial_guess
	best_orientation = None

	# Convert scan points to a more efficient representation for matching
	scan_pcd = o3d.geometry.PointCloud()
	scan_pcd.points = o3d.utility.Vector3dVector(scan_points_3d)

	# Try matching with each reference slice at different orientations
	for orientation, slice_points in self.reference_slices.items():
	roll, pitch, origin = orientation

	# Convert to Open3D format for efficient matching
	slice_pcd = o3d.geometry.PointCloud()
	slice_pcd.points = o3d.utility.Vector3dVector(slice_points)

	# Create initial pose guess incorporating this orientation
	ori_initial_guess = initial_guess.copy()
	ori_initial_guess[3:5] = [roll, pitch] # Use this slice's orientation

	# Match at this orientation
	position, error = self._match_with_slice(scan_pcd, slice_pcd, ori_initial_guess, orientation, max_iterations)

	if error < best_error:
	best_error = error
	best_position = position
	best_orientation = orientation

	if best_orientation is None:
	raise ValueError("Could not find a good match with any orientation")

	best_roll, best_pitch, _ = best_orientation
	print(f"Best match found at orientation: roll={np.degrees(best_roll):.2f}°, pitch={np.degrees(best_pitch):.2f}°")
	return best_position, best_error

	def _match_with_slice(self, scan_pcd, slice_pcd, initial_guess, orientation, max_iterations):
	"""
	Match scan points with a specific oriented slice

	Args:
	scan_pcd: Open3D PointCloud of the scan
	slice_pcd: Open3D PointCloud of the slice
	initial_guess: Initial pose guess [x, y, z, roll, pitch, yaw]
	orientation: (roll, pitch, origin) of the slice
	max_iterations: Maximum ICP iterations

	Returns:
	Estimated [x, y, z, roll, pitch, yaw] and error
	"""
	# Extract orientation parameters
	slice_roll, slice_pitch, slice_origin = orientation

	# Create initial transformation from initial guess
	x, y, z, roll, pitch, yaw = initial_guess

	# Combine the slice orientation with the yaw component of the initial guess
	# This creates a transformation that aligns the scan with the slicing plane
	R_slice = t3d.euler.euler2mat(slice_roll, slice_pitch, yaw, 'sxyz')
	T_slice = np.eye(4)
	T_slice[:3, :3] = R_slice
	T_slice[:3, 3] = [x, y, z]

	# Convert numpy transformation to Open3D format
	init_transform = o3d.geometry.TransformationMatrix(T_slice)

	# Run point-to-point ICP
	result = o3d.pipelines.registration.registration_icp(
	scan_pcd, slice_pcd, 0.2, # max_correspondence_distance
	init_transform.get_matrix(),
	o3d.pipelines.registration.TransformationEstimationPointToPoint(),
	o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=max_iterations)
	)

	# Extract the transformation matrix
	transformation = result.transformation

	# Convert back to our 6-DOF representation [x, y, z, roll, pitch, yaw]
	translation = transformation[:3, 3]
	rotation_matrix = transformation[:3, :3]

	# Extract Euler angles (might need refinement for gimbal lock cases)
	angles = t3d.euler.mat2euler(rotation_matrix, 'sxyz')

	# Final pose estimation
	estimated_pose = [translation[0], translation[1], translation[2], angles[0], angles[1], angles[2]]

	return estimated_pose, result.inlier_rmse

	def visualize_result(self, scan_points_3d, estimated_position, best_orientation=None):
	"""
	Visualize the localization result

	Args:
	scan_points_3d: Original 3D scan points
	estimated_position: Estimated [x, y, z, roll, pitch, yaw] position
	best_orientation: The orientation parameters of the best matching slice
	"""
	x, y, z, roll, pitch, yaw = estimated_position

	# Create full 3D transformation matrix from estimated position
	R = t3d.euler.euler2mat(roll, pitch, yaw, 'sxyz')
	transformation_3d = np.eye(4)
	transformation_3d[:3, :3] = R
	transformation_3d[:3, 3] = [x, y, z]

	# Apply transformation to 3D scan points
	scan_points_hom = np.column_stack((scan_points_3d, np.ones(len(scan_points_3d))))
	transformed_points_3d = np.dot(scan_points_hom, transformation_3d.T)[:, :3]

	# Get the best matching slice for visualization
	if best_orientation is None:
	# Find the closest orientation in our stored slices
	orient_diffs = []
	for orientation in self.reference_slices.keys():
	o_roll, o_pitch, _ = orientation
	diff = np.sqrt((o_roll - roll)2 + (o_pitch - pitch)2)
	orient_diffs.append((diff, orientation))

	_, best_orientation = min(orient_diffs)

	best_slice = self.reference_slices[best_orientation]
	best_roll, best_pitch, slice_origin = best_orientation

	# Create a 3D visualization
	# For better visualization, we can use Open3D's visualization tools
	scan_pcd = o3d.geometry.PointCloud()
	scan_pcd.points = o3d.utility.Vector3dVector(transformed_points_3d)
	scan_pcd.paint_uniform_color([1, 0, 0]) # Red for scan

	slice_pcd = o3d.geometry.PointCloud()
	slice_pcd.points = o3d.utility.Vector3dVector(best_slice)
	slice_pcd.paint_uniform_color([0, 0, 1]) # Blue for CAD slice

	# Create a coordinate frame to show the sensor position
	coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
	coordinate_frame.transform(transformation_3d)

	# Plot using matplotlib for simpler integration
	plt.figure(figsize=(12, 10))

	# Create a 3D subplot
	ax = plt.subplot(111, projection='3d')

	# Plot the scan points and slice
	ax.scatter(transformed_points_3d[:, 0], transformed_points_3d[:, 1], transformed_points_3d[:, 2],
	c='red', s=1, label='Aligned LiDAR Scan')
	ax.scatter(best_slice[:, 0], best_slice[:, 1], best_slice[:, 2],
	c='blue', s=1, alpha=0.5, label='Best CAD Model Slice')
	ax.scatter([x], [y], [z], c='green', s=100, marker='*', label='Estimated Sensor Position')

	# Draw sensor orientation (principal axes)
	axis_length = 0.5
	colors = ['g', 'b', 'r'] # x=green, y=blue, z=red

	for i, color in enumerate(colors):
	axis = np.zeros(3)
	axis[i] = axis_length
	rotated_axis = np.dot(R, axis)
	ax.quiver(x, y, z, rotated_axis[0], rotated_axis[1], rotated_axis[2],
	color=color, linewidth=2)

	# Add a plane representing the slice orientation
	# Create a grid of points on the best matching plane
	plane_size = 2.0
	xx, yy = np.meshgrid(np.linspace(-plane_size, plane_size, 5),
	np.linspace(-plane_size, plane_size, 5))
	zz = np.zeros_like(xx)
	plane_points = np.stack([xx.flatten(), yy.flatten(), zz.flatten()]).T

	# Rotate and position the plane
	R_plane = t3d.euler.euler2mat(best_roll, best_pitch, 0, 'sxyz')
	plane_points = np.dot(plane_points, R_plane.T) + slice_origin

	# Plot the plane wireframe
	ax.plot_wireframe(plane_points.reshape(5, 5, 3)[:,:,0],
	plane_points.reshape(5, 5, 3)[:,:,1],
	plane_points.reshape(5, 5, 3)[:,:,2],
	color='cyan', alpha=0.3)

	ax.set_xlabel('X')
	ax.set_ylabel('Y')
	ax.set_zlabel('Z')
	ax.set_title(f'LiDAR Localization using Arbitrary Slice Matching\n'
	f'Roll: {np.degrees(roll):.1f}°, Pitch: {np.degrees(pitch):.1f}°, Yaw: {np.degrees(yaw):.1f}°')
	ax.legend()

	plt.tight_layout()
	plt.show()

	# Return Open3D visualization objects so they can be viewed interactively if desired
	return [scan_pcd, slice_pcd, coordinate_frame]


	# Example usage
	if __name__ == "__main__":
	# Example data (would come from your sensors in a real application)

	# Simulate a LiDAR scan (ranges and angles)
	angles = np.linspace(0, 2*np.pi, 360, endpoint=False)
	ranges = np.full_like(angles, 5.0) # 5 meter range in all directions

	# Add some "walls" to the simulated data
	ranges[0:60] = 3.0 # Wall at 3m from 0 to 60 degrees
	ranges[180:240] = 2.0 # Wall at 2m from 180 to 240 degrees

	# Add noise to make it more realistic
	ranges += np.random.normal(0, 0.05, ranges.shape)

	# Simulated IMU data (in radians)
	imu_roll = 0.05 # Small roll
	imu_pitch = 0.02 # Small pitch
	imu_yaw = np.pi/6 # 30 degrees yaw

	# Path to your CAD model (replace with your actual model)
	cad_model_path = "room_model.stl"

	# Set the scale factor based on your CAD model units
	# Examples:
	# - If CAD is in millimeters and real world is in meters: scale_factor = 0.001
	# - If CAD is in inches and real world is in meters: scale_factor = 0.0254
	# - If CAD is in feet and real world is in meters: scale_factor = 0.3048
	scale_factor = 1.0 # Adjust this to your CAD model's scale

	# For demonstration purposes, if you don't have a CAD model,
	# uncomment this code to create a simple model with walls and features:
	"""
	import trimesh
	# Create a more complex room with walls at different heights and features
	room = trimesh.Trimesh()

	# Outer walls (full height)
	walls = []

	# Floor
	floor = trimesh.creation.box(extents=[10, 8, 0.1], transform=trimesh.transformations.translation_matrix([0, 0, -0.05]))

	# Ceiling
	ceiling = trimesh.creation.box(extents=[10, 8, 0.1], transform=trimesh.transformations.translation_matrix([0, 0, 2.95]))

	# Front wall
	front_wall = trimesh.creation.box(extents=[10, 0.2, 3], transform=trimesh.transformations.translation_matrix([0, -4, 1.5]))

	# Back wall
	back_wall = trimesh.creation.box(extents=[10, 0.2, 3], transform=trimesh.transformations.translation_matrix([0, 4, 1.5]))

	# Left wall
	left_wall = trimesh.creation.box(extents=[0.2, 8, 3], transform=trimesh.transformations.translation_matrix([-5, 0, 1.5]))

	# Right wall
	right_wall = trimesh.creation.box(extents=[0.2, 8, 3], transform=trimesh.transformations.translation_matrix([5, 0, 1.5]))

	# Add a partial height divider in the middle
	divider = trimesh.creation.box(extents=[5, 0.2, 1.5], transform=trimesh.transformations.translation_matrix([0, 1, 0.75]))

	# Add a column for feature recognition
	column = trimesh.creation.cylinder(radius=0.3, height=3, transform=trimesh.transformations.translation_matrix([3, -2, 1.5]))

	# Add furniture-like objects
	table = trimesh.creation.box(extents=[1.5, 0.8, 0.75], transform=trimesh.transformations.translation_matrix([-2, -2, 0.375]))

	# Combine all elements
	room = trimesh.util.concatenate([floor, ceiling, front_wall, back_wall, left_wall, right_wall, divider, column, table])

	# Export the model
	room.export('room_model.stl')
	cad_model_path = 'room_model.stl'
	"""

	try:
	# Initialize the localizer with appropriate scale factor
	localizer = SensorLocalizer(cad_model_path, scale_factor=scale_factor)

	# Process the LiDAR scan with IMU correction
	scan_points_3d = localizer.process_lidar_scan(ranges, angles, imu_roll, imu_pitch, imu_yaw)

	# Estimate position with 6DOF
	initial_guess = [0.5, 0.5, 1.0, imu_roll, imu_pitch, imu_yaw] # Starting guess: x, y, z, roll, pitch, yaw
	estimated_position, error = localizer.estimate_position(scan_points_3d, initial_guess)

	# Extract position components for readability
	x, y, z, roll, pitch, yaw = estimated_position

	print(f"Estimated sensor position: x={x:.2f}m, y={y:.2f}m, z={z:.2f}m")
	print(f"Estimated sensor orientation: roll={np.degrees(roll):.2f}°, pitch={np.degrees(pitch):.2f}°, yaw={np.degrees(yaw):.2f}°")
	print(f"Average error: {error:.3f}m")

	# Visualize the result
	localizer.visualize_result(scan_points_3d, estimated_position, best_height=z)

	except Exception as e:
	print(f"Error: {e}")
	print("If you don't have a CAD model available, use the code in the comments to create a simple one.")