Is it possible to Post-Process the OS1 data collected to get it to fit surveyed ground control points? Does this result in a modified SLAM trajectory that I can use with other cameras and sensors?
Yeah it is definitely possible, however, we don’t have such capability yet. The fitted data would have a modified trajectory.
1 Like
I agree with Samahu. If you have any more information about your data collection, your sensor, or your environment that would help us give you better advice.
What you are trying to do is sometimes called “pose graph optimization” or “bundle adjustment”. The Ouster SDK doesn’t support this, but it is possible to do with Ouster lidar data and with the aid of the SDK if you want. I’m assuming you’ve successfully installed the ouster-sdk (pip-install ouster-sdk) and are familiar with the command line utility ouster-cli.
The approximate steps will be:
- Generate an initial trajectory for your data. You can do this with a terminal command like
ouster-cli source FILE slam save FILE.osfto store poses in a .OSF file. If you want to visualize the map as it’s being saved, you can addviz --accum-num 100 --accum-every-m 10to the CLI command as is, or you can open the file afterwards with just thevizcommand. - Identify your ground control points in as many LidarScans as you can and record their coordinates in a constraints file noting which ground control point each point in the LidarScan is associated with.
- Load the constraints file and re-optimize the poses using the constraints. This is where pose graph optimization comes in and this isn’t a trivial step if you are unfamiliar with it, but there are many existing repos to investigate.
A Quick Shortcut
If you want a quick solution that will get you a registered map in generally the right location but won’t correct for drift error in the initial poses calculated with slam, then you can do a rigid registration (python - Rigid registration of two point clouds with known correspondence - Stack Overflow) of your trajectory to your control points. This approach only works if the initial poses you’ve calculated have reasonably low drift which you can visualize with ouster-cli
Here’s a script showing how simple this can be:
import numpy as np
import cv2
from ouster.sdk.osf import Writer
from ouster.sdk.open_source import open_source
from ouster.sdk.client import XYZLut, ChanField, dewarp, destagger
FILE_WITH_POSES = "file.osf" # Put your filename here
def rigid_registration(a, b):
##Based on Arun et al., 1987
# Take transpose as columns should be the points
p1 = a.transpose()
p2 = b.transpose()
# Calculate centroids
p1_c = np.mean(p1, axis=1).reshape((-1, 1))
p2_c = np.mean(p2, axis=1).reshape((-1, 1))
# Subtract centroids
q1 = p1 - p1_c
q2 = p2 - p2_c
# Calculate covariance matrix
H = np.matmul(q1, q2.transpose())
# Calculate singular value decomposition (SVD)
U, X, V_t = np.linalg.svd(H) # the SVD of linalg gives you Vt
# Calculate rotation matrix
R = np.matmul(V_t.transpose(), U.transpose())
assert np.allclose(np.linalg.det(R), 1.0), "Rotation matrix of N-point registration not 1, see paper Arun et al."
# Calculate translation matrix
T = p2_c - np.matmul(R, p1_c)
M_a2b = np.c_[R, T]
M_a2b = np.r_[M_a2b, [[0, 0, 0, 1]]]
return M_a2b
##############################################################################
# PART 1
# Define at least three control point coordinates
CONTROL_POINTS_XYZ = np.array([
[0, 0, 0],
[0, 1, 0],
[1, 0, 0]
])
lidar_points_xyz = np.empty((0, 3))
control_points_xyz = np.empty((0, 3))
def click_callback(event, x, y, flags, param):
global lidar_points_xyz, control_points_xyz
if event == cv2.EVENT_LBUTTONDOWN:
if valid[y, x]:
while True:
print(f"Select which control point you are assigning: 0-{CONTROL_POINTS_XYZ.shape[0]-1}")
key = chr(cv2.waitKey(0))
if int(key) in np.arange(CONTROL_POINTS_XYZ.shape[0]):
ctrl_idx = int(key)
print(f"XYZ value at ({x},{y}): {xyz[y, x, :]} for control point {ctrl_idx}")
lidar_points_xyz = np.vstack([lidar_points_xyz, xyz[y, x, :]])
control_points_xyz = np.vstack([control_points_xyz, CONTROL_POINTS_XYZ[ctrl_idx, :]])
cv2.imshow("image", cv2.circle(img, (x, y), 3, (0, 0, 1)))
break
else:
print("Selected point has no valid range.")
scans = open_source(FILE_WITH_POSES)
xyzlut = XYZLut(scans.metadata)
cv2.namedWindow('image')
cv2.setMouseCallback('image', click_callback)
for scan in scans[::10]: # Look at every 10th scan
# destagger all data so it's human-viewable
valid = destagger(scans.metadata, scan.field(ChanField.RANGE))
# Apply the slam calculated poses using the dewarp function to put data in a global frame
xyz = destagger(scans.metadata, dewarp(xyzlut(scan.field(ChanField.RANGE)), scan.pose))
img = destagger(scans.metadata, scan.field(ChanField.REFLECTIVITY)/64)[..., np.newaxis].repeat(3, axis=-1)
cv2.imshow("image", img)
if cv2.waitKey(0) == ord('q'):
break
cv2.destroyAllWindows()
##############################################################################
# PART 2:
print("Transform and re-save")
# Rigidly register them
M_a2b = rigid_registration(a=lidar_points_xyz, b=control_points_xyz)
# Apply the transformation to the original poses and save
scans = open_source(FILE_WITH_POSES)
registered_scans = Writer("registered.osf", scans.metadata)
for i, scan in enumerate(scans):
print(f"Processing frame {i} of {scans.scans_num}") if i % 100 == 0 else None
# Apply the rigid transform to put the poses in the control point coordinate frame
scan.pose[:] = M_a2b[np.newaxis, ...] @ scan.pose
# Save to a new OSF file
registered_scans.save([scan])
registered_scans.close()
Finally, there are programs like interactive_slam that allow you to re-optimize a trajectory. I have not tested any of them. You would have to convert the Ouster lidar data to a compatible format.
2 Likes
@yggsrasil This is exactly what I was looking for> do not have a sensor yet. The poses .OSF file.
Thank you.
@Hutch07 following up on @yggdrasil answer if you’d like to try interactive_slam against any of ouster osf sequences that has a slam trajectory feel free to use the following script to export to the format that interactive_slam can work with:
from pathlib import Path
import argparse
import open3d as o3d
import numpy as np
from ouster.sdk import open_source
from ouster.sdk.client import ChanField, XYZLut
from ouster.sdk.client import (first_valid_packet_ts,
first_valid_column_ts,
first_valid_column_pose)
from tqdm import tqdm
def dump_point_cloud(source_url, output_folder):
"""Dumps invdividual point cloud frames into separate pcd files along with odometry"""
scan_source = open_source(source_url, index=True)
xyzlut = XYZLut(scan_source.metadata, use_extrinsics=True)
if not output_folder:
output_folder = Path(source_url).stem
print(f"saving the results to {output_folder}")
pcd_dir = Path(output_folder)
pcd_dir.mkdir(parents=True, exist_ok=True)
for scan in tqdm(scan_source):
xyz = xyzlut(scan.field(ChanField.RANGE)).astype(np.float32)
pcd = o3d.t.geometry.PointCloud()
pcd.point.positions = o3d.core.Tensor(xyz.reshape(-1, 3))
key = scan.field(ChanField.SIGNAL).astype(np.float32)
pcd.point.intensity = o3d.core.Tensor(key.reshape(-1, 1))
scan_ts = first_valid_column_ts(scan)
ss = int(scan_ts // 1e9)
ns = int(scan_ts % 1e9)
fname = F"{ss}_{ns}"
odom_path = str(pcd_dir / F"{fname}.odom")
scan_odom = first_valid_column_pose(scan)
np.savetxt(odom_path, scan_odom)
pcd_path = str(pcd_dir / F"{fname}.pcd")
o3d.t.io.write_point_cloud(pcd_path, pcd, write_ascii=False)
def main():
parser = argparse.ArgumentParser(description="dumps point loud")
parser.add_argument("input_path", help="path to scan source")
parser.add_argument("-d", "--dir", default="", help="output folder name for dumped pcd")
args = parser.parse_args()
dump_point_cloud(args.input_path, args.dir)
if __name__ == "__main__":
main()
You need ouster-sdk, open3d to run this script, you can invoke this script as follows:
python3 script input.osf --dir output_dir
Then use the odometry2graph tool from interactive_slam to load the output_dir from previous step and save the output to a new folder then you may use interactive_slam to optimize the trajectory.
Note that interactive_slam has plenty of options for automatic optimizations and other tools for configuring certain constraints manually, you can refer to their respective wiki for use cases and further details.
1 Like
Fantastic,
Thank you for the information @Samahu. I will try this. I was looking at the Git from koide3 interactive_slam