Autonomous Driving | Chang Nie

Production-Grade 4D Auto-Annotation: Multi-Camera Rig SfM & High-Precision Geometry

Thu, 20 Jun 2024 00:00:00 +0000

Project context

NETA Auto Collaboration · Automotive 3D Reconstruction & 4D Annotation

Meeting the demand for massive high-precision training ground truth in autonomous driving, this project established an industrial data pipeline spanning sensor sync, semantic perception, fused localization, rig SfM, and multi-run map aggregation, substantially improving offline labeling throughput.

Production Deliverables & Automotive Validation: Automotive 4D auto-annotation production pipeline; processed 1,000+ km fleet data (sub-5 cm cross-run registration), online extrinsic self-calibration, and 80%+ labeling efficiency gain

My contributions

Led a 5-person algorithm team engineering the multi-camera 3D reconstruction and 4D annotation pipeline; processed 1,000+ km multi-run fleet data with sub-5 cm cross-run spatial alignment, boosting annotation efficiency by over 80%.

As autonomous driving perception models shift from single-frame monocular detection toward bird’s-eye-view (BEV) architectures, 3D occupancy networks, and end-to-end foundation models, the availability of massive, temporally consistent 4D ground truth—encompassing metric road surfaces, 3D lane topologies, and static infrastructure—has become the definitive catalyst for algorithmic advancement.

Conventional workflows relying on mobile LiDAR scanning paired with manual outsourcing suffer from long turnaround times, high labeling costs, and limited generalization across heterogeneous sensor layouts. However, naively transplanting classical Structure-from-Motion (SfM) pipelines directly onto vehicle surround-view video streams immediately encounters severe physical and mathematical degeneracies: collinear motion corridors, textureless road surfaces, moving self-vehicle shadows, and unconstrained camera extrinsics drift.

In partnership with leading electric vehicle manufacturer NETA Auto, we developed and deployed an automotive-grade 4D vision auto-annotation data factory. As Project Tech Lead, I directed the overall algorithmic architecture and led the development of the core rigid multi-camera rig SfM reconstruction engine, ground-feature decoupling, spatiotemporal-guided dense multi-view stereo, and multi-session cross-epoch aggregation. The core geometric optimization principles and engineering systems developed during this industrial deployment were subsequently published at .

Why Driving Scenarios Break Classic SfM

In standard photogrammetry textbooks, classic SfM frameworks (such as COLMAP or VisualSFM) are built on the foundational assumption of an operator walking around an isolated, static monument with a single camera, capturing omnidirectional views with wide convergent baselines. Onboard vehicular data collected on highways and urban thoroughfares violates nearly every one of these assumptions:

Straight Corridor Motion & Collinear Triangulation Degeneracy: On highways and arterial avenues, vehicles travel along straight corridors for kilometers at a time. The optical centers of front and rear cameras align along a 1D straight line. In multi-view epipolar geometry, pure forward translation with collinear centers is ill-conditioned. When moving directly forward along the optical axis, the epipole coincides with the image principal center. Rays originating near the optical axis intersect at near-zero parallax angles. Sub-pixel feature matching noise is amplified along the longitudinal axis, causing reconstructed 3D points to stretch and drift along the direction of travel.
Extrinsics Drift in Unconstrained Multi-Camera Optimization: Production autonomous vehicles feature 6 to 8 surround cameras rigidly bolted to the vehicle body (front wide, front telephoto, front-left, front-right, rear-left, rear-right). Standard SfM treats each camera at each timestamp as an unconstrained 6-DoF pose variable. For an $N$-frame sequence, this introduces $6N$ independent pose variables. As pairwise feature matching accumulates noise, unconstrained optimization causes the physically rigid sensor extrinsics to artificially drift over time, introducing severe ghosting and discontinuous stitching artifacts in downstream BEV projections.
Textureless Asphalt & Moving Self-Vehicle Shadows: The lower half of onboard camera images is dominated by asphalt surfaces covering $>40\%$ of the image area. Asphalt is visually homogeneous, lacking sharp corner features under direct sunlight. Even more destructive is the vehicle’s dynamic shadow cast onto the road surface: the shadow boundary presents high-contrast, sharp edges that classical feature detectors (such as SIFT or Harris) aggressively extract as salient landmarks. However, the shadow translates synchronously with the vehicle, violating the static scene assumption. Triangulating shadow features forces the optimizer to fit non-existent points beneath the chassis, pulling vehicle trajectory estimates off course.
Multi-Session Cross-Epoch Aggregation Without Shared Image Matches: Autonomous driving maps require merging multiple independent drives through the same intersection collected across different days (e.g., overcast Monday morning vs. sunset Friday evening). Variable solar angles, changing shadows, and transient traffic mean disparate driving runs share zero visual feature correspondences, leaving independent reconstructions geometrically isolated.

Full Pipeline Architecture: From Raw Logs to 4D Vector Maps

To process tens of thousands of kilometers of production fleet video logs with high throughput, we established a decoupled, fail-safe processing pipeline:

Wider program

4D vision auto-annotation system

From synchronized vehicle data to a structured local map, with multi-camera SfM supplying geometry for annotation and mapping.

Upstream foundations

01Data preparationCameras · IMU · GNSS · chassis · calibration

02Semantic perceptionScene and lane masks

03Sensor-fusion localizationMetric vehicle trajectory

04 · My geometry workMulti-camera SfM reconstruction and system interfacesProgram coordination · poses and extrinsics · sparse/dense geometry · downstream delivery

Downstream applications

05Ground reconstructionDense road surface

06Auto-annotationLanes · signs · static elements

07Multi-run local mapAggregation and topology

Multi-Sensor Spatiotemporal Synchronization: Aligning six surround cameras, high-rate IMUs, GNSS receivers, and wheel odometry into a common timestamp reference and vehicle rear-axle coordinate frame;
Dynamic Semantic Masking: Deploying lightweight segmentation models to isolate moving vehicles, cyclists, pedestrians, and sky regions, shielding static 3D geometry from dynamic clutter;
Fused Inertial-GNSS Initialization: Fusing satellite positioning and dead-reckoning to initialize metric-scale trajectory guesses;
Rig-Centric Multi-Camera SfM (Core Engine): Treating surround cameras as a unified rigid rig to jointly solve vehicle poses and calibrate sensor extrinsics, generating a centimeter-level sparse road skeleton;
Spatiotemporal-Guided Dense MVS: Leveraging motion geometry priors to prune unshared image pairs, accelerating PatchMatch multi-view stereo to produce dense ground meshes;
Multi-Session Cross-Epoch Aggregation: Aligning independent driving passes coarse-to-fine into a globally seamless local high-definition map.

01Inputs & calibration Six-view imagery · multi-sensor pose · semantic masks · initial calibration

02Constrained association SuperPoint · temporal/overlap priors · two-view geometry

03Rig-aware sparse SfM Initialization · rig registration · triangulation · RIG BA · outlier filtering

04Densification & aggregation ACMP depth · COLMAP fusion · ground filtering · multi-run alignment

05Engineering outputs Refined poses/extrinsics · sparse/dense ground cloud · downstream priors

Core Algorithmic Design & Engineering Implementation

1. Rig Bundle Adjustment (Rig BA) with Rigid-Rig Parameterization

To eliminate extrinsics drift and reduce optimization dimensionality, we reformulated the multi-view optimization variables: rather than treating every frame as an independent pose, all surround cameras captured at timestamp $t$ are grouped into a rigid camera set. A camera’s absolute world pose is factored into a time-varying vehicle chassis pose and a static chassis-to-camera mounting extrinsic.

Under this parameterization, the number of pose variables collapses dramatically. We formulate the Rig Bundle Adjustment (Rig BA) framework to jointly optimize vehicle chassis trajectories, mounting extrinsics, and 3D static landmark coordinates.

This rigid formulation delivers two decisive advantages:

Complementary Viewpoint Geometry: When the vehicle drives into direct sun glare or passes alongside large trucks, forward cameras may suffer severe overexposure or visual occlusion. However, lateral and rear cameras continue tracking static architecture, trees, and street furniture. Through fixed internal rig extrinsics, unobstructed cameras constrain the chassis pose, eliminating longitudinal depth stretching;
Online Extrinsic Self-Calibration: By treating mounting extrinsics as optimization parameters, the solver refines mounting extrinsics over multi-kilometer runs to high precision, absorbing mechanical chassis flexing.

Exploiting multi-camera overlap priors removes numerous visually similar but physically impossible cross-camera false matches.

2. Spatial Overlap Constraints & Epipolar Pruning

Exhaustive pairwise matching across $N$ timestamps and $M$ cameras incurs an intractable $O(M \cdot N^2)$ computational explosion.

We introduced structured camera rig overlap priors:

A front-wide camera and front-left camera share only $\sim 30^\circ$ of horizontal FOV overlap. The right half of the front camera image cannot geometrically intersect the front-left camera’s viewing frustum;
Restricting feature searches strictly to overlapping spatial frustums ($u < W/2$ on the front camera) eliminates false associations between visually repetitive facades;
Enforcing Essential matrix $E$ epipolar distance checks and Homography $H$ planar constraints removes $>95\%$ of outlier correspondences while reducing matching runtime by $80\%$.

3. Decoupled Ground-Plane Filtering Prior to Triangulation

To prevent dynamic vehicle shadows and weak-texture asphalt from corrupting 3D point clouds:

Within the coarse sparse point cloud, we partition the road corridor into 5-meter sliding longitudinal cells;
Locally Optimal RANSAC (LO-RANSAC) fits local tangent ground planes $n_{\text{road}}^\top X + d = 0$;
Feature points residing within a 10 cm slice of the fitted plane are tagged and excluded from sparse bundle adjustment. The bundle adjustment solver converges purely on high-elevation rigid structures (building facades, light poles, overhead sign gantries), eliminating shadow-induced trajectory bias.

Neighborhood filtering strips isolated outliers while grid-based RANSAC suppresses thickness artifacts along the road surface.

4. Spatiotemporal-Guided Dense PatchMatch Stereo (MVS)

Generating continuous 3D ground meshes and voxel grids for occupancy networks requires high-throughput dense multi-view stereo:

Using the estimated vehicle trajectory, image pairs are screened based on optical parallax angles $\theta_{\text{parallax}} \in [5^\circ, 30^\circ]$ within a sliding temporal window of $\pm 15$ frames, avoiding global point-cloud searches;
GPU-accelerated multi-view PatchMatch (ACMP) iteratively optimizes depth maps and surface normal fields, employing slanted plane support priors to interpolate dense, continuous depth across textureless asphalt;
A multi-view geometric consistency filter fuses depth estimates across all six surround cameras into unified metric point clouds.

Standard COLMAP Dense

ACMP + Optimized Fusion

The hybrid strategy drastically accelerates fusion while improving reconstruction over weak-texture asphalt.

5. Coarse-to-Fine Multi-Session Aggregation Network

To reconcile asynchronous passes through identical intersections collected on different dates:

Coarse Georeferencing: Differential RTK-GNSS coordinates anchor each pass into a common metric coordinate system;
Fine Geometric Registration: The system isolates invariant vertical landmarks (traffic light poles, bridge pillars, structural facades). PnP and RANSAC solve the inter-session rigid transformation $T_{\text{run1} \to \text{run2}}$;
Global Pose Graph Relaxation: Fixing the primary reference pass, all subsequent passes are formulated as nodes connected by relative geometric constraints in a global pose graph. Non-linear least-squares optimization absorbs inter-session drift, stitching disparate runs into a seamless high-definition map.

Direct Overlay Before Aggregation

After Multi-Session Aggregation

Cross-session visual association and transform optimization reconcile distinct driving passes into one consistent spatial map.

Production Quality Assurance & Field Validation

The full-stack pipeline was validated in production data centers against ground-truth 128-beam mobile LiDAR SLAM and RTK reference trajectories:

Dense Surface Accuracy: Reprojecting dense visual point clouds onto forward-facing camera images and overlaying synchronized LiDAR sweeps demonstrated absolute road elevation errors under $\pm 3\text{ cm}$ within a 50-meter range, with sharp curb profiles;
Trajectory Consistency: Over 5-kilometer continuous city and highway loops, pure visual Rig BA suppressed cumulative yaw drift to 0.05°/100m, keeping closed-loop position errors below 0.1% of total travel distance;
Extrinsic Alignment Continuity: Applying optimized extrinsics to 360° BEV stitching eliminated lane marking shear across camera seams, restoring smooth physical continuity.

BEV projection comparison: lane markings across camera boundaries exhibit smooth physical continuity following optimization.

Pipeline Video Recordings

The four video sequences below present the end-to-end journey from raw onboard video and high-throughput dense depth generation to static 3D point cloud extraction and pixel-level reprojection checks:

Surround Camera StreamSynchronized surround-view video captured from production autonomous test fleets under dynamic driving conditions.

Static 3D Point Cloud ReconstructionHigh-precision static road geometry and architectural structures recovered by multi-camera rig SfM.

High-Throughput Dense Depth EstimationDense depth estimation guided by temporal and spatial overlap priors, achieving dramatic pipeline speedups.

Geometry & Appearance ReprojectionPixel-level reprojection verification demonstrating spatial consistency between recovered poses, dense geometry, and raw imagery.

Onboard Surround Video Stream: Synchronized six-camera footage recorded by production vehicles navigating dense city traffic;
Static Scene 3D Reconstruction: Dense, crisp geometric point-cloud models of overpasses, street furniture, and road structures;
High-Accuracy Dense Depth Estimation: Real-time dense disparity and depth maps generated across all views via spatiotemporal PatchMatch;
Appearance & Geometric Reprojection: Pixel-level reprojection checks confirming geometric alignment against original camera frames.

Engineering Summary & Technical Takeaways

Scaling Structure-from-Motion and multi-view stereo to automotive-grade automated annotation demonstrates key engineering principles:

Mechanical Rigidity as an Algorithmic Anchor: Multi-camera physical rigidity must be embedded directly into optimization formulations. Grouping surround cameras into a rigid rig halts extrinsic drift and overcomes collinear triangulation degeneracies through complementary multi-directional views.
Semantic Decoupling Guards Against Environmental Noise: In the presence of textureless asphalt and moving shadows, accepting all features indiscriminately destabilizes optimization. Decoupling ground features from vertical structures ensures robust convergence.
Bridging Isolated Trajectories into Spatial Networks: Autonomous driving requires scalable, repeatable mapping. Linking independent driving sessions through topological pose graphs establishes the foundation for large-scale digital twins and end-to-end driving models.

Autonomous Valet Parking: Production-Grade Full-Stack System for Underground Garages

Mon, 01 Jan 2024 00:00:00 +0000

Project context

Voyager Tech Mass-Production Partnership

Overcoming challenges in multi-floor underground garages—including featureless corridors, dynamic obstacles, tight clearance, and total GNSS denial—this system established an onboard-compute-powered L4 autonomous valet parking stack validated across extensive real-vehicle fleet trials.

Core Deliverables & Real-World Validation: Automotive pure-onboard AVP system; 500+ in-vehicle road tests (95%+ parking success), GNSS-denied multi-sensor localization, narrow-slot maneuvering, and remote summoning

My contributions

As project lead, architected the full-stack AVP system framework; directed planning and control under GNSS denial, completing 500+ in-vehicle road tests with 95%+ parking success.

Automated Valet Parking (AVP) encompasses autonomous stall search, tight-space bay parking, and designated-point summoning. In contrast to infrastructure-heavy vehicle-to-infrastructure (V2I) approaches—which require deploying dense ultra-wideband (UWB) anchors, roadside LiDARs, or dedicated camera networks throughout parking structures—vehicle-centric AVP requires zero structural modifications to parking facilities. By relying entirely on onboard sensors and compute, it delivers superior cross-facility generalization and commercial scalability.

In partnership with Tier-1 smart driving supplier Voyager Tech, we developed and deployed an onboard AVP system utilizing automotive-grade mass-production sensors: 4 surround-view ultra-wide fisheye cameras and 12 ultrasonic sensors. As Project Tech Lead, I directed the top-level system architecture and led algorithm development across the integrated driving-parking planning and control stack, validating GNSS-denied multi-sensor SLAM, corridor dynamic obstacle avoidance, Hybrid A* tight-space maneuvering, and low-speed chassis control on real production test vehicles.

Key Engineering Bottlenecks in Underground Garages

Underground garages represent semi-enclosed, highly constrained operational design domains (ODDs) that present formidable physical and geometric challenges to autonomous systems:

