Survey point-cloud merger

Merge recorded survey clouds into a binary PLY file using TF or URDF transforms, range filtering, and voxel averaging. Processing is offline but requires ROS Python dependencies.

scripts.survey_mapping.merge_survey_bag · Source

Perception and mapping · Guide · Symbol index

How it is used

Use main() to turn a recorded survey bag into a PLY point cloud for the Isaac Sim workflow. Run the script after sourcing ROS and the workspace; it reads files and needs no running ROS graph. The helpers below handle cloud extraction, coordinate transforms, and voxel averaging.

Recorded TF is the default alignment source. Supplying --urdf replaces TF lookup with forward kinematics using the latest preceding joint-state message. Neither mode refines the camera calibration or performs ICP alignment.

Start here

Callable	Purpose
main	Merge the bag’s clouds into one PLY file. Range-filter points in the incoming camera frame, transform them using recorded TF or joint states with the URDF, then voxel-average them. If any included cloud lacks color, omit color from the final map.
UrdfFkSolver.lookup	Return the transform from `source` to `target` using the URDF and supplied joint positions. Both frames must exist; disconnected URDF roots are not explicitly rejected.
voxel_downsample	Group floor(xyz/leaf) cells and average coordinates and optional RGB per cell. Color means are clipped and truncated to uint8.
write_ply	Serialize point coordinates as little-endian float32 vertices with optional unsigned-byte red/green/blue fields.

Example

From the repository root, merge an existing survey bag:

python3 scripts/survey_mapping/merge_survey_bag.py \
  --bag survey_session --out survey_map.ply --voxel 0.005

The voxel size is 5 mm. Use a new output filename to keep an earlier map; an existing PLY file is overwritten.

Functions

`main()`

Merge the bag’s clouds into one PLY file. Range-filter points in the incoming camera frame, transform them using recorded TF or joint states with the URDF, then voxel-average them. If any included cloud lacks color, omit color from the final map.

Entry point · Source

main()

Returns: Integer exit code: 0 after PLY/self-check, 1 for missing inputs or no mergeable clouds.

Errors: Argparse exits for missing –bag or negative range/voxel; known URDF load errors return 1; transform failures skip clouds with warnings. Bag deserialization, extraction and PLY write errors can propagate.

Side effects: Reads bag/optional URDF, prints progress/warnings and overwrites –out (default survey_map.ply); no robot movement.

`voxel_downsample()`

Group floor(xyz/leaf) cells and average coordinates and optional RGB per cell. Color means are clipped and truncated to uint8.

Function · Source

voxel_downsample(xyz, rgb, leaf)

Parameter	Meaning
`xyz`	N×3 coordinate array, normally metres.
`rgb`	Matching N×3 colors, or None to omit color.
`leaf`	Voxel edge in the same units as xyz; zero or negative disables sampling.

Returns: (sampled_xyz, sampled_rgb); returns original objects for leaf <= 0 or empty input, otherwise one centroid per lexicographically ordered occupied cell.

Errors: Shape, nonfinite coordinate and incompatible dtype errors are not caught.

Side effects: Allocates aggregate arrays; does not mutate supplied arrays.

Example

xyz, rgb = voxel_downsample(np.array([[0., 0, 0], [0.4, 0, 0], [1.1, 0, 0]]), None, 1.0)
assert np.allclose(xyz, [[0.2, 0, 0], [1.1, 0, 0]])
assert rgb is None

Referenced in source (2)

main — scripts.survey_mapping.merge_survey_bag
self_check — scripts.survey_mapping.merge_survey_bag

`write_ply()`

Serialize point coordinates as little-endian float32 vertices with optional unsigned-byte red/green/blue fields.

Function · Source

write_ply(path, xyz, rgb)

Parameter	Meaning
`path`	Destination file; opened in wb mode, overwriting an existing file. Parent directories must exist.
`xyz`	N×3 coordinates; cast to float32 for storage.
`rgb`	N×3 colors cast to uint8, or None for XYZ-only PLY.

