more robist calibration
This commit is contained in:
@@ -73,20 +73,15 @@ DATA binary
|
||||
"""
|
||||
self.buffer.write(header.encode('ascii'))
|
||||
for i in range(points.shape[0]):
|
||||
# x, y, z, intensity as float32, rgb as uint32
|
||||
self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
|
||||
|
||||
|
||||
class LidarTransformPublisher(Node):
|
||||
def __init__(self, lidar1_buffer, lidar1_frame, lidar2_buffer, lidar2_frame):
|
||||
super().__init__('static_transform_lidar_offsets')
|
||||
|
||||
# Static TF broadcaster
|
||||
self.br = StaticTransformBroadcaster(self)
|
||||
|
||||
self.lidar1_buffer = lidar1_buffer
|
||||
self.lidar2_buffer = lidar2_buffer
|
||||
|
||||
self.lidar1_frame = lidar1_frame
|
||||
self.lidar2_frame = lidar2_frame
|
||||
|
||||
@@ -94,16 +89,13 @@ class LidarTransformPublisher(Node):
|
||||
self.pcd_1 = self.pcd_buffer_to_o3d(self.lidar1_buffer)
|
||||
self.pcd_2 = self.pcd_buffer_to_o3d(self.lidar2_buffer)
|
||||
|
||||
# Compute transform once
|
||||
self.T = self.compute_transform()
|
||||
self.get_logger().info(f"Computed initial transform:\n{self.T}")
|
||||
|
||||
# Prepare translation and rotation
|
||||
T_copy = np.array(self.T, copy=True)
|
||||
trans = T_copy[:3, 3]
|
||||
rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat() # [x, y, z, w]
|
||||
rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat()
|
||||
|
||||
# Create static TransformStamped
|
||||
t = TransformStamped()
|
||||
t.header.stamp.sec = 0
|
||||
t.header.stamp.nanosec = 0
|
||||
@@ -117,15 +109,12 @@ class LidarTransformPublisher(Node):
|
||||
t.transform.rotation.z = rot_quat[2]
|
||||
t.transform.rotation.w = rot_quat[3]
|
||||
|
||||
# Publish once
|
||||
self.br.sendTransform(t)
|
||||
self.get_logger().info("Published static transform.")
|
||||
|
||||
def pcd_buffer_to_o3d(self, buffer: BytesIO):
|
||||
buffer.seek(0)
|
||||
header_lines = []
|
||||
|
||||
# Read header lines until 'DATA' line
|
||||
while True:
|
||||
line = buffer.readline().decode('ascii').strip()
|
||||
if not line:
|
||||
@@ -135,69 +124,84 @@ class LidarTransformPublisher(Node):
|
||||
data_line = line
|
||||
break
|
||||
|
||||
# Ensure binary format
|
||||
if not data_line.lower().startswith("data binary"):
|
||||
raise NotImplementedError("Only binary PCD supported for buffer parsing")
|
||||
raise NotImplementedError("Only binary PCD supported")
|
||||
|
||||
# Parse number of points
|
||||
num_points = 0
|
||||
for line in header_lines:
|
||||
if line.startswith("POINTS"):
|
||||
num_points = int(line.split()[1])
|
||||
if line.startswith("FIELDS"):
|
||||
fields = line.split()[1:] # Expect: x y z intensity rgb
|
||||
|
||||
if num_points == 0:
|
||||
raise ValueError("PCD header does not specify number of points")
|
||||
raise ValueError("PCD header missing point count")
|
||||
|
||||
# Define numpy dtype based on expected fields
|
||||
dtype = np.dtype([
|
||||
('x', 'f4'),
|
||||
('y', 'f4'),
|
||||
('z', 'f4'),
|
||||
('intensity', 'f4'),
|
||||
('rgb', 'u4')
|
||||
('x', 'f4'), ('y', 'f4'), ('z', 'f4'),
|
||||
('intensity', 'f4'), ('rgb', 'u4')
|
||||
])
|
||||
|
||||
# Read binary data
|
||||
data = buffer.read(num_points * dtype.itemsize)
|
||||
points_array = np.frombuffer(data, dtype=dtype, count=num_points)
|
||||
|
||||
# Convert to Open3D point cloud
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
xyz = np.stack([points_array['x'], points_array['y'], points_array['z']], axis=-1)
|
||||
pcd.points = o3d.utility.Vector3dVector(xyz)
|
||||
|
||||
return pcd
|
||||
|
||||
def compute_transform(self):
|
||||
# Downsample
|
||||
voxel_size = 0.05
|
||||
pcd_1_ds = self.pcd_1.voxel_down_sample(voxel_size)
|
||||
pcd_2_ds = self.pcd_2.voxel_down_sample(voxel_size)
|
||||
voxel_size = 0.2 # coarse-to-fine pyramid base
|
||||
|
||||
# Estimate normals
|
||||
pcd_1_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
|
||||
pcd_2_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
|
||||
# --- Feature extraction ---
|
||||
def preprocess(pcd, voxel):
|
||||
pcd_down = pcd.voxel_down_sample(voxel)
|
||||
pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel*2, max_nn=30))
|
||||
fpfh = o3d.pipelines.registration.compute_fpfh_feature(
|
||||
pcd_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel*5, max_nn=100))
|
||||
return pcd_down, fpfh
|
||||
|
||||
# ICP registration
|
||||
threshold = 0.5
|
||||
reg_result = o3d.pipelines.registration.registration_icp(
|
||||
pcd_2_ds, pcd_1_ds, threshold,
|
||||
np.eye(4),
|
||||
o3d.pipelines.registration.TransformationEstimationPointToPoint()
|
||||
src_down, src_fpfh = preprocess(self.pcd_2, voxel_size)
|
||||
tgt_down, tgt_fpfh = preprocess(self.pcd_1, voxel_size)
|
||||
|
||||
# --- Global alignment with RANSAC ---
|
||||
result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
|
||||
src_down, tgt_down, src_fpfh, tgt_fpfh, True,
|
||||
1.5, # distance threshold
|
||||
o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
|
||||
4,
|
||||
[
|
||||
o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
|
||||
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(1.5)
|
||||
],
|
||||
o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500)
|
||||
)
|
||||
return reg_result.transformation
|
||||
|
||||
trans_init = result_ransac.transformation
|
||||
|
||||
# --- Multi-scale ICP refinement ---
|
||||
voxel_radii = [0.2, 0.1, 0.05]
|
||||
max_iters = [50, 30, 14]
|
||||
transformation = trans_init
|
||||
for radius, iters in zip(voxel_radii, max_iters):
|
||||
src_down = self.pcd_2.voxel_down_sample(radius)
|
||||
tgt_down = self.pcd_1.voxel_down_sample(radius)
|
||||
src_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius*2, max_nn=30))
|
||||
tgt_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius*2, max_nn=30))
|
||||
|
||||
result_icp = o3d.pipelines.registration.registration_icp(
|
||||
src_down, tgt_down, radius*2, transformation,
|
||||
o3d.pipelines.registration.TransformationEstimationPointToPlane()
|
||||
)
|
||||
transformation = result_icp.transformation
|
||||
|
||||
return transformation
|
||||
|
||||
|
||||
def monitor_nodes(nodes, publisher):
|
||||
"""Separate thread that monitors node status and shuts down ROS when done."""
|
||||
while rclpy.ok():
|
||||
if all(node.finished for node in nodes):
|
||||
print("All pointclouds captured")
|
||||
publisher.publish()
|
||||
return
|
||||
time.sleep(0.1) # check periodically
|
||||
time.sleep(0.1)
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
@@ -205,10 +209,9 @@ def main():
|
||||
buffer_livox = BytesIO()
|
||||
|
||||
executor = MultiThreadedExecutor()
|
||||
|
||||
nodes = [
|
||||
PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 200),
|
||||
PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 500),
|
||||
PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 350),
|
||||
PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
|
||||
]
|
||||
publisher = LidarTransformPublisher(buffer_velodyne, 'velodyne', buffer_livox, 'frame_default')
|
||||
|
||||
@@ -226,4 +229,3 @@ def main():
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user