Total Satellite Denial & Severe Perceptual Degeneracy (GNSS-Denied & Perceptual Aliasing): Upon entering a basement structure, satellite positioning is immediately lost. Simultaneously, multi-story garages present uniform, highly repetitive architectural features: mirrored structural pillars, polished epoxy resin flooring with severe specular reflections, and featureless concrete walls. Under dim, flickering fluorescent illumination, standard point-feature extractors (such as ORB or SIFT) suffer from catastrophic false associations, precipitating visual odometry drift and scale collapse.
Severe Geometric Confinement & Non-Holonomic Kinematic Limits: Primary basement aisle widths typically measure between 2.8 and 3.2 meters, with clearance between concrete pillars occasionally narrowing below 2.5 meters. Standard passenger vehicles feature wheelbases approaching 3 meters and overall lengths near 5 meters. Constrained by non-holonomic Ackermann steering kinematics, the vehicle cannot translate laterally. When executing reverse maneuvers into a standard 2.4m $\times$ 5.2m perpendicular bay, the vehicle must execute multi-point forward-reverse (D/R gear shift) maneuvers under safety margins of less than 30 cm, where minor tracking deviations cause structural collisions.
Dynamical & Control Dichotomy Between Cruising and Parking: During corridor cruising, the system requires continuous, smooth motion at 10–15 km/h, with lateral acceleration strictly constrained for passenger comfort. Conversely, during stall insertion, vehicle velocity drops below 3 km/h, while steering actuators must rapidly sweep toward maximum lock within minimal linear displacement. This induces massive static tire scrub torque against the floor. Conventional trajectory-tracking controllers fail under this dual-regime operation, triggering actuator oscillation, severe deadband lag, or tracking overshoot.
Multi-Story Spiral Ramps & Dynamic Grade Distortions: Underground garages incorporate steep connecting spiral ramps with grades ranging between 10% and 15% (5.7°–8.5°). During ramp traversal, dynamic suspension compression induces pronounced chassis pitch and roll angles, introducing geometric skew into inverse perspective mapping (IPM). Furthermore, the longitudinal gravitational component severely perturbs velocity feedback loops, risking roll-back during stop-and-go maneuvers on inclines.

To systematically address diverse user requirements, the system supports four operational workflows:

Prior Map Condition	Mission Mode	System Execution	Target Outcome
Mapped Garage	Memorized Slot	Load local semantic graph and localize	Direct cruise to user’s assigned stall
Mapped Garage	Random Search	Cruise along aisles and detect open spaces	Immediate trajectory planning into first valid slot
Unmapped Facility	Exploratory Cruise	Build topological slot map on the fly	Explore garage, identify valid spot, and park
Mapped Garage	Remote Summon	Receive app command and execute park-out	Navigate through aisles to pickup foyer

System architecture

Campus-scale automated valet parking

From user intent and onboard sensing to mapping, hierarchical planning, vehicle control, and safety replanning.

Task, sensing, and compute

01 · HMIMaps and parking tasksMap download/update · park-in/out · remote summon

02 · SensorsSurround and near-field sensingFour fisheye cameras · ultrasonic sensing · IMU · wheel/GPS

03 · VehicleOnboard compute and chassisEmbedded GPU/ECU · CAN · steering, braking, and drive

Autonomy core

04 · PerceptionBEV multi-task perceptionSlots, markings, and text · drivable space · obstacle detection and tracking

05 · LocalizationLocalization and semantic mapVisual/inertial/wheel fusion · loop closure · semantic ICP · exploration mapping

06 · PlanningHierarchical driving and parking planningGlobal guidance · local avoidance · Hybrid A* · optimal-control smoothing

Program leadership and integrationSystem design, module interfaces, and vehicle integrationArchitecture and milestones · cross-module interfaces · planning/control focus · scenario testing and fault correction

Execution, safety, and application

07 · ControlLateral/longitudinal controlTrack the reference trajectory and command the vehicle

08 · SafetyObstacle response and replanningNear-field coverage · dynamic updates · stop and detour

09 · ServiceExplore, park, retrieve, and summonKnown/unknown maps · random, remembered, or assigned slots · return to pickup

Full-Stack Architecture & Algorithmic Design

To overcome these constraints under onboard embedded compute limits, we developed a tightly coupled, modular processing pipeline encompassing bird’s-eye-view multi-task perception, factor-graph fusion localization, hierarchical planning, and precision chassis actuation.

1. Surround-View Fisheye BEV Multi-Task Perception & Near-Field Shielding

The sensing suite consists of 4 automotive-grade ultra-wide fisheye cameras (each offering $>190^\circ$ horizontal FOV) positioned at the front, rear, and wing mirrors, paired with 12 ultrasonic transceivers.

Dynamic Pitch-Compensated Inverse Perspective Mapping: To eliminate image warping caused by vehicle acceleration, braking, and ramp slope variations, high-frequency IMU pitch and roll estimates are fed directly into the online homography transformation. This dynamic transformation stabilizes the bird’s-eye view, ensuring projected ground markings and stall boundaries maintain rigorous metric scale.
Lightweight Shared Backbone with Multi-Task Branched Heads: Operating within embedded thermal envelopes, the perception network adopts a single feature-extractor backbone paired with four decoupled task heads: parking stall head, ground marking head, obstacle detection head, and free space boundary head.
Ultrasonic Near-Field Time-of-Flight Protection: When approaching obstacles within close range, vehicle blind zones fall outside camera sightlines. Short-range and long-range ultrasonic sensors establish a continuous proximity barrier, using multi-transceiver time-of-flight triangulation to filter multi-path false echoes and provide deterministic hardware-level collision prevention.

2. Factor-Graph Multi-Sensor Fusion Odometry & Hierarchical Mapping

To guarantee drift-free localization across extended GNSS-denied trajectories, we constructed a tightly coupled factor-graph optimization backend:

Slip-Adaptive Wheel Odometry & IMU Preintegration: On polished epoxy basement surfaces, tire micro-slip frequently occurs during cornering and braking. The fusion filter continuously evaluates consistency between non-driven wheel speeds and IMU yaw rates, adaptively tuning observation covariances to bridge disturbances without pose divergence;
Visual-Semantic Landmark Constraints & Global Loop Closure: In addition to IMU and odometry factors, the factor graph integrates geometric constraints derived from detected parking slot corners and lane centerlines, triggering full pose-graph relaxation upon loop detection to eliminate accumulated drift;
Hierarchical Lightweight Map Representation: A local rolling occupancy grid caches dynamic obstacles within a sliding window, while a global topological semantic graph stores corridor centerlines and stall attributes with minimal memory footprints.

3. Hierarchical Cruising & Parking Motion Planning

Recognizing the distinct kinematic scales of aisle navigation versus stall maneuvering, the planning pipeline is decoupled into two specialized stages:

01Task and map state Known/unknown map · target slot or pickup point · current pose

02Global driving guide Centerline graph · improved A* · turn-radius arcs · B-spline

03Local avoidance trajectories State sampling · centerline/transition/obstacle/curvature costs · online replanning

04Parking search Orientation-aware Hybrid A* · nonholonomic vehicle constraints · collision checks

05Trajectory optimization and execution Optimal control · IPOPT · lateral/longitudinal tracking · feedback loop

Corridor Cruising: Reference Spline Smoothing & Dynamic Evasion

In aisle transit, the global planner fits continuous-curvature reference corridors using spline curves. The local planner samples polynomial trajectory candidates in the Frenet frame at high frequency, evaluating safety distance, comfort, and lateral acceleration to smoothly evade obstacles or yield to traffic.

Bay Parking: Modified Hybrid A* & Non-Linear Optimal Control

Upon reaching the designated stall entry position, the system transitions to precision parking mode using a two-stage coarse-to-fine framework:

Coarse Geometric Search (Modified Hybrid A)*: Search progresses in continuous configuration space under non-holonomic turning radius constraints and distance field heuristics, generating kinematically feasible initial paths within milliseconds;
Continuous Trajectory Optimization: Taking the Hybrid A* path as an initial guess, constrained non-linear optimal control enforces kinematic differential equations, steering rate limits, and multi-circle collision avoidance, producing continuous, curvature-smooth trajectories that prevent driveline shudder.

4. Precision Chassis Tracking & Delay Compensation

At low crawl speeds with aggressive steering angles, drive-by-wire latency constitutes the primary source of path deviation:

Kinematic Preview with Curvature Feedforward: Lateral control employs delay-adaptive preview lookahead, combining curvature feedforward with tracking error feedback to compensate for steering actuator delay;
Grade-Adaptive Gravitational Feedforward: Longitudinal tracking couples speed feedback with real-time IMU grade estimation, directly compensating for gravity components on steep ramps to prevent roll-back;
Multi-Tier Hardware Safety Watchdog: High-frequency watchdogs monitor system communications, issuing direct emergency brake requests if ultrasonic sensors detect unexpected close-proximity incursions.

