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()