ROS | Saikiran Juttu | Robotics Portfolio

Kimera VIO on EuRoC and Custom Datasets

Fri, 26 Apr 2024 00:00:00 +0000

An evaluation and deployment project rather than an algorithms one. Kimera-VIO from MIT-SPARK was used as shipped; the work here was getting it running reproducibly, reproducing its published benchmark on EuRoC, and then pointing it at a sequence it had never seen — recorded on an Intel RealSense D455 in an ordinary office — to find out what breaks when you leave the benchmark behind.

What Kimera-VIO is doing

Worth stating, because the interesting failure later only makes sense against it.

Front end. Shi-Tomasi corners on the left stereo image, tracked temporally with Lucas–Kanade optical flow, matched left-to-right and triangulated for depth, with outlier rejection on the resulting correspondences. IMU measurements between keyframes are preintegrated into a single relative-motion constraint and used for gravity alignment.

Back end. A factor graph over position, orientation and velocity, with IMU factors and visual reprojection factors as edges, optimised incrementally by iSAM2 in GTSAM. Robust losses (Huber, Cauchy) absorb the outliers that survive the front end. Output is a pose trajectory, velocity estimates and a sparse landmark map.

Loop closure. Kimera-RPGO detects loops and runs robust pose-graph optimisation over them to produce a globally consistent trajectory.

Reproducibility

The deliverable that mattered most was a saved Docker image with Kimera-VIO, ROS and every dependency preinstalled — distributed as a tarball so the whole pipeline comes up in three commands:

docker load -i kimera-vio.tar
docker run -it kimera-vio
python3 visual.py data_pgo.csv # inside the container

Anyone who has built a VIO stack from source knows why this is worth doing. Kimera-VIO wants a specific GTSAM, which wants a specific Boost and Eigen, and the failure mode of getting it wrong is a pipeline that compiles and then silently estimates garbage. Pinning the whole environment means a result someone else can actually reproduce, and it makes “does this parameter change help?” a question you can answer in an afternoon instead of a week.

Stereo image pairs and IMU streams were synchronised and fed in through ROS.

EuRoC benchmark

Run on the EuRoC MAV Vicon Room 1 sequences — V1_01, V1_02 and V1_03, spanning easy, medium and difficult, with motion-capture ground truth. That’s three of the dataset’s eleven sequences; the Machine Hall and Vicon Room 2 sets weren’t covered.

ATE RMSE in metres, reproducing the published comparison across VIO pipelines:

Sequence	OKVIS	MSCKF	ROVIO	VINS-Mono	Kimera-VIO
V1_01	0.09	0.34	0.10	0.08	0.05
V1_02	0.20	0.20	0.10	0.11	0.08
V1_03	0.24	0.67	0.14	0.18	0.07

The pattern worth noting is the third row. Every other pipeline degrades on V1_03 — the difficult sequence, with faster and more aggressive motion — while Kimera-VIO holds at 0.07 m. Aggressive motion is good for a visual-inertial system: it excites the IMU, which is what makes scale and gravity direction observable in the first place. Getting parameters into the range where that benchmark reproduces meant tuning IMU noise models, feature thresholds and optimiser tolerances. Throughput came out around 25 FPS.

Custom dataset

Recorded with a RealSense D455 in a room of desks, monitors and the usual office clutter, walking an approximately oval path back to the starting point.

Trajectory after pose-graph optimisation. The plot renders the path in the x–y plane, so vertical motion isn’t shown.

There is no ground truth for this sequence — no motion capture in an office — so everything here is qualitative. No ATE, no drift percentage, and any number attached to it would be invented. What it does show is loop closure working: the path returns to where it started rather than spiralling away.

Starting at rest

The most useful thing this sequence taught me was a failure at the very beginning of the recording, and it isn’t a bug — it’s a property of the problem.

Visual-inertial odometry recovers metric scale and gravity direction from the accelerometer. But an accelerometer at rest measures only gravity, and there is no way to separate “which way is down” from “how fast am I accelerating” without motion to disambiguate them. Start recording with the camera stationary and the initialiser has no excitation to work with; it converges on a poor estimate, and that error is baked into every pose that follows.

That’s exactly what happened — visible drift accumulated from the start of the trajectory, and it persisted until loop closure corrected it. It also explains the EuRoC pattern above: the “difficult” sequence is easier for VIO in this one specific respect. The practical lesson is that a data-collection protocol matters as much as a config file: move the camera through a few seconds of varied motion before anything you care about, and the whole run improves.

Elsewhere in the sequence, low-texture regions produced feature sparsity and minor drift, which pose-graph optimisation largely absorbed.

Limits and next steps

Kimera-VIO’s weak points on this data were the ones its authors document: drift in low-texture regions, and sensitivity to outlier visual measurements. The obvious extensions, in order of how much they’d change the picture:

Metric-semantic mapping. This used the VIO half of the Kimera suite only. Kimera-Semantics builds a semantically annotated mesh from the same pipeline, which is where the interesting downstream work is.
Outdoor and larger scale, to see where the factor graph stops being tractable.
LiDAR as an additional modality, for the feature-poor cases where vision alone degrades.

Stack

Kimera-VIO (MIT-SPARK), GTSAM, ROS, Docker, Python, matplotlib. Datasets: EuRoC MAV V1_01–V1_03; self-collected RealSense D455 stereo + IMU.

Semantic Geometric SLAM (SG-SLAM) in Dynamic Scenes

Fri, 19 Apr 2024 00:00:00 +0000

Feature-based visual SLAM assumes the world holds still. It doesn’t. When someone walks through the frame, their features get matched across keyframes like any other, and the optimiser dutifully fits a camera trajectory to a person who was never part of the scene geometry. On the TUM fr3/walking_* sequences this is not a marginal degradation — ORB-SLAM2’s absolute trajectory error goes to roughly half a metre.

This project reimplements SG-SLAM (Cheng, Sun, Zhang & Zhang, IEEE TIM vol. 72, 2023), which addresses this by adding two parallel threads to ORB-SLAM2 — one for object detection, one for semantic mapping — and a feature-rejection stage in the tracking thread that consults both geometry and semantics before deciding what to keep.

The three threads running together. Left: the live frame — note the green ORB features sit on desks, walls and shelving, not on the person. Bottom: the reconstructed point cloud with detected objects labelled and localised.

Why not just mask out the people

The obvious approach is to run a detector, draw boxes around every a priori dynamic class, and discard whatever falls inside. That fails in both directions, and SG-SLAM’s design is a direct response to each failure:

Things move that aren’t in a box. A detector trained on 20 classes will miss a swinging door, a rolling cart, a shadow. Masking gives you no protection outside the boxes it drew.
Things in boxes aren’t always moving. A parked chair, an empty sofa, a person sitting perfectly still — all get classified dynamic, all get thrown away. In a cluttered indoor scene that can mean discarding the best-textured features you have.

The rejection criterion

The mechanism that avoids both is worth reading closely, because it is subtler than a mask. Reproduced from the paper’s Algorithm 1:

Input: Previous frame F1, current frame F2
 Feature points P1 (previous), P2 (current)
 Standard empirical threshold e_std
Output: Set S of static feature points in the current frame

 1: P1 = CalcOpticalFlowPyrLK(F2, F1, P2)
 2: Remove matched pairs at image edges or with large appearance variation
 3: F = FindFundamentalMat(P2, P1, 7-point method with RANSAC)
 4: for each matched pair (p1, p2) in (P1, P2) do
 5: if DynamicObjectsExist and IsInDynamicRegion(p2) then
 6: if CalcEpiLineDistance(p2, p1, F) * GetDynamicWeightValue(p2) < e_std then
 7: append p2 to S
 8: end if
 9: else
10: if CalcEpiLineDistance(p2, p1, F) < e_std then
11: append p2 to S
12: end if
13: end if
14: end for

There is one threshold, e_std, and it is compared against a point-to-epipolar-line distance in both branches. The difference is that inside a detected region (line 6) that distance is first multiplied by the object class’s dynamic weight.

So semantics act as a per-class multiplier on the geometric residual, not as a veto. A high-weight class like a person has its residual inflated, so it gets rejected on much weaker geometric evidence than a low-weight class like a chair. But a genuinely stationary object still passes — its epipolar distance is near zero, and scaling near-zero by three is still near-zero. Meanwhile line 10 runs everywhere the detector saw nothing, so unmodelled motion is still caught geometrically.

Two implementation details that are easy to miss: the optical flow at line 1 runs backwards, tracking the current frame’s points into the previous frame to reconstruct correspondences, and the fundamental matrix uses the seven-point method inside RANSAC rather than the more common normalised eight-point variant.

Detector

Detection runs an SSD with a MobileNetV3 backbone under NCNN, Tencent’s mobile CPU inference framework. That choice is what makes the real-time claim plausible without a GPU in the loop — the whole point of SG-SLAM over heavier semantic-SLAM systems is that it targets mobile platforms. The tracking thread computes its geometric quantities, blocks on the detector’s 2-D result for that frame, then runs rejection and tracks; only surviving features reach local mapping, loop closing and full bundle adjustment, all of which are unmodified ORB-SLAM2.

Detection on a live frame.

Objects localised in 3-D with class labels.

The semantic mapping thread fuses the 2-D detections with per-keyframe point clouds generated from the depth images and camera poses, then extracts each object’s position and extent into a 3-D semantic object database. That database, a global OctoMap and the camera poses are all published over ROS for RViz — which is the real difference from plain ORB-SLAM2, whose output is a sparse cloud with no idea what anything is.

Results

Benchmark figures below are the published results from Cheng et al., reproduced here rather than independently measured. Metric is ATE RMSE in metres — these are error reductions, not accuracy gains.

TUM RGB-D

Sequence	Dynamics	ORB-SLAM2	SG-SLAM	Reduction
`fr3/walking_static`	high	0.4032	0.0079	98.03%
`fr3/walking_xyz`	high	0.6826	0.0171	97.50%
`fr3/walking_rpy`	high	0.5396	0.0326	93.95%
`fr3/walking_halfsphere`	high	0.4462	0.0309	93.07%
`fr3/sitting_static`	low	0.0087	0.0060	31.03%

Bonn RGB-D Dynamic (9 sequences)

Best and worst of the set: synchronous2 improves 1.4069 → 0.0164 m (98.83%), while synchronous — nominally the same scene, different take — only reaches 1.1411 → 0.3262 m (71.41%). The moving_nonobstructing_box pair land around 71–79%, the crowd and person_tracking sequences 93–97%.

Reading the numbers honestly

Two things about this table are worth saying out loud, because they’re the parts a results summary usually hides.

The 31% on sitting_static isn’t a weak result, it’s the control. That sequence is low-dynamic — a person seated, barely moving. There is almost nothing for dynamic-feature rejection to remove, so the gain collapses to a third of what the walking sequences show. The rotational-drift table is starker still: 7.99% on the same sequence. That’s the expected shape of the result, and it’s evidence the mechanism is doing what it claims rather than just globally discarding features.

RMSE improves far more than the median does. On walking_static the ATE RMSE drops 98% while the RPE median improvements sit at 43–53%. The gain is concentrated in catastrophic frames — the ones where a person crossing the view wrecks the pose estimate entirely — not in the typical frame. That’s the right thing for a SLAM system to fix, since a single badly-corrupted keyframe propagates into the map, but it does mean “98% better” describes the tail, not the average.

Where it breaks

The failure mode falls straight out of Algorithm 1, and the paper is upfront about it: an object moving along the epipolar line direction is invisible to this test. Its features produce a point-to-epipolar-line distance of approximately zero despite genuinely moving, so line 10 accepts them as static. The semantic branch doesn’t save you either — multiplying a near-zero residual by a dynamic weight still clears the threshold. In practice that means someone walking directly toward or away from the camera is much harder to reject than someone crossing the view.

The other open item is semantic map precision. Object extents come from thresholding a depth-derived point cloud inside a 2-D box, which is coarse — good enough to say “there is a monitor at roughly here”, not good enough for manipulation.

Stack

C++ (ORB-SLAM2 core), Python, Ubuntu 18.04, ROS Melodic, NCNN, OpenCV, OctoMap, RViz. Evaluated on TUM RGB-D and the Bonn RGB-D Dynamic dataset.

Dead Reckoning with IMU & GPS in Vehicle Navigation

Tue, 09 Apr 2024 00:00:00 +0000

Take away the GPS fix and a vehicle has to work out where it is from acceleration and rotation alone. This project does that end to end on real data — instrument a car, calibrate the magnetometer properly, fuse a heading estimate, integrate forward velocity, and reconstruct the trajectory — then compares the result against GPS to find out how long dead reckoning actually survives.

The short answer: about three minutes. After that it is off by the length of a city block.

Coursework for EECE5554: Robotics Sensing and Navigation at Northeastern.

Setup

A VectorNav VN-100 IMU mounted inside the vehicle in a custom holder, deliberately aligned so its x-axis points along the vehicle’s forward direction and the unit sits level. That alignment is doing real work later — it’s what lets accel_x be treated as forward acceleration and accel_y as lateral without a rotation. A magnetic-mount GPS puck went on the roof, both feeding one laptop over ROS.

Two datasets:

Circles — five laps of the Ruggles station circle, recorded purely to excite the magnetometer through a full 360° of heading. This is calibration data, not navigation data.
Driving — a 2–3 km route around Boston with at least ten turns, roughly 1300 s, routed to avoid tunnels and underpasses so the GPS reference stays continuous.

Magnetometer calibration

An uncalibrated magnetometer driven in a circle should trace a circle. It doesn’t, and the two ways it fails are visually distinguishable from the same scatter plot:

Five laps of the Ruggles circle. Orange is raw, blue is corrected.