Real-Vehicle Field Verification

Surround-View BEV Multi-Task PerceptionDynamic in-vehicle playback: real-time bird-eye view semantics, 4-way unwrapped fisheye streams, and high-precision parking space and lane marking tracking.

Autonomous Garage Exploration & Full-Stack System IntegrationAutonomous vehicle operation in multi-floor garages: synchronized surround perception, visual odometry, vehicle kinematics, and incremental semantic parking map construction.

Field trials were conducted across diverse commercial underground parking facilities, spanning modern multi-story garages, unlit legacy basements, and complex spiral structures:

Perpendicular, Parallel, and Slanted Bay Ingress: In tight stalls flanked by parked vehicles with less than 30 cm of lateral margin, the vehicle consistently parked within two to three forward-reverse maneuvers, achieving alignment within 1° of stall markings and symmetric wheel clearance;
Multi-Floor Spiral Navigation & Dynamic Clearance: Across hundreds of meters of subterranean transit, the vehicle smoothly negotiated consecutive blind corners, executing automatic lane yields when encountering oncoming delivery vans or wandering pedestrians;
End-to-End Remote Summoning: Production vehicles operated completely unmanned in response to mobile app summon commands, autonomously exiting bays and arriving at ground-level passenger staging areas.

Engineering Takeaways & Architectural Lessons

Automated Valet Parking represents one of the most demanding low-speed autonomous driving domains due to tight geometric tolerances and extreme perceptual degradation. Key takeaways include:

Multi-Modal Synergy in Featureless Environments: When individual sensing modalities degrade, combining vehicle kinematics, IMU preintegration, and topological semantic landmarks provides the only viable defense against long-term localization drift.
Explicit Physics Constraints in Trajectory Formulation: Kinematic paths must not be treated as purely geometric splines. Embedding actuator rate limits, acceleration limits, and gear-shift costs directly into the optimization formulation is essential for smooth, production-grade tracking.
Decoupled Architecture with Deterministic Safety Shields: Decoupling open corridor cruising from tight stall optimization preserves high real-time control frequencies, while dedicated hardware-level ultrasonic emergency braking guarantees operational safety under real-world uncertainties.

Full-Stack Autonomous Driving Research Platform: Hardware-Software Co-Design & Real-Vehicle Verification

Sat, 01 Jan 2022 00:00:00 +0000

Autonomous driving research requires validation beyond idealized simulator environments to directly address real-world ground friction, physical sensor noise, bus communication latency, and actuator non-linearities. In 2019, commercial autonomous testbeds were prohibitively expensive and strictly closed-source, precluding in-depth research into chassis dynamics and custom low-level sensor drivers. To establish an accessible, controllable physical research platform, we undertook the ground-up hardware and software construction of an autonomous driving research testbed.

Between 2019 and 2022, we designed and built two complementary research vehicles: a low-speed pure-electric passenger vehicle configured for campus road cruising, and an omnidirectional research AGV featuring 4-wheel independent driving and 4-wheel independent steering (4WID-4WIS). The engineering effort spanned mechanical mounting fabrication, high/low-voltage isolated power distribution, reverse engineering of proprietary CAN bus protocols, development of Linux SocketCAN drivers, microsecond-level hardware time synchronization across heterogeneous sensors, and a full-stack ROS-based software autonomy suite encompassing perception, SLAM, planning, and control. The platform successfully completed closed-loop verification across diverse representative operational scenarios under realistic campus road traffic.

Project context

Autonomous Driving Research Core · Ground-Up Hardware/Software R&D

To evaluate autonomous driving algorithms on physical hardware rather than purely in simulation, we built full-stack research vehicle platforms from the ground up. Covering chassis adaptation, power distribution, SocketCAN drivers, and ROS autonomy modules, the platform provided an experimental testbed for ongoing laboratory research.

Platform Deliverables & Multi-Scenario Real-Vehicle Demonstrations: Two operational drive-by-wire autonomous research platforms; 300+ hours of road testing, 8 real-vehicle closed-loop demonstrations (<5 cm lateral tracking error, <60 ms AEB braking response), and a complete custom ROS autonomy stack.

My contributions

As project lead, built two generations of drive-by-wire autonomous vehicle research platforms (passenger cart and 4WIS AGV) from scratch, accumulating 300+ hours of physical road testing; responsible for drive-by-wire modification, power isolation, CAN bus reverse engineering, and multi-scenario closed-loop validation.

Two Complementary Research Chassis

To comprehensively ground our research across disparate operational domains, we developed two distinct physical platforms:

Low-Speed Campus Research Cart: Configured with passenger-carrying seating, a standard automotive steering column, and hydraulic braking. Its open frame and high payload capacity accommodate heavy sensor payloads: a 32-beam spinning 3D LiDAR, forward stereo and surround wide-angle cameras, dual-antenna RTK-GNSS, a high-rate IMU, and a ruggedized industrial PC equipped with high-performance discrete GPUs. This vehicle serves as the primary testbed for large-scale metric-topological SLAM, multi-modal perception, traffic interaction, and cooperative V2X platooning.
Compact 4-Wheel Independent Drive & Steer (4WID-4WIS) AGV: Equipped with eight independent brushless DC servomotors (four driving wheels, four steering hubs). Bypassing the non-holonomic kinematic limits of traditional Ackermann steering, the vehicle dynamically reconfigures its steering mode in software: standard front-wheel Ackermann, diagonal crab-steering, pure lateral translation, and zero-radius pivot spinning around its chassis center. This platform provides an agile testbed for high-speed obstacle avoidance maneuvers and extreme tight-space parking.

Both vehicles enforce a foundational design paradigm: exposing algorithms directly to real-world tire-ground compliance, CAN transmission jitter, and actuator deadbands, thereby establishing a physical benchmark for subsequent algorithmic robustness.

Full-Stack Autonomous Driving Architecture

Multi-Sensor Fusion → State Estimation → Planning → Drive-by-Wire Actuation

Full-stack onboard ROS autonomy connects perception, SLAM, and planning; the drive-by-wire chassis executes closed-loop control over CAN.

Sensing and computation

01Observe surroundingsCamera · LiDAR · ultrasonics

02Positioning inputsRTK GNSS · IMU · LiDAR map

03Onboard computationIndustrial PC · ROS · HMI

Autonomy and actuation

04Perception and localizationObjects · lanes · obstacles · vehicle pose

05Decision and planningReference route · driving objective

06Control and drive-by-wireMotion commands · CAN · chassis actuation

Vehicle motion → new observations and feedback → next control updateMap-based and RTK route following use their respective localization inputs.

Ground-Up Electrical & Sensor Topology

Without commercial turnkey kits, integrating disparate sensors and actuators into a fail-safe vehicular platform required overcoming fundamental electrical isolation, extrinsic calibration, and temporal synchronization challenges.

Subsystem	Configuration & Role	Core Engineering Challenges Overcome
Drive-by-Wire & CAN Bus	Electronic steering motor, throttle potentiometer, and hydraulic brake booster	Reverse-engineered proprietary cyclic DBC protocols; built 100 Hz deterministic Linux SocketCAN drivers with heartbeat fail-safe routines
3D LiDAR	32-beam / 16-beam mechanical spinning LiDAR for 3D mapping and obstacle ranging	Custom CNC aluminum vibration dampers isolating chassis road shock; calibrated beam reflectivity intensity decay curves
HDR Industrial Cameras	Narrow-FOV (60°) and wide-angle (120°) cameras for lane segmentation and object vision	Mitigated severe lens flare transitioning into shaded tree canopies; designed rigid camera-LiDAR multi-modal calibration targets
RTK-GNSS Differential GPS	Dual-antenna receiver delivering centimeter-level 3D coordinates and true heading	Optimized 1.5 m baseline antenna placement; mitigated carrier-phase slip and multipath jumps beside dense concrete campus structures
Automotive IMU	6-axis tactical-grade inertial unit providing 200 Hz linear acceleration and yaw rates	Mechanically aligned IMU sensing center with chassis rear-axle centerline; performed online thermal bias drift calibration
Ultrasonic Transceiver Array	8 bumper-mounted ultrasonic sensors for near-field blind-spot guarding (0.2–2.5 m)	Optimized acoustic beam-cone overlap geometry, enforcing sequential time-multiplexed firing to eliminate cross-talk
Power Distribution & Isolation	48V traction battery pack converted to isolated 12V/24V clean bus	Custom electrical distribution enclosure with TVS diodes and optocouplers, suppressing motor back-EMF voltage surges and ground loops

