pyhelios_dev/utils.py at main · Leaf2Landscape/pyhelios_dev · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
"""
Utility functions for PyHelios simulation output processing.
"""

import struct
import numpy as np
import pyhelios

def export_to_raycloud(measurements, output_file):
    """
    Export PyHelios measurements to RayCloudTools PLY format with binary little-endian encoding.

    Args:
        measurements: PyHelios measurement vector or output object
        output_file: Path to output PLY file

    Format matches:
        ply
        format binary_little_endian 1.0
        comment generated by raycloudtools library
        element vertex {count}
        property double x
        property double y
        property double z
        property double time
        property float nx
        property float ny
        property float nz
        property uchar red
        property uchar green
        property uchar blue
        property uchar alpha
        end_header
    """
    print(f"Converting measurements to numpy array...")

    # Handle different input types
    if hasattr(measurements, 'measurements'):
        # This is an output wrapper, extract measurements
        measurements_array, _ = pyhelios.outputToNumpy(measurements)
    else:
        # This is already a measurements vector, convert to numpy
        output_wrapper = type('obj', (object,), {'measurements': measurements, 'trajectories': []})()
        measurements_array, _ = pyhelios.outputToNumpy(output_wrapper)

    num_points = measurements_array.shape[0]
    print(f"Exporting {num_points} measurements to PLY format...")

    with open(output_file, 'wb') as f:
        # Write PLY header
        header = f"""ply
format binary_little_endian 1.0
comment generated by raycloudtools library
element vertex {num_points:019d}
property double x
property double y
property double z
property double time
property float nx
property float ny
property float nz
property uchar red
property uchar green
property uchar blue
property uchar alpha
end_header
"""
        f.write(header.encode('ascii'))

        # Extract data from numpy array
        # Columns: [pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, dir.x, dir.y, dir.z, intensity, echoWidth, NumberOfReturns, ReturnNumber, FullwaveIndex, hitObjectId, classification, gpsTime]
        positions = measurements_array[:, :3]  # x, y, z
        directions = measurements_array[:, 6:9]  # dir.x, dir.y, dir.z (beam direction)
        intensities = measurements_array[:, 9]  # intensity
        gps_times = measurements_array[:, 16]  # gpsTime

        # Calculate intensity range for normalization
        valid_intensities = intensities[np.isfinite(intensities) & (intensities >= 0)]
        if len(valid_intensities) > 0:
            intensity_min = np.min(valid_intensities)
            intensity_max = np.max(valid_intensities)
            intensity_range = intensity_max - intensity_min
            print(f"Intensity range: {intensity_min:.1f} - {intensity_max:.1f}")
        else:
            intensity_min = intensity_max = intensity_range = 0

        # Write binary data for each point
        for i in range(num_points):
            # Position coordinates
            x, y, z = positions[i]
            time_val = gps_times[i]

            # Normals should point from measurement point back to sensor (reverse of beam direction)
            nx, ny, nz = -directions[i]  # Reverse the direction
            norm = np.sqrt(nx*nx + ny*ny + nz*nz)
            if norm > 0:
                nx /= norm
                ny /= norm
                nz /= norm
            else:
                # Default normal pointing up if beam direction is invalid
                nx, ny, nz = 0.0, 0.0, 1.0

            # Empty/default colors
            red = green = blue = 0

            # Intensity goes in alpha channel
            intensity = intensities[i]
            if np.isfinite(intensity) and intensity >= 0 and intensity_range > 0:
                # Normalize intensity to 1-255 range using actual intensity range (never 0)
                normalized_intensity = (intensity - intensity_min) / intensity_range
                alpha = min(255, max(1, int(normalized_intensity * 254) + 1))
            elif np.isfinite(intensity) and intensity >= 0:
                # Single intensity value case
                alpha = 128  # Use middle gray value
            else:
                alpha = 1  # Minimum visible intensity if no valid intensity

            # Pack data in binary format
            # doubles: x, y, z, time (8 bytes each)
            # floats: nx, ny, nz (4 bytes each)
            # uchars: red, green, blue, alpha (1 byte each)
            data = struct.pack('<ddddfffBBBB',
                             float(x), float(y), float(z), float(time_val),
                             float(nx), float(ny), float(nz),
                             red, green, blue, alpha)
            f.write(data)

    print(f"PLY file exported to: {output_file}")

def export_ascii_with_everything(measurements, output_file):
    """
    Export PyHelios measurements to space-delimited text file, writing all columns.

    Args:
        measurements: PyHelios measurement vector or output object
        output_file: Path to output text file

    Format: All columns from measurements_array, space-delimited, one point per line.
    Columns: [pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, dir.x, dir.y, dir.z, intensity, echoWidth, NumberOfReturns, ReturnNumber, FullwaveIndex, hitObjectId, classification, gpsTime]
    """

    print(f"Converting measurements to numpy array...")

    # Handle different input types
    if hasattr(measurements, 'measurements'):
        # This is an output wrapper, extract measurements
        measurements_array, _ = pyhelios.outputToNumpy(measurements)
    else:
        # This is already a measurements vector, convert to numpy
        output_wrapper = type('obj', (object,), {'measurements': measurements, 'trajectories': []})()
        measurements_array, _ = pyhelios.outputToNumpy(output_wrapper)

    num_points = measurements_array.shape[0]
    num_cols = measurements_array.shape[1]
    print(f"Exporting {num_points} measurements with {num_cols} columns to text format...")

    # Define column headers
    headers = [
        "pos.x", "pos.y", "pos.z",
        "ori.x", "ori.y", "ori.z",
        "dir.x", "dir.y", "dir.z",
        "intensity", "echoWidth", "NumberOfReturns", "ReturnNumber",
        "FullwaveIndex", "hitObjectId", "classification", "gpsTime"
    ]

    with open(output_file, 'w') as f:
        # Write header line
        f.write(" ".join(headers) + "\n")
        for i in range(num_points):
            values = measurements_array[i]
            # Format each value with reasonable precision
            formatted = " ".join(f"{v:.6f}" if isinstance(v, float) or isinstance(v, np.floating) else str(int(v)) for v in values)
            f.write(formatted + "\n")

    print(f"Text file exported to: {output_file}")