Feature/track saving by MultyXu · Pull Request #43 · MIT-SPARK/Khronos

MultyXu · 2026-09-03T14:01:43Z

PR along with #41, still following #30 to clean up the code. PR points to #41 to better visualize only the track-saving changes.

Will rebase on top of #41 after the change detection code is merged.

…saving mechanism.

- Nan depth values are forward to the module producing Nan bounding boxes - Filter out Nan value when loading frame data - Test tracker association now saves the reprojected mask - Instance forwarding now to Nan filtering

- Red for current frame mask, green for reprojected mask, blended yellow for intersecting pixels.

…o proper name.

nathanhhughes

I think this needs a little bit more work before I'm happy with it, but it seems like this shouldn't break anything and the new debugging utilities are mostly useful. If the boxmot changes require this PR specifically, it's probably okay for this to go in and then I can go clean it up when I get a chance, otherwise we can coordinate who cleans this up and when it actually goes in.
Note: I looked at most of the PR, but kept comments minimal for now (they're representative of my general feedback though)

nathanhhughes · 2026-09-06T22:49:35Z

+   * Extracted from computeIoUPixels so debugging/visualization tools (e.g. test_track_association)
+   * can obtain the same reprojected pixel set the tracker's own pixel-IoU comparison uses, without
+   * duplicating the formula.


Not useful as a comment

nathanhhughes · 2026-09-06T22:49:56Z

+   * @return The set of image pixels the points project onto (points that fail to project, e.g.
+   * behind the camera, are omitted).


I'd drop the parenthetical note

nathanhhughes · 2026-09-06T22:50:11Z

+   * @return The set of image pixels the points project onto (points that fail to project, e.g.
+   * behind the camera, are omitted).
+   */
+  std::set<Pixel> reprojectPoints(const FrameData& data, const Points& points) const;


Pretty sure this can be a static method

nathanhhughes · 2026-09-06T22:51:50Z

+/**
+ * Step 4 of the tracker-debugging pipeline (see tracker_debug.md): replays an ENTIRE saved run
+ * through a fresh MaxIoUTracker, frame by frame in chronological order, with the real multi-object
+ * competition production has (every cluster in every frame, not the single-pair filtering
+ * test_track_association.cpp does for Step 2). Lets different tracker configs (min_semantic_iou,
+ * track_by, etc.) be evaluated end-to-end against the same recorded data.
+ *
+ * Also simulates active-window eviction the same way production does: after each frame's
+ * association, every track's is_active is recomputed via the real hydra::VolumetricWindow
+ * (SpatialWindowChecker/TemporalWindowChecker) inBounds() check, exactly mirroring
+ * ActiveWindow::updateTrackingStatus (active_window.cpp). Evicted tracks are erased from the live
+ * list (mirroring ActiveWindow::extractInactiveObjects) and their final state archived to disk --
+ * once evicted, a track can never be re-associated again, matching production.
+ *
+ * KNOWN APPROXIMATION: pose.txt records getSensorPose() = real_world_T_body * real_body_T_sensor
+ * from the original run. FrameData::load's reconstructed camera uses identity extrinsics, so
+ * input.world_T_body is set to that recorded pose directly -- correct for the tracker's own
+ * reprojection math, but when reused here as "the robot body pose" for the eviction-radius check,
+ * it is off by the real camera's mounting offset (typically tens of cm). Negligible at the
+ * deployed 14m threshold; not solved (would require saving the real extrinsics).
+ *


Can probably drop this

nathanhhughes · 2026-09-07T00:53:00Z

+void saveCameraIntrinsics(const hydra::Camera& camera, const std::string& run_dir) {
+  const auto& cfg = camera.getConfig();
+  std::ofstream stream(run_dir + "/camera_intrinsics.json");
+  if (!stream.is_open()) {
+    LOG(ERROR) << "[FrameData] Failed to open camera_intrinsics.json for writing in " << run_dir;
+    return;
+  }
+  const json j{{"fx", cfg.fx},
+              {"fy", cfg.fy},
+              {"cx", cfg.cx},
+              {"cy", cfg.cy},
+              {"width", cfg.width},
+              {"height", cfg.height}};
+  stream << j.dump(2);
+}
+
+std::shared_ptr<hydra::Camera> loadCameraIntrinsics(const std::string& run_dir) {
+  std::ifstream file(run_dir + "/camera_intrinsics.json");
+  if (!file.is_open()) {
+    LOG(ERROR) << "[FrameData] Missing camera_intrinsics.json in " << run_dir;
+    return nullptr;
+  }
+  json j;
+  file >> j;
+
+  hydra::Camera::Config config;
+  // Sensor::Config requires min_range > 0 and max_range > min_range; the sink does not save the
+  // original sensor's range limits, so use permissive defaults wide enough for indoor/outdoor use.
+  config.min_range = 0.01;
+  config.max_range = 100.0;
+  config.fx = j.at("fx").get<float>();
+  config.fy = j.at("fy").get<float>();
+  config.cx = j.at("cx").get<float>();
+  config.cy = j.at("cy").get<float>();
+  config.width = j.at("width").get<int>();
+  config.height = j.at("height").get<int>();
+  // Identity extrinsics: the saved pose.txt already IS the sensor pose in world frame, so using
+  // it directly as world_T_body with identity body_T_sensor reproduces getSensorPose() exactly.
+  config.extrinsics = hydra::IdentitySensorExtrinsics::Config{};
+
+  return std::make_shared<hydra::Camera>(config, "reconstructed_camera");
+}


You should be able to load and save the sensor information via config::toYaml and config::fromYaml (though I need to fix something in Hydra for whether or not the sensor is a camera or not before you can easily load the full sensor after the fact). This would also fix the issue with the extrinsics not being saved

nathanhhughes · 2026-09-07T00:59:24Z

+    cv::Mat bgr_image;
+    cv::cvtColor(data.input.color_image, bgr_image, cv::COLOR_RGB2BGR);
+    cv::imwrite(dir + "/color.png", bgr_image);


I wouldn't bother swapping RGB and BGR order on both save and load

nathanhhughes · 2026-09-07T01:00:33Z

+    // 16-bit PNG scaled to mm for easy viewing.
+    cv::Mat depth_mm;
+    data.input.depth_image.convertTo(depth_mm, CV_16UC1, 1000.0);
+    cv::imwrite(dir + "/depth.png", depth_mm);
+
+    // Raw float32 binary for exact values.
+    std::ofstream depth_stream(dir + "/depth.bin", std::ios::binary);
+    if (depth_stream.is_open()) {
+      depth_stream.write(reinterpret_cast<const char*>(data.input.depth_image.data),
+                         data.input.depth_image.total() * data.input.depth_image.elemSize());
+    }


this would be better as cv::imwrite(dir + "/depth.tiff", data.input.depth_image);. Most image viewers should support it and it doesn't lose information

MultyXu added 16 commits September 3, 2026 09:56

initial version for track saving.

c323296

add serialization method for Track class.

a3f4be2

step 3 complete, we now have a new formalized tracking and framedata …

01ccb42

…saving mechanism.

initial version of full pipeline test

eb8b6ac

frame data construct rgb too

098d8aa

Getting full tracker replay to work.

8c1936f

- Nan depth values are forward to the module producing Nan bounding boxes - Filter out Nan value when loading frame data - Test tracker association now saves the reprojected mask - Instance forwarding now to Nan filtering

Better color blending.

6382bf5

- Red for current frame mask, green for reprojected mask, blended yellow for intersecting pixels.

better tracker debug message print.

d1d31b7

commit fix

50f018a

separate commit from outlier rejection dbscan

3d7592a

fix track saver after rebase. Observation type change.

76d526e

comment clean up for track

b4a5df2

Follow proper unitest file structure. Change the non-unit-test file t…

2e8c7a0

…o proper name.

move json serialization to measurement cluster.

c4a9673

clean up

20011e7

better json serialization

b76d8bc

MultyXu marked this pull request as ready for review September 3, 2026 14:12

MultyXu mentioned this pull request Sep 3, 2026

Feature/track saving #42

Closed

MultyXu requested a review from nathanhhughes September 3, 2026 14:13

nathanhhughes reviewed Sep 7, 2026

View reviewed changes

Navigation Menu

Uh oh!

Feature/track saving - #43

Feature/track saving#43
MultyXu wants to merge 16 commits into
dcistfrom
feature/track_saving

MultyXu commented Sep 3, 2026

Uh oh!

nathanhhughes left a comment

Uh oh!

nathanhhughes Sep 6, 2026

Uh oh!

nathanhhughes Sep 6, 2026

Uh oh!

nathanhhughes Sep 6, 2026

Uh oh!

nathanhhughes Sep 6, 2026

Uh oh!

nathanhhughes Sep 7, 2026

Uh oh!

nathanhhughes Sep 7, 2026

Uh oh!

nathanhhughes Sep 7, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

		* @return The set of image pixels the points project onto (points that fail to project, e.g.
		* behind the camera, are omitted).

Uh oh!

Conversation

MultyXu commented Sep 3, 2026

Uh oh!

nathanhhughes left a comment

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 6, 2026

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 6, 2026

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 6, 2026

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 6, 2026

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 7, 2026

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 7, 2026

Choose a reason for hiding this comment

Uh oh!

nathanhhughes Sep 7, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants