DynaVIG: Global Navigation & Object Tracking

• DynaVIG is a navigation and object tracking framework that fuses monocular vision, INS, and GNSS to mitigate long-term drift and scale ambiguity.
• It uses a unified factor-graph optimization to jointly estimate vehicle 6-DoF pose and dynamic objects’ states in a globally referenced ENU frame.
• Evaluations on KITTI show reduced translational errors and superior dynamic object tracking compared to traditional monocular VIO systems.

DynaVIG is a navigation and object tracking system for automated ground vehicles in dynamic outdoor scenes that integrates monocular vision, an Inertial Navigation System, and the Global Navigation Satellite System in a unified factor-graph framework. Its stated goal is to provide accurate global estimation of navigation states and object poses in scenes containing moving cars and pedestrians, while addressing long-term drift, monocular scale ambiguity, and the corruption of visual constraints by dynamic objects. Unlike monocular Visual-Inertial Odometry pipelines that primarily deliver local relative pose and are affected by unobservable global position and yaw, DynaVIG introduces GNSS for globally referenced ENU constraints and models moving objects explicitly rather than treating them as nuisances (Jin et al., 2022).

1. Problem setting and scope

DynaVIG targets two coupled estimation problems for AGVs operating in dynamic traffic environments: accurate global navigation and 3D object tracking. The navigation component estimates the AGV’s 6-DoF pose and velocity over long trajectories in a globally referenced ENU frame. The object component estimates the 3D pose, yaw, velocity, and scale of surrounding objects from a single camera, with ground constraints and allowance for mild slope (Jin et al., 2022).

The system is motivated by several limitations of traditional monocular VIO. First, without absolute position, VIO accumulates error in translation and yaw over time. Second, vision and IMU alone leave four components, described as global position and yaw, not fully observed. Third, feature tracks on moving cars and pedestrians violate static-scene epipolar geometry; if these features are treated as static, they introduce inconsistent visual constraints and enlarge drift. Fourth, monocular SLAM has intrinsic scale ambiguity, and methods that fix scale using camera height and planar ground assumptions can fail on slopes, under camera height changes, or with occlusions (Jin et al., 2022).

DynaVIG addresses these issues by combining GNSS-based drift reduction, explicit dynamic-object modeling, and joint optimization of camera, objects, map points, and object scale. This places it within a broader class of dynamic-environment visual-inertial systems, but its defining emphasis is simultaneous global navigation and object tracking under monocular vision/INS/GNSS fusion rather than purely dynamic-feature rejection (Jin et al., 2022). A plausible implication is that DynaVIG occupies an intermediate position between object-aware dynamic-scene SLAM and globally referenced integrated navigation.

2. System architecture and sensing configuration

The architecture uses three sensors: a monocular camera, an IMU at 100 Hz, and GNSS at 1 Hz. The coordinate frames are the navigation frame $N$ in ENU, the body frame $B$ aligned with the IMU, the camera frame $C$, and an object frame $O$ attached to each tracked object. Known extrinsic transforms are $\boldsymbol{T}_{BG}$ for body–GNSS and $\boldsymbol{T}_{BC}$ for body–camera calibration (Jin et al., 2022).

The front-end begins with object detection via YOLOv5. Object features and static scene features are extracted using Good Features to Track, and Lucas–Kanade optical flow is used for inter-frame tracking. Object association combines bidirectional LK optical flow with BRIEF descriptor matching: optical flow predicts correspondences and BRIEF improves robustness under motion. Object motion state is then inferred from the standard deviation of a triangulated depth sequence $\mathbf{d} = [d_0,\dots,d_m]$. Small depth standard deviation indicates that the object is essentially static and its features are treated as part of the static scene; large depth standard deviation indicates a dynamic object that is tracked separately and excluded from the static map (Jin et al., 2022).

The inertial and satellite subsystems are processed in parallel with the visual stream. IMU pre-integration provides high-rate motion constraints between keyframes, while GNSS contributes loosely coupled position measurements in ENU. The back-end is a sliding-window factor graph implemented in GTSAM with approximately 10 keyframes. Its state includes body pose and velocity, static map points, object pose and points, object velocity and yaw rate, and an object scale factor (Jin et al., 2022).

This architecture differs from dynamic visual-inertial SLAM systems such as DynaVINS, which focus on rejecting dynamic and temporarily static features through robust bundle adjustment and loop-closure hypothesis grouping, and from DynaVINS++, which emphasizes adaptive truncated least squares and bias consistency recovery under abruptly dynamic objects (Song et al., 2022, Song et al., 2024). DynaVIG instead uses explicit object states and dynamics inside the estimation graph.

3. Unified estimation framework

The global state vector is defined as

$$\boldsymbol{X} = \left[ \boldsymbol{T}_{NB},\; \boldsymbol{v}_{NB},\; \boldsymbol{P}_{M},\; s,\; \boldsymbol{T}_{NO},\; \boldsymbol{P}_{O},\; v_{\psi},\; \boldsymbol{v}_{NO} \right].$$

Here $\boldsymbol{T}_{NB} \in SE(3)$ is the body pose in the navigation frame, $\boldsymbol{v}_{NB} \in \mathbb{R}^3$ is the body velocity in ENU, $B$0 denotes static map points, $B$1 is an object scale factor, $B$2 is the object pose with constrained translation-plus-yaw structure, $B$3 denotes object feature points in the object frame, $B$4 is object yaw speed, and $B$5 is object velocity in ENU (Jin et al., 2022).

The IMU factor uses standard pre-integration between keyframes: $B$6 The GNSS factor is formulated as a loosely coupled ENU position residual: $B$7 The static-feature reprojection residual is

$B$8

and the object-feature reprojection residual is

$B$9

These terms couple camera pose, object pose, object geometry, and scale directly within the same optimization (Jin et al., 2022).

The object dynamics state is

$C$0

with a random constant model over short intervals: $C$1 This is enforced through

$C$2

with covariance

$C$3

As specified in the paper, increasing object speed enlarges the variance of the dynamics factor, allowing more flexibility in rapid changes of velocity and yaw rate, while slower objects are more strongly regularized by constant-velocity and constant-yaw assumptions (Jin et al., 2022).

All factors are optimized jointly through a single weighted least-squares objective: $C$4 The significance of this formulation is that navigation errors influence object estimates and object dynamics regularize object trajectories without severing the dependence on globally constrained camera motion (Jin et al., 2022).

4. Prior height model, scale resolution, and object state parameterization

A central difficulty in monocular object tracking is scale ambiguity. DynaVIG addresses this using a prior height model. For an object class such as a car or pedestrian, a typical height $C$5 is assumed. Using the vertical image span of the object and similarity of triangles, depth is initialized as

$C$6

hence

$C$7

For any point $C$8 on the object,

$C$9

This provides an initial monocular depth estimate using only image extent and a class-specific height prior (Jin et al., 2022).

Because the true height $O$0 may differ from the prior, DynaVIG introduces the scale factor

$O$1

This variable is estimated continuously in the factor graph through the object reprojection factor

$O$2

The intended coupling is explicit: IMU and GNSS constrain the camera trajectory metrically, and object pose and scale must remain consistent with camera observations, so scale converges toward physically consistent values (Jin et al., 2022).

Object pose is represented with 4 DoF, $O$3, consisting of 3D translation and yaw while fixing roll and pitch. This representation exploits ground constraints yet permits mild slope through non-zero $O$4. The paper contrasts this with 3-DoF ground-plane-only models, which are more restrictive, and with unconstrained 6-DoF object pose, which can be underconstrained. Initial object yaw is computed from initial velocity components as

$O$5

This suggests that DynaVIG treats traffic participants as predominantly ground-constrained bodies whose dominant orientation is captured by yaw rather than full attitude (Jin et al., 2022).

5. Dynamic-scene handling and object tracking

DynaVIG does not define explicit discrete motion modes such as constant-velocity versus turning models. Instead, it separates static and dynamic objects through depth-sequence statistics and uses a continuous dynamics model with adaptive covariance. Objects with low depth standard deviation are treated as part of the static environment and their features help the map; objects with large depth standard deviation are explicitly tracked and their features are excluded from static SLAM factors (Jin et al., 2022).

For dynamic objects, tracking combines prediction and update. Prediction is supplied by the random constant model over translational velocity and yaw rate. Update is supplied by visual reprojection factors on object points, coupled to the camera pose, while the camera itself is constrained by IMU and GNSS. Because objects do not receive GNSS directly, their global consistency is inherited from the globally referenced camera trajectory and from the dynamics prior (Jin et al., 2022).

This separation is important for navigation robustness. Dynamic objects are not discarded wholesale; instead, they are assigned their own states, scale, and dynamics factors. Static objects, including parked vehicles, can enrich the map when identified as non-dynamic. Dynamic objects remain useful for object-level estimation but are prevented from contaminating the static map that supports AGV navigation. In that sense, DynaVIG is object-aware rather than merely outlier-rejecting (Jin et al., 2022).