Returns: None.

Errors: I/O and incompatible array-shape errors propagate; writes are not atomic.

Side effects: Creates or truncates a binary PLY file.

Referenced in source (1)

main — scripts.survey_mapping.merge_survey_bag

`xyz_rpy_matrix()`

Build a homogeneous transform from fixed-axis XYZ Euler rotation and translation.

Function · Source

xyz_rpy_matrix(xyz, rpy)

Parameter	Meaning
`xyz`	Three translation components, conventionally metres.
`rpy`	Roll, pitch, yaw in radians, applied with tf_transformations axes=”sxyz”.

Returns: NumPy 4×4 homogeneous matrix with xyz in its translation column.

Errors: Invalid vector shape or numeric values are not caught.

Side effects: Allocates a matrix only.

Example

m = xyz_rpy_matrix([1, 2, 3], [0, 0, 0])
assert np.allclose(m[:3, 3], [1, 2, 3])

Referenced in source (2)

UrdfFkSolver.__init__ — scripts.survey_mapping.merge_survey_bag
self_check — scripts.survey_mapping.merge_survey_bag

Internal callbacks and helpers

`open_bag()`

Open a rosbag2 sequential reader using auto-selected storage and CDR-to-CDR conversion.

Internal helper · Source

open_bag(path)

Parameter	Meaning
`path`	Bag directory/path accepted by rosbag2; converted to string.

Returns: (reader, topic_types), where topic_types maps topic names to ROS type strings.

Errors: rosbag2 open/storage/metadata errors propagate.

Side effects: Opens and reads bag metadata; returned reader retains the bag resource.

Referenced in source (1)

main — scripts.survey_mapping.merge_survey_bag

`transform_matrix()`

Convert the translation and quaternion in a TransformStamped into a homogeneous matrix.

Internal helper · Source

transform_matrix(transform)

Parameter	Meaning
`transform`	TransformStamped-like object whose transform contains translation xyz and quaternion xyzw.

Returns: NumPy 4×4 source-to-target matrix in the message’s coordinate units.

Errors: Malformed attributes/numeric data are not caught.

Side effects: Allocates a matrix only.

Referenced in source (1)

main — scripts.survey_mapping.merge_survey_bag

`extract_points()`

Read XYZ and optional packed color from PointCloud2, preferring rgba over rgb and skipping NaN records. Unpack the lower RGB bytes and discard alpha.

Internal helper · Source

extract_points(cloud)

Parameter	Meaning
`cloud`	sensor_msgs.msg.PointCloud2 with x/y/z fields and optional rgb or rgba packed 32-bit field.

Returns: (xyz, rgb): xyz is N×3 float64; rgb is N×3 uint8, or None for missing color or an empty cloud. Coordinates keep the cloud units/frame.

Errors: Missing/malformed fields and point_cloud2/NumPy errors propagate; infinities are not explicitly filtered.

Side effects: Reads message buffers and allocates arrays.

Referenced in source (1)

main — scripts.survey_mapping.merge_survey_bag

`self_check()`

Assert voxel centroid/color averaging and translation-column behavior using small in-memory arrays.

Internal helper · Source

self_check()

Returns: None if assertions pass.

Errors: AssertionError on incorrect expected results; module imports still require ROS and NumPy dependencies.

Side effects: Prints self-check passed; no bag or hardware access.

Referenced in source (1)

main — scripts.survey_mapping.merge_survey_bag

`UrdfFkSolver`

Compute frame transforms from an expanded URDF and a joint-position mapping. This is the merger’s optional alternative to recorded TF; it does not model mimic joints.

`UrdfFkSolver.lookup()`

Return the transform from source to target using the URDF and supplied joint positions. Both frames must exist; disconnected URDF roots are not explicitly rejected.

Method · Source