The ring is off-centre → hard iron distortion. A constant field offset from permanent magnets, ferrous structure or current-carrying conductors near the sensor. The raw ring here sits up and to the right of the origin, biased by roughly 0.27 G in x and 0.22 G in y. Correction is a per-axis bias subtraction that recentres the cluster.
The ring is deformed → soft iron distortion. Non-uniform scaling and rotation from nearby ferrous material. You can see it in both rings as a distinct kink near the +x side — the shape isn’t circular and no amount of translation fixes that. Correction is a matrix that scales and rotates the data back toward a circle.

The parameters come from a least-squares ellipse fit to the circle-driving data, applied as bias subtraction followed by the correction matrix. Being able to read which distortion you have off the plot before fixing either is the useful part of this exercise — the two have different causes and different remedies, and the geometry tells you which you’re looking at.

Heading

Neither heading source is usable alone. The magnetometer gives absolute heading but is noisy and sensitive to the vehicle’s own electrical and magnetic environment. The gyro is clean over short intervals but integrates its bias without bound — the raw integrated yaw here climbs to about +405° by t ≈ 560 s and plateaus near 300°, which is meaningless as an absolute heading.

A complementary filter takes the half of each signal that’s trustworthy:

0.5 Hz low-pass on magnetometer yaw — keeps the slow-moving absolute reference, discards the noise
0.1 Hz high-pass on gyro yaw — keeps short-term rate fidelity, discards the drift
blended with coefficient 0.98

The two filtered branches, the fused output, and the VN-100’s internal estimate for comparison.

Forward velocity, and the number that gave the game away

Integrating raw accelerometer output produces nonsense. The raw forward-velocity trace peaks around +35 m/s and dips to −15 m/s — the car did neither. That’s accelerometer bias being integrated into an unbounded ramp, plus road vibration and the gravity component that any unnoticed slope injects into the forward axis.

Two corrections bring it into physical range: a high-pass filter on forward acceleration to strip the low-frequency bias, then clamps — negative velocities to zero, plus an upper bound. That gets the adjusted trace to a plausible 0–11 m/s, against GPS-derived velocity peaking at 12.7 m/s.

Blue: integrate the raw signal and the car reaches 35 m/s and reverses. Orange: after high-pass filtering and clamping.

Then aligning the dead-reckoned track to GPS required scaling the IMU distances by 2.25×.

That factor is the most informative number in the whole project, and it isn’t a calibration constant — it’s a symptom. A 2.25× scale error means the integrated velocity was systematically less than half the true speed, and both corrections above are implicated. The high-pass filter that removed accelerometer bias also removed genuine low-frequency acceleration, because a car accelerating gently from a light and a slowly-drifting bias occupy the same part of the spectrum. And clamping negative velocity to zero doesn’t just discard drift — it rectifies the signal, converting every real deceleration the drifting integrator pushed below zero into “stationary”. Distance travelled comes out short in both cases.

The filter that fixed the drift destroyed the scale. Getting both would need the bias estimated as a state rather than filtered out — which is the argument for a Kalman filter over hand-tuned Butterworths, and the direction I’d take this next.

Trajectory

Dead-reckoned track (left) against GPS (right). The shape survives; the extent doesn’t.

Projecting forward velocity onto the fused heading and integrating gives the track above, aligned to GPS by matching start coordinates and the first straight segment.

The two agree to within about 2 m for the first 150–200 s
After that they diverge, reaching roughly 400 m of displacement error by the end of the ~2–3 km route

Dead reckoning worked best exactly where you’d expect — early, on straight, level road. It degraded with each successive turn, and the two causes are gyro bias instability feeding heading error into every subsequent position update, and unanticipated road slopes tilting gravity onto the forward axis where it gets integrated as acceleration.

One honest wrinkle in the alignment: the IMU track was rotated 14° and the GPS track 25°, in opposite service of making the first straight segments agree. Rotating both by different amounts means the result no longer references true north — a single rotation of the difference would have been the defensible choice.

The takeaway

There’s a usable operating envelope here and it’s narrow. Roughly 150–200 seconds of GPS outage is survivable at 2 m accuracy, which is enough to cross a tunnel or an urban canyon. It is nowhere near enough to navigate on, and the failure is not graceful — error grows superlinearly as heading error compounds. Any real system needs periodic absolute fixes, and the interesting engineering question is how sparse those fixes can be, not whether you need them.

Stack

ROS (rosbag, sensor nodes), Python, SciPy (butter/filtfilt, least-squares fitting), matplotlib, UTM projection. Hardware: VectorNav VN-100, magnetic-mount GPS puck, Northeastern’s instrumented “nuance” vehicle.