Related work illustrates alternative strategies. DynaVINS emphasizes feature-wise weighting in robust bundle adjustment and hypothesis-wise weighting in loop closure to suppress dynamic and temporarily static objects without explicit object-state estimation (Song et al., 2022). DynaVINS++ extends this line by focusing on abrupt motion onset and preventing false visual residuals from corrupting IMU biases through adaptive truncated least squares and stable state recovery (Song et al., 2024). DynaVIG differs in that it models object pose, velocity, yaw rate, and scale directly inside the graph.

6. Evaluation, limitations, and research context

Experiments were conducted on the KITTI Tracking dataset with IMU from KITTI Raw and simulated GNSS created by adding Gaussian noise to ground-truth trajectories. Camera navigation was evaluated using ATE, translational relative pose error $O$6 in m/f, and rotational relative pose error $O$7 in deg/f. Object tracking was evaluated using ATE, $O$8 in m/m, $O$9 in deg/m, and object scale error $\boldsymbol{T}_{BG}$0. Baselines were VDO-SLAM, DynaSLAM II, and TwistSLAM (Jin et al., 2022).

For camera pose estimation, DynaVIG reported mean $\boldsymbol{T}_{BG}$1 m/f, compared with $\boldsymbol{T}_{BG}$2 m/f for DynaSLAM II and $\boldsymbol{T}_{BG}$3 m/f for TwistSLAM. Mean rotational error was $\boldsymbol{T}_{BG}$4 deg/f for DynaVIG and $\boldsymbol{T}_{BG}$5 deg/f for TwistSLAM. The paper attributes the weaker rotational average to relatively short KITTI sequences, limited IMU bias convergence, and the fact that stereo provides better parallax than monocular vision. At the same time, the GNSS and INS constraints improve translational performance substantially despite monocular sensing (Jin et al., 2022).

For object tracking, DynaVIG reported mean object ATE of $\boldsymbol{T}_{BG}$6 m, compared with $\boldsymbol{T}_{BG}$7 m for DynaSLAM II and $\boldsymbol{T}_{BG}$8 m for TwistSLAM. Mean object rotational error was $\boldsymbol{T}_{BG}$9 deg/m for DynaVIG, compared with $\boldsymbol{T}_{BC}$0 deg/m for DynaSLAM II and $\boldsymbol{T}_{BC}$1 deg/m for TwistSLAM. Mean $\boldsymbol{T}_{BC}$2 was slightly worse than TwistSLAM, $\boldsymbol{T}_{BC}$3 versus $\boldsymbol{T}_{BC}$4 m/m, which the paper associates with remaining scale errors. Most objects had scale errors around $\boldsymbol{T}_{BC}$5–$\boldsymbol{T}_{BC}$6, while abnormal cases occurred when cars were static and the camera remained stationary for long intervals, or when small image footprint and occlusion prevented sufficient observations (Jin et al., 2022).

The computational profile reflects the cost of unified multi-sensor estimation. On an Intel i3-4150 at 3.5 GHz with 16 GB RAM, the front-end required approximately $\boldsymbol{T}_{BC}$7–$\boldsymbol{T}_{BC}$8 ms per frame and the back-end approximately $\boldsymbol{T}_{BC}$9–$\mathbf{d} = [d_0,\dots,d_m]$0 ms per frame. The paper characterizes the front-end as similar to DynaSLAM II and the back-end as heavier because of additional features and factors (Jin et al., 2022).

The stated limitations are correspondingly specific. DynaVIG depends on approximate class-specific object heights for initialization; scale convergence depends on motion and can be poor for static objects with stationary cameras. The current system assumes GNSS availability at 1 Hz with Gaussian noise. Monocular vision yields lower rotational accuracy than stereo. The constant-velocity and constant-yaw model with adaptive covariance may not capture abrupt turns or stop-and-go motion fully. Backend optimization is computationally heavier than some baselines (Jin et al., 2022).

In the broader literature, later dynamic-environment VINS systems pursued complementary robustness mechanisms. DynaVINS formalized robust bundle adjustment using IMU-based pose priors and addressed temporarily static objects in loop closure (Song et al., 2022). DynaVINS++ concentrated on abruptly dynamic objects and on preventing dynamic-feature errors from propagating into IMU bias terms (Song et al., 2024). These developments suggest two distinct but compatible trajectories in the field: one centered on explicit object-state estimation and global fusion, exemplified by DynaVIG, and another centered on increasingly robust visual-inertial state estimation under adversarial scene dynamics.