lookup(self, target, source, angles)

Parameter	Meaning
`target`	Destination link frame.
`source`	Source link frame.
`angles`	Mapping of movable joint positions, radians for rotation and metres for translation.

Returns: 4×4 transform taking points from source to target.

Errors: ValueError for absent frames, cycles or missing positions; inversion errors propagate. No explicit check ensures target and source share one root.

Side effects: Computes transforms without publishing TF.

`UrdfFkSolver.init()`

Parse each URDF joint into a child-link-to-joint table containing name, type, parent, origin transform and axis. Missing origin defaults to zero and missing axis to +X; mimic behavior is not modeled.

Lifecycle · Source

__init__(self, path)

Parameter	Meaning
`path`	Filesystem path to a parsed URDF XML document, not a xacro command.

Returns: None; initializes parent_joint.

Errors: File/XML errors, missing parent or child elements and malformed numeric fields propagate.

Side effects: Reads the URDF file and populates memory.

Referenced in source (1)

main — scripts.survey_mapping.merge_survey_bag

`UrdfFkSolver.chain_to_root()`

Walk parent joints from a link and return them in root-to-link multiplication order.

Method · Source

chain_to_root(self, link)

Parameter	Meaning
`link`	Child link name from the parsed URDF; an unknown name produces an empty chain here.

Returns: Reverse iterator over joint dictionaries from root toward link.

Errors: Raises ValueError on a repeated link/cycle.

Side effects: None outside local allocations.

Referenced in source (1)

UrdfFkSolver.world_matrix — scripts.survey_mapping.merge_survey_bag

`UrdfFkSolver.joint_matrix()`

Compose a joint origin with its revolute/continuous rotation or prismatic translation. Other joint types return the stored origin unchanged.

Method · Source

joint_matrix(self, joint, angles)

Parameter	Meaning
`joint`	Joint dictionary from parent_joint, including type, name, origin and axis.
`angles`	Mapping from movable joint names to positions: radians for rotation, metres for prismatic motion.

Returns: 4×4 origin @ motion matrix; fixed/unsupported types return the origin array itself.

Errors: ValueError if a supported movable joint lacks a position; numeric/shape errors propagate.

Side effects: No external effects; fixed results alias stored origin.

Referenced in source (1)

UrdfFkSolver.world_matrix — scripts.survey_mapping.merge_survey_bag

`UrdfFkSolver.world_matrix()`

Multiply the root-to-link chain of origin and motion transforms.

Method · Source

world_matrix(self, link, angles)

Parameter	Meaning
`link`	URDF link name; this helper alone treats an unknown link as the identity.
`angles`	Joint-name-to-position mapping in radians or metres according to joint type.

Returns: 4×4 transform mapping link coordinates into its root coordinates.

Errors: Propagates cycle and missing-joint ValueError from chain/joint helpers.

Side effects: Allocates matrices only.

Referenced in source (1)

UrdfFkSolver.lookup — scripts.survey_mapping.merge_survey_bag

Survey point-cloud merger

How it is used

Start here

Example

Functions

main()

voxel_downsample()

write_ply()

xyz_rpy_matrix()

Internal callbacks and helpers

open_bag()

transform_matrix()

extract_points()

self_check()

UrdfFkSolver

UrdfFkSolver.lookup()

UrdfFkSolver.__init__()

UrdfFkSolver.chain_to_root()

UrdfFkSolver.joint_matrix()

UrdfFkSolver.world_matrix()

`main()`

`voxel_downsample()`

`write_ply()`

`xyz_rpy_matrix()`

`open_bag()`

`transform_matrix()`

`extract_points()`

`self_check()`

`UrdfFkSolver`

`UrdfFkSolver.lookup()`

`UrdfFkSolver.init()`

`UrdfFkSolver.chain_to_root()`

`UrdfFkSolver.joint_matrix()`

`UrdfFkSolver.world_matrix()`