68 lines
2.3 KiB
Python
68 lines
2.3 KiB
Python
import argparse
|
|
from pathlib import Path
|
|
import sys
|
|
import time
|
|
|
|
import cv2
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
if str(ROOT) not in sys.path:
|
|
sys.path.insert(0, str(ROOT))
|
|
|
|
from localcast.sensor_fusion import (
|
|
PointCloud,
|
|
load_fusion_config,
|
|
match_surface_features,
|
|
orb_surface_observations,
|
|
triangulate_surface_tracks,
|
|
)
|
|
|
|
|
|
def main() -> None:
|
|
parser = argparse.ArgumentParser(description="Match surface features between two calibrated views and write a PLY.")
|
|
parser.add_argument("--config", default=str(ROOT / "config" / "sensor-fusion.json"))
|
|
parser.add_argument("--left-id", required=True)
|
|
parser.add_argument("--right-id", required=True)
|
|
parser.add_argument("--left-image", required=True)
|
|
parser.add_argument("--right-image", required=True)
|
|
parser.add_argument("--output", required=True)
|
|
parser.add_argument("--max-features", type=int, default=2000)
|
|
parser.add_argument("--max-descriptor-distance", type=float, default=44.0)
|
|
parser.add_argument("--max-reprojection-error-px", type=float, default=4.0)
|
|
args = parser.parse_args()
|
|
|
|
rig = load_fusion_config(Path(args.config))
|
|
left = cv2.imread(args.left_image)
|
|
right = cv2.imread(args.right_image)
|
|
if left is None:
|
|
raise SystemExit(f"Could not read {args.left_image}")
|
|
if right is None:
|
|
raise SystemExit(f"Could not read {args.right_image}")
|
|
timestamp_ns = time.monotonic_ns()
|
|
left_features = orb_surface_observations(args.left_id, left, timestamp_ns, max_features=args.max_features)
|
|
right_features = orb_surface_observations(args.right_id, right, timestamp_ns, max_features=args.max_features)
|
|
tracks = match_surface_features(
|
|
left_features,
|
|
right_features,
|
|
max_descriptor_distance=args.max_descriptor_distance,
|
|
max_dt_ns=25_000_000,
|
|
)
|
|
points = triangulate_surface_tracks(
|
|
tracks,
|
|
rig.cameras,
|
|
max_reprojection_error_px=args.max_reprojection_error_px,
|
|
)
|
|
PointCloud(points).write_ply(Path(args.output))
|
|
print(
|
|
{
|
|
"left_features": len(left_features),
|
|
"right_features": len(right_features),
|
|
"tracks": len(tracks),
|
|
"points": len(points),
|
|
"output": args.output,
|
|
}
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|