File size: 14,723 Bytes
740910f | 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 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 | """
Vecros Assignment - Main Entry Point
=====================================
This script provides a unified interface to run both problems:
- Problem 1: Stereo Vision Feature Detection, Matching & Camera Pose Estimation
- Problem 2: 3D Multi-Agent Pathfinding with Collision Avoidance
Usage:
python main.py --problem 1 --dataset kitti --sequence 00
python main.py --problem 2 --start1 0,0,0 --end1 50,50,50 --start2 100,100,100 --end2 50,50,50
Author: Vecros Assignment Submission
"""
import argparse
import numpy as np
from pathlib import Path
import sys
def run_problem1_kitti(data_path: str, sequence: str = "00", output_dir: str = "results/problem1"):
"""
Run Problem 1 with KITTI odometry dataset.
Args:
data_path: Path to KITTI odometry dataset
sequence: Sequence number (e.g., "00")
output_dir: Output directory for results
"""
from problem1_stereo_vision import run_problem1
try:
import pykitti
except ImportError:
print("ERROR: pykitti not installed. Run: pip install pykitti")
return None
print(f"\nLoading KITTI Odometry Sequence {sequence}...")
# Load KITTI data
data = pykitti.odometry(data_path, sequence)
# Get camera intrinsics from calibration
# P0 is the projection matrix for left grayscale camera
P = data.calib.P_rect_00
K = P[:3, :3]
print(f"Camera intrinsic matrix K:\n{K}")
print(f"Number of frames: {len(data.cam0_files)}")
# Use first two frames for demonstration
img1_path = data.cam0_files[0]
img2_path = data.cam0_files[1]
print(f"Image 1: {img1_path}")
print(f"Image 2: {img2_path}")
# Get ground truth poses
poses = data.poses
if len(poses) >= 2:
T1 = poses[0] # 4x4 transformation matrix
T2 = poses[1]
# Relative transformation from frame 1 to frame 2
T_rel = np.linalg.inv(T1) @ T2
R_gt = T_rel[:3, :3]
t_gt = T_rel[:3, 3:]
print(f"\nGround truth relative pose:")
print(f"Rotation:\n{R_gt}")
print(f"Translation: {t_gt.T}")
else:
print("\nWARNING: Ground truth poses not available")
R_gt = None
t_gt = None
# Run the pipeline
results = run_problem1(
img1_path=img1_path,
img2_path=img2_path,
K=K,
R_gt=R_gt,
t_gt=t_gt,
output_dir=output_dir
)
return results
def run_problem1_middlebury(data_path: str, output_dir: str = "results/problem1"):
"""
Run Problem 1 with Middlebury stereo dataset.
Args:
data_path: Path to Middlebury scene folder
output_dir: Output directory for results
"""
from problem1_stereo_vision import run_problem1
data_path = Path(data_path)
# Find stereo images
img1_path = data_path / "im0.png"
img2_path = data_path / "im1.png"
if not img1_path.exists():
# Try alternative names
for ext in ['png', 'jpg', 'pgm']:
img1_path = list(data_path.glob(f"*left*.{ext}")) or list(data_path.glob(f"*0.{ext}"))
img2_path = list(data_path.glob(f"*right*.{ext}")) or list(data_path.glob(f"*1.{ext}"))
if img1_path and img2_path:
img1_path = img1_path[0]
img2_path = img2_path[0]
break
print(f"Image 1: {img1_path}")
print(f"Image 2: {img2_path}")
# Load calibration if available
calib_path = data_path / "calib.txt"
if calib_path.exists():
# Parse Middlebury calibration format
with open(calib_path, 'r') as f:
lines = f.readlines()
# This is simplified - actual parsing depends on format
# Default focal length for Middlebury (approximate)
K = np.array([
[3000, 0, 1500],
[0, 3000, 1000],
[0, 0, 1]
], dtype=float)
else:
# Default camera matrix (approximate for Middlebury)
K = np.array([
[3000, 0, 1500],
[0, 3000, 1000],
[0, 0, 1]
], dtype=float)
print(f"\nUsing camera matrix K:\n{K}")
print("(Note: For Middlebury, ground truth pose is not directly available)")
# Run pipeline without ground truth
results = run_problem1(
img1_path=str(img1_path),
img2_path=str(img2_path),
K=K,
R_gt=None,
t_gt=None,
output_dir=output_dir
)
return results
def run_problem1_custom(img1_path: str, img2_path: str, fx: float, fy: float,
cx: float, cy: float, output_dir: str = "results/problem1"):
"""
Run Problem 1 with custom images and camera parameters.
Args:
img1_path: Path to first image
img2_path: Path to second image
fx, fy: Focal lengths
cx, cy: Principal point
output_dir: Output directory for results
"""
from problem1_stereo_vision import run_problem1
K = np.array([
[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]
])
results = run_problem1(
img1_path=img1_path,
img2_path=img2_path,
K=K,
R_gt=None,
t_gt=None,
output_dir=output_dir
)
return results
def run_problem2_unified(path_requests: list, grid_size: int = 101, velocity: float = 1.0,
weight_ratio: float = 0.1, seed: int = 42,
solver: str = 'fast', output_dir: str = "results/problem2"):
"""
Run Problem 2 using the consolidated solver.
Args:
path_requests: List of (start, goal) tuples
grid_size: Size of grid
velocity: Velocity
weight_ratio: Weight ratio
seed: Random seed
solver: 'fast' or 'cbs'
output_dir: Output directory
"""
try:
from problem2 import run_problem2
results = run_problem2(
path_requests=path_requests,
grid_size=grid_size,
velocity=velocity,
weight_ratio=weight_ratio,
seed=seed,
solver=solver,
output_dir=output_dir
)
return results
except ImportError as e:
print(f"ERROR: Could not import problem2.py: {e}")
sys.exit(1)
def parse_point(s: str) -> tuple:
"""Parse a comma-separated point string like '0,0,0' to tuple."""
parts = s.strip().split(',')
if len(parts) != 3:
raise ValueError(f"Expected 3 coordinates, got {len(parts)}")
return tuple(int(p.strip()) for p in parts)
def parse_path(s: str) -> tuple:
"""Parse a path string like '0,0,0:50,50,50' to (start, goal) tuple."""
parts = s.split(':')
if len(parts) != 2:
raise ValueError(f"Expected format 'start:end', got '{s}'")
return (parse_point(parts[0]), parse_point(parts[1]))
def main():
parser = argparse.ArgumentParser(
description="Vecros Assignment - Stereo Vision & 3D Pathfinding",
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Examples:
# Run Problem 1 with KITTI dataset
python main.py --problem 1 --dataset kitti --data-path ./data/kitti --sequence 00
# Run Problem 1 with Middlebury dataset
python main.py --problem 1 --dataset middlebury --data-path ./data/Adirondack
# Run Problem 1 with custom images
python main.py --problem 1 --dataset custom --img1 left.png --img2 right.png --fx 700 --fy 700 --cx 320 --cy 240
# Run Problem 2 with 2 agents (legacy format)
python main.py --problem 2 --start1 0,0,0 --end1 50,50,50 --start2 100,100,100 --end2 50,50,50
# Run Problem 2 with 3+ agents using --paths (NEW!)
python main.py --problem 2 --paths "0,0,0:20,20,20" "40,40,40:20,20,20" "0,40,0:20,20,20"
# Run Problem 2 with CBS optimal solver
python main.py --problem 2 --solver cbs --paths "0,0,0:15,15,15" "30,30,30:15,15,20"
"""
)
# General arguments
parser.add_argument('--problem', type=int, choices=[1, 2], required=True,
help='Problem number (1 or 2)')
parser.add_argument('--output-dir', type=str, default='results',
help='Output directory for results')
# Problem 1 arguments
parser.add_argument('--dataset', type=str, choices=['kitti', 'middlebury', 'custom'],
help='Dataset type for Problem 1')
parser.add_argument('--data-path', type=str,
help='Path to dataset folder')
parser.add_argument('--sequence', type=str, default='00',
help='KITTI sequence number')
parser.add_argument('--img1', type=str,
help='Path to first image (custom dataset)')
parser.add_argument('--img2', type=str,
help='Path to second image (custom dataset)')
parser.add_argument('--fx', type=float, default=700,
help='Focal length x (custom dataset)')
parser.add_argument('--fy', type=float, default=700,
help='Focal length y (custom dataset)')
parser.add_argument('--cx', type=float, default=320,
help='Principal point x (custom dataset)')
parser.add_argument('--cy', type=float, default=240,
help='Principal point y (custom dataset)')
# Problem 2 arguments
parser.add_argument('--paths', type=str, nargs='+',
help='Paths in format "start:end" e.g. "0,0,0:50,50,50" (supports 3+ agents)')
parser.add_argument('--start1', type=str, default='0,0,0',
help='Start point for agent 1 (x,y,z)')
parser.add_argument('--end1', type=str, default='25,25,25',
help='End point for agent 1 (x,y,z)')
parser.add_argument('--start2', type=str, default='50,50,50',
help='Start point for agent 2 (x,y,z)')
parser.add_argument('--end2', type=str, default='25,25,25',
help='End point for agent 2 (x,y,z)')
parser.add_argument('--start3', type=str, default=None,
help='Start point for agent 3 (x,y,z)')
parser.add_argument('--end3', type=str, default=None,
help='End point for agent 3 (x,y,z)')
parser.add_argument('--grid-size', type=int, default=101,
help='Grid size (0 to grid-size-1)')
parser.add_argument('--velocity', type=float, default=1.0,
help='Travel velocity (m/s)')
parser.add_argument('--weight-ratio', type=float, default=0.1,
help='Ratio of weighted grid points')
parser.add_argument('--seed', type=int, default=42,
help='Random seed for reproducibility')
parser.add_argument('--solver', type=str, choices=['optimized', 'cbs', 'compare'], default='optimized',
help='Solver: "optimized" (fast), "cbs" (optimal), or "compare" (run both)')
args = parser.parse_args()
if args.problem == 1:
# Run Problem 1
if not args.dataset:
print("ERROR: --dataset is required for Problem 1")
print("Options: kitti, middlebury, custom")
sys.exit(1)
output_dir = f"{args.output_dir}/problem1"
if args.dataset == 'kitti':
if not args.data_path:
print("ERROR: --data-path is required for KITTI dataset")
sys.exit(1)
run_problem1_kitti(args.data_path, args.sequence, output_dir)
elif args.dataset == 'middlebury':
if not args.data_path:
print("ERROR: --data-path is required for Middlebury dataset")
sys.exit(1)
run_problem1_middlebury(args.data_path, output_dir)
elif args.dataset == 'custom':
if not args.img1 or not args.img2:
print("ERROR: --img1 and --img2 are required for custom dataset")
sys.exit(1)
run_problem1_custom(args.img1, args.img2, args.fx, args.fy,
args.cx, args.cy, output_dir)
elif args.problem == 2:
output_dir = f"{args.output_dir}/problem2"
# Check if using new --paths format
if args.paths:
try:
path_requests = [parse_path(p) for p in args.paths]
print(f"Planning {len(path_requests)} agent paths...")
except ValueError as e:
print(f"ERROR: Invalid path format: {e}")
print("Use format: 'start_x,start_y,start_z:end_x,end_y,end_z'")
sys.exit(1)
else:
# Check if any legacy start/end args were provided
if args.start1 != '0,0,0' or args.end1 != '25,25,25':
# User provided specific points via legacy args
try:
path_requests = [
(parse_point(args.start1), parse_point(args.end1)),
(parse_point(args.start2), parse_point(args.end2))
]
if args.start3 and args.end3:
path_requests.append((parse_point(args.start3), parse_point(args.end3)))
except ValueError as e:
print(f"ERROR: Invalid point format: {e}")
sys.exit(1)
else:
# No paths provided -> Use 4-Agent Default
path_requests = [
((0, 0, 0), (15, 15, 15)),
((30, 30, 30), (15, 15, 20)),
((0, 30, 0), (20, 15, 15)),
((30, 0, 30), (15, 20, 15)),
]
# Map 'optimized' to 'fast' for problem2.py
solver_map = {'optimized': 'fast', 'cbs': 'cbs', 'compare': 'compare'}
solver_type = solver_map.get(args.solver, 'fast')
run_problem2_unified(
path_requests=path_requests,
grid_size=args.grid_size,
velocity=args.velocity,
weight_ratio=args.weight_ratio,
seed=args.seed,
solver=solver_type,
output_dir=output_dir
)
if __name__ == "__main__":
main()
|