1. Hardware Hard-Triggering & Microsecond Temporal Synchronization

At vehicle speeds of 25 km/h, a 100 ms timing desynchronization between sensors induces an spatial projection error approaching 0.7 meters. Relying on software system timestamps (ROS Time) introduces non-deterministic OS scheduling jitter incompatible with feature-level sensor fusion.

We engineered a multi-tier hardware synchronization architecture:

PTP and PPS Nanosecond Clock Locking: The RTK-GNSS receiver, upon locking satellite constellations, generates a precision Pulse Per Second (PPS) square wave and GPRMC NMEA timing packets. The 32-beam LiDAR ingests the PPS signal, synchronizing its internal clock to absolute UTC time within nanoseconds;
FPGA/Microcontroller External Optical Exposure Triggering: A dedicated microcontroller captures the PPS pulse and synthesizes a phase-locked 20 Hz square wave sent to the external trigger pins of industrial cameras. This forces camera sensor exposure to coincide exactly with the moment the LiDAR spinning beam sweeps through the forward field of view;
Driver nodes read hardware frame timestamps directly from peripheral registers, compressing inter-sensor temporal alignment error below 1 millisecond.

2. Rigid Extrinsic Joint Calibration (LiDAR-Camera-IMU)

To eliminate spatial misalignments:

The rigid transformation between the camera optical center and LiDAR coordinate frame is optimized using a custom calibration board featuring hollow circular cutouts, simultaneously minimizing 3D laser point-to-plane fitting errors and 2D visual circular re-projection errors;
IMU-to-LiDAR spatial extrinsics are identified through continuous sinusoidal steering maneuvers and rapid acceleration profiles in open areas, parameterizing trajectories via continuous B-splines to minimize angular velocity and linear acceleration residuals, reaching millimeter and 0.1-degree alignment accuracy.

3. Proprietary CAN Bus Reverse Engineering & Deterministic SocketCAN Drivers

With native chassis controller (VCU) documentation withheld, we captured raw bus traffic using CAN analyzers during manual driving. By correlating driver throttle inputs, pedal pressure, steering wheel sweeps, and wheel encoder pulses, we systematically isolated and mapped the cyclic DBC communication matrix.

We subsequently built an industrial-grade Linux SocketCAN driver:

Implemented a 100 Hz deterministic periodic thread transmitting steering angle, throttle voltage, and brake pressure setpoints;
Integrated a dual-directional watchdog supervisor: if the VCU misses valid heartbeat packets for $>100\text{ ms}$, onboard firmware immediately disengages autonomous drive and engages gentle mechanical braking, preventing runaway faults caused by upstream software crashes.

Full-Stack ROS Autonomy Architecture

The software stack operates on Ubuntu Linux and ROS with optimized shared-memory IPC, partitioned into three core decoupled pipelines:

1. Localization & High-Precision Mapping Pipeline

Tightly Coupled LiDAR-Inertial SLAM: Extracting geometric edge and planar features from raw 32-beam point clouds, the frontend fuses high-rate IMU pre-integration priors within a factor graph optimization backend. Between keyframes, relative motion increments and bias updates are factored alongside LiDAR point-to-plane geometric residuals into an incremental smoothing and mapping (iSAM2) factor graph, rendering globally consistent 3D point-cloud maps of campus roadways;
50 Hz Online NDT Localization: Within the pre-built metric voxel map, incoming point clouds are registered using the Normal Distributions Transform (NDT) at 50 Hz. Each spatial voxel approximates local point distributions via 3D Gaussians. Even when traversing covered pedestrian corridors and dense underpasses where GNSS signals vanish, NDT scan registration maintains continuous state estimation without trajectory drift or discrete position jumps;
Dual-Antenna Differential Kinematic Heading: Fixed dual antennas establish an instantaneous physical baseline across the vehicle roof, determining true vehicular heading even at zero velocity without dead-reckoning integration drift.

2. Environmental Perception & Scene Understanding Pipeline

Bird’s-Eye-View Visual Lane Tracking: Monocular camera images undergo distortion rectification and Inverse Perspective Mapping (IPM). A lightweight segmentation network extracts lane boundary pixels, fitting polynomial curves to output continuous road curvature and lateral offset for steering regulation;
Multimodal 3D Object Detection & Tracking: 2D deep learning detectors classify pedestrians, bicycles, and vehicles in image space. Detected bounding frustums are back-projected into spatial point clouds to segment candidate point clusters and fit 3D oriented bounding boxes. An Interacting Multiple Model Unscented Kalman Filter (IMM-UKF) bridges constant-velocity and constant-turn-rate motion hypotheses to publish stable obstacle velocity vectors.

3. Hierarchical Planning & High-Rate Actuation

Hierarchical Decision State Machine: Finite state machines (FSM) govern macroscopic mission transitions across cruise, dynamic yield, intersection negotiation, bay parking, and emergency stop. Local trajectory generators sample candidate polynomial trajectories in the Frenet frame to dodge static obstacles and slow pedestrians;
Dual-Loop Dynamic Tracking Control: Lateral control combines Pure Pursuit lookahead with Linear Time-Varying Model Predictive Control (LTV-MPC) over a dynamic bicycle model, enforcing steering angle limits and tire lateral slip angle saturation bounds to eliminate overshoot during high-curvature turns. Longitudinal control employs closed-loop PID with speed feedforward, transmitting steering and braking commands at 100 Hz over CAN.

Seven Real-Vehicle Closed-Loop Scenarios

Real-Time 3D LiDAR SLAM MappingOnboard 3D LiDAR incrementally constructing metric point-cloud maps of complex campus terrain with crisp structural fidelity.

High-Precision SLAM Waypoint TrackingAutonomous waypoint navigation localized entirely within self-built 3D LiDAR point-cloud maps.

Vision-Based Lane Centering on 4WIS AGVMonocular front-facing camera segmenting lane boundaries to drive smooth centering control at speed.

Dynamic Multi-Object Roadside TrackingSynchronized camera-LiDAR fusion tracking pedestrians, vehicles, and dynamic campus traffic.

Autonomous Emergency Braking (AEB)Sub-millisecond emergency brake actuation upon sudden obstacle intrusion across the vehicle path.

Autonomous Precision Bay ParkingCoordinated ultrasonic-vision trajectory optimization backing the vehicle into narrow parking stalls.

V2X Multi-Vehicle PlatooningDirect inter-vehicle ad-hoc wireless communications coordinating two autonomous cars in synchronized following along curves.

The seven video recordings document extensive field testing across campus thoroughfares:

Real-Time 3D LiDAR Mapping: The passenger vehicle cruises smoothly under dense tree canopies, incrementally rendering metric 3D point-cloud geometry of building facades and road curbstones;
High-Precision SLAM Waypoint Following: Operating with GNSS antennas disconnected, the vehicle navigates autonomously through long, winding campus avenues guided solely by real-time NDT point-cloud matching;
Vision Lane Centering on 4WIS AGV: The forward camera parses faded white road lines, while the 4WIS chassis dynamically modulates steer angles to remain centered in the travel lane;
Dynamic Roadside Object Tracking: Onboard detectors identify pedestrians crossing intersections and oncoming bicycles, maintaining 3D bounding boxes and trajectory predictions;
Autonomous Emergency Braking (AEB) Verification: When an obstacle mannequin is rapidly thrust into the vehicle path, perception computes Time-to-Collision ($\text{TTC} < 1.2\text{s}$) within 60 ms, triggering full hydraulic clamp lockup to halt the vehicle 0.8 meters before the target;
Precision Bay Parking: Ultrasonic sensors measure bay clearance, allowing the planner to execute a two-stage reverse trajectory that parks the 4WIS AGV neatly aligned with painted stall markers;
V2X Connected Platooning: Over 5.8 GHz ad-hoc wireless mesh links, lead-vehicle acceleration and braking intentions are shared at 100 Hz, enabling two autonomous vehicles to maintain tight, zero-lag platoon spacing through multi-turn courses.

Engineering Summary & Technical Takeaways

Building two complete physical autonomous platforms from bare metal established foundational systems engineering insights:

Physical Ground Truth Governs Algorithmic Success: Simulators hide bus dropouts, high-frequency frame vibration, electrical ground loops, and motor deadbands. Only by directly debugging power distribution surges and actuator dead zones on real iron can one design resilient control architectures with adequate feedforward and fail-safe watchdogs.
Deterministic Timing Underpins Sensor Fusion: Sophisticated geometric algorithms fail if multi-modal sensor streams suffer millisecond-scale timestamp jitter. Enforcing hardware-level PPS clock discipline and external exposure triggering is a prerequisite for reliable real-world perception.
Reusable Modular System Foundation: The full-stack hardware-software architecture established in this project provided directly transferable foundational modules for subsequent laboratory research and industry partnerships in mobile robotics and autonomous valet parking.
Hardware-Level Heartbeat Watchdogs as Safety Primitives: Upstream Linux software stacks inevitably experience occasional thread freezes, driver crashes, or kernel scheduling delays. Guaranteeing that the low-level chassis microcontroller automatically cuts throttle and engages gradual hydraulic braking when cyclic CAN heartbeats drop for 100 ms is what keeps physical autonomous prototypes safe during high-speed live road trials.

MRASfM: Multi-Camera Reconstruction and Aggregation through Structure-from-Motion in Driving Scenes

Sat, 31 Jan 2026 00:00:00 +0000

Driving is the wrong shape for photogrammetry

Image-based reconstruction for driving is usually handled by visual SLAM, and for HD mapping and novel-view synthesis the SLAM route has a structural limitation. Its estimates are incremental and locally optimized, which blocks global refinement, and the result is drift that accumulates over a long trajectory. Structure-from-motion does the opposite: it processes everything in a batch with a global bundle adjustment at the end. The offline nature that makes SfM impractical for a live robot is exactly what makes it attractive here, because it can spend computation a real-time system cannot.

So the obvious move is to use SfM for driving. Three facts about driving data make that harder than it should be.

The first is the capture geometry. Classic SfM assumes you walk around the subject and photograph it from many directions. A car drives down a corridor. Viewpoints are almost collinear, which is the worst case for the translation estimation that SfM depends on.

The second is that data arrives fragmented. Driving scenes are captured over short durations with limited overlap, and drives covering the same area may be recorded weeks apart. There are often no shared images at all between two datasets that need to end up in the same map.

The third is calibration, and this is the one that changed how we designed everything. Vehicle calibration is difficult, initial values contain errors, and those errors get worse on uneven terrain. So the extrinsics you have at the start of a drive are not the extrinsics you need, and they are also the strongest prior available. That tension runs through the whole design.

What prior methods assume

Multi-camera SLAM systems are accurate and require precise internal relative poses and intrinsics as input. They need calibration done before deployment, and they struggle to accommodate different camera configurations, which restricts flexibility in practice.

Incremental SfM is the closest baseline, and our criticism of it is specific. It ignores camera-set priors during incremental reconstruction and applies them only afterward, in rigid bundle adjustment once the reconstruction is already complete. By that point, errors that accumulated during incremental estimation cannot be corrected. Rigid BA cannot undo drift that has already been baked into the structure. Per-image bundle adjustment has a related problem: it can leave the internal relative poses inconsistent from frame to frame.

Global multi-camera SfM does fix the relative camera poses from the start, and it is sensitive to outlier matches and tends to degenerate when the estimated relative translations are collinear. In a driving scene the trajectory is close to collinear by default, so this is a structural hazard rather than an edge case.

Odometry-guided SfM takes the opposite bet and uses odometry poses and camera extrinsics directly for registration. That demands highly precise extrinsic calibration and accurate odometry, which are exactly the two things field deployment cannot reliably supply. We borrow this line of work’s idea of using homography to identify overlapping image pairs. We do not borrow the assumption that the priors can be trusted.

There is also a matching problem specific to the setting. Traditional descriptors produce limited feature points in low-texture images, and road scenes are full of low-texture surfaces.

Finally, existing aggregation modules for merging reconstructions typically assume shared imagery between the segments being merged. In practice that imagery usually does not exist.

What makes it hard

Priors have to be used without being trusted. Extrinsics and odometry are the strongest signal available and the least reliable thing on the vehicle. Trust them entirely and you get one family of failures; ignore them and you get another. The design space in between is narrow.

Collinearity is a real degeneracy, not a numerical nuisance. Fixed-rig global methods hinge on relative translation estimates, and straight-line driving makes those ill-conditioned.

The failure modes sit exactly where the useful signal is. The road surface occupies a large part of every image and has almost no texture, so it produces both few matches and bad ones. Worse, the edges of vehicle shadows are high-contrast and get detected as feature points. A shadow edge is not a scene point. When the shadow moves, that feature moves, which injects motion into the road surface precisely where the structure matters most.

Occlusion behaves differently on a rig than on a single camera. One blocked camera on a monocular system is a total failure; on a surround rig the other cameras still see the world. But that redundancy is only worth anything if registration actually shares the pose, and per-camera estimation throws the advantage away.

And robustness has a price. More cameras mean more poses to optimize in bundle adjustment, which reduces efficiency. The coverage that buys robustness is paid for in optimization size, and cancelling that cost is what a large part of our design exists to do.

The camera set is the unit

Our idea is to change the atomic element of reconstruction. Instead of treating each image as an independent camera to be solved, we take the rigid unit, every image captured at the same instant, as the thing that gets a pose. That pose is by definition the vehicle’s pose at that moment.

The benefit is that the pose of a rig factorizes into a time-varying vehicle pose and a static internal relative pose. One well-observed camera can carry the poses of the cameras that see nothing useful. The rig is estimated once rather than re-learned at every frame.

The important part is how that factorization is applied. We use it as a reparameterization inside registration and bundle adjustment, not as a constraint imposed afterward. That single decision does three things at once. It reduces the pose variables from one per camera per timestamp down to one per timestamp plus one per camera. It keeps the optimized rig consistent across frames, because there is only one rig to be consistent. And it turns calibration from an input that must be correct into a quantity that can be corrected.

Building a reconstruction

Correspondence search comes first. SuperPoint detects and SuperGlue matches, pairs whose matched points fall into different semantic categories are discarded, and geometric verification removes the rest. Pairs are not all tried: with the rough internal relative poses and vehicle poses in hand, we compute the approximate field of view of each image and only match pairs that could plausibly overlap. Filtering this way improves both efficiency and accuracy, since matching everything against everything is expensive and injects noise from pairs that were never going to share content.

Registration then runs in three steps. PnP is estimated on the images with the richest correspondences. Those per-image estimates are converted into per-unit estimates and fused by local robust rotation averaging and translation averaging into a single unit pose. Every remaining image in that unit gets its pose by composing the unit pose with the calibration.

That middle step is where the method sits between the two failure modes described earlier. It averages, like the global family, but it averages over unit hypotheses derived from reliable images rather than over raw relative translations, which is what makes it survive collinearity.

The honest accounting of this module is narrow. It has minimal effect on average pose accuracy and efficiency, and it is particularly useful for handling occluded viewpoints. In scenes with rich correspondences the impact is less noticeable. We state that tradeoff rather than presenting the module as uniformly beneficial, because the value of redundancy is concentrated in the tail.

View full-size image ↗

Occlusion on one camera no longer fails the unit.

Getting the road out of the way

After a unit is registered, we fit a plane model to the road points with LO-RANSAC and remove them. This targets the shadow-edge problem directly. A moving shadow edge produces a feature that is not attached to the scene, and it lands on the largest and least textured surface in view, so the triangulation noise concentrates there by default. Removing those points improves reconstruction accuracy and accelerates convergence.

The assumption being made is local flatness of the road, and our own real-world scenes include slope changes, so this is an approximation rather than a guarantee.

View full-size image ↗

Removing shadow-driven and texture-poor points where the noise concentrates.

Bundle adjustment on the rig

Camera set bundle adjustment optimizes vehicle poses and internal relative poses under substitution constraints that express each camera pose in terms of the vehicle pose and the rig geometry, with a Cauchy loss for the local stage. The variables include intrinsics, vehicle poses, internal relative poses and scene points.

There are two modes and the split matters. Local CSBA fixes intrinsics and internal relative poses and touches only newly registered units and their neighbours. Global CSBA optimizes everything across all registered units, and we trigger it when many units remain unoptimized.

Global CSBA re-optimizing intrinsics and relative poses is the online recalibration capability claimed in the paper. It is also the practical payoff, because it converts the field’s least reliable input into an output. Local CSBA is cheaper, and it freezes exactly those parameters, which is the tradeoff: the cheap mode cannot fix calibration, and the mode that can fix it costs more.

The variable reduction from one pose per camera per timestamp to one per timestamp plus one per camera is worth dwelling on. Six cameras over a long trajectory means the per-image formulation optimizes six times as many poses as it needs to, and every extra variable is another degree of freedom that noise can hide in. Fewer variables makes convergence faster and the result more consistent. Robustness and efficiency turn out to be the same lever here, which is not usually the case.

View full-size image ↗

Vehicle poses and internal relative poses are refined jointly, which is what allows calibration to improve in the field.

Aggregation has to work without shared imagery, so the whole strategy is coarse-to-fine. Scenes are first placed in a global frame using satellite positioning, represented by trajectory midpoints: one scene sits at the geometric centre, its nearest neighbours become candidates, and homography-guided spatial pairs pick whichever candidate has the greatest visual overlap.

The coarse placement is wrong by an unknown transform, because positioning error is larger than the alignment tolerance. So the transform itself becomes the variable. One image from the incoming scene is relocalized to get a refined pose, which yields the transform between the coarse placement and the true one, and that transform registers the entire rigid unit at once. Triangulation follows, then a transformation-based bundle adjustment optimizes the transform and the scene points while keeping the reference scene’s camera poses fixed.

Registration, triangulation and adjustment iterate until every unit of the incoming scene has been absorbed. The merged result then becomes the new reference for the next scene, so aggregation proceeds as a chain rather than against a single global anchor. The reason is simple: chaining keeps error from originating at one fallible alignment.

The whole stage is designed so aggregation never has to be exact at the coarse step, only close enough to relocalize.

View full-size image ↗

Satellite positioning places the scene; vision refines the placement.

What we take from this

A physical invariant is best used as a reparameterization rather than a constraint. Enforcing rigidity after the fact cannot prevent the error that accumulated before. Making rigidity part of the variable structure prevents it from accumulating in the first place.

Priors should prune, not conclude. Rough poses decide which pairs to match and where to start, and the geometry still comes from matched evidence. That is the line between our approach and the one we criticize.

Design so priors can be corrected rather than merely trusted. Making relative poses optimizable is what turns the least reliable input into an output.

And judge a module where its failure mode lives. Camera set registration barely moves average pose error, and it is the difference between working and failing when a camera is occluded. Aggregate metrics can hide a module’s entire value.

What it still assumes

The method takes rough trajectories, rough calibration and semantic information as inputs. The semantic source is assumed rather than specified, and we report no sensitivity to the quality of those labels.

Multi-scene aggregation is not ablated. The choice of roughly three candidate scenes and the geometric-centre reference heuristic are stated without justification or failure analysis.

The rigid unit is defined purely by simultaneity, and we do not test what happens when that assumption weakens. Camera time-synchronization error and rig flex would both violate it, and neither is discussed. Online calibration is claimed but not characterized: we do not say under what motion diversity or camera count the global stage’s refinement of intrinsics and relative poses is well posed.

The plane model presumes a locally flat road while our own deployment includes slope changes. Generality across different multi-camera configurations is asserted but exercised only on six- and seven-camera rigs. Dynamic objects beyond shadows appear in the motivation but have no dedicated treatment.

Where it leads

The contribution we would state is a change of unit, and everything else follows from it. What the change buys is worth spelling out: camera sets as atomic units for registration and refinement give robust pose estimation in complex environments and better efficiency than conventional per-camera methods. Combined with the road filtering and the aggregation stage, the pipeline produces a single map from drives that were never meant to be merged.

We validated it in two directions on purpose. Real instrumented vehicles demonstrate that it holds up outside a dataset, and public benchmarks establish the accuracy claim against a conventional baseline. The practical statement of the stake is that intelligent vehicles can perceive their environment and estimate their own pose more reliably, which is what the downstream tasks actually consume.

3D Object Detection and Tracking Based on Lidar-Camera Fusion and IMM-UKF Algorithm Towards Highway Driving

Tue, 04 Apr 2023 00:00:00 +0000

The design constraint behind this work is not accuracy. It is latency.

Most fusion-based 3D detection algorithms run below 15 Hz on a workstation, and a workstation is not what drives a car. An onboard computing centre is slower still, and it is not dedicated to perception: planning, control and the rest of the stack have to run concurrently, which takes a further bite out of the same budget.

Now put a number on what 15 Hz costs. At highway speed the vehicle covers about 1.94 metres between two consecutive frames. For that distance the perception system effectively does not see anything that has just happened, and we call that being time blind. The consequences are not abstract. A system that reacts to where a car was two metres ago is reacting to a situation that no longer exists.

Thirty hertz substantially reduces the blind distance and leaves room for the other modules to run. That target, on affordable hardware rather than a development machine, is what shaped every choice in this paper. It also means we made a deliberate trade: LiDAR-only methods achieve better detection accuracy than fusion methods do, and we did not set out to beat them on that axis. We set out to be competitive while running at a frequency they cannot reach.

What the fusion families cost

There are three ways to combine an image with a point cloud, and each has a price.

Detecting in 2D and lifting the boxes into the point cloud, the frustum approach, makes the whole system depend on the 2D detector. An error in the image branch propagates into 3D with no way to recover it.

Fusing at the feature or proposal level and training end to end produces strong results, and it loses original information in the process of fusing at the region level. These methods also tend to be complex and to run slowly, which is the specific problem we were trying to solve.

The third family projects the point cloud into an image feature map and augments each point with what it finds there. The architecture stays close to a LiDAR-only detector, which is what we wanted, and the pillar encoder keeps the runtime low. We chose this family for exactly that reason.

Within LiDAR-only detection the lineage matters too. Voxel-based 3D convolution was accurate and slow. Sparse convolution improved efficiency without making it fast enough. Pillars, which reduce the problem to 2D convolutions, hit the best tradeoff between accuracy and speed, and we built on that.

For tracking, the choice of a classical filter over a learned one was deliberate rather than conservative. Learning-based trackers perform poorly in real time, and neural data association is time-consuming, so the Hungarian algorithm stays. Batch tracking is ruled out for a different reason: it uses all data, and a driving system only ever has history. 2D tracking is ruled out because it cannot tell you an object’s position and size in 3D.

The closest filter-based alternative is a well-known 3D tracker that is simple and efficient and uses a single constant-velocity model. A constant-velocity model has no way to represent a car that is turning, so its state estimate degrades in exactly the situations where prediction matters most. A monocular tracker uses an extended Kalman filter under one camera. An urban JPDA-IMM-UKF system exists and uses traditional clustering for detection, which produces fragmented trajectories and hurts tracking performance downstream.

IMM itself is standard in aviation and nearly absent in vehicles, and the reason is instructive. Aircraft maneuver drastically: large speed changes, fast heading changes, so a switching model earns its complexity. Vehicle motion is comparatively mild. And with little prior work in the automotive setting, the parameters that IMM needs are hard to set.

What makes it hard

Fusing at full image resolution, per point, at thirty hertz, is a budget problem before it is an accuracy problem. The number of image channels you attach to each point is the knob you turn, and we tuned it as a tradeoff rather than maximizing it.

Data-layer fusion is also fragile in a specific and quiet way. Because each point is matched to an image location, a calibration error puts the sampled feature at the wrong pixel for every point, and the resulting input looks entirely plausible. There is no error signal. The system does not know it is being fed garbage.

Choosing a motion model is a lose-lose if you must choose one. Constant velocity is simple and strong for linear motion. Constant turn rate and velocity handles rotation. Either alone is wrong in the other regime. Switching between them is the answer, and switching introduces a set of prior parameters: the state switching matrix, the state covariance, the process and measurement noise covariances. These are conventionally set from experience, and empirical values are not guaranteed to apply to a different scenario. That is a real methodological problem, not a tuning nuisance.

There is a nonlinearity problem too. Constant-velocity transitions are linear in the state. Constant-turn transitions involve position expressed through velocity and yaw rate multiplied by sine and cosine of the heading, which is not linear, so the filter needs sigma points rather than a plain Kalman update.

Tracking also has to survive gaps and resist noise at the same time. Occlusion causes detections to drop, and being too permissive about starting new tracks admits detector false positives. The two failure modes pull in opposite directions.

And the physics caps detection regardless of architecture. In one of our examples, three very distant cars return only three to five LiDAR points each. Too few points and the detection simply does not fire.

Attach a neighbourhood, not a pixel

The first half of the idea addresses the fragility of data-layer fusion. Instead of sampling a single projected pixel for each point, we sample a 5×5 window of image features around the projection and attach the whole neighbourhood to the point.

The consequence is that the network learns the matching relationship between the two modalities rather than trusting the calibration exactly. A point whose projection lands a pixel or two off still finds relevant appearance features in its window. This was the single most useful conceptual move in the detection half, and it is what makes the difference between fusion that survives calibration error and fusion that quietly degrades.

The features come from a full-resolution extractor, which matters for a related reason: the point has to map to a precise image location for the window to mean anything, and a downsampled feature map destroys that precision. The encoder is VGG-based with two modifications. We reduced the channel count to keep optimization manageable, and we cut the network off in the middle so that the feature map does not shrink too far, which would make it useless for generating fusion data. A top-down decoder upsamples and merges with same-resolution encoder maps, with 1×1 convolutions controlling the output dimension.

View full-size image ↗

A local window learns the correspondence between the two modalities instead of assuming the calibration is exact.

Collapsing the height axis to get the frame rate

The augmented points then go through a pillar encoder. Points are binned onto a bird’s-eye-view grid with cell size and a cap on how many points a cell keeps, with random subsampling beyond the cap. The height axis is collapsed to a single cell, which is the trick that turns 3D convolution into 2D convolution and is where the frame rate comes from.

Each point carries its geometry and appearance plus five offset features that describe where it sits relative to its cell’s mean and its voxel centre. A PointNet layer expands these, max-pooling reduces each cell to one representative, and the cells are scattered back into a pseudo-image. A 2D convolutional backbone and a region proposal head then predict oriented 3D boxes.

The cost of the pillar representation is that height structure inside a cell is discarded and the detection range is bounded by the camera’s field of view, since fusion needs pixels. We accept both as the price of the frequency target.

One loss term deserves a note because it looks redundant and is not. The yaw residual is computed as the sine of the difference between predicted and anchor angle, and sine is symmetric. A car facing forward and a car facing backward produce the same residual. Regression alone cannot break the tie, which is why a separate direction classification term exists in the loss.

View full-size image ↗

The fusion-augmented point cloud is encoded as a bird’s-eye-view pseudo-image.

Two motion models, and a search for the priors

The tracker predicts every existing trajectory, associates predictions to detections by the Hungarian algorithm on 3D overlap, updates matched tracks, and manages births and deaths for everything unmatched.

The interesting part is the update. Two models run in parallel, one with a constant-velocity state and one with a constant turn rate and velocity state. Mixing probabilities are computed from the previous model weights and the switching matrix, mixed estimates are formed for each model, each model’s sigma-point filter predicts forward, and the two are combined into the IMM prediction. After association, each model is updated for the matched detection and the model probabilities are recomputed from the measurement likelihoods. The pairing is complementary by construction: the constant-velocity model estimates linear motion cheaply and well, and the constant-turn model covers the rotational case it cannot.

Life-cycle management handles the gap-versus-noise tension with two guardrails. An unmatched trajectory keeps predicting for a bounded number of frames before it is terminated, because an unmatched track usually means occlusion rather than disappearance, and restarting the trajectory later would fragment it. An unmatched detection has to persist for several consecutive frames before a track is born, which filters out occasional false positives. The two rules guard opposite failure modes.

The priors are where we departed from convention. Rather than setting the switching matrix and the covariances from experience, we treat them as an optimization problem and fit them with particle swarm optimization. The name invites a misreading worth clearing up: particle swarm optimization is a derivative-free search over parameters, it has nothing to do with particle filtering, and no particle filter appears anywhere in this work.

Two caveats on that fitting, and we would rather state them than have them discovered. The optimization runs offline and per dataset, so it is not online adaptation; the parameters are fitted to the data they were fitted on. And a symmetric switching matrix does not by itself tell you that vehicle motion in a dataset favors the constant-velocity model, even though it is tempting to read it that way.

View full-size image ↗

Bridging a missed detection is often cheaper than detecting it.

What we take from this

Latency budgets are a design input, not a measurement you take afterward. Once the real-time threshold is crossed, the remaining budget belongs to accuracy, and the architecture should be organized around that ordering.

A local receptive field is a cheap substitute for perfect calibration. When two sensors are aligned imperfectly, learning the correspondence in a neighbourhood beats assuming a pixel.

Reduce the dimensionality of the problem before optimizing the network. Pillarization is the enabling trick, not an afterthought.

Hybrid representations outperform pure ones here: learned appearance features, an explicit geometric projection, and an explicit state estimator, each doing what it is good at. The modularity also helps when something goes wrong, because each component has a distinct role and a distinct failure signature.

Complementarity beats tuning. Two models that fail in different regimes are better than one model that is mediocre in both.

And when the priors genuinely cannot be guessed, search for them. The admission that empirical IMM values may not transfer is the most useful methodological statement in the paper.

What it does not do

The honest summary is that the system is competitive in accuracy rather than leading. It improves substantially on detection speed and remains behind the best LiDAR-only methods on accuracy, which is the trade we chose.

The IMM-UKF has trouble with objects whose motion direction changes little. A filter that switches between hypotheses needs observable maneuver to be worth switching for.

Detection range is bounded by the camera field of view, which is inherent to this fusion design. Sparse distant objects fail, as the three-to-five-point example shows. The tracker is a separate stage from the detector rather than jointly optimized with it, which is the direction we point to for future work, along with improving accuracy without giving up the frame rate.

Closing

What we built integrates 3D detection and 3D multi-object tracking into a single pipeline measured against the real-time requirements of highway driving: a detector that produces oriented boxes efficiently, and a tracker that follows objects through uncertain motion using association, an interacting multiple model filter, and tuned parameters. The claim is not that it is the most accurate system available. It is that the accuracy is real at a frequency the fusion alternatives cannot reach, and that on a highway, being two metres behind the world is its own kind of inaccuracy.

Autonomous Driving | Chang Nie

Production-Grade 4D Auto-Annotation: Multi-Camera Rig SfM & High-Precision Geometry

Project context

My contributions

Why Driving Scenarios Break Classic SfM

Full Pipeline Architecture: From Raw Logs to 4D Vector Maps

Core Algorithmic Design & Engineering Implementation

1. Rig Bundle Adjustment (Rig BA) with Rigid-Rig Parameterization

2. Spatial Overlap Constraints & Epipolar Pruning

3. Decoupled Ground-Plane Filtering Prior to Triangulation

4. Spatiotemporal-Guided Dense PatchMatch Stereo (MVS)

5. Coarse-to-Fine Multi-Session Aggregation Network

Production Quality Assurance & Field Validation

Pipeline Video Recordings

Engineering Summary & Technical Takeaways

Autonomous Valet Parking: Production-Grade Full-Stack System for Underground Garages

Project context

My contributions

Key Engineering Bottlenecks in Underground Garages

Full-Stack Architecture & Algorithmic Design

1. Surround-View Fisheye BEV Multi-Task Perception & Near-Field Shielding

2. Factor-Graph Multi-Sensor Fusion Odometry & Hierarchical Mapping

3. Hierarchical Cruising & Parking Motion Planning

Corridor Cruising: Reference Spline Smoothing & Dynamic Evasion

Bay Parking: Modified Hybrid A* & Non-Linear Optimal Control

4. Precision Chassis Tracking & Delay Compensation

Real-Vehicle Field Verification

Engineering Takeaways & Architectural Lessons

Full-Stack Autonomous Driving Research Platform: Hardware-Software Co-Design & Real-Vehicle Verification

Project context

My contributions

Two Complementary Research Chassis

Ground-Up Electrical & Sensor Topology

1. Hardware Hard-Triggering & Microsecond Temporal Synchronization

2. Rigid Extrinsic Joint Calibration (LiDAR-Camera-IMU)

3. Proprietary CAN Bus Reverse Engineering & Deterministic SocketCAN Drivers

Full-Stack ROS Autonomy Architecture

1. Localization & High-Precision Mapping Pipeline

2. Environmental Perception & Scene Understanding Pipeline

3. Hierarchical Planning & High-Rate Actuation

Seven Real-Vehicle Closed-Loop Scenarios

Engineering Summary & Technical Takeaways

MRASfM: Multi-Camera Reconstruction and Aggregation through Structure-from-Motion in Driving Scenes

Driving is the wrong shape for photogrammetry

What prior methods assume

What makes it hard

The camera set is the unit

Building a reconstruction

Getting the road out of the way

Bundle adjustment on the rig

Merging drives that share no images

What we take from this

What it still assumes

Where it leads

3D Object Detection and Tracking Based on Lidar-Camera Fusion and IMM-UKF Algorithm Towards Highway Driving

Perception that is blind for two metres at a time

What the fusion families cost

What makes it hard

Attach a neighbourhood, not a pixel

Collapsing the height axis to get the frame rate

Two motion models, and a search for the priors

What we take from this

What it does not do

Closing